1,5 → 1,7 |
#include "Handler.h" |
#include "FlightLog.h" |
#include <sstream> |
#include <string> |
|
/** |
* Constructor that gets a communication instance |
14,6 → 16,7 |
* read settings from FlightCtrl (settings index 0x00-0x05) |
*/ |
void Handler::get_flightctrl_settings(int index) { |
FlightLog::info_FC("getting FlightCtrl settings"); |
char tx_data[2] = {index, 0}; |
com->send_cmd('q', ADDRESS_FC, tx_data, 1, true); |
} |
22,6 → 25,7 |
* write settings to FlightCtrl |
*/ |
void Handler::set_flightctrl_settings(char * tx_data) { |
FlightLog::info_FC("setting FlightCtrl settings"); |
com->send_cmd('s', ADDRESS_FC, tx_data, MaxParameter+2, true); |
} |
|
34,6 → 38,7 |
{ |
tx_data[z] = motor.desired_speed[z]; |
} |
FlightLog::info_FC("testing motor"); |
com->send_cmd('t', ADDRESS_FC, tx_data, MAX_MOTORS, false); |
} |
|
50,6 → 55,7 |
* read mixer values from FlightCtrl |
*/ |
void Handler::read_motor_mixer() { |
FlightLog::info_FC("reading motor mixer values"); |
char tx_data[1] = {0}; |
//com->log("read motor mixer"); |
com->send_cmd('n', ADDRESS_FC, tx_data, 1, true); |
59,6 → 65,7 |
* write motor mixer values to FlightCtrl |
*/ |
void Handler::write_motor_mixer(char * tx_data, int length) { |
FlightLog::info_FC("setting motor mixer values"); |
com->send_cmd('m', ADDRESS_FC, tx_data, length, true); |
} |
|
71,6 → 78,7 |
* set debug values for NaviCtrl |
*/ |
void Handler::set_navictrl_debug(int speed) { |
FlightLog::info_NC("setting NaviCtrl debug speed"); |
char tx_data[1] = { speed }; |
com->send_cmd('o', ADDRESS_NC, tx_data, 1, false); |
} |
86,6 → 94,7 |
* send a waypoint to the NaviCtrl (the MikroKopter will fly to the position emidiately) |
*/ |
void Handler::send_waypoint(Waypoint_t desired_pos) { |
FlightLog::info_GPS("sending Waypoint"); |
com->send_cmd('s', ADDRESS_NC, (char *)&desired_pos, sizeof(desired_pos), false); |
} |
|
93,6 → 102,7 |
* add waypoint to waypoint list |
*/ |
void Handler::add_waypoint(Waypoint_t wp) { |
FlightLog::info_GPS("adding Waypoint"); |
com->send_cmd('w', ADDRESS_NC, (char *)&wp, sizeof(wp), false); |
} |
|
106,16 → 116,19 |
} |
//-------------switch between Hardware-------------------- |
void Handler::switch_navictrl() { |
FlightLog::info("switching to NaviCtrl"); |
char tx_data[6] = { 0x1B, 0x1B, 0x55, 0xAA, 0x00, '\r'}; |
com->send_cmd('#', ADDRESS_NC, tx_data, 6, false); |
} |
|
void Handler::switch_flightctrl() { |
FlightLog::info("switching to FlightCtrl"); |
char tx_data[1] = { 0 }; |
com->send_cmd('u', ADDRESS_NC, tx_data, 1, false); |
} |
|
void Handler::switch_mk3mag() { |
FlightLog::info("switching to MK3MAG"); |
char tx_data[1] = { 1 }; |
com->send_cmd('u', ADDRESS_NC, tx_data, 1, false); |
} |
129,6 → 142,7 |
* set debug values for all components |
*/ |
void Handler::set_all_debug(int speed) { |
FlightLog::info("setting debug speed"); |
char tx_data[1] = { speed }; |
com->send_cmd('d', ADDRESS_ALL, tx_data, 1, false); |
} |
144,6 → 158,7 |
* get all analog labels |
*/ |
void Handler::get_analog() { |
FlightLog::info("get analog values"); |
char tx_data[1] = { 0 }; |
com->send_cmd('a', ADDRESS_ALL, tx_data, 1, true); |
} |
152,6 → 167,7 |
* get values from LCD / show LCD |
*/ |
void Handler::show_lcd() { |
FlightLog::info("show LCD"); |
char tx_data[1] = {0}; |
com->send_cmd('l', ADDRESS_ALL, tx_data, 1, true); |
} |
160,6 → 176,7 |
* got to next LCD Page |
*/ |
void Handler::lcd_up() { |
FlightLog::info("going up one LCD page"); |
char tx_data[2] = { 0, 0 }; |
if (data->lcd_cur != data->lcd_max) |
tx_data[0] = data->lcd_cur+1; |
170,6 → 187,7 |
* got to previous LCD Page |
*/ |
void Handler::lcd_down() { |
FlightLog::info("going down one LCD page"); |
char tx_data[2] = { 0, 0 }; |
if (data->lcd_cur != 0) |
tx_data[0] = data->lcd_cur-1; |
177,6 → 195,7 |
} |
|
void Handler::get_version() { |
FlightLog::info("get version"); |
//TODO: Check if is this correct or do we need data from switch_... |
char tx_data[1] = { 0 }; |
com->send_cmd('v', ADDRESS_ALL, tx_data, 0, true); |
183,24 → 202,41 |
} |
|
void Handler::get_ppm_channels() { |
FlightLog::info("get PPM channels"); |
char tx_data[1] = { 0 }; |
com->send_cmd('p', ADDRESS_ALL, tx_data, 0, false); |
} |
|
void Handler::new_navi_data(char * data) { |
std::string mode; |
switch(data[N_NC_FLAGS]) |
{ |
case 0x01 : mode = "Free"; break; |
case 0x02 : mode = "Position Hold"; break; |
case 0x04 : mode = "Coming Home"; break; |
case 0x08 : mode = "Range Limit"; break; |
case 0x10 : mode = "Serial Error"; break; |
case 0x20 : mode = "Target reached"; break; |
case 0x40 : mode = "Manual Control"; break; |
} |
|
} |
|
/** |
* receive data |
*/ |
void Handler::receive_data(char * incomming, int length) { |
if (incomming[0] != '#') |
if (incomming[0] != '#') { |
FlightLog::error("this frame is not correct"); |
FlightLog::info(incomming); |
return; |
} |
int hardwareID = incomming[1] - 'a'; |
|
//The cmd is also known as ID-Byte (or ID for short) in the wiki-dokumentation |
char cmd = incomming[2]; |
//decode data |
unsigned char data[150]; |
//decoded data |
unsigned char data[MAX_DATA_SIZE]; |
|
Parser::decode64(incomming, length, data, 3); |
|
211,16 → 247,14 |
{ |
// Motor-Mixer |
case 'N' : |
//if (Parser::decode64(RX)) |
//{ |
com->stop_resend(); |
//decoded data |
FlightLog::info("received motortest values from FlightCtrl"); |
FlightLog::info_FC("received motortest values from FlightCtrl"); |
if (data[0] == VERSION_MIXER) |
{ |
//TODO: Handler::receivedMotorConfig(incomming) |
//f_MotorMixer->set_MotorConfig(RX); |
} |
//} |
break; |
// Motor-Mixer Schreib-Bestätigung |
case 'M' : |
228,7 → 262,7 |
|
if (data[0] == 1) |
{ |
FlightLog::info("motor values written to FlightCtrl."); |
FlightLog::info_FC("motor values written to FlightCtrl."); |
//lb_Status->setText(tr("MotorMixer-Daten in FC geschrieben.")); |
} else { |
FlightLog::warning("could not write motor values to FlightCtrl!"); |
237,7 → 271,14 |
|
// Stick-Belegung der Fernsteuerung |
case 'P' : // DONE 0.71g |
FlightLog::info("received stick-settings from FlightCtrl:"); |
FlightLog::info_FC("received stick-settings:"); |
for (int i = 1; i <= 8; i++) { |
std::stringstream d; |
d << "Stickvalue for Stick nr. " << i << ": " << Parser::dataToInt((char *)data, i*2, true);; |
std::string val; |
d >> val; |
FlightLog::info_FC((char *)val.c_str()); |
} |
/*f_Settings->pb_K1->setValue(Parser::dataToInt(RX.decode, 2,true)); |
f_Settings->pb_K2->setValue(Parser::dataToInt(RX.decode, 4,true)); |
f_Settings->pb_K3->setValue(Parser::dataToInt(RX.decode, 6,true)); |
250,10 → 291,22 |
// Settings lesen |
case 'Q' : // DONE 0.71g |
com->stop_resend(); |
FlightLog::info("received settings from FlightCtrl"); |
FlightLog::info_FC("received settings"); |
if (data[1] == VERSION_SETTINGS) |
{ |
int Settings_ID = data[0]; |
int settings_id = data[0]; |
std::stringstream d; |
d << "Settings-ID: " << settings_id; |
std::string val; |
d >> val; |
FlightLog::info_FC((char *)val.c_str()); |
for (int i = 0; i < MaxParameter; i++) { |
d.flush(); |
d << "Value for Parameter nr. " << i << ": " << (int)data[i+2]; |
d >> val; |
FlightLog::info_FC((char *)val.c_str()); |
} |
|
/*for (int a = 0; a < MaxParameter; a++) |
{ |
FCSettings[a] = RX.decode[a + 2]; |
281,7 → 334,7 |
// Settings written |
case 'S' : // DONE 0.71g |
com->stop_resend(); |
FlightLog::info("settings written successfully to FlightCtrl"); |
FlightLog::info_FC("settings written successfully"); |
//TODO: QMessagebox("settings written successful") ? |
break; |
} |
292,7 → 345,7 |
// Navigationsdaten |
case 'O' : // NOT DONE 0.12h |
//new_NaviData(RX); |
FlightLog::info("received navigation data from NaviCtrl"); |
FlightLog::info_NC("received navigation data"); |
break; |
} |
// case ADDRESS_MK3MAG : |
304,7 → 357,7 |
case 'L' : // DONE 0.71g |
com->stop_resend(); |
FlightLog::info("received LCD page."); |
/*int LCD[150]; |
/*int LCD[MAX_DATA_SIZE]; |
memcpy(LCD,RX.decode, sizeof(RX.decode)); |
|
f_LCD->show_Data(LCD); |