Merge branch 'new_state_machine_modes' of https://github.com/DrTon/qgroundcontrol into config

src/uas/UAS.cc

@@ -57,9 +57,9 @@ UAS::UAS(MAVLinkProtocol* protocol, int id) : UASInterface(),
     airframe(QGC_AIRFRAME_GENERIC),
     autopilot(-1),
     systemIsArmed(false),
-    mode(-1),
+    base_mode(0),
+    custom_mode(0),
     // custom_mode not initialized
-    navMode(-1),
     status(-1),
     // shortModeText not initialized
     // shortStateText not initialized
@@ -337,7 +337,7 @@ void UAS::updateState()
     }
     else
     {
-        if (((mode&MAV_MODE_FLAG_DECODE_POSITION_AUTO) || (mode&MAV_MODE_FLAG_DECODE_POSITION_GUIDED)) && positionLock)
+        if (((base_mode & MAV_MODE_FLAG_DECODE_POSITION_AUTO) || (base_mode & MAV_MODE_FLAG_DECODE_POSITION_GUIDED)) && positionLock)
         {
             GAudioOutput::instance()->notifyNegative();
         }
@@ -568,24 +568,18 @@ void UAS::receiveMessage(LinkInterface* link, mavlink_message_t message)
                 stateAudio = uasState;
             }
 
-            if (this->mode != static_cast<int>(state.base_mode))
+            if (this->base_mode != state.base_mode || this->custom_mode != state.custom_mode)
             {
                 modechanged = true;
-                this->mode = static_cast<int>(state.base_mode);
-                shortModeText = getShortModeTextFor(this->mode);
+                this->base_mode = state.base_mode;
+                this->custom_mode = state.custom_mode;
+                shortModeText = getShortModeTextFor(this->base_mode, this->custom_mode, this->autopilot);
 
                 emit modeChanged(this->getUASID(), shortModeText, "");
 
                 modeAudio = " is now in " + audiomodeText;
             }
 
