From c3d569d6aef228d7fd4f1100692fd8d5bb078138 Mon Sep 17 00:00:00 2001 From: =?UTF-8?q?Beat=20K=C3=BCng?= Date: Mon, 21 Nov 2022 21:51:37 +0100 Subject: [PATCH] Vehicle: implement MAVLink standard modes --- qgroundcontrol.pro | 2 + src/Vehicle/CMakeLists.txt | 2 + src/Vehicle/InitialConnectStateMachine.cc | 20 ++++ src/Vehicle/InitialConnectStateMachine.h | 2 + src/Vehicle/StandardModes.cc | 179 ++++++++++++++++++++++++++++++ src/Vehicle/StandardModes.h | 69 ++++++++++++ src/Vehicle/Vehicle.cc | 32 +++++- src/Vehicle/Vehicle.h | 3 + 8 files changed, 307 insertions(+), 2 deletions(-) create mode 100644 src/Vehicle/StandardModes.cc create mode 100644 src/Vehicle/StandardModes.h diff --git a/qgroundcontrol.pro b/qgroundcontrol.pro index 73ea552..4f9ca79 100644 --- a/qgroundcontrol.pro +++ b/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 \ diff --git a/src/Vehicle/CMakeLists.txt b/src/Vehicle/CMakeLists.txt index b58cb58..eaabd67 100644 --- a/src/Vehicle/CMakeLists.txt +++ b/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 diff --git a/src/Vehicle/InitialConnectStateMachine.cc b/src/Vehicle/InitialConnectStateMachine.cc index 13ec32e..1d178c7 100644 --- a/src/Vehicle/InitialConnectStateMachine.cc +++ b/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(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(requestAllCompleteFnData); diff --git a/src/Vehicle/InitialConnectStateMachine.h b/src/Vehicle/InitialConnectStateMachine.h index 7218e39..44ed2e8 100644 --- a/src/Vehicle/InitialConnectStateMachine.h +++ b/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); diff --git a/src/Vehicle/StandardModes.cc b/src/Vehicle/StandardModes.cc new file mode 100644 index 0000000..cdd5fcb --- /dev/null +++ b/src/Vehicle/StandardModes.cc @@ -0,0 +1,179 @@ +/**************************************************************************** + * + * (c) 2022 QGROUNDCONTROL PROJECT + * + * 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(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; +} diff --git a/src/Vehicle/StandardModes.h b/src/Vehicle/StandardModes.h new file mode 100644 index 0000000..948511a --- /dev/null +++ b/src/Vehicle/StandardModes.h @@ -0,0 +1,69 @@ +/**************************************************************************** + * + * (c) 2022 QGROUNDCONTROL PROJECT + * + * 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 +#include + +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 _nextModes; ///< Modes added by current request + + bool _hasModes{false}; + + int _lastSeq{-1}; + + QMap _modes; ///< key is custom_mode +}; + diff --git a/src/Vehicle/Vehicle.cc b/src/Vehicle/Vehicle.cc index 0faa325..1b2de23 100644 --- a/src/Vehicle/Vehicle.cc +++ b/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!"; diff --git a/src/Vehicle/Vehicle.h b/src/Vehicle/Vehicle.h index 18acaec..3262192 100644 --- a/src/Vehicle/Vehicle.h +++ b/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;