Vehicle: implement MAVLink standard modes

qgroundcontrol.pro

@@ -724,6 +724,7 @@ HEADERS += \
     src/Vehicle/MultiVehicleManager.h \
     src/Vehicle/RemoteIDManager.h \
     src/Vehicle/StateMachine.h \
+    src/Vehicle/StandardModes.h \
     src/Vehicle/SysStatusSensorInfo.h \
     src/Vehicle/TerrainFactGroup.h \
     src/Vehicle/TerrainProtocolHandler.h \
@@ -979,6 +980,7 @@ SOURCES += \
     src/Vehicle/MultiVehicleManager.cc \
     src/Vehicle/RemoteIDManager.cc \
     src/Vehicle/StateMachine.cc \
+    src/Vehicle/StandardModes.cc \
     src/Vehicle/SysStatusSensorInfo.cc \
     src/Vehicle/TerrainFactGroup.cc \
     src/Vehicle/TerrainProtocolHandler.cc \

src/Vehicle/CMakeLists.txt

@@ -56,6 +56,8 @@ add_library(Vehicle
 	MultiVehicleManager.h
 	StateMachine.cc
 	StateMachine.h
+	StandardModes.cc
+	StandardModes.h
 	SysStatusSensorInfo.cc
 	SysStatusSensorInfo.h
 	TerrainFactGroup.cc

src/Vehicle/InitialConnectStateMachine.cc

@@ -21,6 +21,7 @@ QGC_LOGGING_CATEGORY(InitialConnectStateMachineLog, "InitialConnectStateMachineL
 const StateMachine::StateFn InitialConnectStateMachine::_rgStates[] = {
     InitialConnectStateMachine::_stateRequestAutopilotVersion,
     InitialConnectStateMachine::_stateRequestProtocolVersion,
+    InitialConnectStateMachine::_stateRequestStandardModes,
     InitialConnectStateMachine::_stateRequestCompInfo,
     InitialConnectStateMachine::_stateRequestParameters,
     InitialConnectStateMachine::_stateRequestMission,
@@ -32,6 +33,7 @@ const StateMachine::StateFn InitialConnectStateMachine::_rgStates[] = {
 const int InitialConnectStateMachine::_rgProgressWeights[] = {
     1, //_stateRequestCapabilities
     1, //_stateRequestProtocolVersion
+    1, //_stateRequestStandardModes
     5, //_stateRequestCompInfo
     5, //_stateRequestParameters
     2, //_stateRequestMission
@@ -282,6 +284,24 @@ void InitialConnectStateMachine::_stateRequestCompInfo(StateMachine* stateMachin
     vehicle->_componentInformationManager->requestAllComponentInformation(_stateRequestCompInfoComplete, connectMachine);
 }
 
+void InitialConnectStateMachine::_stateRequestStandardModes(StateMachine *stateMachine)
+{
+    InitialConnectStateMachine* connectMachine  = static_cast<InitialConnectStateMachine*>(stateMachine);
+    Vehicle*                    vehicle         = connectMachine->_vehicle;
+
+    qCDebug(InitialConnectStateMachineLog) << "_stateRequestStandardModes";
+    connect(vehicle->_standardModes, &StandardModes::requestCompleted, connectMachine,
+            &InitialConnectStateMachine::standardModesRequestCompleted);
+    vehicle->_standardModes->request();
+}
+
+void InitialConnectStateMachine::standardModesRequestCompleted()
+{
+    disconnect(_vehicle->_standardModes, &StandardModes::requestCompleted, this,
+               &InitialConnectStateMachine::standardModesRequestCompleted);
+    advance();
+}
+
 void InitialConnectStateMachine::_stateRequestCompInfoComplete(void* requestAllCompleteFnData)
 {
     InitialConnectStateMachine* connectMachine  = static_cast<InitialConnectStateMachine*>(requestAllCompleteFnData);

src/Vehicle/InitialConnectStateMachine.h

@@ -37,11 +37,13 @@ signals:
 
 private slots:
     void gotProgressUpdate(float progressValue);
+    void standardModesRequestCompleted();
 
 private:
     static void _stateRequestAutopilotVersion           (StateMachine* stateMachine);
     static void _stateRequestProtocolVersion            (StateMachine* stateMachine);
     static void _stateRequestCompInfo                   (StateMachine* stateMachine);
+    static void _stateRequestStandardModes              (StateMachine* stateMachine);
     static void _stateRequestCompInfoComplete           (void* requestAllCompleteFnData);
     static void _stateRequestParameters                 (StateMachine* stateMachine);
     static void _stateRequestMission                    (StateMachine* stateMachine);

src/Vehicle/StandardModes.cc

@@ -0,0 +1,179 @@
+/****************************************************************************
+ *
+ * (c) 2022 QGROUNDCONTROL PROJECT <http://www.qgroundcontrol.org>
+ *
+ * QGroundControl is licensed according to the terms in the file
+ * COPYING.md in the root of the source code directory.
+ *
+ ****************************************************************************/
+
+#include "Vehicle.h"
+#include "StandardModes.h"
+
+QGC_LOGGING_CATEGORY(StandardModesLog, "StandardModesLog")
+
+
+static void requestMessageResultHandler(void* resultHandlerData, MAV_RESULT result,
+                                        Vehicle::RequestMessageResultHandlerFailureCode_t failureCode, const mavlink_message_t &message)
+{
+    StandardModes* standardModes = static_cast<StandardModes*>(resultHandlerData);
+    standardModes->gotMessage(result, message);
+}
+
+StandardModes::StandardModes(QObject *parent, Vehicle *vehicle)
+        : QObject(parent), _vehicle(vehicle)
+{
+}
+
+void StandardModes::gotMessage(MAV_RESULT result, const mavlink_message_t &message)
+{
+    _requestActive = false;
+    if (_wantReset) {
+        _wantReset = false;
+        request();
+        return;
+    }
+
+    if (result == MAV_RESULT_ACCEPTED) {
+        mavlink_available_modes_t availableModes;
+        mavlink_msg_available_modes_decode(&message, &availableModes);
+        bool cannotBeSet = availableModes.properties & MAV_MODE_PROPERTY_NOT_USER_SELECTABLE;
+        bool advanced = availableModes.properties & MAV_MODE_PROPERTY_ADVANCED;
+        availableModes.mode_name[sizeof(availableModes.mode_name)-1] = '\0';
+        QString name = availableModes.mode_name;
+        switch (availableModes.standard_mode) {
+            case MAV_STANDARD_MODE_POSITION_HOLD:
+                name = "Position";
+                break;
+            case MAV_STANDARD_MODE_ORBIT:
+                name = "Orbit";
+                break;
+            case MAV_STANDARD_MODE_CRUISE:
+                name = "Cruise";
+                break;
+            case MAV_STANDARD_MODE_ALTITUDE_HOLD:
+                name = "Altitude";
+                break;
+            case MAV_STANDARD_MODE_RETURN_HOME:
+                name = "Return";
+                break;
+            case MAV_STANDARD_MODE_SAFE_RECOVERY:
+                name = "Safe Recovery";
+                break;
+            case MAV_STANDARD_MODE_MISSION:
+                name = "Mission";
+                break;
+            case MAV_STANDARD_MODE_LAND:
+                name = "Land";
+                break;
+            case MAV_STANDARD_MODE_TAKEOFF:
+                name = "Takeoff";
+                break;
+        }
+
+        if (name == "Takeoff" || name == "VTOL Takeoff" || name == "Orbit" || name == "Land" || name == "Return") { // These are exposed in the UI as separate buttons
+            cannotBeSet = true;
+        }
+
+        qCDebug(StandardModesLog) << "Got mode:" << name << ", idx:" << availableModes.mode_index << ", custom_mode" << availableModes.custom_mode;
+
+        _nextModes[availableModes.custom_mode] = Mode{name, availableModes.standard_mode, advanced, cannotBeSet};
+
+        if (availableModes.mode_index >= availableModes.number_modes) { // We are done
+            qCDebug(StandardModesLog) << "Completed, num modes:" << _nextModes.size();
+            _modes = _nextModes;
+            ensureUniqueModeNames();
+            _hasModes = true;
+            emit modesUpdated();
+            emit requestCompleted();
+
+        } else {
+            requestMode(availableModes.mode_index + 1);
+        }
+
+    } else {
+        qCDebug(StandardModesLog) << "Failed to retrieve available modes" << result;
+        emit requestCompleted();
+    }
+}
+
+void StandardModes::ensureUniqueModeNames()
+{
+    // Ensure mode names are unique. This should generally already be the case, but e.g. during development when
+    // restarting dynamic modes, it might not be.
+    for (auto iter = _modes.begin(); iter != _modes.end(); ++iter) {
+        int duplicateIdx = 0;
+        for (auto iter2 = iter + 1; iter2 != _modes.end(); ++iter2) {
+            if (iter.value().name == iter2.value().name) {
+                iter2.value().name += QStringLiteral(" (%1)").arg(duplicateIdx + 1);
+                ++duplicateIdx;
+            }
+        }
+    }
+}
+
+void StandardModes::request()
+{
+    if (_requestActive) {
+        // If we are in the middle of waiting for a request, wait for the response first
+        _wantReset = true;
+        return;
+    }
+
+    _nextModes.clear();
+
+    qCDebug(StandardModesLog) << "Requesting available modes";
+    // Request one at a time. This could be improved by requesting all, but we can't use Vehicle::requestMessage for that
+    StandardModes::requestMode(1);
+}
+
+void StandardModes::requestMode(int modeIndex)
+{
+    _requestActive = true;
+    _vehicle->requestMessage(
+            requestMessageResultHandler,
+            this,
+            MAV_COMP_ID_AUTOPILOT1,
+            MAVLINK_MSG_ID_AVAILABLE_MODES, modeIndex);
+}
+
+void StandardModes::availableModesMonitorReceived(uint8_t seq)
+{
+    if (_lastSeq != seq) {
+        qCDebug(StandardModesLog) << "Available modes changed, re-requesting";
+        _lastSeq = seq;
+        request();
+    }
+}
+
+QStringList StandardModes::flightModes()
+{
+    QStringList ret;
+    for (const auto& mode : _modes) {
+        if (mode.cannotBeSet) {
+            continue;
+        }
+        ret += mode.name;
+    }
+    return ret;
+}
+
+QString StandardModes::flightMode(uint32_t custom_mode) const
+{
+    auto iter = _modes.find(custom_mode);
+    if (iter != _modes.end()) {
+        return iter->name;
+    }
+    return tr("Unknown %2").arg(custom_mode);
+}
+
+bool StandardModes::setFlightMode(const QString &flightMode, uint32_t *custom_mode)
+{
+    for (auto iter = _modes.constBegin(); iter != _modes.constEnd(); ++iter) {
+        if (iter->name == flightMode) {
+            *custom_mode = iter.key();
+            return true;
+        }
+    }
+    return false;
+}

src/Vehicle/StandardModes.h

@@ -0,0 +1,69 @@
+/****************************************************************************
+ *
+ * (c) 2022 QGROUNDCONTROL PROJECT <http://www.qgroundcontrol.org>
+ *
+ * QGroundControl is licensed according to the terms in the file
+ * COPYING.md in the root of the source code directory.
+ *
+ ****************************************************************************/
+
+#pragma once
+
+#include "QGCLoggingCategory.h"
+#include "QGCMAVLink.h"
+#include <QString>
+#include <QMap>
+
+Q_DECLARE_LOGGING_CATEGORY(StandardModesLog)
+
+class Vehicle;
+
+class StandardModes : public QObject
+{
+Q_OBJECT
+
+public:
+    struct Mode {
+        QString name;
+        uint8_t standardMode;
+        bool advanced;
+        bool cannotBeSet;
+    };
+
+    StandardModes(QObject* parent, Vehicle* vehicle);
+
+    void request();
+
+    void availableModesMonitorReceived(uint8_t seq);
+
+    bool supported() const { return _hasModes; }
+
+    QStringList flightModes();
+
+    QString flightMode(uint32_t custom_mode) const;
+
+    bool setFlightMode(const QString& flightMode, uint32_t* custom_mode);
+
+    void gotMessage(MAV_RESULT result, const mavlink_message_t &message);
+signals:
+    void modesUpdated();
+    void requestCompleted();
+
+private:
+
+    void requestMode(int modeIndex);
+    void ensureUniqueModeNames();
+
+    Vehicle*const _vehicle;
+
+    bool _requestActive{false};
+    bool _wantReset{false};
+    QMap<uint32_t, Mode> _nextModes; ///< Modes added by current request
+
+    bool _hasModes{false};
+
+    int _lastSeq{-1};
+
+    QMap<uint32_t, Mode> _modes; ///< key is custom_mode
+};
+

src/Vehicle/Vehicle.cc

@@ -372,12 +372,15 @@ void Vehicle::_commonInit()
     connect(_missionManager, &MissionManager::sendComplete,             _trajectoryPoints, &TrajectoryPoints::clear);
     connect(_missionManager, &MissionManager::newMissionItemsAvailable, _trajectoryPoints, &TrajectoryPoints::clear);
 
+    _standardModes                  = new StandardModes                 (this, this);
     _componentInformationManager    = new ComponentInformationManager   (this);
     _initialConnectStateMachine     = new InitialConnectStateMachine    (this);
     _ftpManager                     = new FTPManager                    (this);
     _imageProtocolManager           = new ImageProtocolManager          ();
     _vehicleLinkManager             = new VehicleLinkManager            (this);
 
+    connect(_standardModes, &StandardModes::modesUpdated, this, &Vehicle::flightModesChanged);
+
     _parameterManager = new ParameterManager(this);
     connect(_parameterManager, &ParameterManager::parametersReadyChanged, this, &Vehicle::_parametersReady);
 
@@ -762,6 +765,16 @@ void Vehicle::_mavlinkMessageReceived(LinkInterface* link, mavlink_message_t mes
         }
     }
         break;
+    case MAVLINK_MSG_ID_AVAILABLE_MODES_MONITOR:
+    {
+        // Avoid duplicate requests during initial connection setup
+        if (!_initialConnectStateMachine || !_initialConnectStateMachine->active()) {
+            mavlink_available_modes_monitor_t availableModesMonitor;
+            mavlink_msg_available_modes_monitor_decode(&message, &availableModesMonitor);
+            _standardModes->availableModesMonitorReceived(availableModesMonitor.seq);
+        }
+        break;
+    }
 
         // Following are ArduPilot dialect messages
 #if !defined(NO_ARDUPILOT_DIALECT)
@@ -1677,7 +1690,7 @@ void Vehicle::setEventsMetadata(uint8_t compid, const QString& metadataJsonFileN
     for (size_t i = 0; i < sizeof(modeGroups)/sizeof(modeGroups[0]); ++i) {
         uint8_t     base_mode;
         uint32_t    custom_mode;
-        if (_firmwarePlugin->setFlightMode(modes[i], &base_mode, &custom_mode)) {
+        if (setFlightModeCustom(modes[i], &base_mode, &custom_mode)) {
             modeGroups[i] = _eventHandler(compid).getModeGroup(custom_mode);
             if (modeGroups[i] == -1) {
                 qCDebug(VehicleLog) << "Failed to get mode group for mode" << modes[i] << "(Might not be in metadata)";
@@ -2182,20 +2195,35 @@ bool Vehicle::flightModeSetAvailable()
 
 QStringList Vehicle::flightModes()
 {
+	if (_standardModes->supported()) {
+		return _standardModes->flightModes();
+	}
     return _firmwarePlugin->flightModes(this);
 }
 
 QString Vehicle::flightMode() const
 {
+    if (_standardModes->supported()) {
+        return _standardModes->flightMode(_custom_mode);
+    }
     return _firmwarePlugin->flightMode(_base_mode, _custom_mode);
 }
 
+bool Vehicle::setFlightModeCustom(const QString& flightMode, uint8_t* base_mode, uint32_t* custom_mode)
+{
+    if (_standardModes->supported()) {
+        *base_mode = MAV_MODE_FLAG_CUSTOM_MODE_ENABLED;
+        return _standardModes->setFlightMode(flightMode, custom_mode);
+    }
+    return _firmwarePlugin->setFlightMode(flightMode, base_mode, custom_mode);
+}
+
 void Vehicle::setFlightMode(const QString& flightMode)
 {
     uint8_t     base_mode;
     uint32_t    custom_mode;
 
-    if (_firmwarePlugin->setFlightMode(flightMode, &base_mode, &custom_mode)) {
+    if (setFlightModeCustom(flightMode, &base_mode, &custom_mode)) {
         SharedLinkInterfacePtr sharedLink = vehicleLinkManager()->primaryLink().lock();
         if (!sharedLink) {
             qCDebug(VehicleLog) << "setFlightMode: primary link gone!";

src/Vehicle/Vehicle.h

@@ -48,6 +48,7 @@
 #include "ImageProtocolManager.h"
 #include "HealthAndArmingCheckReport.h"
 #include "TerrainQuery.h"
+#include "StandardModes.h"
 
 class Actuators;
 class EventHandler;
@@ -1086,6 +1087,7 @@ private:
     void _chunkedStatusTextCompleted    (uint8_t compId);
     void _setMessageInterval            (int messageId, int rate);
     EventHandler& _eventHandler         (uint8_t compid);
+    bool setFlightModeCustom            (const QString& flightMode, uint8_t* base_mode, uint32_t* custom_mode);
 
     static void _rebootCommandResultHandler(void* resultHandlerData, int compId, MAV_RESULT commandResult, uint8_t progress, MavCmdResultFailureCode_t failureCode);
 
@@ -1401,6 +1403,7 @@ private:
     InitialConnectStateMachine*     _initialConnectStateMachine = nullptr;
     Actuators*                      _actuators                  = nullptr;
     RemoteIDManager*                _remoteIDManager            = nullptr;
+    StandardModes*                  _standardModes              = nullptr;
 
     static const char* _rollFactName;
     static const char* _pitchFactName;