Rev 462 | Rev 513 | 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" |
391 | Brean | 3 | |
4 | /** |
||
392 | Brean | 5 | * Constructor that gets a communication instance |
6 | */ |
||
440 | Brean | 7 | Handler::Handler(Communication * com, KopterData * data) { |
392 | Brean | 8 | this->com = com; |
440 | Brean | 9 | this->data = data; |
392 | Brean | 10 | } |
11 | |||
399 | Brean | 12 | //-------------FlightCtrl commands-------------------- |
392 | Brean | 13 | /** |
399 | Brean | 14 | * read settings from FlightCtrl (settings index 0x00-0x05) |
15 | */ |
||
16 | void Handler::get_flightctrl_settings(int index) { |
||
17 | char tx_data[2] = {index, 0}; |
||
18 | com->send_cmd('q', ADDRESS_FC, tx_data, 1, true); |
||
19 | } |
||
20 | |||
21 | /** |
||
22 | * write settings to FlightCtrl |
||
23 | */ |
||
24 | void Handler::set_flightctrl_settings(char * tx_data) { |
||
25 | com->send_cmd('s', ADDRESS_FC, tx_data, MaxParameter+2, true); |
||
26 | } |
||
27 | |||
28 | /** |
||
29 | * test one or more motors |
||
30 | */ |
||
442 | Brean | 31 | void Handler::motor_test(sMotorData motor) { |
32 | char tx_data[MAX_MOTORS]; |
||
33 | for (int z = 0; z<MAX_MOTORS; z++) |
||
399 | Brean | 34 | { |
442 | Brean | 35 | tx_data[z] = motor.desired_speed[z]; |
399 | Brean | 36 | } |
442 | Brean | 37 | com->send_cmd('t', ADDRESS_FC, tx_data, MAX_MOTORS, false); |
399 | Brean | 38 | } |
39 | |||
440 | Brean | 40 | void Handler::reset_motor() { |
442 | Brean | 41 | for (int z = 0; z<MAX_MOTORS; z++) |
440 | Brean | 42 | { |
442 | Brean | 43 | data->motor.desired_speed[z] = 0; |
440 | Brean | 44 | } |
45 | |||
442 | Brean | 46 | motor_test(data->motor); |
440 | Brean | 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 | /** |
||
449 | Brean | 86 | * send a waypoint to the NaviCtrl (the MikroKopter will fly to the position emidiately) |
399 | Brean | 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 | */ |
||
500 | Brean | 193 | void Handler::receive_data(char * incomming, int length) { |
194 | if (incomming[0] != '#') |
||
195 | FlightLog::error("this frame is not correct"); |
||
196 | FlightLog::info(incomming); |
||
197 | return; |
||
198 | int hardwareID = incomming[1] - 'a'; |
||
199 | |||
200 | //The cmd is also known as ID-Byte (or ID for short) in the wiki-dokumentation |
||
201 | char cmd = incomming[2]; |
||
202 | //decode data |
||
203 | unsigned char data[150]; |
||
204 | |||
205 | Parser::decode64(incomming, length, data, 3); |
||
206 | |||
393 | Brean | 207 | switch(hardwareID) |
208 | { |
||
209 | case ADDRESS_FC : |
||
449 | Brean | 210 | switch(cmd) |
393 | Brean | 211 | { |
212 | // Motor-Mixer |
||
213 | case 'N' : |
||
449 | Brean | 214 | //if (Parser::decode64(RX)) |
215 | //{ |
||
397 | Brean | 216 | com->stop_resend(); |
449 | Brean | 217 | //decoded data |
500 | Brean | 218 | FlightLog::info("received motortest values from FlightCtrl"); |
449 | Brean | 219 | if (data[0] == VERSION_MIXER) |
393 | Brean | 220 | { |
396 | Brean | 221 | //f_MotorMixer->set_MotorConfig(RX); |
393 | Brean | 222 | } |
449 | Brean | 223 | //} |
224 | break; |
||
393 | Brean | 225 | // Motor-Mixer Schreib-Bestätigung |
226 | case 'M' : |
||
449 | Brean | 227 | com->stop_resend(); |
228 | |||
229 | if (data[0] == 1) |
||
393 | Brean | 230 | { |
500 | Brean | 231 | FlightLog::info("motor values written to FlightCtrl."); |
449 | Brean | 232 | //lb_Status->setText(tr("MotorMixer-Daten in FC geschrieben.")); |
500 | Brean | 233 | } else { |
234 | FlightLog::warning("could not write motor values to FlightCtrl!"); |
||
393 | Brean | 235 | } |
449 | Brean | 236 | break; |
393 | Brean | 237 | |
238 | // Stick-Belegung der Fernsteuerung |
||
239 | case 'P' : // DONE 0.71g |
||
500 | Brean | 240 | FlightLog::info("received stick-settings from FlightCtrl:"); |
449 | Brean | 241 | /*f_Settings->pb_K1->setValue(Parser::dataToInt(RX.decode, 2,true)); |
242 | f_Settings->pb_K2->setValue(Parser::dataToInt(RX.decode, 4,true)); |
||
243 | f_Settings->pb_K3->setValue(Parser::dataToInt(RX.decode, 6,true)); |
||
244 | f_Settings->pb_K4->setValue(Parser::dataToInt(RX.decode, 8,true)); |
||
245 | f_Settings->pb_K5->setValue(Parser::dataToInt(RX.decode, 10 ,true)); |
||
246 | f_Settings->pb_K6->setValue(Parser::dataToInt(RX.decode, 12,true)); |
||
247 | f_Settings->pb_K7->setValue(Parser::dataToInt(RX.decode, 14,true)); |
||
248 | f_Settings->pb_K8->setValue(Parser::dataToInt(RX.decode, 16,true));*/ |
||
249 | break; |
||
393 | Brean | 250 | // Settings lesen |
251 | case 'Q' : // DONE 0.71g |
||
449 | Brean | 252 | com->stop_resend(); |
500 | Brean | 253 | FlightLog::info("received settings from FlightCtrl"); |
449 | Brean | 254 | if (data[1] == VERSION_SETTINGS) |
393 | Brean | 255 | { |
449 | Brean | 256 | int Settings_ID = data[0]; |
257 | /*for (int a = 0; a < MaxParameter; a++) |
||
393 | Brean | 258 | { |
449 | Brean | 259 | FCSettings[a] = RX.decode[a + 2]; |
393 | Brean | 260 | } |
449 | Brean | 261 | f_Settings->show_FCSettings(Settings_ID, FCSettings); |
262 | f_Settings->pb_Read->setEnabled(true); |
||
263 | f_Settings->pb_Write->setEnabled(true);*/ |
||
264 | } |
||
265 | else |
||
266 | { |
||
500 | Brean | 267 | FlightLog::error("wrong FlightCtrl version"); |
449 | Brean | 268 | /*f_Settings->pb_Read->setDisabled(true); |
269 | f_Settings->pb_Write->setDisabled(true); |
||
393 | Brean | 270 | |
449 | Brean | 271 | QString name = QString("Versionen inkompatibel.\n") + |
272 | QString("Version von GroundStation benoetigt: ") + |
||
273 | QString(VERSION_SETTINGS) + |
||
274 | QString("\nVersion auf der FlightCtrl: ") + |
||
275 | QString(RX.decode[1]) + |
||
276 | QString("\nParameterbearbeitung nicht moeglich."); |
||
277 | QMessageBox::warning(this, QA_NAME, |
||
278 | name, QMessageBox::Ok);*/ |
||
393 | Brean | 279 | } |
449 | Brean | 280 | break; |
396 | Brean | 281 | // Settings written |
393 | Brean | 282 | case 'S' : // DONE 0.71g |
397 | Brean | 283 | com->stop_resend(); |
500 | Brean | 284 | FlightLog::info("settings written successfully to FlightCtrl"); |
396 | Brean | 285 | //TODO: QMessagebox("settings written successful") ? |
449 | Brean | 286 | break; |
393 | Brean | 287 | } |
288 | |||
289 | case ADDRESS_NC : |
||
449 | Brean | 290 | switch(cmd) |
393 | Brean | 291 | { |
292 | // Navigationsdaten |
||
293 | case 'O' : // NOT DONE 0.12h |
||
449 | Brean | 294 | //new_NaviData(RX); |
500 | Brean | 295 | FlightLog::info("received navigation data from NaviCtrl"); |
449 | Brean | 296 | break; |
393 | Brean | 297 | } |
298 | // case ADDRESS_MK3MAG : |
||
299 | |||
300 | default : |
||
449 | Brean | 301 | switch(cmd) |
393 | Brean | 302 | { |
303 | // LCD-Anzeige |
||
304 | case 'L' : // DONE 0.71g |
||
449 | Brean | 305 | com->stop_resend(); |
500 | Brean | 306 | FlightLog::info("received LCD page."); |
449 | Brean | 307 | /*int LCD[150]; |
308 | memcpy(LCD,RX.decode, sizeof(RX.decode)); |
||
393 | Brean | 309 | |
449 | Brean | 310 | f_LCD->show_Data(LCD); |
393 | Brean | 311 | |
449 | Brean | 312 | LCD_Page = RX.decode[0]; |
313 | LCD_MAX_Page = RX.decode[1]; |
||
314 | */ |
||
315 | break; |
||
393 | Brean | 316 | // Analoglabels |
317 | case 'A' : // DONE 0.71g |
||
449 | Brean | 318 | com->stop_resend(); |
500 | Brean | 319 | FlightLog::info("received analog labels"); |
449 | Brean | 320 | //check position |
321 | if (data[0] != 31) { |
||
322 | /* |
||
323 | Settings->Analog1.Label[Position] = ToolBox::dataToQString(RX.decode,1,17).trimmed(); |
||
324 | if (Settings->Analog1.Label[Position] == "") |
||
393 | Brean | 325 | { |
449 | Brean | 326 | Settings->Analog1.Label[Position] = "A-" + QString("%1").arg(Position); |
393 | Brean | 327 | } |
449 | Brean | 328 | Position ++; |
329 | TX_Data[0] = Position; |
||
330 | o_Connection->send_Cmd('a', ADDRESS_ALL, TX_Data, 1, true);*/ |
||
331 | } else { |
||
332 | /* |
||
333 | for (int a = 0; a < MaxAnalog; a++) |
||
393 | Brean | 334 | { |
449 | Brean | 335 | lb_Analog[a]->setText(Settings->Analog1.Label[a]); |
393 | Brean | 336 | } |
449 | Brean | 337 | Settings->Analog1.Version = QString(Mode.Version); |
338 | Settings->write_Settings_AnalogLabels(HardwareID); |
||
339 | config_Plot();*/ |
||
393 | Brean | 340 | } |
449 | Brean | 341 | break; |
393 | Brean | 342 | // Debug-Daten |
343 | case 'D' : // DONE 0.71g |
||
500 | Brean | 344 | FlightLog::info("received debug data:"); |
449 | Brean | 345 | for (int i = 0; i < MaxAnalog; i++) { |
500 | Brean | 346 | /* |
347 | Parser::dataToInt((char *)data, (i * 2) + 2, false); |
||
348 | */ |
||
393 | Brean | 349 | } |
449 | Brean | 350 | //show_DebugData(); |
351 | break; |
||
393 | Brean | 352 | // Version |
353 | case 'V' : // DONE 0.71h |
||
449 | Brean | 354 | com->stop_resend(); |
500 | Brean | 355 | FlightLog::info("received version"); |
449 | Brean | 356 | /* |
357 | Mode.ID = HardwareID; |
||
358 | Mode.VERSION_MAJOR = RX.decode[0]; |
||
359 | Mode.VERSION_MINOR = RX.decode[1]; |
||
360 | Mode.VERSION_PATCH = RX.decode[4]; |
||
361 | Mode.VERSION_SERIAL_MAJOR = RX.decode[2]; |
||
362 | Mode.VERSION_SERIAL_MINOR = RX.decode[3]; |
||
393 | Brean | 363 | |
449 | Brean | 364 | Mode.Hardware = HardwareType[Mode.ID]; |
365 | //TODO: Funktion im Handler get_version() oder sowas |
||
366 | QString version = QString("%1").arg(RX.decode[0]) + "." + |
||
367 | QString("%1").arg(RX.decode[1]) + |
||
368 | QString(RX.decode[4] + 'a'); |
||
369 | Mode.Version = version.toLatin1().data; |
||
370 | setWindowTitle(QA_NAME + " v" + QA_VERSION + " - " + |
||
371 | Mode.Hardware + " " + |
||
372 | Mode.Version); |
||
393 | Brean | 373 | |
449 | Brean | 374 | if (Mode.VERSION_SERIAL_MAJOR != VERSION_SERIAL_MAJOR) |
375 | { |
||
393 | Brean | 376 | // AllowSend = false; |
449 | Brean | 377 | QMessageBox::warning(this, QA_NAME, |
378 | tr("Serielles Protokoll Inkompatibel. \nBitte neue Programmversion installieren,"), QMessageBox::Ok); |
||
379 | } |
||
393 | Brean | 380 | |
449 | Brean | 381 | if (ac_NoDebug->isChecked()) |
382 | { |
||
383 | TX_Data[0] = 0; |
||
384 | } |
||
385 | else |
||
386 | if (ac_FastDebug->isChecked()) |
||
387 | { |
||
388 | TX_Data[0] = Settings->Data.Debug_Fast / 10; |
||
389 | } |
||
390 | else |
||
391 | { |
||
392 | TX_Data[0] = Settings->Data.Debug_Slow / 10; |
||
393 | } |
||
394 | |||
395 | o_Connection->send_Cmd('d', ADDRESS_ALL, TX_Data, 1, false); |
||
396 | |||
397 | // Wenn MK3MAG dann andauernd Daten neu anfragen. |
||
398 | if (Mode.ID == ADDRESS_MK3MAG) |
||
399 | { |
||
400 | TickerEvent[3] = true; |
||
401 | rb_SelMag->setChecked(true); |
||
402 | } |
||
403 | |||
404 | // Wenn NaviCtrl dann hier. |
||
405 | if (Mode.ID == ADDRESS_NC) |
||
406 | { |
||
407 | rb_SelNC->setChecked(true); |
||
408 | |||
409 | if (ac_NoNavi->isChecked()) |
||
393 | Brean | 410 | { |
411 | TX_Data[0] = 0; |
||
412 | } |
||
413 | else |
||
449 | Brean | 414 | if (ac_FastNavi->isChecked()) |
393 | Brean | 415 | { |
449 | Brean | 416 | TX_Data[0] = Settings->Data.Navi_Fast / 10; |
393 | Brean | 417 | } |
418 | else |
||
419 | { |
||
449 | Brean | 420 | TX_Data[0] = Settings->Data.Navi_Slow / 10; |
393 | Brean | 421 | } |
422 | |||
449 | Brean | 423 | o_Connection->send_Cmd('o', ADDRESS_NC, TX_Data, 1, false); |
424 | } |
||
393 | Brean | 425 | |
426 | |||
449 | Brean | 427 | // Wenn FlightCtrl dann Settings abfragen. |
428 | if (Mode.ID == ADDRESS_FC) |
||
429 | { |
||
430 | rb_SelFC->setChecked(true); |
||
393 | Brean | 431 | { |
449 | Brean | 432 | TX_Data[0] = 0xff; |
433 | TX_Data[1] = 0; |
||
393 | Brean | 434 | |
449 | Brean | 435 | // DEP: Raus wenn Resend implementiert. |
393 | Brean | 436 | // ToolBox::Wait(SLEEP); |
449 | Brean | 437 | o_Connection->send_Cmd('q', ADDRESS_FC, TX_Data, 1, true); |
438 | qDebug("FC - Get Settings"); |
||
393 | Brean | 439 | } |
449 | Brean | 440 | } |
441 | // Wenn nicht Lesen und Schreiben der Settings deaktivieren. |
||
442 | else |
||
443 | { |
||
444 | f_Settings->pb_Read->setDisabled(true); |
||
445 | f_Settings->pb_Write->setDisabled(true); |
||
446 | } |
||
393 | Brean | 447 | |
449 | Brean | 448 | Settings->read_Settings_Analog(HardwareID); |
449 | Settings->read_Settings_AnalogLabels(HardwareID); |
||
393 | Brean | 450 | |
449 | Brean | 451 | if (Settings->Analog1.Version != QString(Mode.Version)) |
452 | { |
||
453 | lb_Status->setText(tr("Analoglabel-Version unterschiedlich. Lese Analoglabels neu aus.")); |
||
454 | slot_ac_GetLabels(); |
||
393 | Brean | 455 | } |
449 | Brean | 456 | else |
457 | for (int a = 0; a < MaxAnalog; a++) |
||
458 | { |
||
459 | lb_Analog[a]->setText(Settings->Analog1.Label[a]); |
||
460 | } |
||
461 | config_Plot();*/ |
||
462 | break; |
||
393 | Brean | 463 | } |
464 | } |
||
391 | Brean | 465 | } |