Fixed bug in named value decoding, added additional functionality to HIL config widget

qgroundcontrol.pro

@@ -50,7 +50,7 @@ OBJECTS_DIR = $${BUILDDIR}/obj
 MOC_DIR = $${BUILDDIR}/moc
 UI_DIR = $${BUILDDIR}/ui
 RCC_DIR = $${BUILDDIR}/rcc
-MAVLINK_CONF = ""
+MAVLINK_CONF = "pixhawk"
 MAVLINKPATH = $$BASEDIR/libs/mavlink/include/mavlink/v1.0
 DEFINES += MAVLINK_NO_DATA
 

src/comm/QGCXPlaneLink.cc

@@ -466,6 +466,70 @@ bool QGCXPlaneLink::disconnectSimulation()
     return !connectState;
 }
 
+void QGCXPlaneLink::selectPlane(const QString& plane)
+{
+
+}
+
+void QGCXPlaneLink::setPositionAttitude(double lat, double lon, double alt, double roll, double pitch, double yaw)
+{
+    struct VEH1_struct
+    {
+        quint32 p;
+        double lat_lon_ele[3];
+        float psi_the_phi[3];
+        float gear_flap_vect[3];
+    } pos;
+
+    pos.p = 0;
+    pos.lat_lon_ele[0] = lat;
+    pos.lat_lon_ele[1] = lon;
+    pos.lat_lon_ele[2] = alt;
+}
+
+/**
+ * Sets a random position with an offset of max 1/1000 degree
+ * and max 100 m altitude
+ */
+void QGCXPlaneLink::setRandomPosition()
+{
+    // Initialize generator
+    srand(0);
+
+    double offLat = rand() / static_cast<double>(RAND_MAX) / 500.0 + 1.0/500.0;
+    double offLon = rand() / static_cast<double>(RAND_MAX) / 500.0 + 1.0/500.0;
+    double offAlt = rand() / static_cast<double>(RAND_MAX) * 200.0 + 100.0;
+
+    if (mav->getAltitude() + offAlt < 0)
+    {
+        offAlt *= -1.0;
+    }
+
+    setPositionAttitude(mav->getLatitude() + offLat,
+                        mav->getLongitude() + offLon,
+                        mav->getAltitude() + offAlt,
+                        mav->getRoll(),
+                        mav->getPitch(),
+                        mav->getYaw());
+}
+
+void QGCXPlaneLink::setRandomAttitude()
+{
+    // Initialize generator
+    srand(0);
+
+    double roll = rand() / static_cast<double>(RAND_MAX) * 2.0 - 1.0;
+    double pitch = rand() / static_cast<double>(RAND_MAX) * 2.0 - 1.0;
+    double yaw = rand() / static_cast<double>(RAND_MAX) * 2.0 - 1.0;
+
+    setPositionAttitude(mav->getLatitude(),
+                        mav->getLongitude(),
+                        mav->getAltitude(),
+                        roll,
+                        pitch,
+                        yaw);
+}
+
 /**
  * @brief Connect the connection.
  *

src/comm/QGCXPlaneLink.h

@@ -87,6 +87,31 @@ public slots:
     void writeBytes(const char* data, qint64 length);
     bool connectSimulation();
     bool disconnectSimulation();
+    /**
+     * @brief Select airplane model
+     * @param plane the name of the airplane
+     */
+    void selectPlane(const QString& plane);
+    /**
+     * @brief Set the airplane position and attitude
+     * @param lat
+     * @param lon
+     * @param alt
+     * @param roll
+     * @param pitch
+     * @param yaw
+     */
+    void setPositionAttitude(double lat, double lon, double alt, double roll, double pitch, double yaw);
+
+    /**
+     * @brief Set a random position
+     */
+    void setRandomPosition();
+
+    /**
+     * @brief Set a random attitude
+     */
+    void setRandomAttitude();
 
 protected:
     UASInterface* mav;

src/ui/MAVLinkDecoder.cc

