Subversion Repositories Projects

Rev

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

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