diff --git a/src/FirmwarePlugin/APM/APMFirmwarePlugin.cc b/src/FirmwarePlugin/APM/APMFirmwarePlugin.cc index e54d29c..2a428a5 100644 --- a/src/FirmwarePlugin/APM/APMFirmwarePlugin.cc +++ b/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; diff --git a/src/FirmwarePlugin/APM/APMFirmwarePlugin.h b/src/FirmwarePlugin/APM/APMFirmwarePlugin.h index 2bf2461..e8ba74b 100644 --- a/src/FirmwarePlugin/APM/APMFirmwarePlugin.h +++ b/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); diff --git a/src/FirmwarePlugin/APM/ArduRoverFirmwarePlugin.cc b/src/FirmwarePlugin/APM/ArduRoverFirmwarePlugin.cc index 9c8a67a..4bb3157 100644 --- a/src/FirmwarePlugin/APM/ArduRoverFirmwarePlugin.cc +++ b/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.")); } diff --git a/src/FirmwarePlugin/APM/ArduRoverFirmwarePlugin.h b/src/FirmwarePlugin/APM/ArduRoverFirmwarePlugin.h index 1338b3f..c82ef43 100644 --- a/src/FirmwarePlugin/APM/ArduRoverFirmwarePlugin.h +++ b/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; diff --git a/src/FirmwarePlugin/FirmwarePlugin.cc b/src/FirmwarePlugin/FirmwarePlugin.cc index 2f5d552..a568715 100644 --- a/src/FirmwarePlugin/FirmwarePlugin.cc +++ b/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); diff --git a/src/FirmwarePlugin/FirmwarePlugin.h b/src/FirmwarePlugin/FirmwarePlugin.h index 467bdc9..fc1785d 100644 --- a/src/FirmwarePlugin/FirmwarePlugin.h +++ b/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 diff --git a/src/FirmwarePlugin/PX4/PX4FirmwarePlugin.cc b/src/FirmwarePlugin/PX4/PX4FirmwarePlugin.cc index a6e28c8..bbe21bf 100644 --- a/src/FirmwarePlugin/PX4/PX4FirmwarePlugin.cc +++ b/src/FirmwarePlugin/PX4/PX4FirmwarePlugin.cc @@ -7,10 +7,6 @@ * ****************************************************************************/ - -/// @file -/// @author Don Gagne - #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(resultHandlerData); + pData->plugin->_changeAltAfterPause(resultHandlerData, commandResult == MAV_RESULT_ACCEPTED /* pauseSucceeded */); +} + +void PX4FirmwarePlugin::_changeAltAfterPause(void* resultHandlerData, bool pauseSucceeded) +{ + PauseVehicleThenChangeAltData_t* pData = static_cast(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(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(), - MAV_CMD_DO_REPOSITION, - true, // show error is fails - -1.0f, - MAV_DO_REPOSITION_FLAGS_CHANGE_MODE, - 0.0f, - NAN, - NAN, - NAN, - static_cast(vehicle->homePosition().altitude() + newAltRel)); + 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, + -1.0f, // Don't change groundspeed + MAV_DO_REPOSITION_FLAGS_CHANGE_MODE, + 0.0f, // Reserved + qQNaN(), qQNaN(), qQNaN(), qQNaN()); // No change to yaw, lat, lon, alt + } else { + _changeAltAfterPause(resultData, true /* pauseSucceeded */); + } } void PX4FirmwarePlugin::startMission(Vehicle* vehicle) diff --git a/src/FirmwarePlugin/PX4/PX4FirmwarePlugin.h b/src/FirmwarePlugin/PX4/PX4FirmwarePlugin.h index 9421602..27d239a 100644 --- a/src/FirmwarePlugin/PX4/PX4FirmwarePlugin.h +++ b/src/FirmwarePlugin/PX4/PX4FirmwarePlugin.h @@ -7,12 +7,7 @@ * ****************************************************************************/ - -/// @file -/// @author Don Gagne - -#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 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 diff --git a/src/FlightDisplay/GuidedActionsController.qml b/src/FlightDisplay/GuidedActionsController.qml index 41252a3..8f2d4f4 100644 --- a/src/FlightDisplay/GuidedActionsController.qml +++ b/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 diff --git a/src/Vehicle/RequestMessageTest.cc b/src/Vehicle/RequestMessageTest.cc index a097270..8d4d5ea 100644 --- a/src/Vehicle/RequestMessageTest.cc +++ b/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 }, diff --git a/src/Vehicle/Vehicle.cc b/src/Vehicle/Vehicle.cc index becff0d..648d183 100644 --- a/src/Vehicle/Vehicle.cc +++ b/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(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(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; } } diff --git a/src/Vehicle/Vehicle.h b/src/Vehicle/Vehicle.h index 33ed9f0..5e4bd1d 100644 --- a/src/Vehicle/Vehicle.h +++ b/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);