From 1432ebededeaf1e71a59707509d851b59d13f442 Mon Sep 17 00:00:00 2001 From: DonLakeFlyer Date: Fri, 20 Oct 2017 11:50:04 -0700 Subject: [PATCH] Assume VTOL type is coming from HEARTBEAT --- src/FirmwarePlugin/APM/APMFirmwarePlugin.cc | 16 +--------------- src/FirmwarePlugin/APM/APMFirmwarePlugin.h | 1 - 2 files changed, 1 insertion(+), 16 deletions(-) diff --git a/src/FirmwarePlugin/APM/APMFirmwarePlugin.cc b/src/FirmwarePlugin/APM/APMFirmwarePlugin.cc index 8f7f35953..be893cf1f 100644 --- a/src/FirmwarePlugin/APM/APMFirmwarePlugin.cc +++ b/src/FirmwarePlugin/APM/APMFirmwarePlugin.cc @@ -162,9 +162,7 @@ bool APMFirmwarePlugin::isCapable(const Vehicle* vehicle, FirmwareCapabilities c uint32_t available = SetFlightModeCapability | PauseVehicleCapability | GuidedModeCapability; if (vehicle->multiRotor()) { available |= TakeoffVehicleCapability; - } else if (vehicle->fixedWing() || vehicle->vtol()) { - // Due to the way ArduPilot marks a vtol aircraft, we don't know if something is a vtol at initial connection. - // So we always enabled takeoff for fixed wing. + } else if (vehicle->vtol()) { available |= TakeoffVehicleCapability; } @@ -887,18 +885,6 @@ void APMFirmwarePlugin::guidedModeChangeAltitude(Vehicle* vehicle, double altitu vehicle->sendMessageOnLink(vehicle->priorityLink(), msg); } -bool APMFirmwarePlugin::isVtol(const Vehicle* vehicle) const -{ - if (vehicle->fixedWing()) { - if (vehicle->parameterManager()->parameterExists(FactSystem::defaultComponentId, QStringLiteral("Q_ENABLE")) && - vehicle->parameterManager()->getParameter(FactSystem::defaultComponentId, QStringLiteral("Q_ENABLE"))->rawValue().toBool()) { - return true; - } - } - - return false; -} - void APMFirmwarePlugin::guidedModeTakeoff(Vehicle* vehicle) { _guidedModeTakeoff(vehicle); diff --git a/src/FirmwarePlugin/APM/APMFirmwarePlugin.h b/src/FirmwarePlugin/APM/APMFirmwarePlugin.h index 7d1e3cc19..491c843e1 100644 --- a/src/FirmwarePlugin/APM/APMFirmwarePlugin.h +++ b/src/FirmwarePlugin/APM/APMFirmwarePlugin.h @@ -74,7 +74,6 @@ public: QList supportedMissionCommands(void) override; AutoPilotPlugin* autopilotPlugin (Vehicle* vehicle) override; - bool isVtol (const Vehicle* vehicle) const override; bool isCapable (const Vehicle *vehicle, FirmwareCapabilities capabilities) override; void setGuidedMode (Vehicle* vehicle, bool guidedMode) override; void guidedModeTakeoff (Vehicle* vehicle) override; -- 2.22.0