Subversion Repositories Projects

Rev

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