Merge branch 'config' of github.com:mavlink/qgroundcontrol into config

qgroundcontrol.pro

@@ -488,7 +488,8 @@ HEADERS += src/MG.h \
     src/uas/UASParameterCommsMgr.h \
     src/ui/QGCPendingParamWidget.h \
     src/ui/px4_configuration/QGCPX4AirframeConfig.h \
-    src/ui/QGCBaseParamWidget.h
+    src/ui/QGCBaseParamWidget.h \
+    src/comm/px4_custom_mode.h
 
 # Google Earth is only supported on Mac OS and Windows with Visual Studio Compiler
 macx|macx-g++|macx-g++42|win32-msvc2008|win32-msvc2010|win32-msvc2012::HEADERS += src/ui/map3D/QGCGoogleEarthView.h

src/comm/px4_custom_mode.h

+#ifndef PX4_CUSTOM_MODE_H
+#define PX4_CUSTOM_MODE_H
+
+enum PX4_CUSTOM_MAIN_MODE {
+    PX4_CUSTOM_MAIN_MODE_MANUAL = 1,
+    PX4_CUSTOM_MAIN_MODE_SEATBELT,
+    PX4_CUSTOM_MAIN_MODE_EASY,
+    PX4_CUSTOM_MAIN_MODE_AUTO,
+};
+
+enum PX4_CUSTOM_SUB_MODE_AUTO {
+    PX4_CUSTOM_SUB_MODE_AUTO_READY = 1,
+    PX4_CUSTOM_SUB_MODE_AUTO_TAKEOFF,
+    PX4_CUSTOM_SUB_MODE_AUTO_LOITER,
+    PX4_CUSTOM_SUB_MODE_AUTO_MISSION,
+    PX4_CUSTOM_SUB_MODE_AUTO_RTL,
+    PX4_CUSTOM_SUB_MODE_AUTO_LAND,
+};
+
+union px4_custom_mode {
+    struct {
+        uint16_t reserved;
+        uint8_t main_mode;
+        uint8_t sub_mode;
+    };
+    uint32_t data;
+    float data_float;
+};
+
+#endif // PX4_CUSTOM_MODE_H

src/uas/UAS.cc

@@ -28,6 +28,7 @@
 #include "SerialLink.h"
 #include "UASParameterCommsMgr.h"
 #include <Eigen/Geometry>
+#include <comm/px4_custom_mode.h>
 
 #ifdef QGC_PROTOBUF_ENABLED
 #include <google/protobuf/descriptor.h>
@@ -1837,7 +1838,7 @@ void UAS::setMode(uint8_t newBaseMode, uint32_t newCustomMode)
     mavlink_message_t msg;
     mavlink_msg_set_mode_pack(mavlink->getSystemId(), mavlink->getComponentId(), &msg, (uint8_t)uasId, newBaseMode, newCustomMode);
     sendMessage(msg);
