Subversion Repositories Projects

Rev

Rev 397 | Rev 440 | Go to most recent revision | Details | Compare with Previous | Last modification | View Log | RSS feed

Rev Author Line No. Line
391 Brean 1
#include<Handler.h>
2
 
3
/**
392 Brean 4
 * Constructor that gets a communication instance
5
 */
6
Handler::Handler(Communication * com) {
7
    this->com = com;
8
}
9
 
399 Brean 10
//-------------FlightCtrl commands--------------------
392 Brean 11
/**
399 Brean 12
 * read settings from FlightCtrl (settings index 0x00-0x05)
13
 */
14
void Handler::get_flightctrl_settings(int index) {
15
    char tx_data[2] = {index, 0};
16
    com->send_cmd('q', ADDRESS_FC, tx_data, 1, true);
17
}
18
 
19
/**
20
 * write settings to FlightCtrl
21
 */
22
void Handler::set_flightctrl_settings(char * tx_data) {
23
    com->send_cmd('s', ADDRESS_FC, tx_data, MaxParameter+2, true);
24
}
25
 
26
/**
27
 * test one or more motors
28
 */
29
void Handler::motor_test(sMotor motor) {
30
    char tx_data[12];
31
    for (int z = 0; z<12; z++)
32
    {
33
        tx_data[z] = motor.Speed[z];
34
    }
35
    com->send_cmd('t', ADDRESS_FC, tx_data, 12, false);
36
}
37
 
38
/**
391 Brean 39
 * read mixer values from FlightCtrl
40
 */
41
void Handler::read_mixer() {
396 Brean 42
    char tx_data[1] = {0};
43
    //com->log("read motor mixer");
397 Brean 44
    com->send_cmd('n', ADDRESS_FC, tx_data, 1, true);
393 Brean 45
}
46
 
399 Brean 47
/**
48
 * write motor mixer values to FlightCtrl
49
 */
50
void Handler::write_mixer(char * tx_data, int length) {
51
    com->send_cmd('m', ADDRESS_FC, tx_data, length, true);
52
}
53
 
396 Brean 54
void Handler::get_motor_config() {
55
}
56
 
399 Brean 57
//-------------NaviCtrl commands--------------------
58
/**
59
 * set debug values for NaviCtrl
60
 */
61
void Handler::set_navictrl_debug(int speed) {
62
    char tx_data[1] = { speed };
63
    com->send_cmd('o', ADDRESS_NC, tx_data, 1, false);
64
}
65
 
66
/**
67
 * stop debug for NaviCtrl
68
 */
69
void Handler::stop_navictrl_debug() {
70
    set_navictrl_debug(0);
71
}
72
 
73
/**
74
 * send a waypoint to the NaviCtrl (the copter will fly to the position emidiately)
75
 */
76
void Handler::send_waypoint(Waypoint_t desired_pos) {
77
    com->send_cmd('s', ADDRESS_NC, (char *)&desired_pos, sizeof(desired_pos), false);
78
}
79
 
80
/**
81
 * add waypoint to waypoint list
82
 */
83
void Handler::add_waypoint(Waypoint_t wp) {
84
    com->send_cmd('w', ADDRESS_NC, (char *)&wp, sizeof(wp), false);
85
}
86
 
87
/**
88
 * clear waypoint list on MK
89
 */
90
void Handler::delete_waypoints() {
91
    Waypoint_t wp;
92
    wp.Position.Status = INVALID;
93
    send_waypoint(wp);
94
}
95
//-------------switch between Hardware--------------------
96
void Handler::switch_navictrl() {
97
    char tx_data[6] = { 0x1B, 0x1B, 0x55, 0xAA, 0x00, '\r'};
98
    com->send_cmd('#', ADDRESS_NC, tx_data, 6, false);
99
}
100
 
101
void Handler::switch_flightctrl() {
102
    char tx_data[1] = { 0 };
103
    com->send_cmd('u', ADDRESS_NC, tx_data, 1, false);
104
}
105
 
106
void Handler::switch_mk3mag() {
107
    char tx_data[1] = { 1 };
108
    com->send_cmd('u', ADDRESS_NC, tx_data, 1, false);
109
}
110
 
111
//-------------commands for MK3MAG-----------------
112
 
113
 
114
//-------------commands for all--------------------
115
 
116
/**
117
 * set debug values for all components
118
 */
119
void Handler::set_all_debug(int speed) {
120
    char tx_data[1] = { speed };
121
    com->send_cmd('d', ADDRESS_ALL, tx_data, 1, false);
122
}
123
 
124
/**
125
 * stop debug for all components
126
 */