-            if (navMode != state.custom_mode)
-            {
-                emit navModeChanged(uasId, state.custom_mode, getNavModeText(state.custom_mode));
-                navMode = state.custom_mode;
-                //navModeAudio = tr(" changed nav mode to ") + tr("FIXME");
-            }
-
             // AUDIO
             if (modechanged && statechanged)
             {
@@ -595,7 +589,7 @@ void UAS::receiveMessage(LinkInterface* link, mavlink_message_t message)
             else if (modechanged || statechanged)
             {
                 // Output the one message
-                audiostring += modeAudio + stateAudio + navModeAudio;
+                audiostring += modeAudio + stateAudio;
             }
 
             if (statechanged && ((int)state.system_status == (int)MAV_STATE_CRITICAL || state.system_status == (int)MAV_STATE_EMERGENCY))
@@ -1826,25 +1820,24 @@ QList<int> UAS::getComponentIds()
 /**
 * @param mode that UAS is to be set to.
 */
-void UAS::setMode(int mode)
+void UAS::setMode(uint8_t newBaseMode, uint32_t newCustomMode)
 {
     //this->mode = mode; //no call assignament, update receive message from UAS
 
     // Strip armed / disarmed call, this is not relevant for setting the mode
-    uint8_t newMode = mode;
-    newMode &= (~(uint8_t)MAV_MODE_FLAG_SAFETY_ARMED);
+    newBaseMode &= ~MAV_MODE_FLAG_SAFETY_ARMED;
     // Now set current state (request no change)
-    newMode |= (uint8_t)(this->mode) & (uint8_t)(MAV_MODE_FLAG_SAFETY_ARMED);
+    newBaseMode |= this->base_mode & MAV_MODE_FLAG_SAFETY_ARMED;
 
     // Strip HIL part, replace it with current system state
-    newMode &= (~(uint8_t)MAV_MODE_FLAG_HIL_ENABLED);
+    newBaseMode &= (~MAV_MODE_FLAG_HIL_ENABLED);
     // Now set current state (request no change)
-    newMode |= (uint8_t)(this->mode) & (uint8_t)(MAV_MODE_FLAG_HIL_ENABLED);
+    newBaseMode |= this->base_mode & MAV_MODE_FLAG_HIL_ENABLED;
 
     mavlink_message_t msg;
-    mavlink_msg_set_mode_pack(mavlink->getSystemId(), mavlink->getComponentId(), &msg, (uint8_t)uasId, newMode, (uint16_t)navMode);
+    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 " << newMode;
+    qDebug() << "SENDING REQUEST TO SET MODE TO SYSTEM" << uasId << ", REQUEST TO SET MODE " << newBaseMode;
 }
 
 /**
@@ -1945,35 +1938,6 @@ float UAS::filterVoltage(float value) const
     return lpVoltage * 0.7f + value * 0.3f;
 }
 
-/**
-* The mode can be preflight or unknown.
-* @Return the mode of the autopilot
-*/
-QString UAS::getNavModeText(int mode)
-{
-    if (autopilot == MAV_AUTOPILOT_PIXHAWK)
-    {
-        switch (mode)
-        {
-        case 0:
-            return QString("PREFLIGHT");
-            break;
-        default:
-            return QString("UNKNOWN");
-        }
-    }
-    else if (autopilot == MAV_AUTOPILOT_ARDUPILOTMEGA)
-    {
-        return QString("UNKNOWN");
-    }
-    else if (autopilot == MAV_AUTOPILOT_OPENPILOT)
-    {
-        return QString("UNKNOWN");
-    }
-    // If nothing matches, return unknown
-    return QString("UNKNOWN");
-}
-
 /**
 * Get the status of the code and a description of the status.
 * Status can be unitialized, booting up, calibrating sensors, active
@@ -2750,7 +2714,7 @@ void UAS::launch()
 void UAS::armSystem()
 {
     mavlink_message_t msg;
-    mavlink_msg_set_mode_pack(mavlink->getSystemId(), mavlink->getComponentId(), &msg, this->getUASID(), mode | MAV_MODE_FLAG_SAFETY_ARMED, navMode);
+    mavlink_msg_set_mode_pack(mavlink->getSystemId(), mavlink->getComponentId(), &msg, this->getUASID(), base_mode | MAV_MODE_FLAG_SAFETY_ARMED, custom_mode);
     sendMessage(msg);
 }
 
@@ -2761,35 +2725,35 @@ void UAS::armSystem()
 void UAS::disarmSystem()
 {
     mavlink_message_t msg;
-    mavlink_msg_set_mode_pack(mavlink->getSystemId(), mavlink->getComponentId(), &msg, this->getUASID(), mode & ~MAV_MODE_FLAG_SAFETY_ARMED, navMode);
+    mavlink_msg_set_mode_pack(mavlink->getSystemId(), mavlink->getComponentId(), &msg, this->getUASID(), base_mode & ~MAV_MODE_FLAG_SAFETY_ARMED, custom_mode);
     sendMessage(msg);
 }
 
 void UAS::toggleArmedState()
 {
     mavlink_message_t msg;
-    mavlink_msg_set_mode_pack(mavlink->getSystemId(), mavlink->getComponentId(), &msg, this->getUASID(), mode ^ MAV_MODE_FLAG_SAFETY_ARMED, navMode);
+    mavlink_msg_set_mode_pack(mavlink->getSystemId(), mavlink->getComponentId(), &msg, this->getUASID(), base_mode ^ MAV_MODE_FLAG_SAFETY_ARMED, custom_mode);
     sendMessage(msg);
 }
 
 void UAS::goAutonomous()
 {
     mavlink_message_t msg;
-    mavlink_msg_set_mode_pack(mavlink->getSystemId(), mavlink->getComponentId(), &msg, this->getUASID(), mode & MAV_MODE_FLAG_AUTO_ENABLED & ~MAV_MODE_FLAG_MANUAL_INPUT_ENABLED, navMode);
+    mavlink_msg_set_mode_pack(mavlink->getSystemId(), mavlink->getComponentId(), &msg, this->getUASID(), MAV_MODE_FLAG_AUTO_ENABLED, 0);
     sendMessage(msg);
 }
 
 void UAS::goManual()
 {
     mavlink_message_t msg;
-    mavlink_msg_set_mode_pack(mavlink->getSystemId(), mavlink->getComponentId(), &msg, this->getUASID(), mode & ~MAV_MODE_FLAG_AUTO_ENABLED & MAV_MODE_FLAG_MANUAL_INPUT_ENABLED, navMode);
+    mavlink_msg_set_mode_pack(mavlink->getSystemId(), mavlink->getComponentId(), &msg, this->getUASID(), MAV_MODE_FLAG_MANUAL_INPUT_ENABLED, 0);
     sendMessage(msg);
 }
 
 void UAS::toggleAutonomy()
 {
     mavlink_message_t msg;
-    mavlink_msg_set_mode_pack(mavlink->getSystemId(), mavlink->getComponentId(), &msg, this->getUASID(), mode ^ MAV_MODE_FLAG_AUTO_ENABLED ^ MAV_MODE_FLAG_MANUAL_INPUT_ENABLED, navMode);
+    mavlink_msg_set_mode_pack(mavlink->getSystemId(), mavlink->getComponentId(), &msg, this->getUASID(), base_mode ^ MAV_MODE_FLAG_AUTO_ENABLED ^ MAV_MODE_FLAG_MANUAL_INPUT_ENABLED, 0);
     sendMessage(msg);
 }
 
@@ -2813,7 +2777,7 @@ void UAS::setManualControlCommands(double roll, double pitch, double yaw, double
     manualThrust = thrust * thrustScaling;
 
     // If system has manual inputs enabled and is armed
-    if(((mode & MAV_MODE_FLAG_DECODE_POSITION_MANUAL) && (mode & MAV_MODE_FLAG_DECODE_POSITION_SAFETY)) || (mode & MAV_MODE_FLAG_HIL_ENABLED))
+    if(((base_mode & MAV_MODE_FLAG_DECODE_POSITION_MANUAL) && (base_mode & MAV_MODE_FLAG_DECODE_POSITION_SAFETY)) || (base_mode & MAV_MODE_FLAG_HIL_ENABLED))
     {
         mavlink_message_t message;
         mavlink_msg_manual_control_pack(mavlink->getSystemId(), mavlink->getComponentId(), &message, this->uasId, (float)manualPitchAngle, (float)manualRollAngle, (float)manualThrust, (float)manualYawAngle, buttons);
@@ -2831,7 +2795,7 @@ void UAS::setManualControlCommands(double roll, double pitch, double yaw, double
 void UAS::setManual6DOFControlCommands(double x, double y, double z, double roll, double pitch, double yaw)
 {
     // If system has manual inputs enabled and is armed
-    if(((mode & MAV_MODE_FLAG_DECODE_POSITION_MANUAL) && (mode & MAV_MODE_FLAG_DECODE_POSITION_SAFETY)) || (mode & MAV_MODE_FLAG_HIL_ENABLED))
+    if(((base_mode & MAV_MODE_FLAG_DECODE_POSITION_MANUAL) && (base_mode & MAV_MODE_FLAG_DECODE_POSITION_SAFETY)) || (base_mode & MAV_MODE_FLAG_HIL_ENABLED))
     {
         mavlink_message_t message;
         mavlink_msg_setpoint_6dof_pack(mavlink->getSystemId(), mavlink->getComponentId(), &message, this->uasId, (float)x, (float)y, (float)z, (float)roll, (float)pitch, (float)yaw);
@@ -3110,7 +3074,7 @@ void UAS::sendHilState(quint64 time_us, float roll, float pitch, float yaw, floa
                        float pitchspeed, float yawspeed, double lat, double lon, double alt,
                        float vx, float vy, float vz, float ind_airspeed, float true_airspeed, float xacc, float yacc, float zacc)
 {
-    if (this->mode & MAV_MODE_FLAG_HIL_ENABLED)
+    if (this->base_mode & MAV_MODE_FLAG_HIL_ENABLED)
     {
         float q[4];
 
@@ -3139,7 +3103,7 @@ void UAS::sendHilState(quint64 time_us, float roll, float pitch, float yaw, floa
     {
         // Attempt to set HIL mode
         mavlink_message_t msg;
-        mavlink_msg_set_mode_pack(mavlink->getSystemId(), mavlink->getComponentId(), &msg, this->getUASID(), mode | MAV_MODE_FLAG_HIL_ENABLED, navMode);
+        mavlink_msg_set_mode_pack(mavlink->getSystemId(), mavlink->getComponentId(), &msg, this->getUASID(), base_mode | MAV_MODE_FLAG_HIL_ENABLED, custom_mode);
         sendMessage(msg);
         qDebug() << __FILE__ << __LINE__ << "HIL is onboard not enabled, trying to enable.";
     }
@@ -3148,7 +3112,7 @@ void UAS::sendHilState(quint64 time_us, float roll, float pitch, float yaw, floa
 void UAS::sendHilSensors(quint64 time_us, float xacc, float yacc, float zacc, float rollspeed, float pitchspeed, float yawspeed,
                                     float xmag, float ymag, float zmag, float abs_pressure, float diff_pressure, float pressure_alt, float temperature, quint32 fields_changed)
 {
-    if (this->mode & MAV_MODE_FLAG_HIL_ENABLED)
+    if (this->base_mode & MAV_MODE_FLAG_HIL_ENABLED)
     {
         mavlink_message_t msg;
         mavlink_msg_hil_sensor_pack(mavlink->getSystemId(), mavlink->getComponentId(), &msg,
@@ -3162,7 +3126,7 @@ void UAS::sendHilSensors(quint64 time_us, float xacc, float yacc, float zacc, fl
     {
         // Attempt to set HIL mode
         mavlink_message_t msg;
-        mavlink_msg_set_mode_pack(mavlink->getSystemId(), mavlink->getComponentId(), &msg, this->getUASID(), mode | MAV_MODE_FLAG_HIL_ENABLED, navMode);
+        mavlink_msg_set_mode_pack(mavlink->getSystemId(), mavlink->getComponentId(), &msg, this->getUASID(), base_mode | MAV_MODE_FLAG_HIL_ENABLED, custom_mode);
         sendMessage(msg);
         qDebug() << __FILE__ << __LINE__ << "HIL is onboard not enabled, trying to enable.";
     }
@@ -3174,7 +3138,7 @@ void UAS::sendHilGps(quint64 time_us, double lat, double lon, double alt, int fi
     if (QGC::groundTimeMilliseconds() - lastSendTimeGPS < 100)
         return;
 
-    if (this->mode & MAV_MODE_FLAG_HIL_ENABLED)
+    if (this->base_mode & MAV_MODE_FLAG_HIL_ENABLED)
     {
         float course = cog;
         // map to 0..2pi
@@ -3193,7 +3157,7 @@ void UAS::sendHilGps(quint64 time_us, double lat, double lon, double alt, int fi
     {
         // Attempt to set HIL mode
         mavlink_message_t msg;
-        mavlink_msg_set_mode_pack(mavlink->getSystemId(), mavlink->getComponentId(), &msg, this->getUASID(), mode | MAV_MODE_FLAG_HIL_ENABLED, navMode);
+        mavlink_msg_set_mode_pack(mavlink->getSystemId(), mavlink->getComponentId(), &msg, this->getUASID(), base_mode | MAV_MODE_FLAG_HIL_ENABLED, custom_mode);
         sendMessage(msg);
         qDebug() << __FILE__ << __LINE__ << "HIL is onboard not enabled, trying to enable.";
     }
@@ -3209,7 +3173,7 @@ void UAS::startHil()
     hilEnabled = true;
     sensorHil = false;
     mavlink_message_t msg;
-    mavlink_msg_set_mode_pack(mavlink->getSystemId(), mavlink->getComponentId(), &msg, this->getUASID(), mode | MAV_MODE_FLAG_HIL_ENABLED, navMode);
+    mavlink_msg_set_mode_pack(mavlink->getSystemId(), mavlink->getComponentId(), &msg, this->getUASID(), base_mode | MAV_MODE_FLAG_HIL_ENABLED, custom_mode);
     sendMessage(msg);
     // Connect HIL simulation link
     simulation->connectSimulation();
@@ -3222,7 +3186,7 @@ void UAS::stopHil()
 {
     if (simulation) simulation->disconnectSimulation();
     mavlink_message_t msg;
-    mavlink_msg_set_mode_pack(mavlink->getSystemId(), mavlink->getComponentId(), &msg, this->getUASID(), mode & !MAV_MODE_FLAG_HIL_ENABLED, navMode);
+    mavlink_msg_set_mode_pack(mavlink->getSystemId(), mavlink->getComponentId(), &msg, this->getUASID(), base_mode & !MAV_MODE_FLAG_HIL_ENABLED, custom_mode);
     sendMessage(msg);
     hilEnabled = false;
     sensorHil = false;
@@ -3350,51 +3314,57 @@ QString UAS::getAudioModeTextFor(int id)
 * The mode returned can be auto, stabilized, test, manual, preflight or unknown.
 * @return the short text of the mode for the id given.
 */
-QString UAS::getShortModeTextFor(int id)
+QString UAS::getShortModeTextFor(uint8_t base_mode, uint32_t custom_mode, int autopilot)
 {
     QString mode = "";
-    uint8_t modeid = id;
-
 
-    // BASE MODE DECODING
-
-    if (modeid == 0)
-    {
-        mode = "|PREFLIGHT";
-    }
-    else {
-        if (modeid & (uint8_t)MAV_MODE_FLAG_DECODE_POSITION_AUTO){
-            mode += "|AUTO";
+    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) {
+                mode += "|MANUAL";
+            } else if (custom_mode == PX4_CUSTOM_MODE_SEATBELT) {
+                mode += "|SEATBELT";
+            } else if (custom_mode == PX4_CUSTOM_MODE_EASY) {
+                mode += "|EASY";
+            } else if (custom_mode == PX4_CUSTOM_MODE_AUTO) {
+                mode += "|AUTO";
+            }
         }
+    }
 
-        if (modeid & (uint8_t)MAV_MODE_FLAG_DECODE_POSITION_MANUAL){
+    // 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";
+            }
         }
