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 */