diff --git a/src/comm/px4_custom_mode.h b/src/comm/px4_custom_mode.h deleted file mode 100644 index eaf309288fc416e466cc023f538a551c3d68164e..0000000000000000000000000000000000000000 --- a/src/comm/px4_custom_mode.h +++ /dev/null @@ -1,42 +0,0 @@ -/* - * px4_custom_mode.h - * - * Created on: 09.08.2013 - * Author: ton - */ - -#ifndef PX4_CUSTOM_MODE_H_ -#define PX4_CUSTOM_MODE_H_ - -#include - -enum PX4_CUSTOM_MAIN_MODE { - PX4_CUSTOM_MAIN_MODE_MANUAL = 1, - PX4_CUSTOM_MAIN_MODE_ALTCTL, - PX4_CUSTOM_MAIN_MODE_POSCTL, - PX4_CUSTOM_MAIN_MODE_AUTO, - PX4_CUSTOM_MAIN_MODE_ACRO, - PX4_CUSTOM_MAIN_MODE_OFFBOARD, -}; - -enum PX4_CUSTOM_SUB_MODE_AUTO { - PX4_CUSTOM_SUB_MODE_AUTO_READY = 1, - PX4_CUSTOM_SUB_MODE_AUTO_TAKEOFF, - PX4_CUSTOM_SUB_MODE_AUTO_LOITER, - PX4_CUSTOM_SUB_MODE_AUTO_MISSION, - PX4_CUSTOM_SUB_MODE_AUTO_RTL, - PX4_CUSTOM_SUB_MODE_AUTO_LAND, - PX4_CUSTOM_SUB_MODE_AUTO_RTGS -}; - -union px4_custom_mode { - struct { - uint16_t reserved; - uint8_t main_mode; - uint8_t sub_mode; - }; - uint32_t data; - float data_float; -}; - -#endif /* PX4_CUSTOM_MODE_H_ */ diff --git a/src/uas/UAS.cc b/src/uas/UAS.cc index 7a3ecb2f5aea1cbc8f2ba5ea0dde9eb533b7960f..6ace8d9a10cf76333b1303e6f360e5f3ff8d1150 100644 --- a/src/uas/UAS.cc +++ b/src/uas/UAS.cc @@ -28,7 +28,7 @@ #include "SerialLink.h" #include "UASParameterCommsMgr.h" #include -#include +#include "AutoPilotPlugin.h" /** * Gets the settings from the previous UAS (name, airframe, autopilot, battery specs) @@ -3326,56 +3326,7 @@ QString UAS::getAudioModeTextFor(int id) */ QString UAS::getShortModeTextFor(uint8_t base_mode, uint32_t custom_mode, int autopilot) { - QString mode = ""; - - if (base_mode & MAV_MODE_FLAG_CUSTOM_MODE_ENABLED) { - // use custom_mode - autopilot-specific - if (autopilot == MAV_AUTOPILOT_PX4) { - union px4_custom_mode px4_mode; - px4_mode.data = custom_mode; - if (px4_mode.main_mode == PX4_CUSTOM_MAIN_MODE_MANUAL) { - mode += "|MANUAL"; - } else if (px4_mode.main_mode == PX4_CUSTOM_MAIN_MODE_ALTCTL) { - mode += "|ALTCTL"; - } else if (px4_mode.main_mode == PX4_CUSTOM_MAIN_MODE_POSCTL) { - mode += "|POSCTL"; - } else if (px4_mode.main_mode == PX4_CUSTOM_MAIN_MODE_AUTO) { - mode += "|AUTO"; - 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 += "|TAKEOFF"; - } else if (px4_mode.sub_mode == PX4_CUSTOM_SUB_MODE_AUTO_LOITER) { - mode += "|LOITER"; - } else if (px4_mode.sub_mode == PX4_CUSTOM_SUB_MODE_AUTO_MISSION) { - mode += "|MISSION"; - } else if (px4_mode.sub_mode == PX4_CUSTOM_SUB_MODE_AUTO_RTL) { - mode += "|RTL"; - } else if (px4_mode.sub_mode == PX4_CUSTOM_SUB_MODE_AUTO_LAND) { - mode += "|LAND"; - } - } else if (px4_mode.main_mode == PX4_CUSTOM_MAIN_MODE_OFFBOARD) { - mode += "|OFFBOARD"; - } - } - } - - // 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"; - } - } - } + QString mode = AutoPilotPlugin::getInstanceForAutoPilotPlugin(autopilot)->getShortModeText(base_mode, custom_mode); if (mode.length() == 0) { diff --git a/src/ui/uas/UASControlWidget.cc b/src/ui/uas/UASControlWidget.cc index 3d9b150c635430aa98aa10e4e0b1629c97834d95..af7786b61062b09fcb7a8b65534beb8e6ecfc1ed 100644 --- a/src/ui/uas/UASControlWidget.cc +++ b/src/ui/uas/UASControlWidget.cc @@ -40,24 +40,10 @@ This file is part of the PIXHAWK project #include #include #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[5]; +#include "AutoPilotPlugin.h" UASControlWidget::UASControlWidget(QWidget *parent) : QWidget(parent), uasID(-1), - modesList(NULL), - modesNum(0), modeIdx(0), armed(false) { @@ -74,47 +60,23 @@ UASControlWidget::UASControlWidget(QWidget *parent) : QWidget(parent), void UASControlWidget::updateModesList() { - union px4_custom_mode px4_cm; - px4_cm.data = 0; - modes_list_px4[0].baseMode = MAV_MODE_FLAG_CUSTOM_MODE_ENABLED | MAV_MODE_FLAG_MANUAL_INPUT_ENABLED; - px4_cm.main_mode = PX4_CUSTOM_MAIN_MODE_MANUAL; - modes_list_px4[0].customMode = px4_cm.data; - modes_list_px4[1].baseMode = MAV_MODE_FLAG_CUSTOM_MODE_ENABLED | MAV_MODE_FLAG_MANUAL_INPUT_ENABLED | MAV_MODE_FLAG_STABILIZE_ENABLED; - px4_cm.main_mode = PX4_CUSTOM_MAIN_MODE_ALTCTL; - modes_list_px4[1].customMode = px4_cm.data; - modes_list_px4[2].baseMode = MAV_MODE_FLAG_CUSTOM_MODE_ENABLED | MAV_MODE_FLAG_MANUAL_INPUT_ENABLED | MAV_MODE_FLAG_STABILIZE_ENABLED | MAV_MODE_FLAG_GUIDED_ENABLED; - px4_cm.main_mode = PX4_CUSTOM_MAIN_MODE_POSCTL; - modes_list_px4[2].customMode = px4_cm.data; - modes_list_px4[3].baseMode = MAV_MODE_FLAG_CUSTOM_MODE_ENABLED | MAV_MODE_FLAG_AUTO_ENABLED | MAV_MODE_FLAG_STABILIZE_ENABLED | MAV_MODE_FLAG_GUIDED_ENABLED; - px4_cm.main_mode = PX4_CUSTOM_MAIN_MODE_AUTO; - modes_list_px4[3].customMode = px4_cm.data; - modes_list_px4[4].baseMode = MAV_MODE_FLAG_CUSTOM_MODE_ENABLED | MAV_MODE_FLAG_AUTO_ENABLED | MAV_MODE_FLAG_STABILIZE_ENABLED | MAV_MODE_FLAG_GUIDED_ENABLED; - px4_cm.main_mode = PX4_CUSTOM_MAIN_MODE_OFFBOARD; - modes_list_px4[4].customMode = px4_cm.data; - // Detect autopilot type - int autopilot = 0; + int autopilot = MAV_AUTOPILOT_GENERIC; if (this->uasID >= 0) { UASInterface *uas = UASManager::instance()->getUASForId(this->uasID); if (uas) { autopilot = UASManager::instance()->getUASForId(this->uasID)->getAutopilotType(); } } - - // 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); - } + + AutoPilotPlugin* autopilotPlugin = AutoPilotPlugin::getInstanceForAutoPilotPlugin(autopilot); + + _modeList = autopilotPlugin->getModes(); // 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); + foreach (AutoPilotPlugin::FullMode_t fullMode, _modeList) { + ui.modeComboBox->addItem(UAS::getShortModeTextFor(fullMode.baseMode, fullMode.customMode, autopilot).remove(0, 2)); } // Select first mode in list @@ -235,24 +197,24 @@ void UASControlWidget::transmitMode() { UASInterface* uas_iface = UASManager::instance()->getUASForId(this->uasID); if (uas_iface) { - if (modeIdx >= 0 && modeIdx < modesNum) { - struct full_mode_s mode = modesList[modeIdx]; + if (modeIdx >= 0 && modeIdx < _modeList.count()) { + AutoPilotPlugin::FullMode_t fullMode = _modeList[modeIdx]; // include armed state if (armed) { - mode.baseMode |= MAV_MODE_FLAG_SAFETY_ARMED; + fullMode.baseMode |= MAV_MODE_FLAG_SAFETY_ARMED; } else { - mode.baseMode &= ~MAV_MODE_FLAG_SAFETY_ARMED; + fullMode.baseMode &= ~MAV_MODE_FLAG_SAFETY_ARMED; } UAS* uas = dynamic_cast(uas_iface); if (uas->isHilEnabled() || uas->isHilActive()) { - mode.baseMode |= MAV_MODE_FLAG_HIL_ENABLED; + fullMode.baseMode |= MAV_MODE_FLAG_HIL_ENABLED; } else { - mode.baseMode &= ~MAV_MODE_FLAG_HIL_ENABLED; + fullMode.baseMode &= ~MAV_MODE_FLAG_HIL_ENABLED; } - uas->setMode(mode.baseMode, mode.customMode); + uas->setMode(fullMode.baseMode, fullMode.customMode); QString modeText = ui.modeComboBox->currentText(); ui.lastActionLabel->setText(QString("Sent new mode %1 to %2").arg(modeText).arg(uas->getUASName())); diff --git a/src/ui/uas/UASControlWidget.h b/src/ui/uas/UASControlWidget.h index 9601bf506dc6cabb74ba81b778497f53a0c79371..997849df98016593987c97a1c1527f0abe01d0f4 100644 --- a/src/ui/uas/UASControlWidget.h +++ b/src/ui/uas/UASControlWidget.h @@ -38,12 +38,7 @@ This file is part of the QGROUNDCONTROL project #include #include #include -#include - -struct full_mode_s { - uint8_t baseMode; - uint32_t customMode; -}; +#include "AutoPilotPlugin.h" /** * @brief Widget controlling one MAV @@ -83,11 +78,10 @@ protected slots: void setBackgroundColor(QColor color); protected: - 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 + int uasID; ///< Reference to the current uas + QList _modeList; ///< Mode list for the current UAS + int modeIdx; ///< Current uas mode index + bool armed; ///< Engine state private: Ui::uasControl ui;