Update send message CMDS and CTRL

src/uas/SlugsMAV.cc

@@ -135,11 +135,11 @@ void SlugsMAV::receiveMessage(LinkInterface* link, mavlink_message_t message)
             break;
 
       case MAVLINK_MSG_ID_MID_LVL_CMDS:     //180
-
+        mavlink_msg_mid_lvl_cmds_decode(&message, &mlMidLevelCommands);
       break;
 
       case MAVLINK_MSG_ID_CTRL_SRFC_PT:     //181
-
+        mavlink_msg_ctrl_srfc_pt_decode(&message, &mlPassthrough);
             break;
 
       case MAVLINK_MSG_ID_PID:              //182

src/ui/MainWindow.cc

@@ -625,6 +625,7 @@ void MainWindow::buildSlugsWidgets()
         addToToolsMenu (slugsHilSimWidget, tr("HIL Sim Configuration"), SLOT(showToolWidget(bool)), MENU_SLUGS_HIL, Qt::LeftDockWidgetArea);
     }
 
+
 //    if (!slugsCamControlWidget)
 //    {
 //        slugsCamControlWidget = new QDockWidget(tr("Slugs Video Camera Control"), this);
@@ -1017,11 +1018,6 @@ void MainWindow::connectCommonWidgets()
         connect(waypointsDockWidget->widget(), SIGNAL(changePointList()), mapWidget, SLOT(clearWaypoints()));
     }
 
-    if(controlDockWidget && controlParameterWidget)
-    {
-       connect(controlDockWidget->widget(), SIGNAL(changedMode(int)), controlParameterWidget->widget(), SLOT(changedMode(int)));
-    }
-
     //TODO temporaly debug
     if (slugsHilSimWidget && slugsHilSimWidget->widget()){
         connect(UASManager::instance(), SIGNAL(activeUASSet(UASInterface*)),
@@ -1032,6 +1028,11 @@ void MainWindow::connectCommonWidgets()
         connect(UASManager::instance(), SIGNAL(activeUASSet(UASInterface*)),
                 controlParameterWidget->widget(), SLOT(activeUasSet(UASInterface*)));
     }
+
+    if(controlDockWidget && controlParameterWidget)
+    {
+       connect(controlDockWidget->widget(), SIGNAL(changedMode(int)), controlParameterWidget->widget(), SLOT(changedMode(int)));
+    }
 }
 
 void MainWindow::createCustomWidget()

src/ui/UASControlParameters.ui

@@ -6,8 +6,8 @@
    <rect>
     <x>0</x>
     <y>0</y>
-    <width>200</width>
+    <width>204</width>
-    <height>228</height>
+    <height>246</height>
    </rect>
   </property>
   <property name="minimumSize">
@@ -26,19 +26,6 @@
    <string>Form</string>
   </property>
   <layout class="QGridLayout" name="gridLayout">
-   <item row="1" column="0">
-    <widget class="QTabWidget" name="tabWidget">
-     <property name="styleSheet">
-      <string notr="true"/>
-     </property>
-     <property name="currentIndex">
-      <number>0</number>
-     </property>
-     <widget class="QWidget" name="tab">
-      <attribute name="title">
-       <string>Tab 1</string>
-      </attribute>
-      <layout class="QGridLayout" name="gridLayout_2">
    <item row="0" column="0">
     <layout class="QHBoxLayout" name="horizontalLayout_5">
      <item>
@@ -92,6 +79,19 @@
     </layout>
    </item>
    <item row="1" column="0">
+    <widget class="QTabWidget" name="tabWidget">
+     <property name="styleSheet">
+      <string notr="true"/>
+     </property>
+     <property name="currentIndex">
+      <number>0</number>
+     </property>
+     <widget class="QWidget" name="tab">
+      <attribute name="title">
+       <string>Commands</string>
+      </attribute>
+      <layout class="QGridLayout" name="gridLayout_2">
+       <item row="1" column="0">
         <layout class="QHBoxLayout" name="horizontalLayout">
          <item>
           <widget class="QLabel" name="label_2">
@@ -180,8 +180,98 @@
      </widget>
      <widget class="QWidget" name="tab_2">
       <attribute name="title">
