Merge pull request #1702 from mavlink/audio_fix

Audio fix for PX4

src/AutoPilotPlugins/AutoPilotPluginManager.cc

@@ -124,6 +124,17 @@ QList<AutoPilotPluginManager::FullMode_t> AutoPilotPluginManager::getModes(int a
     }
 }
 
+QString AutoPilotPluginManager::getAudioModeText(uint8_t baseMode, uint32_t customMode, int autopilotType) const
+{
+    switch (autopilotType) {
+        case MAV_AUTOPILOT_PX4:
+            return PX4AutoPilotPlugin::getAudioModeText(baseMode, customMode);
+        case MAV_AUTOPILOT_GENERIC:
+        default:
+            return GenericAutoPilotPlugin::getAudioModeText(baseMode, customMode);
+    }
+}
+
 QString AutoPilotPluginManager::getShortModeText(uint8_t baseMode, uint32_t customMode, int autopilotType) const
 {
     switch (autopilotType) {

src/AutoPilotPlugins/AutoPilotPluginManager.h

@@ -63,6 +63,9 @@ public:
     /// @brief Returns a human readable short description for the specified mode.
     QString getShortModeText(uint8_t baseMode, uint32_t customMode, int autopilotType) const;
 
+    /// @brief Returns a human hearable short description for the specified mode.
+    QString getAudioModeText(uint8_t baseMode, uint32_t customMode, int autopilotType) const;
+
 private slots:
     void _uasCreated(UASInterface* uas);
     void _uasDeleted(UASInterface* uas);

src/AutoPilotPlugins/Generic/GenericAutoPilotPlugin.cc

@@ -60,6 +60,16 @@ QList<AutoPilotPluginManager::FullMode_t> GenericAutoPilotPlugin::getModes(void)
     return modeList;
 }
 
+QString GenericAutoPilotPlugin::getAudioModeText(uint8_t baseMode, uint32_t customMode)
+{
+    Q_UNUSED(baseMode);
+    Q_UNUSED(customMode);
+
+    QString mode = "";
+
+    return mode;
+}
+
 QString GenericAutoPilotPlugin::getShortModeText(uint8_t baseMode, uint32_t customMode)
 {
     Q_UNUSED(customMode);

src/AutoPilotPlugins/Generic/GenericAutoPilotPlugin.h

@@ -44,6 +44,7 @@ public:
     virtual const QVariantList& vehicleComponents(void);
 
     static QList<AutoPilotPluginManager::FullMode_t> getModes(void);
+    static QString getAudioModeText(uint8_t baseMode, uint32_t customMode);
     static QString getShortModeText(uint8_t baseMode, uint32_t customMode);
     static void clearStaticData(void);
     

src/AutoPilotPlugins/PX4/PX4AutoPilotPlugin.cc

@@ -141,6 +141,63 @@ QList<AutoPilotPluginManager::FullMode_t> PX4AutoPilotPlugin::getModes(void)
     return modeList;
 }
 
+QString PX4AutoPilotPlugin::getAudioModeText(uint8_t baseMode, uint32_t customMode)
+{
+    QString mode;
+
+    Q_ASSERT(baseMode & MAV_MODE_FLAG_CUSTOM_MODE_ENABLED);
+
+    if (baseMode & MAV_MODE_FLAG_CUSTOM_MODE_ENABLED) {
+        union px4_custom_mode px4_mode;
+        px4_mode.data = customMode;
+
+        if (px4_mode.main_mode == PX4_CUSTOM_MAIN_MODE_MANUAL) {
+            mode = "manual";
+        } else if (px4_mode.main_mode == PX4_CUSTOM_MAIN_MODE_ACRO) {
+            mode = "caro";
+        } else if (px4_mode.main_mode == PX4_CUSTOM_MAIN_MODE_STABILIZED) {
+            mode = "stabilized";
+        } else if (px4_mode.main_mode == PX4_CUSTOM_MAIN_MODE_ALTCTL) {
+            mode = "altitude control";
+        } else if (px4_mode.main_mode == PX4_CUSTOM_MAIN_MODE_POSCTL) {
+            mode = "position control";
+        } else if (px4_mode.main_mode == PX4_CUSTOM_MAIN_MODE_AUTO) {
+            mode = "auto and ";
+            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 += "taking off";
+            } else if (px4_mode.sub_mode == PX4_CUSTOM_SUB_MODE_AUTO_LOITER) {
+                mode += "loitering";
+            } else if (px4_mode.sub_mode == PX4_CUSTOM_SUB_MODE_AUTO_MISSION) {
+                mode += "following waypoints";
+            } else if (px4_mode.sub_mode == PX4_CUSTOM_SUB_MODE_AUTO_RTL) {
+                mode += "returning to land";
+            } else if (px4_mode.sub_mode == PX4_CUSTOM_SUB_MODE_AUTO_LAND) {
+                mode += "landing";
+            }
+        } else if (px4_mode.main_mode == PX4_CUSTOM_MAIN_MODE_OFFBOARD) {
+            mode = "offboard controlled";
+        }
+
+        if (baseMode != 0)
+        {
+            mode += " mode";
+        }
+
+        // ARMED STATE DECODING
+        if (baseMode & (uint8_t)MAV_MODE_FLAG_DECODE_POSITION_SAFETY)
+        {
+            mode.append(" and armed");
+        }
+
+    } else {
+        mode = "";
+    }
+
+    return mode;
+}
+
 QString PX4AutoPilotPlugin::getShortModeText(uint8_t baseMode, uint32_t customMode)
 {
     QString mode;

src/AutoPilotPlugins/PX4/PX4AutoPilotPlugin.h

@@ -53,6 +53,7 @@ public:
     virtual const QVariantList& vehicleComponents(void);
 
     static QList<AutoPilotPluginManager::FullMode_t> getModes(void);
+    static QString getAudioModeText(uint8_t baseMode, uint32_t customMode);
     static QString getShortModeText(uint8_t baseMode, uint32_t customMode);
     static void clearStaticData(void);
     

src/uas/UAS.cc

@@ -510,7 +510,7 @@ void UAS::receiveMessage(LinkInterface* link, mavlink_message_t message)
             bool statechanged = false;
             bool modechanged = false;
 
-            QString audiomodeText = getAudioModeTextFor(static_cast<int>(state.base_mode));
+            QString audiomodeText = getAudioModeTextFor(state.base_mode, state.custom_mode);
 
             if ((state.system_status != this->status) && state.system_status != MAV_STATE_UNINIT)
             {
@@ -3141,60 +3141,63 @@ const QString& UAS::getShortState() const
 * hardware in the loop is being used.
 * @return the audio mode text for the id given.
 */
-QString UAS::getAudioModeTextFor(int id)
+QString UAS::getAudioModeTextFor(uint8_t base_mode, uint32_t custom_mode) const
 {
-    QString mode;
-    uint8_t modeid = id;
+    QString mode = AutoPilotPluginManager::instance()->getAudioModeText(base_mode, custom_mode, autopilot);
 
-    // BASE MODE DECODING
-    if (modeid & (uint8_t)MAV_MODE_FLAG_DECODE_POSITION_AUTO)
-    {
-        mode += "autonomous";
-    }
-    else if (modeid & (uint8_t)MAV_MODE_FLAG_DECODE_POSITION_GUIDED)
-    {
-        mode += "guided";
-    }
-    else if (modeid & (uint8_t)MAV_MODE_FLAG_DECODE_POSITION_STABILIZE)
-    {
-        mode += "stabilized";
-    }
-    else if (modeid & (uint8_t)MAV_MODE_FLAG_DECODE_POSITION_MANUAL)
-    {
-        mode += "manual";
-    }
-    else
+    if (mode.length() == 0)
     {
-        // Nothing else applies, we're in preflight
-        mode += "preflight";
-    }
+        // Fall back to generic decoding
 
-    if (modeid != 0)
-    {
-        mode += " mode";
-    }
+        QString mode;
+        uint8_t modeid = base_mode;
 
-    // ARMED STATE DECODING
-    if (modeid & (uint8_t)MAV_MODE_FLAG_DECODE_POSITION_SAFETY)
-    {
-        mode.append(" and armed");
-    }
+        // BASE MODE DECODING
+        if (modeid & (uint8_t)MAV_MODE_FLAG_DECODE_POSITION_AUTO)
+        {
+            mode += "autonomous";
+        }
+        else if (modeid & (uint8_t)MAV_MODE_FLAG_DECODE_POSITION_GUIDED)
+        {
+            mode += "guided";
+        }
+        else if (modeid & (uint8_t)MAV_MODE_FLAG_DECODE_POSITION_STABILIZE)
+        {
+            mode += "stabilized";
+        }
+        else if (modeid & (uint8_t)MAV_MODE_FLAG_DECODE_POSITION_MANUAL)
+        {
+            mode += "manual";
+        }
+        else
+        {
+            // Nothing else applies, we're in preflight
+            mode += "preflight";
+        }
 
-    // HARDWARE IN THE LOOP DECODING
-    if (modeid & (uint8_t)MAV_MODE_FLAG_DECODE_POSITION_HIL)
-    {
-        mode.append(" using hardware in the loop simulation");
+        if (modeid != 0)
+        {
+            mode += " mode";
+        }
+
+        // ARMED STATE DECODING
+        if (modeid & (uint8_t)MAV_MODE_FLAG_DECODE_POSITION_SAFETY)
+        {
+            mode.append(" and armed");
+        }
+
+        // HARDWARE IN THE LOOP DECODING
+        if (modeid & (uint8_t)MAV_MODE_FLAG_DECODE_POSITION_HIL)
+        {
+            mode.append(" using hardware in the loop simulation");
+        }
     }
 
     return mode;
 }
 
 /**
-* The mode returned can be auto, stabilized, test, manual, preflight or unknown.
-* @return the short text of the mode for the id given.
-*/
-/**
-* The mode returned can be auto, stabilized, test, manual, preflight or unknown.
+* The mode returned depends on the specific autopilot used.
 * @return the short text of the mode for the id given.
 */
 QString UAS::getShortModeTextFor(uint8_t base_mode, uint32_t custom_mode) const

src/uas/UAS.h

@@ -76,7 +76,7 @@ public:
     /** @brief Translate from mode id to text */
     QString getShortModeTextFor(uint8_t base_mode, uint32_t custom_mode) const;
     /** @brief Translate from mode id to audio text */
-    static QString getAudioModeTextFor(int id);
+    QString getAudioModeTextFor(uint8_t base_mode, uint32_t custom_mode) const;
     /** @brief Get the unique system id */
     int getUASID() const;
     /** @brief Get the airframe */