diff --git a/src/FirmwarePlugin/APM/APMFirmwarePlugin.cc b/src/FirmwarePlugin/APM/APMFirmwarePlugin.cc index 185cd8853a9108c35cf4834803f1ca5bb3821e65..a9b993f72d3647e7d32846c3e6a0f3dfe92811e3 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 7d1e3cc196433f9d629d092f98eb493e6724929f..491c843e1b6a42177e2f5b5060b0ccf1d351c2c6 100644 --- a/src/FirmwarePlugin/APM/APMFirmwarePlugin.h +++ b/src/FirmwarePlugin/APM/APMFirmwarePlugin.h @@ -74,7 +74,6 @@ public: QList<MAV_CMD> 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; diff --git a/src/FirmwarePlugin/APM/APMParameterMetaData.cc b/src/FirmwarePlugin/APM/APMParameterMetaData.cc index 7d9b848ef3280c39da4beb3077c30b0b55f2db08..f5154f921b16e52488b436624990fedb46e38195 100644 --- a/src/FirmwarePlugin/APM/APMParameterMetaData.cc +++ b/src/FirmwarePlugin/APM/APMParameterMetaData.cc @@ -83,6 +83,13 @@ QString APMParameterMetaData::mavTypeToString(MAV_TYPE vehicleTypeEnum) switch(vehicleTypeEnum) { case MAV_TYPE_FIXED_WING: + case MAV_TYPE_VTOL_DUOROTOR: + case MAV_TYPE_VTOL_QUADROTOR: + case MAV_TYPE_VTOL_TILTROTOR: + case MAV_TYPE_VTOL_RESERVED2: + case MAV_TYPE_VTOL_RESERVED3: + case MAV_TYPE_VTOL_RESERVED4: + case MAV_TYPE_VTOL_RESERVED5: vehicleName = "ArduPlane"; break; case MAV_TYPE_QUADROTOR: @@ -111,13 +118,6 @@ QString APMParameterMetaData::mavTypeToString(MAV_TYPE vehicleTypeEnum) case MAV_TYPE_FLAPPING_WING: case MAV_TYPE_KITE: case MAV_TYPE_ONBOARD_CONTROLLER: - case MAV_TYPE_VTOL_DUOROTOR: - case MAV_TYPE_VTOL_QUADROTOR: - case MAV_TYPE_VTOL_TILTROTOR: - case MAV_TYPE_VTOL_RESERVED2: - case MAV_TYPE_VTOL_RESERVED3: - case MAV_TYPE_VTOL_RESERVED4: - case MAV_TYPE_VTOL_RESERVED5: case MAV_TYPE_GIMBAL: case MAV_TYPE_ENUM_END: default: