From 3733318c93981d4da2bf37225d691a4cbc37fc39 Mon Sep 17 00:00:00 2001 From: davidsastresas Date: Mon, 19 Jun 2023 19:19:51 +0200 Subject: [PATCH] Ardupilot: Add support for fixed wing airspeed change --- src/FirmwarePlugin/APM/APMFirmwarePlugin.cc | 42 +++++++++++++++++++++++++++++ src/FirmwarePlugin/APM/APMFirmwarePlugin.h | 4 +++ 2 files changed, 46 insertions(+) diff --git a/src/FirmwarePlugin/APM/APMFirmwarePlugin.cc b/src/FirmwarePlugin/APM/APMFirmwarePlugin.cc index 987826a..58cd5a0 100644 --- a/src/FirmwarePlugin/APM/APMFirmwarePlugin.cc +++ b/src/FirmwarePlugin/APM/APMFirmwarePlugin.cc @@ -1133,3 +1133,45 @@ QMutex& APMFirmwarePlugin::_reencodeMavlinkChannelMutex() static QMutex _mutex{}; return _mutex; } + +double APMFirmwarePlugin::maximumEquivalentAirspeed(Vehicle* vehicle) +{ + QString airspeedMax("ARSPD_FBW_MAX"); + + if (vehicle->parameterManager()->parameterExists(FactSystem::defaultComponentId, airspeedMax)) { + return vehicle->parameterManager()->getParameter(FactSystem::defaultComponentId, airspeedMax)->rawValue().toDouble(); + } + + return FirmwarePlugin::maximumEquivalentAirspeed(vehicle); +} + +double APMFirmwarePlugin::minimumEquivalentAirspeed(Vehicle* vehicle) +{ + QString airspeedMin("ARSPD_FBW_MIN"); + + if (vehicle->parameterManager()->parameterExists(FactSystem::defaultComponentId, airspeedMin)) { + return vehicle->parameterManager()->getParameter(FactSystem::defaultComponentId, airspeedMin)->rawValue().toDouble(); + } + + return FirmwarePlugin::minimumEquivalentAirspeed(vehicle); +} + +bool APMFirmwarePlugin::fixedWingAirSpeedLimitsAvailable(Vehicle* vehicle) +{ + return vehicle->parameterManager()->parameterExists(FactSystem::defaultComponentId, "ARSPD_FBW_MIN") && + vehicle->parameterManager()->parameterExists(FactSystem::defaultComponentId, "ARSPD_FBW_MAX"); +} + +void APMFirmwarePlugin::guidedModeChangeEquivalentAirspeed(Vehicle* vehicle, double airspeed_equiv) +{ + + vehicle->sendMavCommand( + vehicle->defaultComponentId(), + MAV_CMD_DO_CHANGE_SPEED, + true, // show error is fails + 0, // 0: airspeed, 1: groundspeed + static_cast(airspeed_equiv), // speed setpoint + -1, // throttle, no change + 0 // 0: absolute speed, 1: relative to current + ); // param 5-7 unused +} \ No newline at end of file diff --git a/src/FirmwarePlugin/APM/APMFirmwarePlugin.h b/src/FirmwarePlugin/APM/APMFirmwarePlugin.h index 4a1dd06..4dd3d2f 100644 --- a/src/FirmwarePlugin/APM/APMFirmwarePlugin.h +++ b/src/FirmwarePlugin/APM/APMFirmwarePlugin.h @@ -80,6 +80,10 @@ public: QString getHobbsMeter (Vehicle* vehicle) override; bool hasGripper (const Vehicle* vehicle) const override; const QVariantList& toolIndicators (const Vehicle* vehicle) override; + double maximumEquivalentAirspeed (Vehicle* vehicle) override; + double minimumEquivalentAirspeed (Vehicle* vehicle) override; + bool fixedWingAirSpeedLimitsAvailable(Vehicle* vehicle) override; + void guidedModeChangeEquivalentAirspeed(Vehicle* vehicle, double airspeed_equiv) override; protected: /// All access to singleton is through stack specific implementation