From 6043cf9515fb73f125f2fa9098845bfb3bfe212d Mon Sep 17 00:00:00 2001 From: Daniel Agar Date: Thu, 12 May 2016 20:39:18 -0400 Subject: [PATCH] PX4 only show FW/MC modes --- src/FirmwarePlugin/APM/APMFirmwarePlugin.cc | 3 +- src/FirmwarePlugin/APM/APMFirmwarePlugin.h | 2 +- src/FirmwarePlugin/FirmwarePlugin.h | 5 ++- src/FirmwarePlugin/PX4/PX4FirmwarePlugin.cc | 44 +++++++++++++-------- src/FirmwarePlugin/PX4/PX4FirmwarePlugin.h | 2 +- src/Vehicle/Vehicle.cc | 2 +- 6 files changed, 36 insertions(+), 22 deletions(-) diff --git a/src/FirmwarePlugin/APM/APMFirmwarePlugin.cc b/src/FirmwarePlugin/APM/APMFirmwarePlugin.cc index 48c46f62a..f794a985a 100644 --- a/src/FirmwarePlugin/APM/APMFirmwarePlugin.cc +++ b/src/FirmwarePlugin/APM/APMFirmwarePlugin.cc @@ -167,8 +167,9 @@ QList APMFirmwarePlugin::componentsForVehicle(AutoPilotPlugin return QList(); } -QStringList APMFirmwarePlugin::flightModes(void) +QStringList APMFirmwarePlugin::flightModes(Vehicle* vehicle) { + Q_UNUSED(vehicle) QStringList flightModesList; foreach (const APMCustomMode& customMode, _supportedModes) { if (customMode.canBeSet()) { diff --git a/src/FirmwarePlugin/APM/APMFirmwarePlugin.h b/src/FirmwarePlugin/APM/APMFirmwarePlugin.h index 0c7b7701c..f8a91fe87 100644 --- a/src/FirmwarePlugin/APM/APMFirmwarePlugin.h +++ b/src/FirmwarePlugin/APM/APMFirmwarePlugin.h @@ -87,7 +87,7 @@ public: QList supportedMissionCommands(void) final; bool isCapable(FirmwareCapabilities capabilities); - QStringList flightModes(void) final; + QStringList flightModes(Vehicle* vehicle) final; QString flightMode(uint8_t base_mode, uint32_t custom_mode) const final; bool setFlightMode(const QString& flightMode, uint8_t* base_mode, uint32_t* custom_mode) final; bool isGuidedMode(const Vehicle* vehicle) const final; diff --git a/src/FirmwarePlugin/FirmwarePlugin.h b/src/FirmwarePlugin/FirmwarePlugin.h index b2afe94de..6578f9f39 100644 --- a/src/FirmwarePlugin/FirmwarePlugin.h +++ b/src/FirmwarePlugin/FirmwarePlugin.h @@ -84,7 +84,10 @@ public: virtual QList componentsForVehicle(AutoPilotPlugin* vehicle); /// Returns the list of available flight modes - virtual QStringList flightModes(void) { return QStringList(); } + virtual QStringList flightModes(Vehicle* vehicle) { + Q_UNUSED(vehicle); + return QStringList(); + } /// Returns the name for this flight mode. Flight mode names must be human readable as well as audio speakable. /// @param base_mode Base mode from mavlink HEARTBEAT message diff --git a/src/FirmwarePlugin/PX4/PX4FirmwarePlugin.cc b/src/FirmwarePlugin/PX4/PX4FirmwarePlugin.cc index 8b19a6e1c..4492accc0 100644 --- a/src/FirmwarePlugin/PX4/PX4FirmwarePlugin.cc +++ b/src/FirmwarePlugin/PX4/PX4FirmwarePlugin.cc @@ -68,6 +68,8 @@ struct Modes2Name { uint8_t sub_mode; const char* name; ///< Name for flight mode bool canBeSet; ///< true: Vehicle can be set to this flight mode + bool fixedWing; /// fixed wing compatible + bool multiRotor; /// multi rotor compatible }; const char* PX4FirmwarePlugin::manualFlightMode = "Manual"; @@ -89,21 +91,21 @@ const char* PX4FirmwarePlugin::followMeFlightMode = "Follow Me"; /// Tranlates from PX4 custom modes to flight mode names static const struct Modes2Name rgModes2Name[] = { - { PX4_CUSTOM_MAIN_MODE_MANUAL, 0, PX4FirmwarePlugin::manualFlightMode, true }, - { PX4_CUSTOM_MAIN_MODE_ACRO, 0, PX4FirmwarePlugin::acroFlightMode, true }, - { PX4_CUSTOM_MAIN_MODE_STABILIZED, 0, PX4FirmwarePlugin::stabilizedFlightMode, true }, - { PX4_CUSTOM_MAIN_MODE_RATTITUDE, 0, PX4FirmwarePlugin::rattitudeFlightMode, true }, - { PX4_CUSTOM_MAIN_MODE_ALTCTL, 0, PX4FirmwarePlugin::altCtlFlightMode, true }, - { PX4_CUSTOM_MAIN_MODE_POSCTL, 0, PX4FirmwarePlugin::posCtlFlightMode, true }, - { PX4_CUSTOM_MAIN_MODE_OFFBOARD, 0, PX4FirmwarePlugin::offboardFlightMode, true }, - { PX4_CUSTOM_MAIN_MODE_AUTO, PX4_CUSTOM_SUB_MODE_AUTO_READY, PX4FirmwarePlugin::readyFlightMode, false }, - { PX4_CUSTOM_MAIN_MODE_AUTO, PX4_CUSTOM_SUB_MODE_AUTO_TAKEOFF, PX4FirmwarePlugin::takeoffFlightMode, false }, - { PX4_CUSTOM_MAIN_MODE_AUTO, PX4_CUSTOM_SUB_MODE_AUTO_LOITER, PX4FirmwarePlugin::pauseFlightMode, true }, - { PX4_CUSTOM_MAIN_MODE_AUTO, PX4_CUSTOM_SUB_MODE_AUTO_MISSION, PX4FirmwarePlugin::missionFlightMode, true }, - { PX4_CUSTOM_MAIN_MODE_AUTO, PX4_CUSTOM_SUB_MODE_AUTO_RTL, PX4FirmwarePlugin::rtlFlightMode, true }, - { PX4_CUSTOM_MAIN_MODE_AUTO, PX4_CUSTOM_SUB_MODE_AUTO_LAND, PX4FirmwarePlugin::landingFlightMode, false }, - { PX4_CUSTOM_MAIN_MODE_AUTO, PX4_CUSTOM_SUB_MODE_AUTO_RTGS, PX4FirmwarePlugin::rtgsFlightMode, false }, - { PX4_CUSTOM_MAIN_MODE_AUTO, PX4_CUSTOM_SUB_MODE_AUTO_FOLLOW_ME, PX4FirmwarePlugin::followMeFlightMode, true }, + { 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}, }; QList PX4FirmwarePlugin::componentsForVehicle(AutoPilotPlugin* vehicle) @@ -113,7 +115,7 @@ QList PX4FirmwarePlugin::componentsForVehicle(AutoPilotPlugin return QList(); } -QStringList PX4FirmwarePlugin::flightModes(void) +QStringList PX4FirmwarePlugin::flightModes(Vehicle* vehicle) { QStringList flightModes; @@ -121,7 +123,15 @@ QStringList PX4FirmwarePlugin::flightModes(void) const struct Modes2Name* pModes2Name = &rgModes2Name[i]; if (pModes2Name->canBeSet) { - flightModes += pModes2Name->name; + bool fw = (vehicle->fixedWing() && pModes2Name->fixedWing); + bool mc = (vehicle->multiRotor() && pModes2Name->multiRotor); + + // show all modes for generic, vtol, etc + bool other = !vehicle->fixedWing() && !vehicle->multiRotor(); + + if (fw || mc || other) { + flightModes += pModes2Name->name; + } } } diff --git a/src/FirmwarePlugin/PX4/PX4FirmwarePlugin.h b/src/FirmwarePlugin/PX4/PX4FirmwarePlugin.h index d41af304b..04e5c6a35 100644 --- a/src/FirmwarePlugin/PX4/PX4FirmwarePlugin.h +++ b/src/FirmwarePlugin/PX4/PX4FirmwarePlugin.h @@ -42,7 +42,7 @@ public: QList supportedMissionCommands(void) final; bool isCapable (FirmwareCapabilities capabilities) final; - QStringList flightModes (void) final; + QStringList flightModes (Vehicle* vehicle) final; QString flightMode (uint8_t base_mode, uint32_t custom_mode) const final; bool setFlightMode (const QString& flightMode, uint8_t* base_mode, uint32_t* custom_mode) final; void setGuidedMode(Vehicle* vehicle, bool guidedMode) final; diff --git a/src/Vehicle/Vehicle.cc b/src/Vehicle/Vehicle.cc index 961fab555..058d93e84 100644 --- a/src/Vehicle/Vehicle.cc +++ b/src/Vehicle/Vehicle.cc @@ -1193,7 +1193,7 @@ bool Vehicle::flightModeSetAvailable(void) QStringList Vehicle::flightModes(void) { - return _firmwarePlugin->flightModes(); + return _firmwarePlugin->flightModes(this); } QString Vehicle::flightMode(void) const -- 2.22.0