diff --git a/src/AutoPilotPlugins/AutoPilotPluginManager.cc b/src/AutoPilotPlugins/AutoPilotPluginManager.cc index 086400fe242e711f6784fd9ffa5c1fd5748cd444..80172536b3436e24838a7c002792e796cb4397f3 100644 --- a/src/AutoPilotPlugins/AutoPilotPluginManager.cc +++ b/src/AutoPilotPlugins/AutoPilotPluginManager.cc @@ -124,6 +124,17 @@ QList 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) { diff --git a/src/AutoPilotPlugins/AutoPilotPluginManager.h b/src/AutoPilotPlugins/AutoPilotPluginManager.h index aa3808bae42c5b93164b04583d62542a211d63a8..9abc6993589daec50605796c45e4c36b1222aac4 100644 --- a/src/AutoPilotPlugins/AutoPilotPluginManager.h +++ b/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); diff --git a/src/AutoPilotPlugins/Generic/GenericAutoPilotPlugin.cc b/src/AutoPilotPlugins/Generic/GenericAutoPilotPlugin.cc index f2ddfac47d791dfdd2bfa41789ddd7dfc87c0446..e8897483cbb0b267ceb19f0e97041b7a71faf237 100644 --- a/src/AutoPilotPlugins/Generic/GenericAutoPilotPlugin.cc +++ b/src/AutoPilotPlugins/Generic/GenericAutoPilotPlugin.cc @@ -60,6 +60,16 @@ QList 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); diff --git a/src/AutoPilotPlugins/Generic/GenericAutoPilotPlugin.h b/src/AutoPilotPlugins/Generic/GenericAutoPilotPlugin.h index 87f37de0880c17be7fdee1c3c3edb8edd481655d..c9b2848e30895d861f0ee28ea597b806ff2c962a 100644 --- a/src/AutoPilotPlugins/Generic/GenericAutoPilotPlugin.h +++ b/src/AutoPilotPlugins/Generic/GenericAutoPilotPlugin.h @@ -44,6 +44,7 @@ public: virtual const QVariantList& vehicleComponents(void); static QList getModes(void); + static QString getAudioModeText(uint8_t baseMode, uint32_t customMode); static QString getShortModeText(uint8_t baseMode, uint32_t customMode); static void clearStaticData(void); diff --git a/src/AutoPilotPlugins/PX4/PX4AutoPilotPlugin.cc b/src/AutoPilotPlugins/PX4/PX4AutoPilotPlugin.cc index 643fe81f6824a0ad31e8ff27cc58a04933c6fe6e..c282deb2d291e115dad3dd168aa4a8865cf1c540 100644 --- a/src/AutoPilotPlugins/PX4/PX4AutoPilotPlugin.cc +++ b/src/AutoPilotPlugins/PX4/PX4AutoPilotPlugin.cc @@ -141,6 +141,63 @@ QList 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; diff --git a/src/AutoPilotPlugins/PX4/PX4AutoPilotPlugin.h b/src/AutoPilotPlugins/PX4/PX4AutoPilotPlugin.h index e757900d452541536a7390649492c4c44a033671..d391e361315b9d4985480cf12bcbd8c48392712c 100644 --- a/src/AutoPilotPlugins/PX4/PX4AutoPilotPlugin.h +++ b/src/AutoPilotPlugins/PX4/PX4AutoPilotPlugin.h @@ -53,6 +53,7 @@ public: virtual const QVariantList& vehicleComponents(void); static QList getModes(void); + static QString getAudioModeText(uint8_t baseMode, uint32_t customMode); static QString getShortModeText(uint8_t baseMode, uint32_t customMode); static void clearStaticData(void); diff --git a/src/uas/UAS.cc b/src/uas/UAS.cc index 50009d5c8446bfd3e79e6ff4b165af7afd1dadc4..f982c94e1f62155a7869ab4090270f8a626a3777 100644 --- a/src/uas/UAS.cc +++ b/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(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 diff --git a/src/uas/UAS.h b/src/uas/UAS.h index af5ffee512e59ae99f5dd47dce6674f8cfcc5f25..ba8e9b7617df7f37568d9256c7a286e604e9984d 100644 --- a/src/uas/UAS.h +++ b/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 */