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 <don@thegagnes.com>
-
#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 <QTcpSocket>
@@ -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<VehicleComponent*> 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<MAV_CMD> 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.