127
void Handler::stop_all_debug() {
128
    set_all_debug(0);
129
}
130
 
131
/**
132
 * get all analog labels
133
 */
134
void Handler::get_analog() {
135
    char tx_data[1] = { 0 };
136
    com->send_cmd('a', ADDRESS_ALL, tx_data, 1, true);
137
}
138
 
139
/**
140
 * get values from LCD / show LCD
141
 */
142
void Handler::show_lcd() {
143
    char tx_data[1] = {0};
144
    com->send_cmd('l', ADDRESS_ALL, tx_data, 1, true);
145
}
146
 
147
/**
148
 * got to next LCD Page
149
 */
150
void Handler::lcd_up() {
151
    char tx_data[2] = { 0, 0 };
152
    if (lcd_cur != lcd_max)
153
        tx_data[0] = lcd_cur+1;
154
    com->send_cmd('l', ADDRESS_ALL, tx_data, 1, true);
155
}
156
 
157
/**
158
 * got to previous LCD Page
159
 */
160
void Handler::lcd_down() {
161
    char tx_data[2] = { 0, 0 };
162
    if (lcd_cur != 0)
163
        tx_data[0] = lcd_cur-1;
164
    com->send_cmd('l', ADDRESS_ALL, tx_data, 1, true);
165
}
166
 
167
void Handler::get_version() {
168
    //TODO: Check if is this correct or do we need data from switch_...
169
    char tx_data[1] = { 0 };
170
    com->send_cmd('v', ADDRESS_ALL, tx_data, 0, true);
171
}
172
 
173
void Handler::get_ppm_channels() {
174
    char tx_data[1] = { 0 };
175
    com->send_cmd('p', ADDRESS_ALL, tx_data, 0, false);
176
}
177
 
178
/**
179
 * receive data
180
 */