-
-        if (modeid & (uint8_t)MAV_MODE_FLAG_DECODE_POSITION_GUIDED){
-            mode += "|VECTOR";
-        }
-
-        if (modeid & (uint8_t)MAV_MODE_FLAG_DECODE_POSITION_STABILIZE){
-            mode += "|STABILIZED";
-        }
-
-
-        if (modeid & (uint8_t)MAV_MODE_FLAG_DECODE_POSITION_TEST){
-            mode += "|TEST";
-        }
-
-
     }
 
     if (mode.length() == 0)
     {
         mode = "|UNKNOWN";
-        qDebug() << __FILE__ << __LINE__ << " Unknown modeid: " << modeid;
+        qDebug() << __FILE__ << __LINE__ << " Unknown mode: base_mode=" << base_mode << " custom_mode=" << custom_mode << " autopilot=" << autopilot;
     }
 
     // ARMED STATE DECODING
-    if (modeid & (uint8_t)MAV_MODE_FLAG_DECODE_POSITION_SAFETY)
+    if (base_mode & MAV_MODE_FLAG_DECODE_POSITION_SAFETY)
     {
         mode.prepend("A");
     }
@@ -3404,12 +3374,12 @@ QString UAS::getShortModeTextFor(int id)
     }
 
     // HARDWARE IN THE LOOP DECODING