@@ -216,7 +216,10 @@ void MAVLinkDecoder::emitFieldValue(mavlink_message_t* msg, int fieldid, quint64
     {
         mavlink_debug_vect_t debug;
         mavlink_msg_debug_vect_decode(msg, &debug);
-        name = name.arg(QString(debug.name), fieldName);
+        char buf[11];
+        strncpy(buf, debug.name, 10);
+        buf[10] = '\0';
+        name = QString(buf);
         time = getUnixTimeFromMs(msg->sysid, (debug.time_usec+500)/1000); // Scale to milliseconds, round up/down correctly
     }
     else if (msgid == MAVLINK_MSG_ID_DEBUG)
@@ -230,14 +233,20 @@ void MAVLinkDecoder::emitFieldValue(mavlink_message_t* msg, int fieldid, quint64
     {
         mavlink_named_value_float_t debug;
         mavlink_msg_named_value_float_decode(msg, &debug);
-        name = debug.name;
+        char buf[11];
+        strncpy(buf, debug.name, 10);
+        buf[10] = '\0';
+        name = QString(buf);
         time = getUnixTimeFromMs(msg->sysid, debug.time_boot_ms);
     }
     else if (msgid == MAVLINK_MSG_ID_NAMED_VALUE_INT)
     {
         mavlink_named_value_int_t debug;
         mavlink_msg_named_value_int_decode(msg, &debug);
-        name = name.arg(debug.name).arg(fieldName);
+        char buf[11];
+        strncpy(buf, debug.name, 10);
+        buf[10] = '\0';
+        name = QString(buf);
         time = getUnixTimeFromMs(msg->sysid, debug.time_boot_ms);
     }
 //    else if (msgid == MAVLINK_MSG_ID_HIGHRES_IMU)

src/ui/QGCHilConfiguration.cc

 #include "QGCHilConfiguration.h"
 #include "ui_QGCHilConfiguration.h"
+#include "QGCXPlaneLink.h"
 
 QGCHilConfiguration::QGCHilConfiguration(QGCHilLink* link, QWidget *parent) :
     QWidget(parent),
@@ -12,6 +13,16 @@ QGCHilConfiguration::QGCHilConfiguration(QGCHilLink* link, QWidget *parent) :
     connect(ui->hostComboBox, SIGNAL(activated(QString)), link, SLOT(setRemoteHost(QString)));
 
     ui->startButton->setText(tr("Connect"));
+
+    QGCXPlaneLink* xplane = dynamic_cast<QGCXPlaneLink*>(link);
+
+    if (xplane)
+    {
+        connect(ui->randomAttitudeButton, SIGNAL(clicked()), link, SLOT(setRandomAttitude()));
+        connect(ui->randomPositionButton, SIGNAL(clicked()), link, SLOT(setRandomPosition()));
+        connect(ui->airframeComboBox, SIGNAL(activated(QString)), link, SLOT(setAirframe(QString)));
+    }
+//    connect(ui->)
 }
 
 void QGCHilConfiguration::toggleSimulation(bool connect)

src/ui/QGCHilConfiguration.ui

@@ -7,28 +7,28 @@
     <x>0</x>
     <y>0</y>
     <width>305</width>
-    <height>172</height>
+    <height>202</height>
    </rect>
   </property>
   <property name="windowTitle">
    <string>Form</string>
   </property>
   <layout class="QGridLayout" name="gridLayout" columnstretch="40,60,50,50">
-   <item row="4" column="0" colspan="3">
+   <item row="5" column="0" colspan="3">
     <widget class="QLabel" name="statusLabel">
      <property name="text">
       <string>Status</string>
      </property>
     </widget>
    </item>
-   <item row="5" column="0" colspan="2">
+   <item row="6" column="0" colspan="2">
     <widget class="QPushButton" name="randomAttitudeButton">
      <property name="text">
       <string>Random ATT</string>
      </property>
     </widget>
    </item>
-   <item row="3" column="0" colspan="2">
+   <item row="4" column="0" colspan="2">
     <widget class="QPushButton" name="startButton">
      <property name="text">
       <string>Start</string>
@@ -42,21 +42,21 @@
      </property>
     </widget>
    </item>
-   <item row="1" column="0">
+   <item row="2" column="0">
     <widget class="QLabel" name="hostLabel">
      <property name="text">
       <string>Host</string>
      </property>
     </widget>
    </item>
-   <item row="3" column="2" colspan="2">
+   <item row="4" column="2" colspan="2">
     <widget class="QPushButton" name="setHomeButton">
      <property name="text">
       <string>Set HOME</string>
      </property>
     </widget>
    </item>
-   <item row="1" column="1" colspan="3">
+   <item row="2" column="1" colspan="3">
     <widget class="QComboBox" name="hostComboBox">
      <property name="editable">
       <bool>true</bool>
@@ -77,13 +77,62 @@
      </item>
     </widget>
    </item>
-   <item row="5" column="2" colspan="2">
+   <item row="6" column="2" colspan="2">
     <widget class="QPushButton" name="randomPositionButton">
      <property name="text">
       <string>Random POS</string>
      </property>
     </widget>
    </item>
+   <item row="1" column="0">
+    <widget class="QLabel" name="label">
+     <property name="text">
+      <string>Airframe</string>
+     </property>
+    </widget>
+   </item>
+   <item row="1" column="1" colspan="3">
+    <widget class="QComboBox" name="airframeComboBox">
+     <property name="editable">
+      <bool>true</bool>
+     </property>
+     <item>
+      <property name="text">
+       <string>QRO_X</string>
+      </property>
+     </item>
+     <item>
+      <property name="text">
+       <string>Unlimited</string>
+      </property>
+     </item>
+     <item>
+      <property name="text">
+       <string>Twinjet</string>
+      </property>
+     </item>
+     <item>
+      <property name="text">
+       <string>Early Bird</string>
+      </property>
+     </item>
+     <item>
+      <property name="text">
+       <string>Reno Racer</string>
+      </property>
+     </item>
+     <item>
+      <property name="text">
+       <string>Slowstick</string>
+      </property>
+     </item>
+     <item>
+      <property name="text">
+       <string>Tiny</string>
+      </property>
+     </item>
+    </widget>
+   </item>
   </layout>
  </widget>
  <resources/>