From 3808a1f9b5e0657710767c5ab5c2acada5e67ece Mon Sep 17 00:00:00 2001 From: Daniel Agar Date: Tue, 14 Jun 2016 16:15:26 -0400 Subject: [PATCH] PX4 mode cleanup --- qgroundcontrol.pro | 1 + .../PX4/PX4AdvancedFlightModes.qml | 6 +- src/FirmwarePlugin/PX4/PX4FirmwarePlugin.cc | 90 ++++++++-------------- src/FirmwarePlugin/PX4/PX4FirmwarePlugin.h | 2 +- src/FirmwarePlugin/PX4/px4_custom_mode.h | 77 ++++++++++++++++++ src/comm/MockLink.cc | 33 +------- 6 files changed, 116 insertions(+), 93 deletions(-) create mode 100644 src/FirmwarePlugin/PX4/px4_custom_mode.h diff --git a/qgroundcontrol.pro b/qgroundcontrol.pro index fd8501c..b49c2dc 100644 --- a/qgroundcontrol.pro +++ b/qgroundcontrol.pro @@ -664,6 +664,7 @@ HEADERS+= \ src/FirmwarePlugin/APM/ArduCopterFirmwarePlugin.h \ src/FirmwarePlugin/APM/ArduPlaneFirmwarePlugin.h \ src/FirmwarePlugin/APM/ArduRoverFirmwarePlugin.h \ + src/FirmwarePlugin/PX4/px4_custom_mode.h \ src/FirmwarePlugin/PX4/PX4FirmwarePlugin.h \ src/FirmwarePlugin/PX4/PX4ParameterMetaData.h \ src/Vehicle/MultiVehicleManager.h \ diff --git a/src/AutoPilotPlugins/PX4/PX4AdvancedFlightModes.qml b/src/AutoPilotPlugins/PX4/PX4AdvancedFlightModes.qml index ef9a979..e63d57d 100644 --- a/src/AutoPilotPlugins/PX4/PX4AdvancedFlightModes.qml +++ b/src/AutoPilotPlugins/PX4/PX4AdvancedFlightModes.qml @@ -63,7 +63,7 @@ Item { readonly property string fwAcroModeDescription: qsTr("The angular rates are controlled, but not the attitude. ") readonly property string mrAcroModeDescription: qsTr("The angular rates are controlled, but not the attitude. ") - readonly property string altCtlModeName: qsTr("Altitude Control") + readonly property string altCtlModeName: qsTr("Altitude") readonly property string fwAltCtlModeDescription: qsTr("Roll stick controls banking, pitch stick altitude ") + qsTr("Throttle stick controls speed. ") + qsTr("With no stick inputs the plane holds heading, but drifts off in wind. ") @@ -76,10 +76,10 @@ Item { readonly property string mrPosCtlModeDescription: qsTr("Roll and Pitch sticks control sideways and forward speed ") + qsTr("Throttle stick controls climb / sink rade. ") - readonly property string missionModeName: qsTr("Auto Mission") + readonly property string missionModeName: qsTr("Mission") readonly property string missionModeDescription: qsTr("The aircraft obeys the programmed mission sent by QGroundControl. ") - readonly property string loiterModeName: qsTr("Auto Pause") + readonly property string loiterModeName: qsTr("Hold") readonly property string fwLoiterModeDescription: qsTr("The aircraft flies in a circle around the current position at the current altitude. ") readonly property string mrLoiterModeDescription: qsTr("The multirotor hovers at the current position and altitude. ") diff --git a/src/FirmwarePlugin/PX4/PX4FirmwarePlugin.cc b/src/FirmwarePlugin/PX4/PX4FirmwarePlugin.cc index 4f5e26d..0c781d9 100644 --- a/src/FirmwarePlugin/PX4/PX4FirmwarePlugin.cc +++ b/src/FirmwarePlugin/PX4/PX4FirmwarePlugin.cc @@ -18,37 +18,7 @@ #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, - PX4_CUSTOM_MAIN_MODE_STABILIZED, - PX4_CUSTOM_MAIN_MODE_RATTITUDE -}; - -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, - PX4_CUSTOM_SUB_MODE_AUTO_FOLLOW_ME -}; - -union px4_custom_mode { - struct { - uint16_t reserved; - uint8_t main_mode; - uint8_t sub_mode; - }; - uint32_t data; - float data_float; -}; +#include "px4_custom_mode.h" struct Modes2Name { uint8_t main_mode; @@ -60,39 +30,43 @@ struct Modes2Name { }; const char* PX4FirmwarePlugin::manualFlightMode = "Manual"; +const char* PX4FirmwarePlugin::altCtlFlightMode = "Altitude"; +const char* PX4FirmwarePlugin::posCtlFlightMode = "Position"; +const char* PX4FirmwarePlugin::missionFlightMode = "Mission"; +const char* PX4FirmwarePlugin::holdFlightMode = "Hold"; +const char* PX4FirmwarePlugin::takeoffFlightMode = "Takeoff"; +const char* PX4FirmwarePlugin::landingFlightMode = "Land"; +const char* PX4FirmwarePlugin::rtlFlightMode = "Return"; const char* PX4FirmwarePlugin::acroFlightMode = "Acro"; +const char* PX4FirmwarePlugin::offboardFlightMode = "Offboard"; const char* PX4FirmwarePlugin::stabilizedFlightMode = "Stabilized"; const char* PX4FirmwarePlugin::rattitudeFlightMode = "Rattitude"; -const char* PX4FirmwarePlugin::altCtlFlightMode = "Altitude Control"; -const char* PX4FirmwarePlugin::posCtlFlightMode = "Position Control"; -const char* PX4FirmwarePlugin::offboardFlightMode = "Offboard Control"; -const char* PX4FirmwarePlugin::readyFlightMode = "Ready"; -const char* PX4FirmwarePlugin::takeoffFlightMode = "Takeoff"; -const char* PX4FirmwarePlugin::pauseFlightMode = "Hold"; -const char* PX4FirmwarePlugin::missionFlightMode = "Mission"; -const char* PX4FirmwarePlugin::rtlFlightMode = "Return To Land"; -const char* PX4FirmwarePlugin::landingFlightMode = "Landing"; -const char* PX4FirmwarePlugin::rtgsFlightMode = "Return, Link Loss"; const char* PX4FirmwarePlugin::followMeFlightMode = "Follow Me"; +const char* PX4FirmwarePlugin::rtgsFlightMode = "Return to Groundstation"; + +const char* PX4FirmwarePlugin::readyFlightMode = "Ready"; // unused + /// Tranlates from PX4 custom modes to flight mode names static const struct Modes2Name rgModes2Name[] = { - { PX4_CUSTOM_MAIN_MODE_MANUAL, 0, PX4FirmwarePlugin::manualFlightMode, true, true, true}, - { PX4_CUSTOM_MAIN_MODE_ACRO, 0, PX4FirmwarePlugin::acroFlightMode, true, false, true}, - { PX4_CUSTOM_MAIN_MODE_STABILIZED, 0, PX4FirmwarePlugin::stabilizedFlightMode, true, true, true}, - { PX4_CUSTOM_MAIN_MODE_RATTITUDE, 0, PX4FirmwarePlugin::rattitudeFlightMode, true, false, true}, - { PX4_CUSTOM_MAIN_MODE_ALTCTL, 0, PX4FirmwarePlugin::altCtlFlightMode, true, true, true}, - { PX4_CUSTOM_MAIN_MODE_POSCTL, 0, PX4FirmwarePlugin::posCtlFlightMode, true, true, true}, - { PX4_CUSTOM_MAIN_MODE_OFFBOARD, 0, PX4FirmwarePlugin::offboardFlightMode, true, true, true}, - { PX4_CUSTOM_MAIN_MODE_AUTO, PX4_CUSTOM_SUB_MODE_AUTO_READY, PX4FirmwarePlugin::readyFlightMode, false, true, true}, - { PX4_CUSTOM_MAIN_MODE_AUTO, PX4_CUSTOM_SUB_MODE_AUTO_TAKEOFF, PX4FirmwarePlugin::takeoffFlightMode, false, true, true}, - { PX4_CUSTOM_MAIN_MODE_AUTO, PX4_CUSTOM_SUB_MODE_AUTO_LOITER, PX4FirmwarePlugin::pauseFlightMode, true, true, true}, - { PX4_CUSTOM_MAIN_MODE_AUTO, PX4_CUSTOM_SUB_MODE_AUTO_MISSION, PX4FirmwarePlugin::missionFlightMode, true, true, true}, - { PX4_CUSTOM_MAIN_MODE_AUTO, PX4_CUSTOM_SUB_MODE_AUTO_RTL, PX4FirmwarePlugin::rtlFlightMode, true, true, true}, - { PX4_CUSTOM_MAIN_MODE_AUTO, PX4_CUSTOM_SUB_MODE_AUTO_LAND, PX4FirmwarePlugin::landingFlightMode, false, true, true}, - { PX4_CUSTOM_MAIN_MODE_AUTO, PX4_CUSTOM_SUB_MODE_AUTO_RTGS, PX4FirmwarePlugin::rtgsFlightMode, false, true, true}, - { PX4_CUSTOM_MAIN_MODE_AUTO, PX4_CUSTOM_SUB_MODE_AUTO_FOLLOW_ME, PX4FirmwarePlugin::followMeFlightMode, true, true, true}, + //main_mode sub_mode name canBeSet FW MC + { PX4_CUSTOM_MAIN_MODE_MANUAL, 0, PX4FirmwarePlugin::manualFlightMode, true, true, true }, + { PX4_CUSTOM_MAIN_MODE_STABILIZED, 0, PX4FirmwarePlugin::stabilizedFlightMode, true, true, true }, + { PX4_CUSTOM_MAIN_MODE_ACRO, 0, PX4FirmwarePlugin::acroFlightMode, true, false, true }, + { PX4_CUSTOM_MAIN_MODE_RATTITUDE, 0, PX4FirmwarePlugin::rattitudeFlightMode, true, false, true }, + { PX4_CUSTOM_MAIN_MODE_ALTCTL, 0, PX4FirmwarePlugin::altCtlFlightMode, true, true, true }, + { PX4_CUSTOM_MAIN_MODE_POSCTL, 0, PX4FirmwarePlugin::posCtlFlightMode, true, true, true }, + { PX4_CUSTOM_MAIN_MODE_AUTO, PX4_CUSTOM_SUB_MODE_AUTO_LOITER, PX4FirmwarePlugin::holdFlightMode, true, true, true }, + { PX4_CUSTOM_MAIN_MODE_AUTO, PX4_CUSTOM_SUB_MODE_AUTO_MISSION, PX4FirmwarePlugin::missionFlightMode, true, true, true }, + { PX4_CUSTOM_MAIN_MODE_AUTO, PX4_CUSTOM_SUB_MODE_AUTO_RTL, PX4FirmwarePlugin::rtlFlightMode, true, true, true }, + { PX4_CUSTOM_MAIN_MODE_AUTO, PX4_CUSTOM_SUB_MODE_AUTO_FOLLOW_TARGET, PX4FirmwarePlugin::followMeFlightMode, true, true, true }, + { PX4_CUSTOM_MAIN_MODE_OFFBOARD, 0, PX4FirmwarePlugin::offboardFlightMode, true, true, true }, + // modes that can't be directly set by the user + { PX4_CUSTOM_MAIN_MODE_AUTO, PX4_CUSTOM_SUB_MODE_AUTO_LAND, PX4FirmwarePlugin::landingFlightMode, false, true, true }, + { PX4_CUSTOM_MAIN_MODE_AUTO, PX4_CUSTOM_SUB_MODE_AUTO_READY, PX4FirmwarePlugin::readyFlightMode, false, true, true }, + { PX4_CUSTOM_MAIN_MODE_AUTO, PX4_CUSTOM_SUB_MODE_AUTO_RTGS, PX4FirmwarePlugin::rtgsFlightMode, false, true, true }, + { PX4_CUSTOM_MAIN_MODE_AUTO, PX4_CUSTOM_SUB_MODE_AUTO_TAKEOFF, PX4FirmwarePlugin::takeoffFlightMode, false, true, true }, }; QList PX4FirmwarePlugin::componentsForVehicle(AutoPilotPlugin* vehicle) @@ -379,7 +353,7 @@ void PX4FirmwarePlugin::guidedModeChangeAltitude(Vehicle* vehicle, double altitu void PX4FirmwarePlugin::setGuidedMode(Vehicle* vehicle, bool guidedMode) { if (guidedMode) { - vehicle->setFlightMode(pauseFlightMode); + vehicle->setFlightMode(holdFlightMode); } else { pauseVehicle(vehicle); } @@ -388,6 +362,6 @@ void PX4FirmwarePlugin::setGuidedMode(Vehicle* vehicle, bool guidedMode) bool PX4FirmwarePlugin::isGuidedMode(const Vehicle* vehicle) const { // Not supported by generic vehicle - return (vehicle->flightMode() == pauseFlightMode || vehicle->flightMode() == takeoffFlightMode + return (vehicle->flightMode() == holdFlightMode || vehicle->flightMode() == takeoffFlightMode || vehicle->flightMode() == landingFlightMode); } diff --git a/src/FirmwarePlugin/PX4/PX4FirmwarePlugin.h b/src/FirmwarePlugin/PX4/PX4FirmwarePlugin.h index 7ad652e..a5c2229 100644 --- a/src/FirmwarePlugin/PX4/PX4FirmwarePlugin.h +++ b/src/FirmwarePlugin/PX4/PX4FirmwarePlugin.h @@ -63,7 +63,7 @@ public: static const char* offboardFlightMode; static const char* readyFlightMode; static const char* takeoffFlightMode; - static const char* pauseFlightMode; + static const char* holdFlightMode; static const char* missionFlightMode; static const char* rtlFlightMode; static const char* landingFlightMode; diff --git a/src/FirmwarePlugin/PX4/px4_custom_mode.h b/src/FirmwarePlugin/PX4/px4_custom_mode.h new file mode 100644 index 0000000..b3e9de3 --- /dev/null +++ b/src/FirmwarePlugin/PX4/px4_custom_mode.h @@ -0,0 +1,77 @@ +/**************************************************************************** + * + * Copyright (c) 2013-2016 PX4 Development Team. All rights reserved. + * + * Redistribution and use in source and binary forms, with or without + * modification, are permitted provided that the following conditions + * are met: + * + * 1. Redistributions of source code must retain the above copyright + * notice, this list of conditions and the following disclaimer. + * 2. Redistributions in binary form must reproduce the above copyright + * notice, this list of conditions and the following disclaimer in + * the documentation and/or other materials provided with the + * distribution. + * 3. Neither the name PX4 nor the names of its contributors may be + * used to endorse or promote products derived from this software + * without specific prior written permission. + * + * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS + * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT + * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS + * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE + * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, + * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, + * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS + * OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED + * AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT + * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN + * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE + * POSSIBILITY OF SUCH DAMAGE. + * + ****************************************************************************/ + +/** + * @file px4_custom_mode.h + * PX4 custom flight modes + * + */ + +#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, + PX4_CUSTOM_MAIN_MODE_STABILIZED, + PX4_CUSTOM_MAIN_MODE_RATTITUDE +}; + +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, + PX4_CUSTOM_SUB_MODE_AUTO_FOLLOW_TARGET +}; + +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/comm/MockLink.cc b/src/comm/MockLink.cc index 6cc291e..ecf6334 100644 --- a/src/comm/MockLink.cc +++ b/src/comm/MockLink.cc @@ -18,6 +18,8 @@ #include +#include "px4_custom_mode.h" + QGC_LOGGING_CATEGORY(MockLinkLog, "MockLinkLog") QGC_LOGGING_CATEGORY(MockLinkVerboseLog, "MockLinkVerboseLog") @@ -26,37 +28,6 @@ QGC_LOGGING_CATEGORY(MockLinkVerboseLog, "MockLinkVerboseLog") /// /// @author Don Gagne -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, - PX4_CUSTOM_MAIN_MODE_STABILIZED, - PX4_CUSTOM_MAIN_MODE_RATTITUDE -}; - -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; -}; - float MockLink::_vehicleLatitude = 47.633033f; float MockLink::_vehicleLongitude = -122.08794f; float MockLink::_vehicleAltitude = 3.5f;