-    if (modeid & (uint8_t)MAV_MODE_FLAG_DECODE_POSITION_HIL)
+    if (base_mode & MAV_MODE_FLAG_DECODE_POSITION_HIL)
     {
         mode.prepend("HIL:");
     }
 
-    qDebug() << "MODE: " << modeid << " " << mode;
+    //qDebug() << "base_mode=" << base_mode << " custom_mode=" << custom_mode << " autopilot=" << autopilot << ": " << mode;
 
     return mode;
 }

src/uas/UAS.h

@@ -69,7 +69,7 @@ public:
     /** @brief Get short mode */
     const QString& getShortMode() const;
     /** @brief Translate from mode id to text */
-    static QString getShortModeTextFor(int id);
+    static QString getShortModeTextFor(uint8_t base_mode, uint32_t custom_mode, int autopilot);
     /** @brief Translate from mode id to audio text */
     static QString getAudioModeTextFor(int id);
     /** @brief Get the unique system id */
@@ -362,9 +362,8 @@ protected: //COMMENTS FOR TEST UNIT
     int airframe;                 ///< The airframe type
     int autopilot;                ///< Type of the Autopilot: -1: None, 0: Generic, 1: PIXHAWK, 2: SLUGS, 3: Ardupilot (up to 15 types), defined in MAV_AUTOPILOT_TYPE ENUM
     bool systemIsArmed;           ///< If the system is armed