-       <string>Tab 2</string>
+       <string>Passthrough</string>
       </attribute>
+      <layout class="QGridLayout" name="gridLayout_3">
+       <item row="0" column="0">
+        <layout class="QHBoxLayout" name="horizontalLayout_6">
+         <item>
+          <layout class="QVBoxLayout" name="verticalLayout_2">
+           <item>
+            <widget class="QCheckBox" name="cxElevator">
+             <property name="text">
+              <string>Elevator</string>
+             </property>
+            </widget>
+           </item>
+           <item>
+            <widget class="QCheckBox" name="cxRudder">
+             <property name="text">
+              <string>Rudder</string>
+             </property>
+            </widget>
+           </item>
+           <item>
+            <widget class="QCheckBox" name="cxThrottle">
+             <property name="text">
+              <string>Throttle</string>
+             </property>
+            </widget>
+           </item>
+           <item>
+            <widget class="QCheckBox" name="cxAilerons">
+             <property name="text">
+              <string>Ailerons</string>
+             </property>
+            </widget>
+           </item>
+          </layout>
+         </item>
+         <item>
+          <layout class="QVBoxLayout" name="verticalLayout">
+           <item>
+            <widget class="QCheckBox" name="cxRightAileron">
+             <property name="text">
+              <string>Right Aileron</string>
+             </property>
+            </widget>
+           </item>
+           <item>
+            <widget class="QCheckBox" name="cxRightElevator">
+             <property name="text">
+              <string>Right Elevator</string>
+             </property>
+            </widget>
+           </item>
+           <item>
+            <widget class="QCheckBox" name="cxLeftFlap">
+             <property name="text">
+              <string>Left Flap</string>
+             </property>
+            </widget>
+           </item>
+           <item>
+            <widget class="QCheckBox" name="cxRightFlap">
+             <property name="text">
+              <string>Right Flap</string>
+             </property>
+            </widget>
+           </item>
+          </layout>
+         </item>
+        </layout>
+       </item>
+       <item row="1" column="0">
+        <widget class="QPushButton" name="btSetCtrl">
+         <property name="text">
+          <string>Set</string>
+         </property>
+        </widget>
+       </item>
+       <item row="2" column="0">
+        <spacer name="verticalSpacer_2">
+         <property name="orientation">
+          <enum>Qt::Vertical</enum>
+         </property>
+         <property name="sizeHint" stdset="0">
+          <size>
+           <width>20</width>
+           <height>26</height>
+          </size>
+         </property>
+        </spacer>
+       </item>
+      </layout>
      </widget>
     </widget>
    </item>

src/ui/uas/UASControlParameters.cpp

@@ -27,14 +27,9 @@ UASControlParameters::UASControlParameters(QWidget *parent) :
 {
     ui->setupUi(this);
 
-    //this->mode = "MAV_MODE_UNKNOWN";
-    //connect(UASManager::instance(), SIGNAL(activeUASSet(UASInterface*)), this, SLOT(activeUasSet(UASInterface*)));
-
     connect(ui->btGetCommands, SIGNAL(clicked()), this, SLOT(getCommands()));
 
-    //QColor groupColor = QColor(231,72,28);
+    connect(ui->btSetCtrl, SIGNAL(clicked()), this, SLOT(setPassthrough()));
-    //QString borderColor = "#FA4A4F";
-    //groupColor = groupColor.darker(475);
 }
 
 UASControlParameters::~UASControlParameters()
