From b4d3e248c1c02964ba713c085ccd555c8acf05a3 Mon Sep 17 00:00:00 2001 From: Don Gagne Date: Mon, 10 Dec 2018 11:24:07 -0800 Subject: [PATCH] Add support for Orbit flight mode --- src/FirmwarePlugin/PX4/PX4FirmwarePlugin.cc | 49 +++++++++++---------- src/FirmwarePlugin/PX4/PX4FirmwarePlugin.h | 1 + src/FirmwarePlugin/PX4/px4_custom_mode.h | 17 ++++--- 3 files changed, 37 insertions(+), 30 deletions(-) diff --git a/src/FirmwarePlugin/PX4/PX4FirmwarePlugin.cc b/src/FirmwarePlugin/PX4/PX4FirmwarePlugin.cc index c85bb70db9..7b19ebf5c0 100644 --- a/src/FirmwarePlugin/PX4/PX4FirmwarePlugin.cc +++ b/src/FirmwarePlugin/PX4/PX4FirmwarePlugin.cc @@ -36,23 +36,24 @@ PX4FirmwarePluginInstanceData::PX4FirmwarePluginInstanceData(QObject* parent) } PX4FirmwarePlugin::PX4FirmwarePlugin(void) - : _manualFlightMode(tr("Manual")) - , _acroFlightMode(tr("Acro")) - , _stabilizedFlightMode(tr("Stabilized")) - , _rattitudeFlightMode(tr("Rattitude")) - , _altCtlFlightMode(tr("Altitude")) - , _posCtlFlightMode(tr("Position")) - , _offboardFlightMode(tr("Offboard")) - , _readyFlightMode(tr("Ready")) - , _takeoffFlightMode(tr("Takeoff")) - , _holdFlightMode(tr("Hold")) - , _missionFlightMode(tr("Mission")) - , _rtlFlightMode(tr("Return")) - , _landingFlightMode(tr("Land")) - , _preclandFlightMode(tr("Precision Land")) - , _rtgsFlightMode(tr("Return to Groundstation")) - , _followMeFlightMode(tr("Follow Me")) - , _simpleFlightMode(tr("Simple")) + : _manualFlightMode (tr("Manual")) + , _acroFlightMode (tr("Acro")) + , _stabilizedFlightMode (tr("Stabilized")) + , _rattitudeFlightMode (tr("Rattitude")) + , _altCtlFlightMode (tr("Altitude")) + , _posCtlFlightMode (tr("Position")) + , _offboardFlightMode (tr("Offboard")) + , _readyFlightMode (tr("Ready")) + , _takeoffFlightMode (tr("Takeoff")) + , _holdFlightMode (tr("Hold")) + , _missionFlightMode (tr("Mission")) + , _rtlFlightMode (tr("Return")) + , _landingFlightMode (tr("Land")) + , _preclandFlightMode (tr("Precision Land")) + , _rtgsFlightMode (tr("Return to Groundstation")) + , _followMeFlightMode (tr("Follow Me")) + , _simpleFlightMode (tr("Simple")) + , _orbitFlightMode (tr("Orbit")) { qmlRegisterType ("QGroundControl.Controllers", 1, 0, "PX4AdvancedFlightModesController"); qmlRegisterType ("QGroundControl.Controllers", 1, 0, "PX4SimpleFlightModesController"); @@ -76,15 +77,14 @@ PX4FirmwarePlugin::PX4FirmwarePlugin(void) { PX4_CUSTOM_MAIN_MODE_ACRO, 0, true, true, true }, { PX4_CUSTOM_MAIN_MODE_RATTITUDE, 0, true, true, true }, { PX4_CUSTOM_MAIN_MODE_ALTCTL, 0, true, true, true }, - { PX4_CUSTOM_MAIN_MODE_POSCTL, 0, true, true, true }, - // simple can't be set by the user right now - { PX4_CUSTOM_MAIN_MODE_SIMPLE, 0, false, false, true }, + { PX4_CUSTOM_MAIN_MODE_OFFBOARD, 0, true, false, true }, + { PX4_CUSTOM_MAIN_MODE_SIMPLE, 0, false, false, true }, + { PX4_CUSTOM_MAIN_MODE_POSCTL, PX4_CUSTOM_SUB_MODE_POSCTL_POSCTL, true, true, true }, + { PX4_CUSTOM_MAIN_MODE_POSCTL, PX4_CUSTOM_SUB_MODE_POSCTL_ORBIT, false, false, false }, { PX4_CUSTOM_MAIN_MODE_AUTO, PX4_CUSTOM_SUB_MODE_AUTO_LOITER, true, true, true }, { PX4_CUSTOM_MAIN_MODE_AUTO, PX4_CUSTOM_SUB_MODE_AUTO_MISSION, true, true, true }, { PX4_CUSTOM_MAIN_MODE_AUTO, PX4_CUSTOM_SUB_MODE_AUTO_RTL, true, true, true }, { PX4_CUSTOM_MAIN_MODE_AUTO, PX4_CUSTOM_SUB_MODE_AUTO_FOLLOW_TARGET, true, false, true }, - { PX4_CUSTOM_MAIN_MODE_OFFBOARD, 0, true, false, true }, - // modes that can't be directly set by the user { PX4_CUSTOM_MAIN_MODE_AUTO, PX4_CUSTOM_SUB_MODE_AUTO_LAND, false, true, true }, { PX4_CUSTOM_MAIN_MODE_AUTO, PX4_CUSTOM_SUB_MODE_AUTO_PRECLAND, false, false, true }, { PX4_CUSTOM_MAIN_MODE_AUTO, PX4_CUSTOM_SUB_MODE_AUTO_READY, false, true, true }, @@ -99,13 +99,14 @@ PX4FirmwarePlugin::PX4FirmwarePlugin(void) &_acroFlightMode, &_rattitudeFlightMode, &_altCtlFlightMode, - &_posCtlFlightMode, + &_offboardFlightMode, &_simpleFlightMode, + &_posCtlFlightMode, + &_orbitFlightMode, &_holdFlightMode, &_missionFlightMode, &_rtlFlightMode, &_followMeFlightMode, - &_offboardFlightMode, &_landingFlightMode, &_preclandFlightMode, &_readyFlightMode, diff --git a/src/FirmwarePlugin/PX4/PX4FirmwarePlugin.h b/src/FirmwarePlugin/PX4/PX4FirmwarePlugin.h index 24628433a0..fa2a4e8796 100644 --- a/src/FirmwarePlugin/PX4/PX4FirmwarePlugin.h +++ b/src/FirmwarePlugin/PX4/PX4FirmwarePlugin.h @@ -105,6 +105,7 @@ protected: QString _rtgsFlightMode; QString _followMeFlightMode; QString _simpleFlightMode; + QString _orbitFlightMode; private slots: void _mavCommandResult(int vehicleId, int component, int command, int result, bool noReponseFromVehicle); diff --git a/src/FirmwarePlugin/PX4/px4_custom_mode.h b/src/FirmwarePlugin/PX4/px4_custom_mode.h index f6b3c90550..3e65a862af 100644 --- a/src/FirmwarePlugin/PX4/px4_custom_mode.h +++ b/src/FirmwarePlugin/PX4/px4_custom_mode.h @@ -34,7 +34,7 @@ /** * @file px4_custom_mode.h * PX4 custom flight modes - * Copied from PX4 2018-04-07 - https://github.com/PX4/Firmware/blob/master/src/modules/commander/px4_custom_mode.h#L45 + * */ #ifndef PX4_CUSTOM_MODE_H_ @@ -66,18 +66,23 @@ enum PX4_CUSTOM_SUB_MODE_AUTO { PX4_CUSTOM_SUB_MODE_AUTO_PRECLAND }; +enum PX4_CUSTOM_SUB_MODE_POSCTL { + PX4_CUSTOM_SUB_MODE_POSCTL_POSCTL = 0, + PX4_CUSTOM_SUB_MODE_POSCTL_ORBIT +}; + union px4_custom_mode { struct { uint16_t reserved; uint8_t main_mode; uint8_t sub_mode; }; - struct { - uint16_t reserved_hl; - uint16_t custom_mode_hl; - }; - uint32_t data; + uint32_t data; float data_float; + struct { + uint16_t reserved_hl; + uint16_t custom_mode_hl; + }; }; #endif /* PX4_CUSTOM_MODE_H_ */ -- GitLab