-    qDebug() << "SENDING REQUEST TO SET MODE TO SYSTEM" << uasId << ", REQUEST TO SET MODE " << newBaseMode;
+    qDebug() << "SENDING REQUEST TO SET MODE TO SYSTEM" << uasId << ", MODE " << newBaseMode << " " << newCustomMode;
 }
 
 /**
@@ -3318,24 +3319,32 @@ QString UAS::getShortModeTextFor(uint8_t base_mode, uint32_t custom_mode, int au
 {
     QString mode = "";
 
-    enum PX4_CUSTOM_MODE {
-        PX4_CUSTOM_MODE_MANUAL = 1,
-        PX4_CUSTOM_MODE_SEATBELT,
-        PX4_CUSTOM_MODE_EASY,
-        PX4_CUSTOM_MODE_AUTO
-    };
-
     if (base_mode & MAV_MODE_FLAG_CUSTOM_MODE_ENABLED) {
         // use custom_mode - autopilot-specific
         if (autopilot == MAV_AUTOPILOT_PX4) {
-            if (custom_mode == PX4_CUSTOM_MODE_MANUAL) {
+            union px4_custom_mode px4_mode;
+            px4_mode.data = custom_mode;
+            if (px4_mode.main_mode == PX4_CUSTOM_MAIN_MODE_MANUAL) {
                 mode += "|MANUAL";
-            } else if (custom_mode == PX4_CUSTOM_MODE_SEATBELT) {
+            } else if (px4_mode.main_mode == PX4_CUSTOM_MAIN_MODE_SEATBELT) {
                 mode += "|SEATBELT";
-            } else if (custom_mode == PX4_CUSTOM_MODE_EASY) {
+            } else if (px4_mode.main_mode == PX4_CUSTOM_MAIN_MODE_EASY) {
                 mode += "|EASY";
-            } else if (custom_mode == PX4_CUSTOM_MODE_AUTO) {
+            } else if (px4_mode.main_mode == PX4_CUSTOM_MAIN_MODE_AUTO) {
                 mode += "|AUTO";
+                if (px4_mode.sub_mode == PX4_CUSTOM_SUB_MODE_AUTO_READY) {
+                    mode += "|READY";
+                } else if (px4_mode.sub_mode == PX4_CUSTOM_SUB_MODE_AUTO_TAKEOFF) {
+                    mode += "|TAKEOFF";
+                } else if (px4_mode.sub_mode == PX4_CUSTOM_SUB_MODE_AUTO_LOITER) {
+                    mode += "|LOITER";
+                } else if (px4_mode.sub_mode == PX4_CUSTOM_SUB_MODE_AUTO_MISSION) {
+                    mode += "|MISSION";
+                } else if (px4_mode.sub_mode == PX4_CUSTOM_SUB_MODE_AUTO_RTL) {
+                    mode += "|RTL";
+                } else if (px4_mode.sub_mode == PX4_CUSTOM_SUB_MODE_AUTO_LAND) {
+                    mode += "|LAND";
+                }
             }
         }
     }

src/ui/uas/UASControlWidget.cc

@@ -52,16 +52,7 @@ static struct full_mode_s modes_list_common[] = {
             0 },
 };
 
-static struct full_mode_s modes_list_px4[] = {
-    { (MAV_MODE_FLAG_CUSTOM_MODE_ENABLED | MAV_MODE_FLAG_MANUAL_INPUT_ENABLED),
-            PX4_CUSTOM_MODE_MANUAL },
-    { (MAV_MODE_FLAG_CUSTOM_MODE_ENABLED | MAV_MODE_FLAG_MANUAL_INPUT_ENABLED | MAV_MODE_FLAG_STABILIZE_ENABLED),
-            PX4_CUSTOM_MODE_SEATBELT },
-    { (MAV_MODE_FLAG_CUSTOM_MODE_ENABLED | MAV_MODE_FLAG_MANUAL_INPUT_ENABLED | MAV_MODE_FLAG_STABILIZE_ENABLED | MAV_MODE_FLAG_GUIDED_ENABLED),
-            PX4_CUSTOM_MODE_EASY },
-    { (MAV_MODE_FLAG_CUSTOM_MODE_ENABLED | MAV_MODE_FLAG_AUTO_ENABLED | MAV_MODE_FLAG_STABILIZE_ENABLED | MAV_MODE_FLAG_GUIDED_ENABLED),
-            PX4_CUSTOM_MODE_AUTO },
-};
+static struct full_mode_s modes_list_px4[4];
 
 UASControlWidget::UASControlWidget(QWidget *parent) : QWidget(parent),
     uasID(-1),
@@ -83,6 +74,21 @@ UASControlWidget::UASControlWidget(QWidget *parent) : QWidget(parent),
 
 void UASControlWidget::updateModesList()
 {
+    union px4_custom_mode px4_cm;
+    px4_cm.data = 0;
+    modes_list_px4[0].baseMode = MAV_MODE_FLAG_CUSTOM_MODE_ENABLED | MAV_MODE_FLAG_MANUAL_INPUT_ENABLED;
+    px4_cm.main_mode = PX4_CUSTOM_MAIN_MODE_MANUAL;
+    modes_list_px4[0].customMode = px4_cm.data;
+    modes_list_px4[1].baseMode = MAV_MODE_FLAG_CUSTOM_MODE_ENABLED | MAV_MODE_FLAG_MANUAL_INPUT_ENABLED | MAV_MODE_FLAG_STABILIZE_ENABLED;
+    px4_cm.main_mode = PX4_CUSTOM_MAIN_MODE_SEATBELT;
+    modes_list_px4[1].customMode = px4_cm.data;
+    modes_list_px4[2].baseMode = MAV_MODE_FLAG_CUSTOM_MODE_ENABLED | MAV_MODE_FLAG_MANUAL_INPUT_ENABLED | MAV_MODE_FLAG_STABILIZE_ENABLED | MAV_MODE_FLAG_GUIDED_ENABLED;
+    px4_cm.main_mode = PX4_CUSTOM_MAIN_MODE_EASY;
+    modes_list_px4[2].customMode = px4_cm.data;
+    modes_list_px4[3].baseMode = MAV_MODE_FLAG_CUSTOM_MODE_ENABLED | MAV_MODE_FLAG_AUTO_ENABLED | MAV_MODE_FLAG_STABILIZE_ENABLED | MAV_MODE_FLAG_GUIDED_ENABLED;
+    px4_cm.main_mode = PX4_CUSTOM_MAIN_MODE_AUTO;
+    modes_list_px4[3].customMode = px4_cm.data;
+
     // Detect autopilot type
     int autopilot = 0;
     if (this->uasID >= 0) {

src/ui/uas/UASControlWidget.h

@@ -38,13 +38,7 @@ This file is part of the QGROUNDCONTROL project
 #include <QPushButton>
 #include <ui_UASControl.h>
 #include <UASInterface.h>
-
-enum PX4_CUSTOM_MODE {
-    PX4_CUSTOM_MODE_MANUAL = 1,
-    PX4_CUSTOM_MODE_SEATBELT,
-    PX4_CUSTOM_MODE_EASY,
-    PX4_CUSTOM_MODE_AUTO
-};
+#include <comm/px4_custom_mode.h>
 
 struct full_mode_s {
     uint8_t baseMode;