-    uint8_t mode;                 ///< The current mode of the MAV
+    uint8_t base_mode;                 ///< The current mode of the MAV
     uint32_t custom_mode;         ///< The current mode of the MAV
-    uint32_t navMode;             ///< The current navigation mode of the MAV
     int status;                   ///< The current status of the MAV
     QString shortModeText;        ///< Short textual mode description
     QString shortStateText;       ///< Short textual state description
@@ -506,8 +505,6 @@ public:
     float getChargeLevel();
     /** @brief Get the human-readable status message for this code */
     void getStatusForCode(int statusCode, QString& uasState, QString& stateDescription);
-    /** @brief Get the human-readable navigation mode translation for this mode */
-    QString getNavModeText(int mode);
     /** @brief Check if vehicle is in autonomous mode */
     bool isAuto();
     /** @brief Check if vehicle is armed */
@@ -834,7 +831,7 @@ public slots:
     void setSelected();
 
     /** @brief Set current mode of operation, e.g. auto or manual */
-    void setMode(int mode);
+    void setMode(uint8_t newBaseMode, uint32_t newCustomMode);
 
     /** @brief Request all parameters */
     void requestParameters();

src/uas/UASInterface.h

@@ -295,7 +295,7 @@ public slots:
     /** @brief Start/continue the current robot action */
     virtual void go() = 0;
     /** @brief Set the current mode of operation */
