WebSVN - Projects - Diff - Rev 500 and 513 - /QMK-Groundstation/branches/libMK/libMK/Handler.cpp

 #include "Handler.h"
 #include "FlightLog.h"
+#include <sstream>
+#include <string>
 /**
  * Constructor that gets a communication instance
  */
 //-------------FlightCtrl commands--------------------
 /**
  * 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);
+}
 /**
  * write settings to FlightCtrl
  */
+void Handler::set_flightctrl_settings(char * tx_data) {
-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);
+}
     char tx_data[MAX_MOTORS];
     for (int z = 0; z<MAX_MOTORS; z++)
+    {
         tx_data[z] = motor.desired_speed[z];
+    }
+    FlightLog::info_FC("testing motor");
     com->send_cmd('t', ADDRESS_FC, tx_data, MAX_MOTORS, false);
+}
 void Handler::reset_motor() {
 /**
  * read mixer values from FlightCtrl
  */
+void Handler::read_motor_mixer() {
-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);
+}
 /**
  * write motor mixer values to FlightCtrl
+ */
- */
+void Handler::write_motor_mixer(char * tx_data, int length) {
-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);
+}
 //-------------NaviCtrl commands--------------------
 /**
  * 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);
+}
 /**
  * send a waypoint to the NaviCtrl (the MikroKopter will fly to the position emidiately)
  */
+void Handler::send_waypoint(Waypoint_t desired_pos) {
-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);
+}
 /**
  * add waypoint to waypoint list
+ */
- */
+void Handler::add_waypoint(Waypoint_t wp) {
-void Handler::add_waypoint(Waypoint_t wp) {
+    FlightLog::info_GPS("adding Waypoint");
     com->send_cmd('w', ADDRESS_NC, (char *)&wp, sizeof(wp), false);
+}
     wp.Position.Status = INVALID;
     send_waypoint(wp);
+}
 //-------------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() {
-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() {
-void Handler::switch_mk3mag() {
+    FlightLog::info("switching to MK3MAG");
     char tx_data[1] = { 1 };
     com->send_cmd('u', ADDRESS_NC, tx_data, 1, false);
 /**
  * set debug values for all components
  */
+void Handler::set_all_debug(int speed) {
-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);
+}
 /**
  * get all analog labels
  */
+void Handler::get_analog() {
-void Handler::get_analog() {
+    FlightLog::info("get analog values");
     char tx_data[1] = { 0 };
     com->send_cmd('a', ADDRESS_ALL, tx_data, 1, true);
+}
 /**
  * get values from LCD / show LCD
+ */
- */
+void Handler::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);
+}
 /**
+ * got to next LCD Page
- * got to next LCD Page
+ */
- */
+void Handler::lcd_up() {
-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;
     com->send_cmd('l', ADDRESS_ALL, tx_data, 1, true);
+}
+/**
-/**
+ * got to previous LCD Page
- * got to previous LCD Page
+ */
- */
+void Handler::lcd_down() {
-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;
-        tx_data[0] = data->lcd_cur-1;
+    com->send_cmd('l', ADDRESS_ALL, tx_data, 1, true);
     com->send_cmd('l', ADDRESS_ALL, tx_data, 1, true);
 void Handler::get_version() {
-void Handler::get_version() {
+    FlightLog::info("get version");
+    //TODO: Check if is this correct or do we need data from switch_...
-    //TODO: Check if is this correct or do we need data from switch_...
+    char tx_data[1] = { 0 };
-    char tx_data[1] = { 0 };
+    com->send_cmd('v', ADDRESS_ALL, tx_data, 0, true);
     com->send_cmd('v', ADDRESS_ALL, tx_data, 0, true);
+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;
-void Handler::get_ppm_channels() {
+        case 0x40 : mode = "Manual Control"; break;
     char tx_data[1] = { 0 };
-    com->send_cmd('p', ADDRESS_ALL, tx_data, 0, false);
+}
 /**
+ * receive data
- * receive data
+ */
- */
+void Handler::receive_data(char * incomming, int length) {
-void Handler::receive_data(char * incomming, int length) {
+    if (incomming[0] != '#') {
-    if (incomming[0] != '#')
+        FlightLog::error("this frame is not correct");
-        FlightLog::error("this frame is not correct");
+        FlightLog::info(incomming);
-        FlightLog::info(incomming);
+        return;
         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
+    //decoded data
-    unsigned char data[150];
+    unsigned char data[MAX_DATA_SIZE];
-    Parser::decode64(incomming, length, data, 3);
-    switch(hardwareID)
+    Parser::decode64(incomming, length, data, 3);
-    {
-        case ADDRESS_FC :
+    switch(hardwareID)
-            switch(cmd)
+    {
-            {
+        case ADDRESS_FC :
-                // Motor-Mixer
+            switch(cmd)
-                case 'N' :
+            {
-                    //if (Parser::decode64(RX))
+                // Motor-Mixer
-                    //{
+                case 'N' :
-                        com->stop_resend();
+                    com->stop_resend();
-                        //decoded data
+                    //decoded data
-                        FlightLog::info("received motortest values from FlightCtrl");
+                    FlightLog::info_FC("received motortest values from FlightCtrl");
-                        if (data[0] == VERSION_MIXER)
+                    if (data[0] == VERSION_MIXER)
-                        {
+                    {
-                            //f_MotorMixer->set_MotorConfig(RX);
+                        //TODO: Handler::receivedMotorConfig(incomming)
                         //f_MotorMixer->set_MotorConfig(RX);
                     //}
                     break;
                 // Motor-Mixer Schreib-Bestätigung
                 case 'M' :
                     com->stop_resend();
+                    if (data[0] == 1)
+                    {
+                        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!");
+                    }
-                    if (data[0] == 1)
+                    break;
-                        FlightLog::info("motor values written to FlightCtrl.");
+                // Stick-Belegung der Fernsteuerung
-                        //lb_Status->setText(tr("MotorMixer-Daten in FC geschrieben."));
+                case 'P' : // DONE 0.71g
-                    } else {
+                    FlightLog::info_FC("received stick-settings:");
                     f_Settings->pb_K8->setValue(Parser::dataToInt(RX.decode, 16,true));*/
                     break;
                 // 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];
+                        }
                         f_Settings->show_FCSettings(Settings_ID, FCSettings);
+                    }
                     break;
                 // 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;
+            }
         case ADDRESS_NC :
             switch(cmd)
+            {
                 // 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 :
+            {
                 // LCD-Anzeige
                 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);

Subversion Repositories Projects

(root)/QMK-Groundstation/branches/libMK/libMK/Handler.cpp - Rev 500 → 513