Call AutoPilotPlugin instead of hard-wired code

src/comm/px4_custom_mode.h

-/*
- * px4_custom_mode.h
- *
- *  Created on: 09.08.2013
- *      Author: ton
- */
-
-#ifndef PX4_CUSTOM_MODE_H_
-#define PX4_CUSTOM_MODE_H_
-
-#include <stdint.h>
-
-enum PX4_CUSTOM_MAIN_MODE {
-	PX4_CUSTOM_MAIN_MODE_MANUAL = 1,
-	PX4_CUSTOM_MAIN_MODE_ALTCTL,
-	PX4_CUSTOM_MAIN_MODE_POSCTL,
-	PX4_CUSTOM_MAIN_MODE_AUTO,
-	PX4_CUSTOM_MAIN_MODE_ACRO,
-	PX4_CUSTOM_MAIN_MODE_OFFBOARD,
-};
-
-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,
-	PX4_CUSTOM_SUB_MODE_AUTO_RTGS
-};
-
-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,7 +28,7 @@
 #include "SerialLink.h"
 #include "UASParameterCommsMgr.h"
 #include <Eigen/Geometry>
-#include <comm/px4_custom_mode.h>
+#include "AutoPilotPlugin.h"
 
 /**
 * Gets the settings from the previous UAS (name, airframe, autopilot, battery specs)
@@ -3326,56 +3326,7 @@ QString UAS::getAudioModeTextFor(int id)
 */
 QString UAS::getShortModeTextFor(uint8_t base_mode, uint32_t custom_mode, int autopilot)
 {
-    QString mode = "";
-
-    if (base_mode & MAV_MODE_FLAG_CUSTOM_MODE_ENABLED) {
-        // use custom_mode - autopilot-specific
-        if (autopilot == MAV_AUTOPILOT_PX4) {
-            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 (px4_mode.main_mode == PX4_CUSTOM_MAIN_MODE_ALTCTL) {
-                mode += "|ALTCTL";
-            } else if (px4_mode.main_mode == PX4_CUSTOM_MAIN_MODE_POSCTL) {
-                mode += "|POSCTL";
-            } 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";
-                }
-            } else if (px4_mode.main_mode == PX4_CUSTOM_MAIN_MODE_OFFBOARD) {
-                mode += "|OFFBOARD";
-            }
-        }
-    }
-
-    // fallback to using base_mode
-    if (mode.length() == 0) {
-        // use base_mode - not autopilot-specific
-        if (base_mode == 0) {
-            mode += "|PREFLIGHT";
-        } else if (base_mode & MAV_MODE_FLAG_DECODE_POSITION_AUTO) {
-            mode += "|AUTO";
-        } else if (base_mode & MAV_MODE_FLAG_DECODE_POSITION_MANUAL) {
-            mode += "|MANUAL";
-            if (base_mode & MAV_MODE_FLAG_DECODE_POSITION_GUIDED) {
-                mode += "|GUIDED";
-            } else if (base_mode & MAV_MODE_FLAG_DECODE_POSITION_STABILIZE) {
-                mode += "|STABILIZED";
-            }
-        }
-    }
+    QString mode = AutoPilotPlugin::getInstanceForAutoPilotPlugin(autopilot)->getShortModeText(base_mode, custom_mode);
 
     if (mode.length() == 0)
     {

src/ui/uas/UASControlWidget.cc

@@ -40,24 +40,10 @@ This file is part of the PIXHAWK project
 #include <UASManager.h>
 #include <UAS.h>
 #include "QGC.h"
-
-static struct full_mode_s modes_list_common[] = {
-    { MAV_MODE_FLAG_MANUAL_INPUT_ENABLED,
-            0 },
-    { (MAV_MODE_FLAG_MANUAL_INPUT_ENABLED | MAV_MODE_FLAG_STABILIZE_ENABLED),
-            0 },
-    { (MAV_MODE_FLAG_MANUAL_INPUT_ENABLED | MAV_MODE_FLAG_STABILIZE_ENABLED | MAV_MODE_FLAG_GUIDED_ENABLED),
-            0 },
-    { (MAV_MODE_FLAG_AUTO_ENABLED | MAV_MODE_FLAG_STABILIZE_ENABLED | MAV_MODE_FLAG_GUIDED_ENABLED),
-            0 },
-};
-
-static struct full_mode_s modes_list_px4[5];
+#include "AutoPilotPlugin.h"
 
 UASControlWidget::UASControlWidget(QWidget *parent) : QWidget(parent),
     uasID(-1),
-    modesList(NULL),
-    modesNum(0),
     modeIdx(0),
     armed(false)
 {
@@ -74,47 +60,23 @@ 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_ALTCTL;
-    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_POSCTL;
-    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;
-    modes_list_px4[4].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_OFFBOARD;
-    modes_list_px4[4].customMode = px4_cm.data;
-
     // Detect autopilot type
-    int autopilot = 0;
+    int autopilot = MAV_AUTOPILOT_GENERIC;
     if (this->uasID >= 0) {
         UASInterface *uas = UASManager::instance()->getUASForId(this->uasID);
         if (uas) {
             autopilot = UASManager::instance()->getUASForId(this->uasID)->getAutopilotType();
         }
     }
-
-    // Use corresponding modes list
-    if (autopilot == MAV_AUTOPILOT_PX4) {
-        modesList = modes_list_px4;
-        modesNum = sizeof(modes_list_px4) / sizeof(struct full_mode_s);
-    } else {
-        modesList = modes_list_common;
-        modesNum = sizeof(modes_list_common) / sizeof(struct full_mode_s);
-    }
+    
+    AutoPilotPlugin* autopilotPlugin = AutoPilotPlugin::getInstanceForAutoPilotPlugin(autopilot);
+    
+    _modeList = autopilotPlugin->getModes();
 
     // Set combobox items
     ui.modeComboBox->clear();
-    for (int i = 0; i < modesNum; i++) {
-        struct full_mode_s mode = modesList[i];
-        ui.modeComboBox->insertItem(i, UAS::getShortModeTextFor(mode.baseMode, mode.customMode, autopilot).remove(0, 2), i);
+    foreach (AutoPilotPlugin::FullMode_t fullMode, _modeList) {
+        ui.modeComboBox->addItem(UAS::getShortModeTextFor(fullMode.baseMode, fullMode.customMode, autopilot).remove(0, 2));
     }
 
     // Select first mode in list
@@ -235,24 +197,24 @@ void UASControlWidget::transmitMode()
 {
     UASInterface* uas_iface = UASManager::instance()->getUASForId(this->uasID);
     if (uas_iface) {
-        if (modeIdx >= 0 && modeIdx < modesNum) {
-            struct full_mode_s mode = modesList[modeIdx];
+        if (modeIdx >= 0 && modeIdx < _modeList.count()) {
+            AutoPilotPlugin::FullMode_t fullMode = _modeList[modeIdx];
             // include armed state
             if (armed) {
-                mode.baseMode |= MAV_MODE_FLAG_SAFETY_ARMED;
+                fullMode.baseMode |= MAV_MODE_FLAG_SAFETY_ARMED;
             } else {
-                mode.baseMode &= ~MAV_MODE_FLAG_SAFETY_ARMED;
+                fullMode.baseMode &= ~MAV_MODE_FLAG_SAFETY_ARMED;
             }
 
             UAS* uas = dynamic_cast<UAS*>(uas_iface);
 
             if (uas->isHilEnabled()) {
-                mode.baseMode |= MAV_MODE_FLAG_HIL_ENABLED;
+                fullMode.baseMode |= MAV_MODE_FLAG_HIL_ENABLED;
             } else {
-                mode.baseMode &= ~MAV_MODE_FLAG_HIL_ENABLED;
+                fullMode.baseMode &= ~MAV_MODE_FLAG_HIL_ENABLED;
             }
 
-            uas->setMode(mode.baseMode, mode.customMode);
+            uas->setMode(fullMode.baseMode, fullMode.customMode);
             QString modeText = ui.modeComboBox->currentText();
 
             ui.lastActionLabel->setText(QString("Sent new mode %1 to %2").arg(modeText).arg(uas->getUASName()));

src/ui/uas/UASControlWidget.h

@@ -38,12 +38,7 @@ This file is part of the QGROUNDCONTROL project
 #include <QPushButton>
 #include <ui_UASControl.h>
 #include <UASInterface.h>
-#include <comm/px4_custom_mode.h>
-
-struct full_mode_s {
-    uint8_t baseMode;
-    uint32_t customMode;
-};
+#include "AutoPilotPlugin.h"
 
 /**
  * @brief Widget controlling one MAV
@@ -83,11 +78,10 @@ protected slots:
     void setBackgroundColor(QColor color);
 
 protected:
-    int uasID;                        ///< Reference to the current uas
-    struct full_mode_s *modesList;  ///< Modes list for the current UAS
-    int modesNum;                   ///< Number of modes in list for the current UAS
-    int modeIdx;                    ///< Current uas mode index
-    bool armed;                  ///< Engine state
+    int uasID;                                      ///< Reference to the current uas
+    QList<AutoPilotPlugin::FullMode_t>  _modeList;  ///< Mode list for the current UAS
+    int modeIdx;                                    ///< Current uas mode index
+    bool armed;                                     ///< Engine state
 
 private:
     Ui::uasControl ui;