-    virtual void setMode(int mode) = 0;
+    virtual void setMode(uint8_t newBaseMode, uint32_t newCustomMode) = 0;
     /** Stops the robot system. If it is an MAV, the robot starts the emergency landing procedure **/
     virtual void emergencySTOP() = 0;
     /** Kills the robot. All systems are immediately shut down (e.g. the main power line is cut). This might lead to a crash **/

src/ui/uas/UASControlWidget.cc

@@ -33,7 +33,6 @@ This file is part of the PIXHAWK project
 #include <QTimer>
 #include <QLabel>
 #include <QFileDialog>
-#include <QDebug>
 #include <QProcess>
 #include <QPalette>
 
@@ -42,71 +41,111 @@ This file is part of the PIXHAWK project
 #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[] = {
+    { (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 },
+};
+
 UASControlWidget::UASControlWidget(QWidget *parent) : QWidget(parent),
-    uas(0),
-    uasMode(0),
-    engineOn(false)
+    uasID(-1),
+    modesList(NULL),
+    modesNum(0),
+    modeIdx(0),
+    armed(false)
 {
     ui.setupUi(this);
 
+    this->setUAS(UASManager::instance()->getActiveUAS());
+
     connect(UASManager::instance(), SIGNAL(activeUASSet(UASInterface*)), this, SLOT(setUAS(UASInterface*)));
-    ui.modeComboBox->clear();
-    int modes[] = {
-        MAV_MODE_PREFLIGHT,
-        MAV_MODE_FLAG_MANUAL_INPUT_ENABLED,
-        MAV_MODE_FLAG_STABILIZE_ENABLED | MAV_MODE_FLAG_MANUAL_INPUT_ENABLED,
-        MAV_MODE_FLAG_STABILIZE_ENABLED | MAV_MODE_FLAG_AUTO_ENABLED,
-        MAV_MODE_FLAG_MANUAL_INPUT_ENABLED | MAV_MODE_FLAG_TEST_ENABLED,
-    };
-    for (int i = 0; i < sizeof(modes) / sizeof(int); i++) {
-        int mode = modes[i];
-        ui.modeComboBox->insertItem(i, UAS::getShortModeTextFor(mode).remove(0, 2), mode);
-    }
     connect(ui.modeComboBox, SIGNAL(activated(int)), this, SLOT(setMode(int)));
     connect(ui.setModeButton, SIGNAL(clicked()), this, SLOT(transmitMode()));
 
-    uasMode = ui.modeComboBox->itemData(ui.modeComboBox->currentIndex()).toInt();
+    ui.gridLayout->setAlignment(Qt::AlignTop);
+}
 
-    ui.modeComboBox->setCurrentIndex(0);
+void UASControlWidget::updateModesList()
+{
+    // Detect autopilot type
+    int autopilot = 0;
+    if (this->uasID >= 0) {
+        UASInterface *uas = UASManager::instance()->getUASForId(this->uasID);
+        if (uas) {
+            autopilot = UASManager::instance()->getUASForId(this->uasID)->getAutopilotType();
+        }
+    }
 
-    ui.gridLayout->setAlignment(Qt::AlignTop);
+    // 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);
+    }
+
+    // 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);
+    }
 
