WebSVN - Projects - Diff - Rev 397 and 399 - /QMK-Groundstation/branches/libMK/libMK/Handler.cpp

 #include<Handler.h>
 /**
  * Constructor that gets a communication instance
  */
 Handler::Handler(Communication * com) {
     this->com = com;
+}
+//-------------FlightCtrl commands--------------------
+/**
+ * read settings from FlightCtrl (settings index 0x00-0x05)
+ */
+void Handler::get_flightctrl_settings(int index) {
+    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) {
+    com->send_cmd('s', ADDRESS_FC, tx_data, MaxParameter+2, true);
+}
+/**
+ * test one or more motors
+ */
+void Handler::motor_test(sMotor motor) {
+    char tx_data[12];
+    for (int z = 0; z<12; z++)
+    {
+        tx_data[z] = motor.Speed[z];
+    }
+    com->send_cmd('t', ADDRESS_FC, tx_data, 12, false);
+}
 /**
  * read mixer values from FlightCtrl
  */
 void Handler::read_mixer() {
     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_mixer(char * tx_data, int length) {
+    com->send_cmd('m', ADDRESS_FC, tx_data, length, true);
+}
 void Handler::get_motor_config() {
+}
+//-------------NaviCtrl commands--------------------
+/**
+ * set debug values for NaviCtrl
+ */
+void Handler::set_navictrl_debug(int speed) {
+    char tx_data[1] = { speed };
+    com->send_cmd('o', ADDRESS_NC, tx_data, 1, false);
+}
+/**
+ * stop debug for NaviCtrl
+ */
+void Handler::stop_navictrl_debug() {
+    set_navictrl_debug(0);
+}
+/**
+ * send a waypoint to the NaviCtrl (the copter will fly to the position emidiately)
+ */
+void Handler::send_waypoint(Waypoint_t desired_pos) {
+    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) {
+    com->send_cmd('w', ADDRESS_NC, (char *)&wp, sizeof(wp), false);
+}
+/**
+ * clear waypoint list on MK
+ */
+void Handler::delete_waypoints() {
+    Waypoint_t wp;
+    wp.Position.Status = INVALID;
+    send_waypoint(wp);
+}
+//-------------switch between Hardware--------------------
+void Handler::switch_navictrl() {
+    char tx_data[6] = { 0x1B, 0x1B, 0x55, 0xAA, 0x00, '\r'};
+    com->send_cmd('#', ADDRESS_NC, tx_data, 6, false);
+}
+void Handler::switch_flightctrl() {
+    char tx_data[1] = { 0 };
+    com->send_cmd('u', ADDRESS_NC, tx_data, 1, false);
+}
+void Handler::switch_mk3mag() {
+    char tx_data[1] = { 1 };
+    com->send_cmd('u', ADDRESS_NC, tx_data, 1, false);
+}
+//-------------commands for MK3MAG-----------------
+//-------------commands for all--------------------
+/**
+ * set debug values for all components
+ */
+void Handler::set_all_debug(int speed) {
+    char tx_data[1] = { speed };
+    com->send_cmd('d', ADDRESS_ALL, tx_data, 1, false);
+}
+/**
+ * stop debug for all components
+ */
+void Handler::stop_all_debug() {
+    set_all_debug(0);
+}
+/**
+ * get all analog labels
+ */
+void Handler::get_analog() {
+    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() {
+    char tx_data[1] = {0};
+    com->send_cmd('l', ADDRESS_ALL, tx_data, 1, true);
+}
+/**
+ * got to next LCD Page
+ */
+void Handler::lcd_up() {
+    char tx_data[2] = { 0, 0 };
+    if (lcd_cur != lcd_max)
+        tx_data[0] = lcd_cur+1;
+    com->send_cmd('l', ADDRESS_ALL, tx_data, 1, true);
+}
+/**
+ * got to previous LCD Page
+ */
+void Handler::lcd_down() {
+    char tx_data[2] = { 0, 0 };
+    if (lcd_cur != 0)
+        tx_data[0] = lcd_cur-1;
+    com->send_cmd('l', ADDRESS_ALL, tx_data, 1, true);
+}
+void Handler::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);
+}
+void Handler::get_ppm_channels() {
+    char tx_data[1] = { 0 };
+    com->send_cmd('p', ADDRESS_ALL, tx_data, 0, false);
+}
+/**
+ * receive data
  */
 void Handler::receive_data(sRxData RX) {
     //extract hardware ID from received Data
     int hardwareID = RX.input[1] - 'a';
     switch(hardwareID)
+    {
         case ADDRESS_FC :
             switch(RX.input[2])
+            {
                 // Motor-Mixer
                 case 'N' :
                     if (Parser::decode64(RX))
+                    {
                         com->stop_resend();
                         if (RX.decode[0] == VERSION_MIXER)
+                        {
                             //f_MotorMixer->set_MotorConfig(RX);
+                        }
+                    }
                 break;
                 // Motor-Mixer Schreib-Bestätigung
                 case 'M' :
                     if (Parser::decode64(RX))
+                    {
                         com->stop_resend();
                         if (RX.decode[0] == 1)
+                        {
                             //lb_Status->setText(tr("MotorMixer-Daten in FC geschrieben."));
+                        }
+                    }
                 break;
                 // Stick-Belegung der Fernsteuerung
                 case 'P' : // DONE 0.71g
                     if (Parser::decode64(RX))
+                    {
                         /*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));
                         f_Settings->pb_K4->setValue(Parser::dataToInt(RX.decode,  8,true));
                         f_Settings->pb_K5->setValue(Parser::dataToInt(RX.decode, 10 ,true));
                         f_Settings->pb_K6->setValue(Parser::dataToInt(RX.decode, 12,true));
                         f_Settings->pb_K7->setValue(Parser::dataToInt(RX.decode, 14,true));
                         f_Settings->pb_K8->setValue(Parser::dataToInt(RX.decode, 16,true));*/
+                    }
                 break;
                 // Settings lesen
                 case 'Q' : // DONE 0.71g
                     if (Parser::decode64(RX))
+                    {
                         com->stop_resend();
                         if (RX.decode[1] == VERSION_SETTINGS)
+                        {
                             int Settings_ID = RX.decode[0];
                             /*for (int a = 0; a < MaxParameter; a++)
+                            {
                                 FCSettings[a] = RX.decode[a + 2];
+                            }
                             f_Settings->show_FCSettings(Settings_ID, FCSettings);
                             f_Settings->pb_Read->setEnabled(true);
                             f_Settings->pb_Write->setEnabled(true);*/
+                        }
                         else
+                        {
                             /*f_Settings->pb_Read->setDisabled(true);
                             f_Settings->pb_Write->setDisabled(true);
                             QString name = QString("Versionen inkompatibel.\n") +
                                       QString("Version von GroundStation benoetigt: ") +
                                       QString(VERSION_SETTINGS) +
                                       QString("\nVersion auf der FlightCtrl: ") +
                                       QString(RX.decode[1]) +
                                       QString("\nParameterbearbeitung nicht moeglich.");
                             QMessageBox::warning(this, QA_NAME,
                                    name, QMessageBox::Ok);*/
+                        }
+                    }
                 break;
                 // Settings written
                 case 'S' : // DONE 0.71g
                     com->stop_resend();
                     //TODO: QMessagebox("settings written successful") ?
                 break;
+            }
         case ADDRESS_NC :
             switch(RX.input[2])
+            {
                 // Navigationsdaten
                 case 'O' : // NOT DONE 0.12h
                     if (Parser::decode64(RX))
+                    {
                         //new_NaviData(RX);
+                    }
                 break;
+            }
 //        case ADDRESS_MK3MAG :
         default :
             switch(RX.input[2])
+            {
                 // LCD-Anzeige
                 case 'L' : // DONE 0.71g
                     if (Parser::decode64(RX))
+                    {
                         com->stop_resend();
                         /*int LCD[150];
                         memcpy(LCD,RX.decode, sizeof(RX.decode));
                         f_LCD->show_Data(LCD);
                         LCD_Page     = RX.decode[0];
                         LCD_MAX_Page = RX.decode[1];
                         */
+                    }
                 break;
                 // Analoglabels
                 case 'A' : // DONE 0.71g
                     if (Parser::decode64(RX))
+                    {
                         com->stop_resend();
                         int Position = RX.decode[0];
                         if (Position != 31)
+                        {
                             /*
                             Settings->Analog1.Label[Position] = ToolBox::dataToQString(RX.decode,1,17).trimmed();
                             if (Settings->Analog1.Label[Position] == "")
+                            {
                                 Settings->Analog1.Label[Position] = "A-" + QString("%1").arg(Position);
+                            }
                             Position ++;
                             TX_Data[0] = Position;
                             o_Connection->send_Cmd('a', ADDRESS_ALL, TX_Data, 1, true);*/
+                        }
                         if (Position == 31)
+                        {
                             /*
                             for (int a = 0; a < MaxAnalog; a++)
+                            {
                                 lb_Analog[a]->setText(Settings->Analog1.Label[a]);
+                            }
                             Settings->Analog1.Version = QString(Mode.Version);
                             Settings->write_Settings_AnalogLabels(HardwareID);
                             config_Plot();*/
+                        }
+                    }
                 break;
                 // Debug-Daten
                 case 'D' : // DONE 0.71g
                     if (Parser::decode64(RX))
+                    {
                         for (int i = 0; i < MaxAnalog; i++)
+                        {
                             //AnalogData[i] = Parser::dataToInt(RX.decode, (i * 2) + 2);
+                        }
                         //show_DebugData();
+                    }
                 break;
                 // Version
                 case 'V' : // DONE 0.71h
                     if (Parser::decode64(RX))
+                    {
                         com->stop_resend();
                         /*
                         Mode.ID            = HardwareID;
                         Mode.VERSION_MAJOR = RX.decode[0];
                         Mode.VERSION_MINOR = RX.decode[1];
                         Mode.VERSION_PATCH = RX.decode[4];
                         Mode.VERSION_SERIAL_MAJOR = RX.decode[2];
                         Mode.VERSION_SERIAL_MINOR = RX.decode[3];
                         Mode.Hardware   = HardwareType[Mode.ID];
                         //TODO: Funktion im Handler get_version() oder sowas
                         QString version = QString("%1").arg(RX.decode[0]) + "." +
                                           QString("%1").arg(RX.decode[1]) +
                                           QString(RX.decode[4] + 'a');
                         Mode.Version = version.toLatin1().data;
                         setWindowTitle(QA_NAME + " v" + QA_VERSION + " - " +
                                  Mode.Hardware + " " +
                                  Mode.Version);
                         if (Mode.VERSION_SERIAL_MAJOR != VERSION_SERIAL_MAJOR)
+                        {
 //                                AllowSend = false;
                                 QMessageBox::warning(this, QA_NAME,
                                    tr("Serielles Protokoll Inkompatibel. \nBitte neue Programmversion installieren,"), QMessageBox::Ok);
+                        }
                         if (ac_NoDebug->isChecked())
+                        {
                             TX_Data[0] = 0;
+                        }
                         else
                         if (ac_FastDebug->isChecked())
+                        {
                             TX_Data[0] = Settings->Data.Debug_Fast / 10;
+                        }
                         else
+                        {
                             TX_Data[0] = Settings->Data.Debug_Slow / 10;
+                        }
                         o_Connection->send_Cmd('d', ADDRESS_ALL, TX_Data, 1, false);
                         // Wenn MK3MAG dann andauernd Daten neu anfragen.
                         if (Mode.ID == ADDRESS_MK3MAG)
+                        {
                             TickerEvent[3] = true;
                             rb_SelMag->setChecked(true);
+                        }
                         // Wenn NaviCtrl dann hier.
                         if (Mode.ID == ADDRESS_NC)
+                        {
                             rb_SelNC->setChecked(true);
                             if (ac_NoNavi->isChecked())
+                            {
                                 TX_Data[0] = 0;
+                            }
                             else
                             if (ac_FastNavi->isChecked())
+                            {
                                 TX_Data[0] = Settings->Data.Navi_Fast / 10;
+                            }
                             else
+                            {
                                 TX_Data[0] = Settings->Data.Navi_Slow / 10;
+                            }
                             o_Connection->send_Cmd('o', ADDRESS_NC, TX_Data, 1, false);
+                        }
                         // Wenn FlightCtrl dann Settings abfragen.
                         if (Mode.ID == ADDRESS_FC)
+                        {
                             rb_SelFC->setChecked(true);
+                            {
                                 TX_Data[0] = 0xff;
                                 TX_Data[1] = 0;
                                 // DEP: Raus wenn Resend implementiert.
 //                                ToolBox::Wait(SLEEP);
                                 o_Connection->send_Cmd('q', ADDRESS_FC, TX_Data, 1, true);
                                 qDebug("FC - Get Settings");
+                            }
+                        }
                         // Wenn nicht Lesen und Schreiben der Settings deaktivieren.
                         else
+                        {
                                 f_Settings->pb_Read->setDisabled(true);
                                 f_Settings->pb_Write->setDisabled(true);
+                        }
                         Settings->read_Settings_Analog(HardwareID);
                         Settings->read_Settings_AnalogLabels(HardwareID);
                         if (Settings->Analog1.Version != QString(Mode.Version))
+                        {
                             lb_Status->setText(tr("Analoglabel-Version unterschiedlich. Lese Analoglabels neu aus."));
                             slot_ac_GetLabels();
+                        }
                         else
                         for (int a = 0; a < MaxAnalog; a++)
+                        {
                             lb_Analog[a]->setText(Settings->Analog1.Label[a]);
+                        }
                         config_Plot();*/
+                    }
                 break;
+            }
+    }
+}

Subversion Repositories Projects

(root)/QMK-Groundstation/branches/libMK/libMK/Handler.cpp @ 462 - Rev 397 → 399