Subversion Repositories Projects

Rev

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