Commit 1432ebed authored by DonLakeFlyer's avatar DonLakeFlyer

Assume VTOL type is coming from HEARTBEAT

parent e4e1936d
...@@ -162,9 +162,7 @@ bool APMFirmwarePlugin::isCapable(const Vehicle* vehicle, FirmwareCapabilities c ...@@ -162,9 +162,7 @@ bool APMFirmwarePlugin::isCapable(const Vehicle* vehicle, FirmwareCapabilities c
uint32_t available = SetFlightModeCapability | PauseVehicleCapability | GuidedModeCapability; uint32_t available = SetFlightModeCapability | PauseVehicleCapability | GuidedModeCapability;
if (vehicle->multiRotor()) { if (vehicle->multiRotor()) {
available |= TakeoffVehicleCapability; available |= TakeoffVehicleCapability;
} else if (vehicle->fixedWing() || vehicle->vtol()) { } else if (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.
available |= TakeoffVehicleCapability; available |= TakeoffVehicleCapability;
} }
...@@ -887,18 +885,6 @@ void APMFirmwarePlugin::guidedModeChangeAltitude(Vehicle* vehicle, double altitu ...@@ -887,18 +885,6 @@ void APMFirmwarePlugin::guidedModeChangeAltitude(Vehicle* vehicle, double altitu
vehicle->sendMessageOnLink(vehicle->priorityLink(), msg); 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) void APMFirmwarePlugin::guidedModeTakeoff(Vehicle* vehicle)
{ {
_guidedModeTakeoff(vehicle); _guidedModeTakeoff(vehicle);
......
...@@ -74,7 +74,6 @@ public: ...@@ -74,7 +74,6 @@ public:
QList<MAV_CMD> supportedMissionCommands(void) override; QList<MAV_CMD> supportedMissionCommands(void) override;
AutoPilotPlugin* autopilotPlugin (Vehicle* vehicle) override; AutoPilotPlugin* autopilotPlugin (Vehicle* vehicle) override;
bool isVtol (const Vehicle* vehicle) const override;
bool isCapable (const Vehicle *vehicle, FirmwareCapabilities capabilities) override; bool isCapable (const Vehicle *vehicle, FirmwareCapabilities capabilities) override;
void setGuidedMode (Vehicle* vehicle, bool guidedMode) override; void setGuidedMode (Vehicle* vehicle, bool guidedMode) override;
void guidedModeTakeoff (Vehicle* vehicle) override; void guidedModeTakeoff (Vehicle* vehicle) override;
......
Markdown is supported
0% or
You are about to add 0 people to the discussion. Proceed with caution.
Finish editing this message first!
Please register or to comment