@@ -46,46 +41,48 @@ void UASControlParameters::changedMode(int mode)
 {
     QString modeTemp;
 
-    if (mode == CONTROL_MODE_LOCKED_INDEX)
+    switch (mode)
-    {
+    {
-        modeTemp= CONTROL_MODE_LOCKED;
+    case (uint8_t)MAV_MODE_LOCKED:
-    }
+        modeTemp = "LOCKED MODE";
-    else if (mode == CONTROL_MODE_MANUAL_INDEX)
+        break;
-    {
+    case (uint8_t)MAV_MODE_MANUAL:
-        modeTemp= CONTROL_MODE_MANUAL;
+        modeTemp = "MANUAL MODE";
-    }
+        break;
-    else if (mode == CONTROL_MODE_GUIDED_INDEX)
+    case (uint8_t)MAV_MODE_AUTO:
-    {
+        modeTemp = "AUTO MODE";
-        modeTemp= CONTROL_MODE_GUIDED;
+        break;
-    }
+    case (uint8_t)MAV_MODE_GUIDED:
-    else if (mode == CONTROL_MODE_AUTO_INDEX)
+        modeTemp = "GUIDED MODE";
-    {
+        break;
-        modeTemp= CONTROL_MODE_AUTO;
+    case (uint8_t)MAV_MODE_READY:
-    }
+        modeTemp = "READY MODE";
-    else if (mode == CONTROL_MODE_TEST1_INDEX)
+        break;
-    {
+    case (uint8_t)MAV_MODE_TEST1:
-        modeTemp= CONTROL_MODE_TEST1;
+        modeTemp = "TEST1 MODE";
-    }
+        break;
-    else if (mode == CONTROL_MODE_TEST2_INDEX)
+    case (uint8_t)MAV_MODE_TEST2:
-    {
+        modeTemp = "TEST2 MODE";
-        modeTemp= CONTROL_MODE_TEST2;
+        break;
-    }
+    case (uint8_t)MAV_MODE_TEST3:
-    else if (mode == CONTROL_MODE_TEST3_INDEX)
+        modeTemp = "TEST3 MODE";
-    {
+        break;
-        modeTemp= CONTROL_MODE_TEST3;
+    case (uint8_t)MAV_MODE_RC_TRAINING:
-    }
+        modeTemp = "RC TRAINING MODE";
-    else if (mode == CONTROL_MODE_RC_TRAINING_INDEX)
+        break;
-    {
+    default:
-        modeTemp= CONTROL_MODE_RC_TRAINING;
+        modeTemp = "UNINIT MODE";
-    }
+        break;
-
+    }
-    if( static_cast<QString>(modeTemp) != this->mode)
+
+
+    if(modeTemp != this->mode)
     {
         ui->lbMode->setStyleSheet("background-color: rgb(255, 0, 0)");
     }
     else
     {
-        ui->lbMode->setStyleSheet("");
+        ui->lbMode->setStyleSheet("background-color: rgb(0, 255, 0)");
     }
 }
 
@@ -94,8 +91,12 @@ void UASControlParameters::activeUasSet(UASInterface *uas)
     connect(uas, SIGNAL(globalPositionChanged(UASInterface*,double,double,double,quint64)), this, SLOT(updateGlobalPosition(UASInterface*,double,double,double,quint64)));
     connect(uas, SIGNAL(speedChanged(UASInterface*,double,double,double,quint64)), this, SLOT(speedChanged(UASInterface*,double,double,double,quint64)));
     connect(uas, SIGNAL(attitudeChanged(UASInterface*,double,double,double,quint64)), this, SLOT(updateAttitude(UASInterface*,double,double,double,quint64)));
+
     connect(uas, SIGNAL(modeChanged(int,QString,QString)), this, SLOT(updateMode(int,QString,QString)));
 
+    connect(uas, SIGNAL(thrustChanged(UASInterface*,double)), this, SLOT(thrustChanged(UASInterface*,double)) );
+    //connect(uas, SIGNAL(radioCalibrationReceived(QPointer<RadioCalibrationData>)), this, SLOT(radioChanged(QPointer<RadioCalibrationData>)));
+
     activeUAS= uas;
 }
 
@@ -120,7 +121,18 @@ void UASControlParameters::updateAttitude(UASInterface *uas, double roll, double
 }
 
 void UASControlParameters::setCommands()
-{}
+{
+    UAS* myUas= static_cast<UAS*>(this->activeUAS);
+
+    mavlink_message_t msg;
+
+    tempCmds.uCommand = ui->sbAirSpeed->value();
+    tempCmds.hCommand = ui->sbHeight->value();
+    tempCmds.rCommand = ui->sbTurnRate->value();
+
+    mavlink_msg_mid_lvl_cmds_encode(MG::SYSTEM::ID, MG::SYSTEM::COMPID, &msg, &this->tempCmds);
+    myUas->sendMessage(msg);
+}
 
 void UASControlParameters::getCommands()
 {
@@ -129,10 +141,91 @@ void UASControlParameters::getCommands()
     ui->sbTurnRate->setValue(this->roll);
 }
 
