Fix pause and change alt command sequencing

Work

src/FirmwarePlugin/APM/APMFirmwarePlugin.cc

@@ -699,13 +699,18 @@ void APMFirmwarePlugin::guidedModeRTL(Vehicle* vehicle, bool smartRTL)
     _setFlightModeAndValidate(vehicle, smartRTL ? smartRTLFlightMode() : rtlFlightMode());
 }
 
-void APMFirmwarePlugin::guidedModeChangeAltitude(Vehicle* vehicle, double altitudeChange)
+void APMFirmwarePlugin::guidedModeChangeAltitude(Vehicle* vehicle, double altitudeChange, bool pauseVehicle)
 {
     if (qIsNaN(vehicle->altitudeRelative()->rawValue().toDouble())) {
         qgcApp()->showAppMessage(tr("Unable to change altitude, vehicle altitude not known."));
         return;
     }
 
+    if (pauseVehicle && !_setFlightModeAndValidate(vehicle, pauseFlightMode())) {
+        qgcApp()->showAppMessage(tr("Unable to pause vehicle."));
+        return;
+    }
+
     if (abs(altitudeChange) < 0.01) {
         // This prevents unecessary changes to Guided mode when the users selects pause and doesn't really touch the altitude slider
         return;

src/FirmwarePlugin/APM/APMFirmwarePlugin.h

@@ -68,7 +68,7 @@ public:
     QString             missionFlightMode               (void) const override { return QString("Auto"); }
     void                pauseVehicle                    (Vehicle* vehicle) override;
     void                guidedModeRTL                   (Vehicle* vehicle, bool smartRTL) override;
-    void                guidedModeChangeAltitude        (Vehicle* vehicle, double altitudeChange) override;
+    void                guidedModeChangeAltitude        (Vehicle* vehicle, double altitudeChange, bool pauseVehicle) override;
     bool                adjustIncomingMavlinkMessage    (Vehicle* vehicle, mavlink_message_t* message) override;
     void                adjustOutgoingMavlinkMessageThreadSafe(Vehicle* vehicle, LinkInterface* outgoingLink, mavlink_message_t* message) override;
     virtual void        initializeStreamRates           (Vehicle* vehicle);

src/FirmwarePlugin/APM/ArduRoverFirmwarePlugin.cc

@@ -71,11 +71,8 @@ int ArduRoverFirmwarePlugin::remapParamNameHigestMinorVersionNumber(int majorVer
     return majorVersionNumber == 3 ? 5 : Vehicle::versionNotSetValue;
 }
 
-void ArduRoverFirmwarePlugin::guidedModeChangeAltitude(Vehicle* vehicle, double altitudeChange)
+void ArduRoverFirmwarePlugin::guidedModeChangeAltitude(Vehicle* /*vehicle*/, double /*altitudeChange*/, bool /*pauseVehicle*/)
 {
-    Q_UNUSED(vehicle);
-    Q_UNUSED(altitudeChange);
-
     qgcApp()->showAppMessage(QStringLiteral("Change altitude not supported."));
 }
 

src/FirmwarePlugin/APM/ArduRoverFirmwarePlugin.h

@@ -50,7 +50,7 @@ public:
     // Overrides from FirmwarePlugin
     QString pauseFlightMode                         (void) const override { return QStringLiteral("Hold"); }
     QString followFlightMode                        (void) const override { return QStringLiteral("Follow"); }
-    void    guidedModeChangeAltitude                (Vehicle* vehicle, double altitudeChange) final;
+    void    guidedModeChangeAltitude                (Vehicle* vehicle, double altitudeChange, bool pauseVehicle) final;
     int     remapParamNameHigestMinorVersionNumber  (int majorVersionNumber) const final;
     const   FirmwarePlugin::remapParamNameMajorVersionMap_t& paramNameRemapMajorVersionMap(void) const final { return _remapParamName; }
     bool    supportsNegativeThrust                  (Vehicle *) final;

src/FirmwarePlugin/FirmwarePlugin.cc

@@ -260,7 +260,7 @@ void FirmwarePlugin::guidedModeGotoLocation(Vehicle* vehicle, const QGeoCoordina
     qgcApp()->showAppMessage(guided_mode_not_supported_by_vehicle);
 }
 
-void FirmwarePlugin::guidedModeChangeAltitude(Vehicle*, double)
+void FirmwarePlugin::guidedModeChangeAltitude(Vehicle*, double, bool pauseVehicle)
 {
     // Not supported by generic vehicle
     qgcApp()->showAppMessage(guided_mode_not_supported_by_vehicle);

src/FirmwarePlugin/FirmwarePlugin.h

@@ -155,7 +155,8 @@ public:
 
     /// Command vehicle to change altitude
     ///     @param altitudeChange If > 0, go up by amount specified, if < 0, go down by amount specified
-    virtual void guidedModeChangeAltitude(Vehicle* vehicle, double altitudeChange);
+    ///     @param pauseVehicle true: pause vehicle prior to altitude change
+    virtual void guidedModeChangeAltitude(Vehicle* vehicle, double altitudeChange, bool pauseVehicle);
 
     /// Default tx mode to apply to joystick axes
     /// TX modes are as outlined here: http://www.rc-airplane-world.com/rc-transmitter-modes.html

src/FirmwarePlugin/PX4/PX4FirmwarePlugin.cc

@@ -7,10 +7,6 @@
  *
  ****************************************************************************/
 
-
-/// @file
-///     @author Don Gagne <don@thegagnes.com>
-
 #include "PX4FirmwarePlugin.h"
 #include "PX4ParameterMetaData.h"
 #include "QGCApplication.h"
@@ -462,7 +458,54 @@ void PX4FirmwarePlugin::guidedModeGotoLocation(Vehicle* vehicle, const QGeoCoord
     }
 }
 
-void PX4FirmwarePlugin::guidedModeChangeAltitude(Vehicle* vehicle, double altitudeChange)
+typedef struct {
+    PX4FirmwarePlugin*  plugin;
+    Vehicle*            vehicle;
+    double              newAMSLAlt;
+} PauseVehicleThenChangeAltData_t;
+
+static void _pauseVehicleThenChangeAltResultHandler(void* resultHandlerData, int /*compId*/, MAV_RESULT commandResult, Vehicle::MavCmdResultFailureCode_t failureCode)
+{
+    if (commandResult != MAV_RESULT_ACCEPTED) {
+        switch (failureCode) {
+        case Vehicle::MavCmdResultCommandResultOnly:
+            qDebug() << QStringLiteral("MAV_CMD_DO_REPOSITION error(%1)").arg(commandResult);
+            break;
+        case Vehicle::MavCmdResultFailureNoResponseToCommand:
+            qDebug() << "MAV_CMD_DO_REPOSITION no response from vehicle";
+            break;
+        case Vehicle::MavCmdResultFailureDuplicateCommand:
+            qDebug() << "Internal Error: MAV_CMD_DO_REPOSITION could not be sent due to duplicate command";
+            break;
+        }
+    }
+
+    PauseVehicleThenChangeAltData_t* pData = static_cast<PauseVehicleThenChangeAltData_t*>(resultHandlerData);
+    pData->plugin->_changeAltAfterPause(resultHandlerData, commandResult == MAV_RESULT_ACCEPTED /* pauseSucceeded */);
+}
+
+void PX4FirmwarePlugin::_changeAltAfterPause(void* resultHandlerData, bool pauseSucceeded)
+{
+    PauseVehicleThenChangeAltData_t* pData = static_cast<PauseVehicleThenChangeAltData_t*>(resultHandlerData);
+
+    if (pauseSucceeded) {
+        pData->vehicle->sendMavCommand(
+                    pData->vehicle->defaultComponentId(),
+                    MAV_CMD_DO_REPOSITION,
+                    true,                                   // show error is fails
+                    -1.0f,                                  // Don't change groundspeed
+                    MAV_DO_REPOSITION_FLAGS_CHANGE_MODE,
+                    0.0f,                                   // Reserved
+                    qQNaN(), qQNaN(), qQNaN(),              // No change to yaw, lat, lon
+                    static_cast<float>(pData->newAMSLAlt));
+    } else {
+        qgcApp()->showAppMessage(tr("Unable to pause vehicle."));
+    }
+
+    delete pData;
+}
+
+void PX4FirmwarePlugin::guidedModeChangeAltitude(Vehicle* vehicle, double altitudeChange, bool pauseVehicle)
 {
     if (!vehicle->homePosition().isValid()) {
         qgcApp()->showAppMessage(tr("Unable to change altitude, home position unknown."));
@@ -476,16 +519,24 @@ void PX4FirmwarePlugin::guidedModeChangeAltitude(Vehicle* vehicle, double altitu
     double currentAltRel = vehicle->altitudeRelative()->rawValue().toDouble();
     double newAltRel = currentAltRel + altitudeChange;
 
-    vehicle->sendMavCommand(vehicle->defaultComponentId(),
+    PauseVehicleThenChangeAltData_t* resultData = new PauseVehicleThenChangeAltData_t;
+    resultData->plugin      = this;
+    resultData->vehicle     = vehicle;
+    resultData->newAMSLAlt  = vehicle->homePosition().altitude() + newAltRel;
+
+    if (pauseVehicle) {
+        vehicle->sendMavCommandWithHandler(
+                    _pauseVehicleThenChangeAltResultHandler,
+                    resultData,
+                    vehicle->defaultComponentId(),
                     MAV_CMD_DO_REPOSITION,
-                            true,   // show error is fails
-                            -1.0f,
+                    -1.0f,                                  // Don't change groundspeed
                     MAV_DO_REPOSITION_FLAGS_CHANGE_MODE,
-                            0.0f,
-                            NAN,
-                            NAN,
-                            NAN,
-                            static_cast<float>(vehicle->homePosition().altitude() + newAltRel));
+                    0.0f,                                   // Reserved
+                    qQNaN(), qQNaN(), qQNaN(), qQNaN());    // No change to yaw, lat, lon, alt
+    } else {
+        _changeAltAfterPause(resultData, true /* pauseSucceeded */);
+    }
 }
 
 void PX4FirmwarePlugin::startMission(Vehicle* vehicle)

src/FirmwarePlugin/PX4/PX4FirmwarePlugin.h

@@ -7,12 +7,7 @@
  *
  ****************************************************************************/
 
-
-/// @file
-///     @author Don Gagne <don@thegagnes.com>
-
-#ifndef PX4FirmwarePlugin_H
-#define PX4FirmwarePlugin_H
+#pragma once
 
 #include "FirmwarePlugin.h"
 #include "ParameterManager.h"
@@ -26,6 +21,9 @@ public:
     PX4FirmwarePlugin   ();
     ~PX4FirmwarePlugin  () override;
 
+    // Called internally only
+    void _changeAltAfterPause(void* resultHandlerData, bool pauseSucceeded);
+
     // Overrides from FirmwarePlugin
 
     QList<VehicleComponent*> componentsForVehicle(AutoPilotPlugin* vehicle) override;
@@ -49,7 +47,7 @@ public:
     void                guidedModeLand                  (Vehicle* vehicle) override;
     void                guidedModeTakeoff               (Vehicle* vehicle, double takeoffAltRel) override;
     void                guidedModeGotoLocation          (Vehicle* vehicle, const QGeoCoordinate& gotoCoord) override;
-    void                guidedModeChangeAltitude        (Vehicle* vehicle, double altitudeRel) override;
+    void                guidedModeChangeAltitude        (Vehicle* vehicle, double altitudeRel, bool pauseVehicle) override;
     double              minimumTakeoffAltitude          (Vehicle* vehicle) override;
     void                startMission                    (Vehicle* vehicle) override;
     bool                isGuidedMode                    (const Vehicle* vehicle) const override;
@@ -107,9 +105,10 @@ private slots:
     void _mavCommandResult(int vehicleId, int component, int command, int result, bool noReponseFromVehicle);
 
 private:
-    void _handleAutopilotVersion(Vehicle* vehicle, mavlink_message_t* message);
-    QString _getLatestVersionFileUrl(Vehicle* vehicle) override;
-    QString _versionRegex() override;
+    void    _handleAutopilotVersion         (Vehicle* vehicle, mavlink_message_t* message);
+
+    QString _getLatestVersionFileUrl        (Vehicle* vehicle) override;
+    QString _versionRegex                   () override;
 
     // Any instance data here must be global to all vehicles
     // Vehicle specific data should go into PX4FirmwarePluginInstanceData
@@ -124,5 +123,3 @@ public:
 
     bool versionNotified;  ///< true: user notified over version issue
 };
-
-#endif

src/FlightDisplay/GuidedActionsController.qml

@@ -500,7 +500,7 @@ Item {
             _activeVehicle.emergencyStop()
             break
         case actionChangeAlt:
-            _activeVehicle.guidedModeChangeAltitude(actionAltitudeChange)
+            _activeVehicle.guidedModeChangeAltitude(actionAltitudeChange, false /* pauseVehicle */)
             break
         case actionGoto:
             _activeVehicle.guidedModeGotoLocation(actionData)
@@ -515,8 +515,7 @@ Item {
             _activeVehicle.abortLanding(50)     // hardcoded value for climbOutAltitude that is currently ignored
             break
         case actionPause:
-            _activeVehicle.pauseVehicle()
-            _activeVehicle.guidedModeChangeAltitude(actionAltitudeChange)
+            _activeVehicle.guidedModeChangeAltitude(actionAltitudeChange, true /* pauseVehicle */)
             break
         case actionMVPause:
             rgVehicle = QGroundControl.multiVehicleManager.vehicles

src/Vehicle/RequestMessageTest.cc

@@ -13,7 +13,7 @@
 #include "MockLink.h"
 
 RequestMessageTest::TestCase_t RequestMessageTest::_rgTestCases[] = {
-//    {  MockLink::FailRequestMessageNone,                                MAV_RESULT_ACCEPTED,    Vehicle::RequestMessageNoFailure,                   1,                                  false },
+    {  MockLink::FailRequestMessageNone,                                MAV_RESULT_ACCEPTED,    Vehicle::RequestMessageNoFailure,                   1,                                  false },
     {  MockLink::FailRequestMessageCommandAcceptedMsgNotSent,           MAV_RESULT_FAILED,      Vehicle::RequestMessageFailureMessageNotReceived,   1,                                  false },
     {  MockLink::FailRequestMessageCommandUnsupported,                  MAV_RESULT_UNSUPPORTED, Vehicle::RequestMessageFailureCommandError,         1,                                  false },
     {  MockLink::FailRequestMessageCommandNoResponse,                   MAV_RESULT_FAILED,      Vehicle::RequestMessageFailureCommandNotAcked,      Vehicle::_mavCommandMaxRetryCount,  false },

src/Vehicle/Vehicle.cc

@@ -2386,13 +2386,13 @@ void Vehicle::guidedModeGotoLocation(const QGeoCoordinate& gotoCoord)
     _firmwarePlugin->guidedModeGotoLocation(this, gotoCoord);
 }
 
-void Vehicle::guidedModeChangeAltitude(double altitudeChange)
+void Vehicle::guidedModeChangeAltitude(double altitudeChange, bool pauseVehicle)
 {
     if (!guidedModeSupported()) {
         qgcApp()->showAppMessage(guided_mode_not_supported_by_vehicle);
         return;
     }
-    _firmwarePlugin->guidedModeChangeAltitude(this, altitudeChange);
+    _firmwarePlugin->guidedModeChangeAltitude(this, altitudeChange, pauseVehicle);
 }
 
 void Vehicle::guidedModeOrbit(const QGeoCoordinate& centerCoord, double radius, double amslAltitude)
@@ -2701,16 +2701,19 @@ void Vehicle::_sendMavCommandWorker(bool commandInt, bool requestMessage, bool s
         entry.elapsedTimer.start();
 
         _mavCommandList.append(entry);
-        _sendMavCommandFromList(_mavCommandList.last());
+        _sendMavCommandFromList(_mavCommandList.count() - 1);
     }
 }
 
-void Vehicle::_sendMavCommandFromList(MavCommandListEntry_t& commandEntry)
+void Vehicle::_sendMavCommandFromList(int index)
 {
+    MavCommandListEntry_t commandEntry = _mavCommandList[index];
+
     QString rawCommandName  = _toolbox->missionCommandTree()->rawName(commandEntry.command);
 
-    if (++commandEntry.tryCount > commandEntry.maxTries) {
+    if (++_mavCommandList[index].tryCount > commandEntry.maxTries) {
         qCDebug(VehicleLog) << "_sendMavCommandFromList giving up after max retries" << rawCommandName;
+        _mavCommandList.removeAt(index);
         if (commandEntry.resultHandler) {
             (*commandEntry.resultHandler)(commandEntry.resultHandlerData, commandEntry.targetCompId, MAV_RESULT_FAILED, MavCmdResultFailureNoResponseToCommand);
         } else {
@@ -2719,7 +2722,6 @@ void Vehicle::_sendMavCommandFromList(MavCommandListEntry_t& commandEntry)
         if (commandEntry.showError) {
             qgcApp()->showAppMessage(tr("Vehicle did not respond to command: %1").arg(rawCommandName));
         }
-        _mavCommandList.removeAt(_findMavCommandListEntryIndex(commandEntry.targetCompId, commandEntry.command));
         return;
     }
 
@@ -2799,7 +2801,7 @@ void Vehicle::_sendMavCommandResponseTimeoutCheck(void)
         MavCommandListEntry_t& commandEntry = _mavCommandList[i];
         if (commandEntry.elapsedTimer.elapsed() > commandEntry.ackTimeoutMSecs) {
             // Try sending command again
-            _sendMavCommandFromList(commandEntry);
+            _sendMavCommandFromList(i);
         }
     }
 }
@@ -2835,7 +2837,7 @@ void Vehicle::_handleCommandAck(mavlink_message_t& message)
     int entryIndex = _findMavCommandListEntryIndex(message.compid, static_cast<MAV_CMD>(ack.command));
     bool commandInList = false;
     if (entryIndex != -1) {
-        const MavCommandListEntry_t& commandEntry = _mavCommandList[entryIndex];
+        MavCommandListEntry_t commandEntry = _mavCommandList.takeAt(entryIndex);
         if (commandEntry.command == ack.command) {
             if (commandEntry.requestMessage) {
                 RequestMessageInfo_t* pInfo = static_cast<RequestMessageInfo_t*>(commandEntry.resultHandlerData);
@@ -2881,8 +2883,6 @@ void Vehicle::_handleCommandAck(mavlink_message_t& message)
                     emit mavCommandResult(_id, message.compid, ack.command, ack.result, MavCmdResultCommandResultOnly);
                 }
             }
-
-            _mavCommandList.removeAt(entryIndex);
             commandInList = true;
         }
     }

src/Vehicle/Vehicle.h

@@ -331,7 +331,8 @@ public:
 
     /// Command vehicle to change altitude
     ///     @param altitudeChange If > 0, go up by amount specified, if < 0, go down by amount specified
-    Q_INVOKABLE void guidedModeChangeAltitude(double altitudeChange);
+    ///     @param pauseVehicle true: pause vehicle prior to altitude change
+    Q_INVOKABLE void guidedModeChangeAltitude(double altitudeChange, bool pauseVehicle);
 
     /// Command vehicle to orbit given center point
     ///     @param centerCoord Orit around this point
@@ -1183,7 +1184,7 @@ private:
     static const int                _mavCommandAckTimeoutMSecsHighLatency   = 120000;
 
     void _sendMavCommandWorker  (bool commandInt, bool requestMessage, bool showError, MavCmdResultHandler resultHandler, void* resultHandlerData, int compId, MAV_CMD command, MAV_FRAME frame, float param1, float param2, float param3, float param4, float param5, float param6, float param7);
-    void _sendMavCommandFromList(MavCommandListEntry_t& commandEntry);
+    void _sendMavCommandFromList(int index);
     int  _findMavCommandListEntryIndex(int targetCompId, MAV_CMD command);
     bool _sendMavCommandShouldRetry(MAV_CMD command);