diff --git a/qgroundcontrol.pro b/qgroundcontrol.pro index 383e00dae2e74b4fee37b7efe55233798b3a36aa..c281da90938cdd23f0568f1f498d80526c530458 100644 --- a/qgroundcontrol.pro +++ b/qgroundcontrol.pro @@ -488,7 +488,8 @@ HEADERS += src/MG.h \ src/uas/UASParameterCommsMgr.h \ src/ui/QGCPendingParamWidget.h \ src/ui/px4_configuration/QGCPX4AirframeConfig.h \ - src/ui/QGCBaseParamWidget.h + src/ui/QGCBaseParamWidget.h \ + src/comm/px4_custom_mode.h # Google Earth is only supported on Mac OS and Windows with Visual Studio Compiler macx|macx-g++|macx-g++42|win32-msvc2008|win32-msvc2010|win32-msvc2012::HEADERS += src/ui/map3D/QGCGoogleEarthView.h diff --git a/src/comm/px4_custom_mode.h b/src/comm/px4_custom_mode.h new file mode 100644 index 0000000000000000000000000000000000000000..9c0c8ae91ea11ffbcd6e1d101da32c12ab207174 --- /dev/null +++ b/src/comm/px4_custom_mode.h @@ -0,0 +1,30 @@ +#ifndef PX4_CUSTOM_MODE_H +#define PX4_CUSTOM_MODE_H + +enum PX4_CUSTOM_MAIN_MODE { + PX4_CUSTOM_MAIN_MODE_MANUAL = 1, + PX4_CUSTOM_MAIN_MODE_SEATBELT, + PX4_CUSTOM_MAIN_MODE_EASY, + PX4_CUSTOM_MAIN_MODE_AUTO, +}; + +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, +}; + +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 b2580593763b44277f162e2c5b1ad1a5b1e95624..2c92ff6e89b9add90d7a3a08069bbe5ed0cb935c 100644 --- a/src/uas/UAS.cc +++ b/src/uas/UAS.cc @@ -28,6 +28,7 @@ #include "SerialLink.h" #include "UASParameterCommsMgr.h" #include +#include #ifdef QGC_PROTOBUF_ENABLED #include @@ -1837,7 +1838,7 @@ void UAS::setMode(uint8_t newBaseMode, uint32_t newCustomMode) mavlink_message_t msg; mavlink_msg_set_mode_pack(mavlink->getSystemId(), mavlink->getComponentId(), &msg, (uint8_t)uasId, newBaseMode, newCustomMode); sendMessage(msg); - qDebug() << "SENDING REQUEST TO SET MODE TO SYSTEM" << uasId << ", REQUEST TO SET MODE " << newBaseMode; + qDebug() << "SENDING REQUEST TO SET MODE TO SYSTEM" << uasId << ", MODE " << newBaseMode << " " << newCustomMode; } /** @@ -3318,24 +3319,32 @@ QString UAS::getShortModeTextFor(uint8_t base_mode, uint32_t custom_mode, int au { QString mode = ""; - enum PX4_CUSTOM_MODE { - PX4_CUSTOM_MODE_MANUAL = 1, - PX4_CUSTOM_MODE_SEATBELT, - PX4_CUSTOM_MODE_EASY, - PX4_CUSTOM_MODE_AUTO - }; - if (base_mode & MAV_MODE_FLAG_CUSTOM_MODE_ENABLED) { // use custom_mode - autopilot-specific if (autopilot == MAV_AUTOPILOT_PX4) { - if (custom_mode == PX4_CUSTOM_MODE_MANUAL) { + 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 (custom_mode == PX4_CUSTOM_MODE_SEATBELT) { + } else if (px4_mode.main_mode == PX4_CUSTOM_MAIN_MODE_SEATBELT) { mode += "|SEATBELT"; - } else if (custom_mode == PX4_CUSTOM_MODE_EASY) { + } else if (px4_mode.main_mode == PX4_CUSTOM_MAIN_MODE_EASY) { mode += "|EASY"; - } else if (custom_mode == PX4_CUSTOM_MODE_AUTO) { + } 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"; + } } } } diff --git a/src/ui/uas/UASControlWidget.cc b/src/ui/uas/UASControlWidget.cc index ebc1934c3adf204453ceca76521ff5ef86347b62..91c2d0605c8b404106611b31365ed6e4c0741cfe 100644 --- a/src/ui/uas/UASControlWidget.cc +++ b/src/ui/uas/UASControlWidget.cc @@ -52,16 +52,7 @@ static struct full_mode_s modes_list_common[] = { 0 }, }; -static struct full_mode_s modes_list_px4[] = { - { (MAV_MODE_FLAG_CUSTOM_MODE_ENABLED | MAV_MODE_FLAG_MANUAL_INPUT_ENABLED), - PX4_CUSTOM_MODE_MANUAL }, - { (MAV_MODE_FLAG_CUSTOM_MODE_ENABLED | MAV_MODE_FLAG_MANUAL_INPUT_ENABLED | MAV_MODE_FLAG_STABILIZE_ENABLED), - PX4_CUSTOM_MODE_SEATBELT }, - { (MAV_MODE_FLAG_CUSTOM_MODE_ENABLED | MAV_MODE_FLAG_MANUAL_INPUT_ENABLED | MAV_MODE_FLAG_STABILIZE_ENABLED | MAV_MODE_FLAG_GUIDED_ENABLED), - PX4_CUSTOM_MODE_EASY }, - { (MAV_MODE_FLAG_CUSTOM_MODE_ENABLED | MAV_MODE_FLAG_AUTO_ENABLED | MAV_MODE_FLAG_STABILIZE_ENABLED | MAV_MODE_FLAG_GUIDED_ENABLED), - PX4_CUSTOM_MODE_AUTO }, -}; +static struct full_mode_s modes_list_px4[4]; UASControlWidget::UASControlWidget(QWidget *parent) : QWidget(parent), uasID(-1), @@ -83,6 +74,21 @@ 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_SEATBELT; + 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_EASY; + 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; + // Detect autopilot type int autopilot = 0; if (this->uasID >= 0) { diff --git a/src/ui/uas/UASControlWidget.h b/src/ui/uas/UASControlWidget.h index 7f9a5652b74c00c92ee13a6780ca4ff94d617d40..9601bf506dc6cabb74ba81b778497f53a0c79371 100644 --- a/src/ui/uas/UASControlWidget.h +++ b/src/ui/uas/UASControlWidget.h @@ -38,13 +38,7 @@ This file is part of the QGROUNDCONTROL project #include #include #include - -enum PX4_CUSTOM_MODE { - PX4_CUSTOM_MODE_MANUAL = 1, - PX4_CUSTOM_MODE_SEATBELT, - PX4_CUSTOM_MODE_EASY, - PX4_CUSTOM_MODE_AUTO -}; +#include struct full_mode_s { uint8_t baseMode;