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..3f0fbcc1aed5b2679ba2482eb16ef30bf5cd4647 100644 --- a/src/AutoPilotPlugins/Generic/GenericAutoPilotPlugin.cc +++ b/src/AutoPilotPlugins/Generic/GenericAutoPilotPlugin.cc @@ -60,6 +60,15 @@ QList GenericAutoPilotPlugin::getModes(void) return modeList; } +QString GenericAutoPilotPlugin::getAudioModeText(uint8_t baseMode, uint32_t customMode) +{ + 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);