393 Brean 181
void Handler::receive_data(sRxData RX) {
182
    //extract hardware ID from received Data
183
    int hardwareID = RX.input[1] - 'a';
184
    switch(hardwareID)
185
    {
186
        case ADDRESS_FC :
187
            switch(RX.input[2])
188
            {
189
                // Motor-Mixer
190
                case 'N' :
191
                    if (Parser::decode64(RX))
192
                    {
397 Brean 193
                        com->stop_resend();
393 Brean 194
 
195
                        if (RX.decode[0] == VERSION_MIXER)
196
                        {
396 Brean 197
                            //f_MotorMixer->set_MotorConfig(RX);
393 Brean 198
                        }
199
                    }
200
                break;
201
                // Motor-Mixer Schreib-Best├Ątigung
202
                case 'M' :
203
                    if (Parser::decode64(RX))
204
                    {
397 Brean 205
                        com->stop_resend();
393 Brean 206
 
207
                        if (RX.decode[0] == 1)
208
                        {
396 Brean 209
                            //lb_Status->setText(tr("MotorMixer-Daten in FC geschrieben."));
393 Brean 210
                        }
211
                    }
212
                break;
213
 
214
                // Stick-Belegung der Fernsteuerung
215
                case 'P' : // DONE 0.71g
216
                    if (Parser::decode64(RX))
217
                    {
396 Brean 218
                        /*f_Settings->pb_K1->setValue(Parser::dataToInt(RX.decode,  2,true));
393 Brean 219
                        f_Settings->pb_K2->setValue(Parser::dataToInt(RX.decode,  4,true));
220
                        f_Settings->pb_K3->setValue(Parser::dataToInt(RX.decode,  6,true));
221
                        f_Settings->pb_K4->setValue(Parser::dataToInt(RX.decode,  8,true));
222
                        f_Settings->pb_K5->setValue(Parser::dataToInt(RX.decode, 10 ,true));
223
                        f_Settings->pb_K6->setValue(Parser::dataToInt(RX.decode, 12,true));
224
                        f_Settings->pb_K7->setValue(Parser::dataToInt(RX.decode, 14,true));
396 Brean 225
                        f_Settings->pb_K8->setValue(Parser::dataToInt(RX.decode, 16,true));*/
393 Brean 226
                    }
227
                break;
228
                // Settings lesen
229
                case 'Q' : // DONE 0.71g
230
                    if (Parser::decode64(RX))
231
                    {
397 Brean 232
                        com->stop_resend();
393 Brean 233
 
234
                        if (RX.decode[1] == VERSION_SETTINGS)
235
                        {
236
                            int Settings_ID = RX.decode[0];
396 Brean 237
                            /*for (int a = 0; a < MaxParameter; a++)
393 Brean 238
                            {
239
                                FCSettings[a] = RX.decode[a + 2];
240
                            }
241
                            f_Settings->show_FCSettings(Settings_ID, FCSettings);
242
                            f_Settings->pb_Read->setEnabled(true);
396 Brean 243
                            f_Settings->pb_Write->setEnabled(true);*/
393 Brean 244
                        }
245
                        else
246
                        {
396 Brean 247
                            /*f_Settings->pb_Read->setDisabled(true);
393 Brean 248
                            f_Settings->pb_Write->setDisabled(true);
249
 
250
                            QString name = QString("Versionen inkompatibel.\n") +
251
                                      QString("Version von GroundStation benoetigt: ") +
252
                                      QString(VERSION_SETTINGS) +
253
                                      QString("\nVersion auf der FlightCtrl: ") +
254
                                      QString(RX.decode[1]) +
255
                                      QString("\nParameterbearbeitung nicht moeglich.");
256
                            QMessageBox::warning(this, QA_NAME,
396 Brean 257
                                   name, QMessageBox::Ok);*/
393 Brean 258
                        }
259
                    }
260
                break;
396 Brean 261
                // Settings written
393 Brean 262
                case 'S' : // DONE 0.71g
397 Brean 263
                    com->stop_resend();
396 Brean 264
                    //TODO: QMessagebox("settings written successful") ?
393 Brean 265
                break;
266
            }
267
 
268
        case ADDRESS_NC :
269
            switch(RX.input[2])
270
            {
271
                // Navigationsdaten
272
                case 'O' : // NOT DONE 0.12h
273
                    if (Parser::decode64(RX))
274
                    {
396 Brean 275
                        //new_NaviData(RX);
393 Brean 276
                    }
277
                break;
278
            }
279
//        case ADDRESS_MK3MAG :
280
 
281
        default :
282
            switch(RX.input[2])
283
            {
284
                // LCD-Anzeige
285
                case 'L' : // DONE 0.71g
286
                    if (Parser::decode64(RX))
287
                    {
397 Brean 288
                        com->stop_resend();
393 Brean 289
 
396 Brean 290
                        /*int LCD[150];
393 Brean 291
                        memcpy(LCD,RX.decode, sizeof(RX.decode));
292
 
293
                        f_LCD->show_Data(LCD);
294
 
295
                        LCD_Page     = RX.decode[0];
296
                        LCD_MAX_Page = RX.decode[1];
396 Brean 297
                        */
393 Brean 298
                    }
299
                break;
300
                // Analoglabels
301
                case 'A' : // DONE 0.71g
302
                    if (Parser::decode64(RX))
303
                    {
397 Brean 304
                        com->stop_resend();
393 Brean 305
 
306
                        int Position = RX.decode[0];
307
                        if (Position != 31)
308
                        {
396 Brean 309
                            /*
393 Brean 310
                            Settings->Analog1.Label[Position] = ToolBox::dataToQString(RX.decode,1,17).trimmed();
311
                            if (Settings->Analog1.Label[Position] == "")
312
                            {
313
                                Settings->Analog1.Label[Position] = "A-" + QString("%1").arg(Position);
314
                            }
315
                            Position ++;
316
                            TX_Data[0] = Position;
396 Brean 317
                            o_Connection->send_Cmd('a', ADDRESS_ALL, TX_Data, 1, true);*/
393 Brean 318
                        }
319
                        if (Position == 31)
320
                        {
396 Brean 321
                            /*
393 Brean 322
                            for (int a = 0; a < MaxAnalog; a++)
323
                            {
324
                                lb_Analog[a]->setText(Settings->Analog1.Label[a]);
325
                            }
326
                            Settings->Analog1.Version = QString(Mode.Version);
327
                            Settings->write_Settings_AnalogLabels(HardwareID);
396 Brean 328
                            config_Plot();*/
393 Brean 329
                        }
330
                    }
331
                break;
332
                // Debug-Daten
333
                case 'D' : // DONE 0.71g
334
                    if (Parser::decode64(RX))
335
                    {
336
                        for (int i = 0; i < MaxAnalog; i++)
337
                        {
396 Brean 338
                            //AnalogData[i] = Parser::dataToInt(RX.decode, (i * 2) + 2);
393 Brean 339
                        }
396 Brean 340
                        //show_DebugData();
393 Brean 341
                    }
342
                break;
343
                // Version
344
                case 'V' : // DONE 0.71h
345
                    if (Parser::decode64(RX))
346
                    {
397 Brean 347
                        com->stop_resend();
396 Brean 348
                        /*
393 Brean 349
                        Mode.ID            = HardwareID;
350
                        Mode.VERSION_MAJOR = RX.decode[0];
351
                        Mode.VERSION_MINOR = RX.decode[1];
352
                        Mode.VERSION_PATCH = RX.decode[4];
353
                        Mode.VERSION_SERIAL_MAJOR = RX.decode[2];
354
                        Mode.VERSION_SERIAL_MINOR = RX.decode[3];
355
 
356
                        Mode.Hardware   = HardwareType[Mode.ID];
357
                        //TODO: Funktion im Handler get_version() oder sowas
358
                        QString version = QString("%1").arg(RX.decode[0]) + "." +
359
                                          QString("%1").arg(RX.decode[1]) +
360
                                          QString(RX.decode[4] + 'a');
361
                        Mode.Version = version.toLatin1().data;
362
                        setWindowTitle(QA_NAME + " v" + QA_VERSION + " - " +
363
                                 Mode.Hardware + " " +
364
                                 Mode.Version);
365
 
366
                        if (Mode.VERSION_SERIAL_MAJOR != VERSION_SERIAL_MAJOR)
367
                        {
368
//                                AllowSend = false;
369
                                QMessageBox::warning(this, QA_NAME,
370
                                   tr("Serielles Protokoll Inkompatibel. \nBitte neue Programmversion installieren,"), QMessageBox::Ok);
371
                        }
372
 
373
                        if (ac_NoDebug->isChecked())
374
                        {
375
                            TX_Data[0] = 0;
376
                        }
377
                        else
378
                        if (ac_FastDebug->isChecked())
379
                        {
380
                            TX_Data[0] = Settings->Data.Debug_Fast / 10;
381
                        }
382
                        else
383
                        {
384
                            TX_Data[0] = Settings->Data.Debug_Slow / 10;
385
                        }
386
 
387
                        o_Connection->send_Cmd('d', ADDRESS_ALL, TX_Data, 1, false);
388
 
389
                        // Wenn MK3MAG dann andauernd Daten neu anfragen.
390
                        if (Mode.ID == ADDRESS_MK3MAG)
391
                        {
392
                            TickerEvent[3] = true;
393
                            rb_SelMag->setChecked(true);
394
                        }
395
 
396
                        // Wenn NaviCtrl dann hier.
397
                        if (Mode.ID == ADDRESS_NC)
398
                        {
399
                            rb_SelNC->setChecked(true);
400
 
401
                            if (ac_NoNavi->isChecked())
402
                            {
403
                                TX_Data[0] = 0;
404
                            }
405
                            else
406
                            if (ac_FastNavi->isChecked())
407
                            {
408
                                TX_Data[0] = Settings->Data.Navi_Fast / 10;
409
                            }
410
                            else
411
                            {
412
                                TX_Data[0] = Settings->Data.Navi_Slow / 10;
413
                            }
414
 
415
                            o_Connection->send_Cmd('o', ADDRESS_NC, TX_Data, 1, false);
416
                        }
417
 
418
 
419
                        // Wenn FlightCtrl dann Settings abfragen.
420
                        if (Mode.ID == ADDRESS_FC)
421
                        {
422
                            rb_SelFC->setChecked(true);
423
                            {
424
                                TX_Data[0] = 0xff;
425
                                TX_Data[1] = 0;
426
 
427
                                // DEP: Raus wenn Resend implementiert.
428
//                                ToolBox::Wait(SLEEP);
429
                                o_Connection->send_Cmd('q', ADDRESS_FC, TX_Data, 1, true);
430
                                qDebug("FC - Get Settings");
431
                            }
432
                        }
433
                        // Wenn nicht Lesen und Schreiben der Settings deaktivieren.
434
                        else
435
                        {
436
                                f_Settings->pb_Read->setDisabled(true);
437
                                f_Settings->pb_Write->setDisabled(true);
438
                        }
439
 
440
                        Settings->read_Settings_Analog(HardwareID);
441
                        Settings->read_Settings_AnalogLabels(HardwareID);
442
 
443
                        if (Settings->Analog1.Version != QString(Mode.Version))
444
                        {
445
                            lb_Status->setText(tr("Analoglabel-Version unterschiedlich. Lese Analoglabels neu aus."));
446
                            slot_ac_GetLabels();
447
                        }
448
                        else
449
                        for (int a = 0; a < MaxAnalog; a++)
450
                        {
451
                            lb_Analog[a]->setText(Settings->Analog1.Label[a]);
452
                        }
396 Brean 453
                        config_Plot();*/
393 Brean 454
                    }
455
                break;
456
            }
457
    }
391 Brean 458
}