diff --git a/src/FirmwarePlugin/APM/APMFirmwarePlugin.cc b/src/FirmwarePlugin/APM/APMFirmwarePlugin.cc index a5ce1069ea91f972c8802d509ade49031aab718f..2b280a360952f0b1e0fa47665ac44776d2ffdf31 100644 --- a/src/FirmwarePlugin/APM/APMFirmwarePlugin.cc +++ b/src/FirmwarePlugin/APM/APMFirmwarePlugin.cc @@ -15,6 +15,7 @@ #include "APMAirframeComponentController.h" #include "APMSensorsComponentController.h" #include "MissionManager.h" +#include "ParameterManager.h" #include @@ -158,11 +159,18 @@ AutoPilotPlugin* APMFirmwarePlugin::autopilotPlugin(Vehicle* vehicle) bool APMFirmwarePlugin::isCapable(const Vehicle* vehicle, FirmwareCapabilities capabilities) { - Q_UNUSED(vehicle); - - uint32_t vehicleCapabilities = SetFlightModeCapability | GuidedModeCapability | PauseVehicleCapability; + uint32_t available = SetFlightModeCapability | PauseVehicleCapability | GuidedModeCapability; + if (vehicle->multiRotor()) { + available |= TakeoffVehicleCapability; + } else if (vehicle->fixedWing()) { + // Quad plane supports takeoff + if (vehicle->parameterManager()->parameterExists(FactSystem::defaultComponentId, QStringLiteral("Q_ENABLE")) && + vehicle->parameterManager()->getParameter(FactSystem::defaultComponentId, QStringLiteral("Q_ENABLE"))->rawValue().toBool()) { + available |= TakeoffVehicleCapability; + } + } - return (capabilities & vehicleCapabilities) == capabilities; + return (capabilities & available) == capabilities; } QList APMFirmwarePlugin::componentsForVehicle(AutoPilotPlugin* vehicle) @@ -880,3 +888,88 @@ 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); +} + +bool APMFirmwarePlugin::_guidedModeTakeoff(Vehicle* vehicle) +{ + QString takeoffAltParam(vehicle->vtol() ? QStringLiteral("Q_RTL_ALT") : QStringLiteral("PILOT_TKOFF_ALT")); + float paramDivisor = vehicle->vtol() ? 1.0 : 100.0; // PILOT_TAKEOFF_ALT is in centimeters + + float takeoffAlt = 0; + if (vehicle->parameterManager()->parameterExists(FactSystem::defaultComponentId, takeoffAltParam)) { + Fact* takeoffAltFact = vehicle->parameterManager()->getParameter(FactSystem::defaultComponentId, takeoffAltParam); + takeoffAlt = takeoffAltFact->rawValue().toDouble(); + } + if (takeoffAlt <= 0) { + takeoffAlt = 2.5; + } else { + takeoffAlt /= paramDivisor; // centimeters -> meters + } + + if (!_setFlightModeAndValidate(vehicle, "Guided")) { + qgcApp()->showMessage(tr("Unable to takeoff: Vehicle failed to change to Guided mode.")); + return false; + } + + // FIXME: Is this needed? + if (!_armVehicleAndValidate(vehicle)) { + qgcApp()->showMessage(tr("Unable to takeoff: Vehicle failed to arm.")); + return false; + } + + vehicle->sendMavCommand(vehicle->defaultComponentId(), + MAV_CMD_NAV_TAKEOFF, + true, // show error + 0.0f, 0.0f, 0.0f, 0.0f, 0.0f, 0.0f, + takeoffAlt); + + return true; +} + +// FIXME: Review for a better way to do this +void APMFirmwarePlugin::startMission(Vehicle* vehicle) +{ + double currentAlt = vehicle->altitudeRelative()->rawValue().toDouble(); + + if (!vehicle->flying()) { + if (_guidedModeTakeoff(vehicle)) { + + // Wait for vehicle to get off ground before switching to auto (10 seconds) + bool didTakeoff = false; + for (int i=0; i<100; i++) { + if (vehicle->altitudeRelative()->rawValue().toDouble() >= currentAlt + 1.0) { + didTakeoff = true; + break; + } + QGC::SLEEP::msleep(100); + qgcApp()->processEvents(QEventLoop::ExcludeUserInputEvents); + } + + if (!didTakeoff) { + qgcApp()->showMessage(QStringLiteral("Unable to start mission. Vehicle takeoff failed.")); + return; + } + } + } + + if (!_setFlightModeAndValidate(vehicle, missionFlightMode())) { + qgcApp()->showMessage(QStringLiteral("Unable to start mission. Vehicle failed to change to auto.")); + return; + } +} diff --git a/src/FirmwarePlugin/APM/APMFirmwarePlugin.h b/src/FirmwarePlugin/APM/APMFirmwarePlugin.h index d3f55c94833f398fdaf3a9f4729d17d9a62b0752..a1205829fae20054c3103c6e6502e678b9cf5583 100644 --- a/src/FirmwarePlugin/APM/APMFirmwarePlugin.h +++ b/src/FirmwarePlugin/APM/APMFirmwarePlugin.h @@ -74,9 +74,12 @@ public: QList supportedMissionCommands(void) final; AutoPilotPlugin* autopilotPlugin (Vehicle* vehicle) final; - bool isCapable (const Vehicle *vehicle, FirmwareCapabilities capabilities) final; + bool isVtol (const Vehicle* vehicle) const final; + bool isCapable (const Vehicle *vehicle, FirmwareCapabilities capabilities) override; void setGuidedMode (Vehicle* vehicle, bool guidedMode) final; + void guidedModeTakeoff (Vehicle* vehicle) final; void guidedModeGotoLocation (Vehicle* vehicle, const QGeoCoordinate& gotoCoord) final; + void startMission (Vehicle* vehicle) 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; @@ -120,7 +123,8 @@ private: bool _handleIncomingStatusText(Vehicle* vehicle, mavlink_message_t* message); void _handleIncomingHeartbeat(Vehicle* vehicle, mavlink_message_t* message); void _handleOutgoingParamSet(Vehicle* vehicle, LinkInterface* outgoingLink, mavlink_message_t* message); - void _soloVideoHandshake(Vehicle* vehicle); + void _soloVideoHandshake(Vehicle* vehicle); + bool _guidedModeTakeoff(Vehicle* vehicle); // Any instance data here must be global to all vehicles // Vehicle specific data should go into APMFirmwarePluginInstanceData diff --git a/src/FirmwarePlugin/APM/ArduCopterFirmwarePlugin.cc b/src/FirmwarePlugin/APM/ArduCopterFirmwarePlugin.cc index b32d49f16edc0d152ba0ac9384ff1a6e34d49112..58ff75035fa5bd9d2e53241c00c853ae7a9bc052 100644 --- a/src/FirmwarePlugin/APM/ArduCopterFirmwarePlugin.cc +++ b/src/FirmwarePlugin/APM/ArduCopterFirmwarePlugin.cc @@ -160,45 +160,6 @@ void ArduCopterFirmwarePlugin::guidedModeLand(Vehicle* vehicle) _setFlightModeAndValidate(vehicle, "Land"); } -void ArduCopterFirmwarePlugin::guidedModeTakeoff(Vehicle* vehicle) -{ - _guidedModeTakeoff(vehicle); -} - -bool ArduCopterFirmwarePlugin::_guidedModeTakeoff(Vehicle* vehicle) -{ - QString takeoffAltParam("PILOT_TKOFF_ALT"); - - float takeoffAlt = 0; - if (vehicle->parameterManager()->parameterExists(FactSystem::defaultComponentId, takeoffAltParam)) { - Fact* takeoffAltFact = vehicle->parameterManager()->getParameter(FactSystem::defaultComponentId, takeoffAltParam); - takeoffAlt = takeoffAltFact->rawValue().toDouble(); - } - if (takeoffAlt <= 0) { - takeoffAlt = 2.5; - } else { - takeoffAlt /= 100; // centimeters -> meters - } - - if (!_setFlightModeAndValidate(vehicle, "Guided")) { - qgcApp()->showMessage(tr("Unable to takeoff: Vehicle failed to change to Guided mode.")); - return false; - } - - if (!_armVehicleAndValidate(vehicle)) { - qgcApp()->showMessage(tr("Unable to takeoff: Vehicle failed to arm.")); - return false; - } - - vehicle->sendMavCommand(vehicle->defaultComponentId(), - MAV_CMD_NAV_TAKEOFF, - true, // show error - 0.0f, 0.0f, 0.0f, 0.0f, 0.0f, 0.0f, - takeoffAlt); - - return true; -} - bool ArduCopterFirmwarePlugin::multiRotorCoaxialMotors(Vehicle* vehicle) { Q_UNUSED(vehicle); @@ -223,34 +184,3 @@ bool ArduCopterFirmwarePlugin::vehicleYawsToNextWaypointInMission(const Vehicle* return true; } -void ArduCopterFirmwarePlugin::startMission(Vehicle* vehicle) -{ - double currentAlt = vehicle->altitudeRelative()->rawValue().toDouble(); - - if (!vehicle->flying()) { - if (_guidedModeTakeoff(vehicle)) { - - // Wait for vehicle to get off ground before switching to auto (10 seconds) - bool didTakeoff = false; - for (int i=0; i<100; i++) { - if (vehicle->altitudeRelative()->rawValue().toDouble() >= currentAlt + 1.0) { - didTakeoff = true; - break; - } - QGC::SLEEP::msleep(100); - qgcApp()->processEvents(QEventLoop::ExcludeUserInputEvents); - } - - if (!didTakeoff) { - qgcApp()->showMessage(QStringLiteral("Unable to start mission. Vehicle takeoff failed.")); - return; - } - } - } - - if (!_setFlightModeAndValidate(vehicle, missionFlightMode())) { - qgcApp()->showMessage(QStringLiteral("Unable to start mission. Vehicle failed to change to auto.")); - return; - } -} - diff --git a/src/FirmwarePlugin/APM/ArduCopterFirmwarePlugin.h b/src/FirmwarePlugin/APM/ArduCopterFirmwarePlugin.h index ce1d9c89978d3f3ce3efef637027b2a16092ec68..6bdc9f61910d1cb00d92052da1d72fc9e5f777b0 100644 --- a/src/FirmwarePlugin/APM/ArduCopterFirmwarePlugin.h +++ b/src/FirmwarePlugin/APM/ArduCopterFirmwarePlugin.h @@ -57,7 +57,6 @@ public: // Overrides from FirmwarePlugin void guidedModeLand (Vehicle* vehicle) final; - void guidedModeTakeoff (Vehicle* vehicle) final; const FirmwarePlugin::remapParamNameMajorVersionMap_t& paramNameRemapMajorVersionMap(void) const final { return _remapParamName; } int remapParamNameHigestMinorVersionNumber(int majorVersionNumber) const final; bool multiRotorCoaxialMotors (Vehicle* vehicle) final; @@ -68,13 +67,10 @@ public: QString takeControlFlightMode (void) const override { return QString("Loiter"); } bool vehicleYawsToNextWaypointInMission (const Vehicle* vehicle) const final; QString autoDisarmParameter (Vehicle* vehicle) final { Q_UNUSED(vehicle); return QStringLiteral("DISARM_DELAY"); } - void startMission (Vehicle* vehicle) override; private: static bool _remapParamNameIntialized; static FirmwarePlugin::remapParamNameMajorVersionMap_t _remapParamName; - - bool _guidedModeTakeoff(Vehicle* vehicle); }; #endif diff --git a/src/FirmwarePlugin/APM/ArduPlaneFirmwarePlugin.h b/src/FirmwarePlugin/APM/ArduPlaneFirmwarePlugin.h index 650611ff7c33b587e33563118a4aff1baf77acc2..e4465c01a1fcef636aec21d824eb81c33e30c13b 100644 --- a/src/FirmwarePlugin/APM/ArduPlaneFirmwarePlugin.h +++ b/src/FirmwarePlugin/APM/ArduPlaneFirmwarePlugin.h @@ -60,7 +60,6 @@ public: QString offlineEditingParamFile (Vehicle* vehicle) final { Q_UNUSED(vehicle); return QStringLiteral(":/FirmwarePlugin/APM/Plane.OfflineEditing.params"); } QString autoDisarmParameter (Vehicle* vehicle) final { Q_UNUSED(vehicle); return QStringLiteral("LAND_DISARMDELAY"); } int remapParamNameHigestMinorVersionNumber (int majorVersionNumber) const final; - const FirmwarePlugin::remapParamNameMajorVersionMap_t& paramNameRemapMajorVersionMap(void) const final { return _remapParamName; } private: static bool _remapParamNameIntialized;