Subversion Repositories Projects

Rev

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