+    // Select first mode in list
+    modeIdx = 0;
+    ui.modeComboBox->setCurrentIndex(modeIdx);
+    ui.modeComboBox->update();
 }
 
 void UASControlWidget::setUAS(UASInterface* uas)
 {
-    if (this->uas)
-    {
-        UASInterface* oldUAS = UASManager::instance()->getUASForId(this->uas);
+    if (this->uasID) {
+        UASInterface* oldUAS = UASManager::instance()->getUASForId(this->uasID);
         disconnect(ui.controlButton, SIGNAL(clicked()), oldUAS, SLOT(armSystem()));
         disconnect(ui.liftoffButton, SIGNAL(clicked()), oldUAS, SLOT(launch()));
         disconnect(ui.landButton, SIGNAL(clicked()), oldUAS, SLOT(home()));
         disconnect(ui.shutdownButton, SIGNAL(clicked()), oldUAS, SLOT(shutdown()));
         //connect(ui.setHomeButton, SIGNAL(clicked()), uas, SLOT(setLocalOriginAtCurrentGPSPosition()));
-        disconnect(uas, SIGNAL(modeChanged(int,QString,QString)), this, SLOT(updateMode(int,QString,QString)));
+        disconnect(uas, SIGNAL(modeChanged(int,QString,QString)), this, SLOT(updateMode(int, QString, QString)));
         disconnect(uas, SIGNAL(statusChanged(int)), this, SLOT(updateState(int)));
     }
 
     // Connect user interface controls
-    if (uas)
-    {
-        connect(ui.controlButton, SIGNAL(clicked()), this, SLOT(cycleContextButton()));
-        connect(ui.liftoffButton, SIGNAL(clicked()), uas, SLOT(launch()));
-        connect(ui.landButton, SIGNAL(clicked()), uas, SLOT(home()));
-        connect(ui.shutdownButton, SIGNAL(clicked()), uas, SLOT(shutdown()));
-        //connect(ui.setHomeButton, SIGNAL(clicked()), uas, SLOT(setLocalOriginAtCurrentGPSPosition()));
-        connect(uas, SIGNAL(modeChanged(int,QString,QString)), this, SLOT(updateMode(int,QString,QString)));
-        connect(uas, SIGNAL(statusChanged(int)), this, SLOT(updateState(int)));
-
-        ui.controlStatusLabel->setText(tr("Connected to ") + uas->getUASName());
-
-        this->uas = uas->getUASID();
-        setBackgroundColor(uas->getColor());
-    }
-    else
-    {
-        this->uas = -1;
-    }
+    if (uas) {
+        connect(ui.controlButton, SIGNAL(clicked()), this, SLOT(cycleContextButton()));
+        connect(ui.liftoffButton, SIGNAL(clicked()), uas, SLOT(launch()));
+        connect(ui.landButton, SIGNAL(clicked()), uas, SLOT(home()));
+        connect(ui.shutdownButton, SIGNAL(clicked()), uas, SLOT(shutdown()));
+        //connect(ui.setHomeButton, SIGNAL(clicked()), uas, SLOT(setLocalOriginAtCurrentGPSPosition()));
+        connect(uas, SIGNAL(modeChanged(int, QString, QString)), this, SLOT(updateMode(int, QString, QString)));
+        connect(uas, SIGNAL(statusChanged(int)), this, SLOT(updateState(int)));
+
+        ui.controlStatusLabel->setText(tr("Connected to ") + uas->getUASName());
+
+        this->uasID = uas->getUASID();
+        setBackgroundColor(uas->getColor());
+    } else {
+        this->uasID = -1;
+    }
+    this->updateModesList();
+    this->updateArmText();
 }
 
 UASControlWidget::~UASControlWidget()
@@ -114,15 +153,11 @@ UASControlWidget::~UASControlWidget()
 
 }
 
-void UASControlWidget::updateStatemachine()
+void UASControlWidget::updateArmText()
 {
-
-    if (engineOn)
-    {
+    if (armed) {
         ui.controlButton->setText(tr("DISARM SYSTEM"));
-    }
-    else
-    {
+    } else {
         ui.controlButton->setText(tr("ARM SYSTEM"));
     }
 }
@@ -138,7 +173,7 @@ void UASControlWidget::setBackgroundColor(QColor color)
     QString colorstyle;
     QString borderColor = "#4A4A4F";
     borderColor = "#FA4A4F";
-    uasColor = uasColor.darker(900);
+    uasColor = uasColor.darker(400);
     colorstyle = colorstyle.sprintf("QLabel { border-radius: 3px; padding: 0px; margin: 0px; background-color: #%02X%02X%02X; border: 0px solid %s; }",
                                     uasColor.red(), uasColor.green(), uasColor.blue(), borderColor.toStdString().c_str());
     setStyleSheet(colorstyle);
@@ -149,7 +184,7 @@ void UASControlWidget::setBackgroundColor(QColor color)
 }
 
 
