diff --git a/src/FirmwarePlugin/APM/APMFirmwarePlugin.cc b/src/FirmwarePlugin/APM/APMFirmwarePlugin.cc index 770d7d229254b4dce949420997ee30a7cbd81227..a5ce1069ea91f972c8802d509ade49031aab718f 100644 --- a/src/FirmwarePlugin/APM/APMFirmwarePlugin.cc +++ b/src/FirmwarePlugin/APM/APMFirmwarePlugin.cc @@ -7,10 +7,6 @@ * ****************************************************************************/ - -/// @file -/// @author Don Gagne - #include "APMFirmwarePlugin.h" #include "APMAutoPilotPlugin.h" #include "QGCMAVLink.h" @@ -18,6 +14,7 @@ #include "APMFlightModesComponentController.h" #include "APMAirframeComponentController.h" #include "APMSensorsComponentController.h" +#include "MissionManager.h" #include @@ -159,9 +156,13 @@ AutoPilotPlugin* APMFirmwarePlugin::autopilotPlugin(Vehicle* vehicle) return new APMAutoPilotPlugin(vehicle, vehicle); } -bool APMFirmwarePlugin::isCapable(const Vehicle* /*vehicle*/, FirmwareCapabilities capabilities) +bool APMFirmwarePlugin::isCapable(const Vehicle* vehicle, FirmwareCapabilities capabilities) { - return (capabilities & (SetFlightModeCapability | PauseVehicleCapability)) == capabilities; + Q_UNUSED(vehicle); + + uint32_t vehicleCapabilities = SetFlightModeCapability | GuidedModeCapability | PauseVehicleCapability; + + return (capabilities & vehicleCapabilities) == capabilities; } QList APMFirmwarePlugin::componentsForVehicle(AutoPilotPlugin* vehicle) @@ -725,12 +726,6 @@ bool APMFirmwarePlugin::isGuidedMode(const Vehicle* vehicle) const return vehicle->flightMode() == "Guided"; } -void APMFirmwarePlugin::pauseVehicle(Vehicle* vehicle) -{ - // Best we can do in this case - vehicle->setFlightMode("Loiter"); -} - void APMFirmwarePlugin::_soloVideoHandshake(Vehicle* vehicle) { Q_UNUSED(vehicle); @@ -819,3 +814,69 @@ QString APMFirmwarePlugin::internalParameterMetaDataFile(Vehicle* vehicle) return QString(); } } + +void APMFirmwarePlugin::setGuidedMode(Vehicle* vehicle, bool guidedMode) +{ + if (guidedMode) { + _setFlightModeAndValidate(vehicle, "Guided"); + } else { + pauseVehicle(vehicle); + } +} + +void APMFirmwarePlugin::pauseVehicle(Vehicle* vehicle) +{ + _setFlightModeAndValidate(vehicle, pauseFlightMode()); +} + +void APMFirmwarePlugin::guidedModeGotoLocation(Vehicle* vehicle, const QGeoCoordinate& gotoCoord) +{ + if (qIsNaN(vehicle->altitudeRelative()->rawValue().toDouble())) { + qgcApp()->showMessage(QStringLiteral("Unable to go to location, vehicle position not known.")); + return; + } + + + setGuidedMode(vehicle, true); + + QGeoCoordinate coordWithAltitude = gotoCoord; + coordWithAltitude.setAltitude(vehicle->altitudeRelative()->rawValue().toDouble()); + vehicle->missionManager()->writeArduPilotGuidedMissionItem(coordWithAltitude, false /* altChangeOnly */); +} + +void APMFirmwarePlugin::guidedModeRTL(Vehicle* vehicle) +{ + _setFlightModeAndValidate(vehicle, rtlFlightMode()); +} + +void APMFirmwarePlugin::guidedModeChangeAltitude(Vehicle* vehicle, double altitudeChange) +{ + if (qIsNaN(vehicle->altitudeRelative()->rawValue().toDouble())) { + qgcApp()->showMessage(QStringLiteral("Unable to change altitude, vehicle altitude not known.")); + return; + } + + setGuidedMode(vehicle, true); + + mavlink_message_t msg; + mavlink_set_position_target_local_ned_t cmd; + + memset(&cmd, 0, sizeof(cmd)); + + cmd.target_system = vehicle->id(); + cmd.target_component = vehicle->defaultComponentId(); + cmd.coordinate_frame = MAV_FRAME_LOCAL_OFFSET_NED; + cmd.type_mask = 0xFFF8; // Only x/y/z valid + cmd.x = 0.0f; + cmd.y = 0.0f; + cmd.z = -(altitudeChange); + + MAVLinkProtocol* mavlink = qgcApp()->toolbox()->mavlinkProtocol(); + mavlink_msg_set_position_target_local_ned_encode_chan(mavlink->getSystemId(), + mavlink->getComponentId(), + vehicle->priorityLink()->mavlinkChannel(), + &msg, + &cmd); + + vehicle->sendMessageOnLink(vehicle->priorityLink(), msg); +} diff --git a/src/FirmwarePlugin/APM/APMFirmwarePlugin.h b/src/FirmwarePlugin/APM/APMFirmwarePlugin.h index aa27e876b13d59e85ec7fc9ce35cfa0bbd5c55ac..d3f55c94833f398fdaf3a9f4729d17d9a62b0752 100644 --- a/src/FirmwarePlugin/APM/APMFirmwarePlugin.h +++ b/src/FirmwarePlugin/APM/APMFirmwarePlugin.h @@ -74,12 +74,18 @@ public: QList supportedMissionCommands(void) final; AutoPilotPlugin* autopilotPlugin (Vehicle* vehicle) final; - bool isCapable (const Vehicle *vehicle, FirmwareCapabilities capabilities) override; + bool isCapable (const Vehicle *vehicle, FirmwareCapabilities capabilities) final; + void setGuidedMode (Vehicle* vehicle, bool guidedMode) final; + void guidedModeGotoLocation (Vehicle* vehicle, const QGeoCoordinate& gotoCoord) 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; bool isGuidedMode (const Vehicle* vehicle) const final; - void pauseVehicle (Vehicle* vehicle) override; + QString rtlFlightMode (void) const final { return QString("RTL"); } + QString missionFlightMode (void) const final { return QString("Auto"); } + void pauseVehicle (Vehicle* vehicle) final; + void guidedModeRTL (Vehicle* vehicle) final; + void guidedModeChangeAltitude (Vehicle* vehicle, double altitudeChange) override; int manualControlReservedButtonCount(void) override; bool adjustIncomingMavlinkMessage (Vehicle* vehicle, mavlink_message_t* message) override; void adjustOutgoingMavlinkMessage (Vehicle* vehicle, LinkInterface* outgoingLink, mavlink_message_t* message) final; diff --git a/src/FirmwarePlugin/APM/ArduCopterFirmwarePlugin.cc b/src/FirmwarePlugin/APM/ArduCopterFirmwarePlugin.cc index 9a8fb896d3bcfe994081c252631945a64b06a273..2d05daa46fe37be602626286e6dcfd60ea097cb9 100644 --- a/src/FirmwarePlugin/APM/ArduCopterFirmwarePlugin.cc +++ b/src/FirmwarePlugin/APM/ArduCopterFirmwarePlugin.cc @@ -151,20 +151,6 @@ int ArduCopterFirmwarePlugin::remapParamNameHigestMinorVersionNumber(int majorVe return majorVersionNumber == 3 ? 5 : Vehicle::versionNotSetValue; } -bool ArduCopterFirmwarePlugin::isCapable(const Vehicle* vehicle, FirmwareCapabilities capabilities) -{ - Q_UNUSED(vehicle); - - uint32_t vehicleCapabilities = SetFlightModeCapability | GuidedModeCapability | PauseVehicleCapability; - - return (capabilities & vehicleCapabilities) == capabilities; -} - -void ArduCopterFirmwarePlugin::guidedModeRTL(Vehicle* vehicle) -{ - _setFlightModeAndValidate(vehicle, "RTL"); -} - void ArduCopterFirmwarePlugin::guidedModeLand(Vehicle* vehicle) { _setFlightModeAndValidate(vehicle, "Land"); @@ -209,64 +195,6 @@ bool ArduCopterFirmwarePlugin::_guidedModeTakeoff(Vehicle* vehicle) return true; } -void ArduCopterFirmwarePlugin::guidedModeGotoLocation(Vehicle* vehicle, const QGeoCoordinate& gotoCoord) -{ - if (qIsNaN(vehicle->altitudeRelative()->rawValue().toDouble())) { - qgcApp()->showMessage(QStringLiteral("Unable to go to location, vehicle position not known.")); - return; - } - - QGeoCoordinate coordWithAltitude = gotoCoord; - coordWithAltitude.setAltitude(vehicle->altitudeRelative()->rawValue().toDouble()); - vehicle->missionManager()->writeArduPilotGuidedMissionItem(coordWithAltitude, false /* altChangeOnly */); -} - -void ArduCopterFirmwarePlugin::guidedModeChangeAltitude(Vehicle* vehicle, double altitudeChange) -{ - if (qIsNaN(vehicle->altitudeRelative()->rawValue().toDouble())) { - qgcApp()->showMessage(QStringLiteral("Unable to change altitude, vehicle altitude not known.")); - return; - } - - setGuidedMode(vehicle, true); - - mavlink_message_t msg; - mavlink_set_position_target_local_ned_t cmd; - - memset(&cmd, 0, sizeof(cmd)); - - cmd.target_system = vehicle->id(); - cmd.target_component = vehicle->defaultComponentId(); - cmd.coordinate_frame = MAV_FRAME_LOCAL_OFFSET_NED; - cmd.type_mask = 0xFFF8; // Only x/y/z valid - cmd.x = 0.0f; - cmd.y = 0.0f; - cmd.z = -(altitudeChange); - - MAVLinkProtocol* mavlink = qgcApp()->toolbox()->mavlinkProtocol(); - mavlink_msg_set_position_target_local_ned_encode_chan(mavlink->getSystemId(), - mavlink->getComponentId(), - vehicle->priorityLink()->mavlinkChannel(), - &msg, - &cmd); - - vehicle->sendMessageOnLink(vehicle->priorityLink(), msg); -} - -void ArduCopterFirmwarePlugin::pauseVehicle(Vehicle* vehicle) -{ - _setFlightModeAndValidate(vehicle, "Brake"); -} - -void ArduCopterFirmwarePlugin::setGuidedMode(Vehicle* vehicle, bool guidedMode) -{ - if (guidedMode) { - _setFlightModeAndValidate(vehicle, "Guided"); - } else { - pauseVehicle(vehicle); - } -} - bool ArduCopterFirmwarePlugin::multiRotorCoaxialMotors(Vehicle* vehicle) { Q_UNUSED(vehicle); diff --git a/src/FirmwarePlugin/APM/ArduCopterFirmwarePlugin.h b/src/FirmwarePlugin/APM/ArduCopterFirmwarePlugin.h index e21d4f6d09a051080b9f9d507a44daa644623a14..d5176604e7358c74bc82b97d011e7e363d8da83c 100644 --- a/src/FirmwarePlugin/APM/ArduCopterFirmwarePlugin.h +++ b/src/FirmwarePlugin/APM/ArduCopterFirmwarePlugin.h @@ -55,22 +55,14 @@ public: ArduCopterFirmwarePlugin(void); // Overrides from FirmwarePlugin - bool isCapable (const Vehicle *vehicle, FirmwareCapabilities capabilities) final; - void setGuidedMode (Vehicle* vehicle, bool guidedMode) final; - void pauseVehicle (Vehicle* vehicle) final; - void guidedModeRTL (Vehicle* vehicle) final; void guidedModeLand (Vehicle* vehicle) final; void guidedModeTakeoff (Vehicle* vehicle) final; - void guidedModeGotoLocation (Vehicle* vehicle, const QGeoCoordinate& gotoCoord) final; - void guidedModeChangeAltitude (Vehicle* vehicle, double altitudeChange) final; const FirmwarePlugin::remapParamNameMajorVersionMap_t& paramNameRemapMajorVersionMap(void) const final { return _remapParamName; } int remapParamNameHigestMinorVersionNumber(int majorVersionNumber) const final; bool multiRotorCoaxialMotors (Vehicle* vehicle) final; bool multiRotorXConfig (Vehicle* vehicle) final; QString offlineEditingParamFile (Vehicle* vehicle) final { Q_UNUSED(vehicle); return QStringLiteral(":/FirmwarePlugin/APM/Copter.OfflineEditing.params"); } QString pauseFlightMode (void) const override { return QString("Brake"); } - QString missionFlightMode (void) const override { return QString("Auto"); } - QString rtlFlightMode (void) const override { return QString("RTL"); } QString landFlightMode (void) const override { return QString("Land"); } QString takeControlFlightMode (void) const override { return QString("Loiter"); } bool vehicleYawsToNextWaypointInMission (const Vehicle* vehicle) const final; diff --git a/src/FirmwarePlugin/APM/ArduPlaneFirmwarePlugin.h b/src/FirmwarePlugin/APM/ArduPlaneFirmwarePlugin.h index 7a012e16bb60a870cb5e43cf724572b75abf1cd4..650611ff7c33b587e33563118a4aff1baf77acc2 100644 --- a/src/FirmwarePlugin/APM/ArduPlaneFirmwarePlugin.h +++ b/src/FirmwarePlugin/APM/ArduPlaneFirmwarePlugin.h @@ -56,10 +56,11 @@ public: ArduPlaneFirmwarePlugin(void); // Overrides from FirmwarePlugin - QString offlineEditingParamFile(Vehicle* vehicle) final { Q_UNUSED(vehicle); return QStringLiteral(":/FirmwarePlugin/APM/Plane.OfflineEditing.params"); } + QString pauseFlightMode (void) const override { return QString("Loiter"); } + 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; } - int remapParamNameHigestMinorVersionNumber(int majorVersionNumber) const final; - QString autoDisarmParameter(Vehicle* vehicle) final { Q_UNUSED(vehicle); return QStringLiteral("LAND_DISARMDELAY"); } private: static bool _remapParamNameIntialized; diff --git a/src/FirmwarePlugin/APM/ArduRoverFirmwarePlugin.cc b/src/FirmwarePlugin/APM/ArduRoverFirmwarePlugin.cc index 1e30340f13b01bc04be10f2bdd4d46248fa8a1ac..91c2a8299a666a3be30c246f3c00dabaa6997b43 100644 --- a/src/FirmwarePlugin/APM/ArduRoverFirmwarePlugin.cc +++ b/src/FirmwarePlugin/APM/ArduRoverFirmwarePlugin.cc @@ -8,6 +8,7 @@ ****************************************************************************/ #include "ArduRoverFirmwarePlugin.h" +#include "QGCApplication.h" bool ArduRoverFirmwarePlugin::_remapParamNameIntialized = false; FirmwarePlugin::remapParamNameMajorVersionMap_t ArduRoverFirmwarePlugin::_remapParamName; @@ -98,3 +99,10 @@ int ArduRoverFirmwarePlugin::remapParamNameHigestMinorVersionNumber(int majorVer return majorVersionNumber == 3 ? 2 : Vehicle::versionNotSetValue; } +void ArduRoverFirmwarePlugin::guidedModeChangeAltitude(Vehicle* vehicle, double altitudeChange) +{ + Q_UNUSED(vehicle); + Q_UNUSED(altitudeChange); + + qgcApp()->showMessage(QStringLiteral("Change altitude not supported.")); +} diff --git a/src/FirmwarePlugin/APM/ArduRoverFirmwarePlugin.h b/src/FirmwarePlugin/APM/ArduRoverFirmwarePlugin.h index 88e5e5272560a50678b4c2dd696e09a44e5348f9..e08dd61c93f161c3927d45f875c44981454180c6 100644 --- a/src/FirmwarePlugin/APM/ArduRoverFirmwarePlugin.h +++ b/src/FirmwarePlugin/APM/ArduRoverFirmwarePlugin.h @@ -51,8 +51,10 @@ public: ArduRoverFirmwarePlugin(void); // Overrides from FirmwarePlugin + QString pauseFlightMode (void) const override { return QString("Hold"); } + void guidedModeChangeAltitude (Vehicle* vehicle, double altitudeChange) final; + int remapParamNameHigestMinorVersionNumber (int majorVersionNumber) const final; const FirmwarePlugin::remapParamNameMajorVersionMap_t& paramNameRemapMajorVersionMap(void) const final { return _remapParamName; } - int remapParamNameHigestMinorVersionNumber(int majorVersionNumber) const final; private: static bool _remapParamNameIntialized; diff --git a/src/FlightDisplay/FlightDisplayViewMap.qml b/src/FlightDisplay/FlightDisplayViewMap.qml index 77a22c88574041533d175c37846d911eedd1b3fc..df01d8b74fda9ff6ae0c5fafd7a0188c615a333b 100644 --- a/src/FlightDisplay/FlightDisplayViewMap.qml +++ b/src/FlightDisplay/FlightDisplayViewMap.qml @@ -261,7 +261,7 @@ FlightMap { // GoTo here waypoint MapQuickItem { coordinate: _gotoHereCoordinate - visible: _activeVehicle && _activeVehicle.guidedMode && _gotoHereCoordinate.isValid + visible: _activeVehicle && _activeVehicle.guidedModeSupported && _gotoHereCoordinate.isValid z: QGroundControl.zOrderMapItems anchorPoint.x: sourceItem.anchorPointX anchorPoint.y: sourceItem.anchorPointY diff --git a/src/FlightDisplay/GuidedActionsController.qml b/src/FlightDisplay/GuidedActionsController.qml index f111c6fb018012f3612bbaa633519ff526a75533..db16ef4dec84f7e29c624041eb520167834ef9d9 100644 --- a/src/FlightDisplay/GuidedActionsController.qml +++ b/src/FlightDisplay/GuidedActionsController.qml @@ -101,7 +101,7 @@ Item { property bool showChangeAlt: (_activeVehicle && _vehicleFlying) && _activeVehicle.guidedModeSupported && _vehicleArmed && !_missionActive property bool showOrbit: !_hideOrbit && _activeVehicle && _vehicleFlying && _activeVehicle.orbitModeSupported && _vehicleArmed && !_missionActive property bool showLandAbort: _activeVehicle && _vehicleFlying && _activeVehicle.fixedWing && _vehicleLanding - property bool showGotoLocation: _activeVehicle && _activeVehicle.guidedMode && _vehicleFlying + property bool showGotoLocation: _activeVehicle && _vehicleFlying property bool guidedUIVisible: guidedActionConfirm.visible || guidedActionList.visible diff --git a/src/MissionManager/MissionManager.cc b/src/MissionManager/MissionManager.cc index ab54a7da11981d3f0d41bf89c6ebe1e1c2722bbd..c61eb2c2ab2620eb204158a35ceff6049073f44b 100644 --- a/src/MissionManager/MissionManager.cc +++ b/src/MissionManager/MissionManager.cc @@ -40,6 +40,8 @@ void MissionManager::writeArduPilotGuidedMissionItem(const QGeoCoordinate& gotoC _transactionInProgress = TransactionWrite; + _connectToMavlink(); + mavlink_message_t messageOut; mavlink_mission_item_t missionItem; diff --git a/src/MissionManager/MissionSettingsItem.cc b/src/MissionManager/MissionSettingsItem.cc index 1f330f91a81f5d62245d90ebe0d00b95fa14bb89..8fa2708044c3aa5d0cd22cb9a375821aa99b94d4 100644 --- a/src/MissionManager/MissionSettingsItem.cc +++ b/src/MissionManager/MissionSettingsItem.cc @@ -233,7 +233,6 @@ void MissionSettingsItem::setCoordinate(const QGeoCoordinate& coordinate) if (_plannedHomePositionCoordinate != coordinate) { // ArduPilot tends to send crap home positions at initial vehicel boot, discard them if (coordinate.isValid() && (coordinate.latitude() != 0 || coordinate.longitude() != 0)) { - qDebug() << "Setting home position" << coordinate; _plannedHomePositionCoordinate = coordinate; emit coordinateChanged(coordinate); emit exitCoordinateChanged(coordinate); diff --git a/src/Vehicle/Vehicle.cc b/src/Vehicle/Vehicle.cc index 6131dfa9e86980816db9001363a5859bf906e514..68309dcea73931771f063961376d03b5ef04233e 100644 --- a/src/Vehicle/Vehicle.cc +++ b/src/Vehicle/Vehicle.cc @@ -1733,8 +1733,6 @@ void Vehicle::setFlightMode(const QString& flightMode) uint8_t base_mode; uint32_t custom_mode; - qDebug() << flightMode; - if (_firmwarePlugin->setFlightMode(flightMode, &base_mode, &custom_mode)) { // setFlightMode will only set MAV_MODE_FLAG_CUSTOM_MODE_ENABLED in base_mode, we need to move back in the existing // states.