+void UASControlParameters::setPassthrough()
+{
+    //UAS* myUas= static_cast<UAS*>(this->activeUAS);
+
+    //mavlink_message_t msg;
+
+    int8_t tmpBit=0;
+
+    if(ui->cxAilerons->isChecked())
+    {
+        tmpBit+=1;
+    }
+    if(ui->cxElevator->isChecked())
+    {
+        tmpBit+=2;
+    }
+    if(ui->cxLeftFlap->isChecked())
+    {
+        tmpBit+=4;
+    }
+    if(ui->cxRightAileron->isChecked())
+    {
+        tmpBit+=8;
+    }
+    if(ui->cxRightElevator->isChecked())
+    {
+        tmpBit+=16;
+    }
+    if(ui->cxRightFlap->isChecked())
+    {
+        tmpBit+=32;
+    }
+    if(ui->cxRudder->isChecked())
+    {
+        tmpBit+=64;
+    }
+    if(ui->cxThrottle->isChecked())
+    {
+        tmpBit+=128;
+    }
+
+    generic_16bit r;
+    r.b[1] = 0;
+    r.b[0] = tmpBit;//255;
+
+    tempCtrl.bitfieldPt= (uint16_t)r.s;
+
+    //mavlink_msg_ctrl_srfc_pt_encode(MG::SYSTEM::ID, MG::SYSTEM::COMPID, &msg, &this->tempCtrl);
+    //myUas->sendMessage(msg);
+    qDebug()<<tempCtrl.bitfieldPt;
+}
+
 void UASControlParameters::updateMode(int uas,QString mode,QString description)
 {
     Q_UNUSED(uas);
     Q_UNUSED(description);
     this->mode = mode;
     ui->lbMode->setText(this->mode);
+
+    ui->lbMode->setStyleSheet("background-color: rgb(0, 255, 0)");
+}
+
+void UASControlParameters::thrustChanged(UASInterface *uas, double throttle)
+{
+    Q_UNUSED(uas);
+    this->throttle= throttle;
+}
+
+
+void UASControlParameters::radioChanged(const QPointer<RadioCalibrationData>& radio)
+{
+    if (radio)
+    {
+        if (this->radio)
+        {
+            delete this->radio;
+        }
+
+        this->radio = new RadioCalibrationData(*radio);
+
+//        aileron->set((*radio)(RadioCalibrationData::AILERON));
+//        elevator->set((*radio)(RadioCalibrationData::ELEVATOR));
+//        rudder->set((*radio)(RadioCalibrationData::RUDDER));
+//        gyro->set((*radio)(RadioCalibrationData::GYRO));
+//        pitch->set((*radio)(RadioCalibrationData::PITCH));
+//        throttle->set((*radio)(RadioCalibrationData::THROTTLE));
+    }
 }

src/ui/uas/UASControlParameters.h

@@ -27,7 +27,11 @@ public slots:
     void updateAttitude(UASInterface* uas, double roll, double pitch, double yaw, quint64 time);
     void setCommands();
     void getCommands();
+
+    void setPassthrough();
     void updateMode(int uas,QString mode,QString description);
+    void thrustChanged(UASInterface* uas,double throttle);
+    void radioChanged(const QPointer<RadioCalibrationData>& radio);
 
 private:
     Ui::UASControlParameters *ui;
@@ -36,8 +40,13 @@ private:
     double speed;
     double roll;
     double altitude;
+    double throttle;
     QString mode;
     QString REDcolorStyle;
+    QPointer<RadioCalibrationData> radio;
+    LinkInterface* hilLink;
+    mavlink_mid_lvl_cmds_t tempCmds;
+    mavlink_ctrl_srfc_pt_t tempCtrl;
 };
 
 #endif // UASCONTROLPARAMETERS_H