-void UASControlWidget::updateMode(int uas,QString mode,QString description)
+void UASControlWidget::updateMode(int uas, QString mode, QString description)
 {
     Q_UNUSED(uas);
     Q_UNUSED(mode);
@@ -158,17 +193,15 @@ void UASControlWidget::updateMode(int uas,QString mode,QString description)
 
 void UASControlWidget::updateState(int state)
 {
-    switch (state)
-    {
+    switch (state) {
     case (int)MAV_STATE_ACTIVE:
-        engineOn = true;
-        ui.controlButton->setText(tr("DISARM SYSTEM"));
+        armed = true;
         break;
     case (int)MAV_STATE_STANDBY:
-        engineOn = false;
-        ui.controlButton->setText(tr("ARM SYSTEM"));
+        armed = false;
         break;
     }
+    this->updateArmText();
 }
 
 /**
@@ -177,7 +210,7 @@ void UASControlWidget::updateState(int state)
 void UASControlWidget::setMode(int mode)
 {
     // Adapt context button mode
-    uasMode = ui.modeComboBox->itemData(mode).toInt();
+    modeIdx = mode;
     ui.modeComboBox->blockSignals(true);
     ui.modeComboBox->setCurrentIndex(mode);
     ui.modeComboBox->blockSignals(false);
@@ -187,43 +220,36 @@ void UASControlWidget::setMode(int mode)
 
 void UASControlWidget::transmitMode()
 {
-    UASInterface* mav = UASManager::instance()->getUASForId(this->uas);
-    if (mav)
-    {
-        // include armed state
-        if (engineOn)
-            uasMode |= MAV_MODE_FLAG_SAFETY_ARMED;
-        else
-            uasMode &= ~MAV_MODE_FLAG_SAFETY_ARMED;
-
-        mav->setMode(uasMode);
-        QString mode = ui.modeComboBox->currentText();
-
-        ui.lastActionLabel->setText(QString("Sent new mode %1 to %2").arg(mode).arg(mav->getUASName()));
+    UASInterface* uas = UASManager::instance()->getUASForId(this->uasID);
+    if (uas) {
+        if (modeIdx >= 0 && modeIdx < modesNum) {
+            struct full_mode_s mode = modesList[modeIdx];
+            // include armed state
+            if (armed) {
+                mode.baseMode |= MAV_MODE_FLAG_SAFETY_ARMED;
+            } else {
+                mode.baseMode &= ~MAV_MODE_FLAG_SAFETY_ARMED;
+            }
+
+            uas->setMode(mode.baseMode, mode.customMode);
+            QString modeText = ui.modeComboBox->currentText();
+
+            ui.lastActionLabel->setText(QString("Sent new mode %1 to %2").arg(modeText).arg(uas->getUASName()));
+        }
     }
 }
 
 void UASControlWidget::cycleContextButton()
 {
-    UAS* mav = dynamic_cast<UAS*>(UASManager::instance()->getUASForId(this->uas));
-    if (mav)
-    {
-
-        if (!engineOn)
-        {
-            mav->armSystem();
-            ui.lastActionLabel->setText(QString("Enabled motors on %1").arg(mav->getUASName()));
+    UAS* uas = dynamic_cast<UAS*>(UASManager::instance()->getUASForId(this->uasID));
+    if (uas) {
+        if (!armed) {
+            uas->armSystem();
+            ui.lastActionLabel->setText(QString("Arm %1").arg(uas->getUASName()));
         } else {
-            mav->disarmSystem();
-            ui.lastActionLabel->setText(QString("Disabled motors on %1").arg(mav->getUASName()));
+            uas->disarmSystem();
+            ui.lastActionLabel->setText(QString("Disarm %1").arg(uas->getUASName()));
         }
-        // Update state now and in several intervals when MAV might have changed state
-        updateStatemachine();
-
-        QTimer::singleShot(50, this, SLOT(updateStatemachine()));
-        QTimer::singleShot(200, this, SLOT(updateStatemachine()));
-
     }
-
 }
 

src/ui/uas/UASControlWidget.h

@@ -39,6 +39,18 @@ This file is part of the QGROUNDCONTROL project
 #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
+};
+
+struct full_mode_s {
+    uint8_t baseMode;
+    uint32_t customMode;
+};
+
 /**
  * @brief Widget controlling one MAV
  */
@@ -51,8 +63,10 @@ public:
     ~UASControlWidget();
 
 public slots:
+    /** @brief Update modes list for selected system */
+    void updateModesList();
     /** @brief Set the system this widget controls */
-    void setUAS(UASInterface* uas);
+    void setUAS(UASInterface* uasID);
     /** @brief Trigger next context action */
     void cycleContextButton();
     /** @brief Set the operation mode of the MAV */
@@ -60,11 +74,11 @@ public slots:
     /** @brief Transmit the operation mode */
     void transmitMode();
     /** @brief Update the mode */
-    void updateMode(int uas,QString mode,QString description);
+    void updateMode(int uasID, QString mode, QString description);
     /** @brief Update state */
     void updateState(int state);
     /** @brief Update internal state machine */
-    void updateStatemachine();
+    void updateArmText();
 
 signals:
     void changedMode(int);
@@ -75,9 +89,11 @@ protected slots:
     void setBackgroundColor(QColor color);
 
 protected:
-    int uas;              ///< Reference to the current uas
-    unsigned int uasMode; ///< Current uas mode
-    bool engineOn;        ///< Engine state
+    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
 
 private:
     Ui::uasControl ui;