diff --git a/src/Camera/QGCCameraIO.cc b/src/Camera/QGCCameraIO.cc index ec25f3a3ef8c..9b9140899b39 100644 --- a/src/Camera/QGCCameraIO.cc +++ b/src/Camera/QGCCameraIO.cc @@ -1,5 +1,6 @@ #include "QGCCameraIO.h" #include "MAVLinkLib.h" +#include "QGCMAVLink.h" #include "MavlinkCameraControlInterface.h" #include "LinkInterface.h" #include "MAVLinkProtocol.h" @@ -142,58 +143,10 @@ void QGCCameraParamIO::_sendParameter() mavlink_param_ext_set_t p{}; p.param_type = _mavParamType; - QGCMAVLink::param_ext_union_t union_value{}; - const FactMetaData::ValueType_t factType = _fact->type(); - bool ok = true; - switch (factType) { - case FactMetaData::valueTypeUint8: - case FactMetaData::valueTypeBool: - union_value.param_uint8 = static_cast(_fact->rawValue().toUInt(&ok)); - break; - case FactMetaData::valueTypeInt8: - union_value.param_int8 = static_cast(_fact->rawValue().toInt(&ok)); - break; - case FactMetaData::valueTypeUint16: - union_value.param_uint16 = static_cast(_fact->rawValue().toUInt(&ok)); - break; - case FactMetaData::valueTypeInt16: - union_value.param_int16 = static_cast(_fact->rawValue().toInt(&ok)); - break; - case FactMetaData::valueTypeUint32: - union_value.param_uint32 = static_cast(_fact->rawValue().toUInt(&ok)); - break; - case FactMetaData::valueTypeInt64: - union_value.param_int64 = static_cast(_fact->rawValue().toLongLong(&ok)); - break; - case FactMetaData::valueTypeUint64: - union_value.param_uint64 = static_cast(_fact->rawValue().toULongLong(&ok)); - break; - case FactMetaData::valueTypeFloat: - union_value.param_float = _fact->rawValue().toFloat(&ok); - break; - case FactMetaData::valueTypeDouble: - union_value.param_double = _fact->rawValue().toDouble(&ok); - break; - // String and custom are the same for now - case FactMetaData::valueTypeString: - case FactMetaData::valueTypeCustom: { - const QByteArray custom = _fact->rawValue().toByteArray(); - (void) memcpy(union_value.bytes, custom.constData(), static_cast(std::max(custom.size(), static_cast(MAVLINK_MSG_PARAM_EXT_SET_FIELD_PARAM_VALUE_LEN)))); - break; - } - default: - qCCritical(QGCCameraParamIOLog) << "Unsupported fact type" << factType << "for" << _fact->name(); - Q_FALLTHROUGH(); - case FactMetaData::valueTypeInt32: - union_value.param_int32 = static_cast(_fact->rawValue().toInt(&ok)); - break; - } - - if (!ok) { + if (!QGCMAVLink::variantToParamExtValue(_fact->rawValue(), _mavParamType, &p.param_value[0])) { qCCritical(QGCCameraParamIOLog) << "Invalid value for" << _fact->name() << ":" << _fact->rawValue(); } - (void) memcpy(&p.param_value[0], &union_value.bytes[0], MAVLINK_MSG_PARAM_EXT_SET_FIELD_PARAM_VALUE_LEN); p.target_system = static_cast(_vehicle->id()); p.target_component = static_cast(_control->compID()); (void) qstrncpy(p.param_id, _fact->name().toStdString().c_str(), MAVLINK_MSG_PARAM_EXT_SET_FIELD_PARAM_ID_LEN); @@ -258,52 +211,9 @@ void QGCCameraParamIO::handleParamAck(const mavlink_param_ext_ack_t &ack) QVariant QGCCameraParamIO::_valueFromMessage(const char *value, uint8_t param_type) { - QVariant var; - QGCMAVLink::param_ext_union_t u{}; - (void) memcpy(u.bytes, value, MAVLINK_MSG_PARAM_EXT_SET_FIELD_PARAM_VALUE_LEN); - switch (param_type) { - case MAV_PARAM_EXT_TYPE_REAL32: - var = QVariant(u.param_float); - break; - case MAV_PARAM_EXT_TYPE_UINT8: - var = QVariant(u.param_uint8); - break; - case MAV_PARAM_EXT_TYPE_INT8: - var = QVariant(u.param_int8); - break; - case MAV_PARAM_EXT_TYPE_UINT16: - var = QVariant(u.param_uint16); - break; - case MAV_PARAM_EXT_TYPE_INT16: - var = QVariant(u.param_int16); - break; - case MAV_PARAM_EXT_TYPE_UINT32: - var = QVariant(u.param_uint32); - break; - case MAV_PARAM_EXT_TYPE_INT32: - var = QVariant(u.param_int32); - break; - case MAV_PARAM_EXT_TYPE_UINT64: - var = QVariant(static_cast(u.param_uint64)); - break; - case MAV_PARAM_EXT_TYPE_INT64: - var = QVariant(static_cast(u.param_int64)); - break; - case MAV_PARAM_EXT_TYPE_CUSTOM: { - // This will null terminate the name string - char strValueWithNull[MAVLINK_MSG_PARAM_EXT_SET_FIELD_PARAM_VALUE_LEN + 1] = {}; - (void) strncpy(strValueWithNull, value, MAVLINK_MSG_PARAM_EXT_SET_FIELD_PARAM_VALUE_LEN); - const QString strValue(strValueWithNull); - var = QVariant(strValue); - break; - } - default: - var = QVariant(0); - qCCritical(QGCCameraParamIOLog) << "Invalid param_type used for camera setting:" << param_type; - break; - } + const QVariant var = QGCMAVLink::paramExtValueToVariant(value, param_type); - return var; + return var.isValid() ? var : QVariant(0); } void QGCCameraParamIO::handleParamValue(const mavlink_param_ext_value_t &value) diff --git a/src/Camera/VehicleCameraControl.cc b/src/Camera/VehicleCameraControl.cc index ede5743095ab..b5f2a3d6d67f 100644 --- a/src/Camera/VehicleCameraControl.cc +++ b/src/Camera/VehicleCameraControl.cc @@ -1295,7 +1295,8 @@ void VehicleCameraControl::handleParamExtAck(const mavlink_param_ext_ack_t& para << "\n\tType:" << static_cast(paramExtAck.param_type); if(!_paramIO.contains(paramName)) { - qCWarning(VehicleCameraControlLog) << "Received PARAM_EXT_ACK for unknown param:" << paramName; + // ParameterManager queries the full ext parameter list, so acks for params outside the camera definition are expected + qCDebug(VehicleCameraControlLog) << "Ignoring PARAM_EXT_ACK for param not in camera definition:" << paramName; return; } if(_paramIO[paramName]) { @@ -1315,7 +1316,8 @@ void VehicleCameraControl::handleParamExtValue(const mavlink_param_ext_value_t& << "\n\tCount:" << static_cast(paramExtValue.param_count); if(!_paramIO.contains(paramName)) { - qCWarning(VehicleCameraControlLog) << "Received PARAM_EXT_VALUE for unknown param:" << paramName; + // ParameterManager queries the full ext parameter list, so values outside the camera definition are expected + qCDebug(VehicleCameraControlLog) << "Ignoring PARAM_EXT_VALUE for param not in camera definition:" << paramName; return; } if(_paramIO[paramName]) { diff --git a/src/Comms/MockLink/MockConfiguration.h b/src/Comms/MockLink/MockConfiguration.h index 0a1ca2953738..cb538ad03ff8 100644 --- a/src/Comms/MockLink/MockConfiguration.h +++ b/src/Comms/MockLink/MockConfiguration.h @@ -49,6 +49,7 @@ class MockConfiguration : public LinkConfiguration OptionStayMavlinkV1 = 1 << 5, OptionAPMStartFreshParams = 1 << 6, OptionFtpCapability = 1 << 7, + OptionDropFirstExtParam = 1 << 8, }; Q_DECLARE_FLAGS(Options, Option) Q_FLAG(Options) @@ -171,6 +172,12 @@ class MockConfiguration : public LinkConfiguration bool ftpCapability() const { return _ftpCapability; } void setFtpCapability(bool ftpCapability) { _ftpCapability = ftpCapability; } + // Test-only: when true, the camera omits the first parameter from the PARAM_EXT_REQUEST_LIST + // stream, so it can only be picked up by the indexed re-request. Must be set before the link + // starts, since the list is streamed during initial connect. Not persisted. + bool dropFirstExtParam() const { return _dropFirstExtParam; } + void setDropFirstExtParam(bool dropFirstExtParam) { _dropFirstExtParam = dropFirstExtParam; } + signals: void firmwareChanged(); void vehicleChanged(); @@ -214,6 +221,7 @@ class MockConfiguration : public LinkConfiguration bool _preloadMission = false; bool _stayMavlinkV1 = false; bool _ftpCapability = false; + bool _dropFirstExtParam = false; // Camera capability flags (defaults match current Camera 1 configuration) bool _cameraCaptureVideo = true; diff --git a/src/Comms/MockLink/MockLink.cc b/src/Comms/MockLink/MockLink.cc index 5b75247c0fdc..102a34e681a0 100644 --- a/src/Comms/MockLink/MockLink.cc +++ b/src/Comms/MockLink/MockLink.cc @@ -209,6 +209,10 @@ MockLink::MockLink(SharedLinkConfigurationPtr &config, QObject *parent) _missionItemHandler->loadSimpleMultirotorMission(); } + if (_mockLinkCamera && _mockConfig->dropFirstExtParam()) { + _mockLinkCamera->setExtParamListDropIndex(0); + } + // Initialize ADS-B vehicles with different starting conditions _adsbVehicles.reserve(_numberOfVehicles); for (int i = 0; i < _numberOfVehicles; ++i) { @@ -2631,6 +2635,7 @@ MockLink *MockLink::_startMockLinkWorker(const QString &configName, MAV_AUTOPILO mockConfig->setStayMavlinkV1(options.testFlag(MockConfiguration::OptionStayMavlinkV1)); mockConfig->setApmStartFreshParams(options.testFlag(MockConfiguration::OptionAPMStartFreshParams)); mockConfig->setFtpCapability(options.testFlag(MockConfiguration::OptionFtpCapability)); + mockConfig->setDropFirstExtParam(options.testFlag(MockConfiguration::OptionDropFirstExtParam)); mockConfig->setVideoStreamType(videoStreamType); mockConfig->setFailureMode(failureMode); @@ -3259,6 +3264,11 @@ MockLinkFTP *MockLink::mockLinkFTP() const return _mockLinkFTP; } +MockLinkCamera *MockLink::mockLinkCamera() const +{ + return _mockLinkCamera; +} + void MockLink::_sendAvailableMode(uint8_t modeIndexOneBased) { if (modeIndexOneBased < 1 || modeIndexOneBased > _availableModesCount()) { diff --git a/src/Comms/MockLink/MockLink.h b/src/Comms/MockLink/MockLink.h index 98230c095a54..6b93c47c0caa 100644 --- a/src/Comms/MockLink/MockLink.h +++ b/src/Comms/MockLink/MockLink.h @@ -68,6 +68,9 @@ class MockLink : public LinkInterface MockLinkFTP *mockLinkFTP() const; + /// @return The simulated cameras, nullptr when the camera option is not enabled + MockLinkCamera *mockLinkCamera() const; + /// Set the armed state of the simulated vehicle void setArmed(bool armed) { if (armed) _mavBaseMode |= MAV_MODE_FLAG_SAFETY_ARMED; else _mavBaseMode &= ~MAV_MODE_FLAG_SAFETY_ARMED; } bool armed() const { return (_mavBaseMode & MAV_MODE_FLAG_SAFETY_ARMED) != 0; } diff --git a/src/Comms/MockLink/MockLinkCamera.cc b/src/Comms/MockLink/MockLinkCamera.cc index 3cde87bba400..292853d875ab 100644 --- a/src/Comms/MockLink/MockLinkCamera.cc +++ b/src/Comms/MockLink/MockLinkCamera.cc @@ -3,6 +3,7 @@ #include "MockLink.h" #include "MissionManager/MissionCommandTree.h" #include "QGCLoggingCategory.h" +#include "QGCMAVLink.h" #include #include @@ -48,6 +49,17 @@ MockLinkCamera::MockLinkCamera(MockLink *mockLink, _cameras[1].compId = MAV_COMP_ID_CAMERA2; _cameras[1].capFlags = CAMERA_CAP_FLAGS_CAPTURE_IMAGE; _cameras[1].cameraMode = CAMERA_MODE_IMAGE; + + _extParams = defaultExtParams(); +} + +QVector MockLinkCamera::defaultExtParams() +{ + return { + { QStringLiteral("CAM_EXPMODE"), MAV_PARAM_EXT_TYPE_INT32, QVariant(1) }, + { QStringLiteral("CAM_EV"), MAV_PARAM_EXT_TYPE_REAL32, QVariant(0.0f) }, + { QStringLiteral("CAM_MODEL"), MAV_PARAM_EXT_TYPE_CUSTOM, QVariant(QStringLiteral("MockCam")) }, + }; } MockLinkCamera::CameraState *MockLinkCamera::_findCamera(uint8_t compId) @@ -142,7 +154,16 @@ void MockLinkCamera::run10HzTasks() bool MockLinkCamera::handleMavlinkMessage(const mavlink_message_t &msg) { - if (msg.msgid != MAVLINK_MSG_ID_COMMAND_LONG) { + switch (msg.msgid) { + case MAVLINK_MSG_ID_PARAM_EXT_REQUEST_LIST: + return _handleParamExtRequestList(msg); + case MAVLINK_MSG_ID_PARAM_EXT_REQUEST_READ: + return _handleParamExtRequestRead(msg); + case MAVLINK_MSG_ID_PARAM_EXT_SET: + return _handleParamExtSet(msg); + case MAVLINK_MSG_ID_COMMAND_LONG: + break; + default: return false; } @@ -861,3 +882,156 @@ void MockLinkCamera::_sendCommandAck(uint8_t compId, uint16_t command, uint8_t r qCDebug(MockLinkCameraLog) << logMsg; } + +bool MockLinkCamera::_handleParamExtRequestList(const mavlink_message_t &msg) +{ + mavlink_param_ext_request_list_t request{}; + mavlink_msg_param_ext_request_list_decode(&msg, &request); + + if ((request.target_component != kExtParamCompId) && (request.target_component != MAV_COMP_ID_ALL)) { + return false; + } + + qCDebug(MockLinkCameraLog) << "Streaming" << _extParams.count() << "ext params for compId:" << kExtParamCompId; + for (int index = 0; index < _extParams.count(); index++) { + if (index == _extParamListDropIndex) { + qCDebug(MockLinkCameraLog) << "Dropping ext param index from list stream:" << index; + continue; + } + _sendParamExtValue(index); + } + + return true; +} + +QVariant MockLinkCamera::extParamValue(const QString &name) const +{ + for (const ExtParam &extParam: _extParams) { + if (extParam.name == name) { + return extParam.value; + } + } + + return QVariant(); +} + +bool MockLinkCamera::_handleParamExtRequestRead(const mavlink_message_t &msg) +{ + mavlink_param_ext_request_read_t request{}; + mavlink_msg_param_ext_request_read_decode(&msg, &request); + + if (request.target_component != kExtParamCompId) { + return false; + } + + int index = request.param_index; + if (index >= 0) { + _extParamIndexedReadCount++; + } else { + char paramIdWithNull[MAVLINK_MSG_PARAM_EXT_REQUEST_READ_FIELD_PARAM_ID_LEN + 1] = {}; + (void) strncpy(paramIdWithNull, request.param_id, MAVLINK_MSG_PARAM_EXT_REQUEST_READ_FIELD_PARAM_ID_LEN); + const QString paramName(paramIdWithNull); + for (int i = 0; i < _extParams.count(); i++) { + if (_extParams[i].name == paramName) { + index = i; + break; + } + } + } + + if ((index < 0) || (index >= _extParams.count())) { + qCDebug(MockLinkCameraLog) << "PARAM_EXT_REQUEST_READ for unknown param, ignoring"; + return true; + } + + _sendParamExtValue(index); + + return true; +} + +bool MockLinkCamera::_handleParamExtSet(const mavlink_message_t &msg) +{ + mavlink_param_ext_set_t request{}; + mavlink_msg_param_ext_set_decode(&msg, &request); + + if (request.target_component != kExtParamCompId) { + return false; + } + + char paramIdWithNull[MAVLINK_MSG_PARAM_EXT_SET_FIELD_PARAM_ID_LEN + 1] = {}; + (void) strncpy(paramIdWithNull, request.param_id, MAVLINK_MSG_PARAM_EXT_SET_FIELD_PARAM_ID_LEN); + const QString paramName(paramIdWithNull); + + if (_extParamSetFailureMode == FailExtParamSetNoAck) { + qCDebug(MockLinkCameraLog) << "Not acking PARAM_EXT_SET for" << paramName; + return true; + } + + mavlink_param_ext_ack_t ack{}; + ack.param_result = PARAM_ACK_FAILED; + (void) memcpy(ack.param_id, request.param_id, MAVLINK_MSG_PARAM_EXT_ACK_FIELD_PARAM_ID_LEN); + ack.param_type = request.param_type; + (void) memcpy(ack.param_value, request.param_value, MAVLINK_MSG_PARAM_EXT_ACK_FIELD_PARAM_VALUE_LEN); + + if (_extParamSetInProgressPending) { + // Report the write as still running, the next attempt is accepted normally + _extParamSetInProgressPending = false; + ack.param_result = PARAM_ACK_IN_PROGRESS; + qCDebug(MockLinkCameraLog) << "PARAM_EXT_SET in progress for" << paramName; + } else { + for (ExtParam &extParam: _extParams) { + if (extParam.name != paramName) { + continue; + } + if (_extParamSetFailureMode == FailExtParamSetRejected) { + // Echo back the unchanged stored value, as a camera clamping an out of range write would + ack.param_result = PARAM_ACK_VALUE_UNSUPPORTED; + (void) QGCMAVLink::variantToParamExtValue(extParam.value, extParam.type, &ack.param_value[0]); + qCDebug(MockLinkCameraLog) << "PARAM_EXT_SET rejected for" << paramName; + break; + } + if (extParam.type != request.param_type) { + qCDebug(MockLinkCameraLog) << "PARAM_EXT_SET type mismatch for" << paramName; + break; + } + extParam.value = QGCMAVLink::paramExtValueToVariant(request.param_value, request.param_type); + ack.param_result = PARAM_ACK_ACCEPTED; + qCDebug(MockLinkCameraLog) << "PARAM_EXT_SET" << paramName << "=" << extParam.value; + break; + } + } + + mavlink_message_t ackMsg{}; + (void) mavlink_msg_param_ext_ack_encode_chan( + _mockLink->vehicleId(), + kExtParamCompId, + _mockLink->outgoingMavlinkChannel(), + &ackMsg, + &ack); + _mockLink->respondWithMavlinkMessage(ackMsg); + + return true; +} + +void MockLinkCamera::_sendParamExtValue(int index) +{ + const ExtParam &extParam = _extParams[index]; + + mavlink_param_ext_value_t paramExtValue{}; + char paramId[MAVLINK_MSG_PARAM_EXT_VALUE_FIELD_PARAM_ID_LEN + 1] = {}; + (void) strncpy(paramId, extParam.name.toLocal8Bit().constData(), MAVLINK_MSG_PARAM_EXT_VALUE_FIELD_PARAM_ID_LEN); + (void) memcpy(paramExtValue.param_id, paramId, MAVLINK_MSG_PARAM_EXT_VALUE_FIELD_PARAM_ID_LEN); + paramExtValue.param_type = extParam.type; + paramExtValue.param_count = static_cast(_extParams.count()); + paramExtValue.param_index = static_cast(index); + (void) QGCMAVLink::variantToParamExtValue(extParam.value, extParam.type, ¶mExtValue.param_value[0]); + + mavlink_message_t msg{}; + (void) mavlink_msg_param_ext_value_encode_chan( + _mockLink->vehicleId(), + kExtParamCompId, + _mockLink->outgoingMavlinkChannel(), + &msg, + ¶mExtValue); + _mockLink->respondWithMavlinkMessage(msg); +} diff --git a/src/Comms/MockLink/MockLinkCamera.h b/src/Comms/MockLink/MockLinkCamera.h index 5145f992a1ad..680c15fc689c 100644 --- a/src/Comms/MockLink/MockLinkCamera.h +++ b/src/Comms/MockLink/MockLinkCamera.h @@ -3,6 +3,8 @@ #include "MAVLinkLib.h" #include +#include +#include class MockLink; @@ -32,6 +34,10 @@ class MockLink; /// /// Simulated storage: 16 GiB total, 8 GiB free, SD card. /// +/// Camera 1 additionally serves a small set of extended parameters (PARAM_EXT_REQUEST_LIST / +/// PARAM_EXT_REQUEST_READ / PARAM_EXT_SET). Camera 2 serves none, so tests can check that +/// components which don't implement the protocol are simply left out. +/// class MockLinkCamera { public: @@ -94,7 +100,50 @@ class MockLinkCamera /// @return true if the message was handled by the camera bool handleMavlinkMessage(const mavlink_message_t &msg); + /// Extended parameter served by camera 1 + struct ExtParam { + QString name; + uint8_t type; ///< MAV_PARAM_EXT_TYPE + QVariant value; + }; + + /// @return The extended parameters served by camera 1 + static QVector defaultExtParams(); + + enum ExtParamSetFailureMode_t { + FailExtParamSetNone, ///< Normal behavior + FailExtParamSetNoAck, ///< Do not send PARAM_EXT_ACK + FailExtParamSetRejected, ///< Reject with PARAM_ACK_VALUE_UNSUPPORTED, keep the stored value + FailExtParamSetInProgress, ///< Answer PARAM_ACK_IN_PROGRESS once, then accept on the next attempt + }; + + /// Sets a PARAM_EXT_SET failure mode for unit testing + void setExtParamSetFailureMode(ExtParamSetFailureMode_t mode) { + _extParamSetFailureMode = mode; + _extParamSetInProgressPending = (mode == FailExtParamSetInProgress); + } + + /// Test API: drops this index from the PARAM_EXT_REQUEST_LIST stream so the indexed + /// re-request path can be exercised. Negative disables dropping. Only affects the + /// list stream - an explicit PARAM_EXT_REQUEST_READ for the index is always answered. + /// Set from MockConfiguration::dropFirstExtParam, since the list is already streamed + /// by the time a test body runs. + void setExtParamListDropIndex(int index) { _extParamListDropIndex = index; } + + /// @return Current value of an ext parameter, an invalid QVariant if unknown + QVariant extParamValue(const QString &name) const; + + /// @return Number of PARAM_EXT_REQUEST_READ by index served so far. Nothing but the recovery + /// of a dropped list entry asks for a parameter this way, so a non-zero count is proof + /// the indexed re-request ran. + int extParamIndexedReadCount() const { return _extParamIndexedReadCount; } + private: + bool _handleParamExtRequestList(const mavlink_message_t &msg); + bool _handleParamExtRequestRead(const mavlink_message_t &msg); + bool _handleParamExtSet(const mavlink_message_t &msg); + void _sendParamExtValue(int index); + /// Handle a COMMAND_LONG that targets a camera component. /// @return true if the command was handled (ack already sent) bool _handleCameraCommand(const mavlink_command_long_t &request, uint8_t targetCompId); @@ -121,7 +170,14 @@ class MockLinkCamera static constexpr uint32_t kStorageTotalMiB = 16384; ///< 16 GiB simulated SD card static constexpr uint32_t kStorageFreeMiB = 8192; ///< 8 GiB free + static constexpr uint8_t kExtParamCompId = MAV_COMP_ID_CAMERA; ///< Only camera 1 serves ext params + MockLink *_mockLink = nullptr; + QVector _extParams; ///< Ext parameters served by kExtParamCompId + ExtParamSetFailureMode_t _extParamSetFailureMode = FailExtParamSetNone; + bool _extParamSetInProgressPending = false; + int _extParamListDropIndex = -1; + int _extParamIndexedReadCount = 0; CameraState _cameras[kNumCameras]; ///< Simulated cameras /// Protects _cameras array from race conditions between: /// - Main thread: _handleCameraCommand() modifying camera state on MAVLink commands diff --git a/src/FactSystem/ParameterManager.cc b/src/FactSystem/ParameterManager.cc index e457027f7257..d8b05a3ea3c5 100644 --- a/src/FactSystem/ParameterManager.cc +++ b/src/FactSystem/ParameterManager.cc @@ -6,6 +6,8 @@ #include #include +#include + #include "AutoPilotPlugin.h" #include "CompInfoParam.h" #include "ComponentInformationManager.h" @@ -66,6 +68,10 @@ ParameterManager::ParameterManager(Vehicle *vehicle) (void) connect(&_waitingParamTimeoutTimer, &QTimer::timeout, this, &ParameterManager::_waitingParamTimeout); } + _extParamTimeoutTimer.setSingleShot(true); + _extParamTimeoutTimer.setInterval(QGC::runningUnitTests() ? 500 : kExtParamTimeoutMs); + (void) connect(&_extParamTimeoutTimer, &QTimer::timeout, this, &ParameterManager::_extParamTimeout); + // Ensure the cache directory exists (void) QDir().mkpath(parameterCacheDir().absolutePath()); } @@ -124,6 +130,31 @@ void ParameterManager::mavlinkMessageReceived(const mavlink_message_t &message) } _handleParamValue(message.compid, parameterName, param_value.param_count, param_value.param_index, static_cast(param_value.param_type), parameterValue); + return; + } + + if (message.msgid == MAVLINK_MSG_ID_PARAM_EXT_VALUE) { + mavlink_param_ext_value_t paramExtValue{}; + mavlink_msg_param_ext_value_decode(&message, ¶mExtValue); + _handleParamExtValue(message.compid, paramExtValue); + return; + } + + if (message.msgid == MAVLINK_MSG_ID_PARAM_EXT_ACK) { + mavlink_param_ext_ack_t paramExtAck{}; + mavlink_msg_param_ext_ack_decode(&message, ¶mExtAck); + _handleParamExtAck(message.compid, paramExtAck); + return; + } + + if (message.msgid == MAVLINK_MSG_ID_HEARTBEAT) { + // Extended parameters live on peripherals such as cameras and gimbals, never on the autopilot + if (message.compid != MAV_COMP_ID_AUTOPILOT1) { + (void) _seenComponentIds.insert(message.compid); + if (_initialLoadComplete) { + _startExtParameterDownload(message.compid); + } + } } } @@ -530,9 +561,312 @@ void ParameterManager::_factRawValueUpdated(const QVariant &rawValue) return; } + if (_extFact(fact->componentId(), fact->name()) == fact) { + _mavlinkParamExtSet(fact->componentId(), fact->name(), factTypeToMavExtType(fact->type()), rawValue); + return; + } + _mavlinkParamSet(fact->componentId(), fact->name(), fact->type(), rawValue); } +Fact *ParameterManager::_extFact(int componentId, const QString ¶mName) const +{ + return _mapCompId2ExtFactMap.value(componentId).value(paramName, nullptr); +} + +void ParameterManager::_startExtParameterDownload(int componentId) +{ + if (_extParamRequestedCompIds.contains(componentId)) { + return; + } + + const SharedLinkInterfacePtr sharedLink = _vehicle->vehicleLinkManager()->primaryLink().lock(); + if (!sharedLink) { + return; + } + if (sharedLink->linkConfiguration()->isHighLatency() || _logReplay) { + return; + } + + (void) _extParamRequestedCompIds.insert(componentId); + + qCDebug(ParameterManagerLog) << _logVehiclePrefix(componentId) << "Requesting extended parameter list"; + + mavlink_message_t msg{}; + (void) mavlink_msg_param_ext_request_list_pack_chan(MAVLinkProtocol::instance()->getSystemId(), + MAVLinkProtocol::getComponentId(), + sharedLink->mavlinkChannel(), + &msg, + _vehicle->id(), + static_cast(componentId)); + (void) _vehicle->sendMessageOnLinkThreadSafe(sharedLink.get(), msg); +} + +void ParameterManager::_handleParamExtValue(int componentId, const mavlink_param_ext_value_t ¶mExtValue) +{ + // This will null terminate the name string + char parameterNameWithNull[MAVLINK_MSG_PARAM_EXT_VALUE_FIELD_PARAM_ID_LEN + 1] = {}; + (void) strncpy(parameterNameWithNull, paramExtValue.param_id, MAVLINK_MSG_PARAM_EXT_VALUE_FIELD_PARAM_ID_LEN); + const QString parameterName(parameterNameWithNull); + + const auto mavParamExtType = static_cast(paramExtValue.param_type); + const QVariant parameterValue = QGCMAVLink::paramExtValueToVariant(paramExtValue.param_value, mavParamExtType); + + qCDebug(ParameterManagerVerbose1Log) << _logVehiclePrefix(componentId) << + "_handleParamExtValue" << + "name:" << parameterName << + "count:" << paramExtValue.param_count << + "index:" << paramExtValue.param_index << + "mavExtType:" << mavParamExtType << + "value:" << parameterValue; + + // The index has been answered whatever we end up doing with the value, so book keep it before + // any early return below. Otherwise it stays marked missing, is re-requested until the retries + // run out and is then reported as never received. + if (!_extWaitingReadParamIndexMap.contains(componentId)) { + for (int waitingIndex = 0; waitingIndex < paramExtValue.param_count; waitingIndex++) { + _extWaitingReadParamIndexMap[componentId][waitingIndex] = 0; + } + qCDebug(ParameterManagerLog) << _logVehiclePrefix(componentId) << "Seeing ext params for first time - paramcount:" << paramExtValue.param_count; + } + (void) _extWaitingReadParamIndexMap[componentId].remove(paramExtValue.param_index); + _extParamTimeoutTimer.start(); + + if (!parameterValue.isValid()) { + return; + } + + if (mavParamExtType == MAV_PARAM_EXT_TYPE_CUSTOM) { + // Custom is an opaque blob of bytes. The Fact system has nothing that can hold it, and + // decoding it as text mangles anything that isn't printable UTF-8, so leave it to whoever + // knows the component rather than showing a value we can neither trust nor write back. + qCDebug(ParameterManagerLog) << _logVehiclePrefix(componentId) << "Ignoring custom type ext param" << parameterName; + return; + } + + if (_mapCompId2FactMap.value(componentId).contains(parameterName)) { + // The classic protocol already owns this name for this component, don't shadow it with a second fact + qCDebug(ParameterManagerLog) << _logVehiclePrefix(componentId) << "Ignoring ext param which also exists as a classic param" << parameterName; + return; + } + + Fact *fact = _extFact(componentId, parameterName); + if (!fact) { + fact = new Fact(componentId, parameterName, mavExtTypeToFactType(mavParamExtType), this); + FactMetaData *const factMetaData = _vehicle->compInfoManager()->compInfoParam(componentId)->factMetaDataForName(parameterName, fact->type()); + fact->setMetaData(factMetaData); + + _mapCompId2ExtFactMap[componentId][parameterName] = fact; + + (void) connect(fact, &Fact::containerRawValueChanged, this, &ParameterManager::_factRawValueUpdated); + + emit factAdded(componentId, fact); + } + + fact->containerSetRawValue(parameterValue); +} + +void ParameterManager::_handleParamExtAck(int componentId, const mavlink_param_ext_ack_t ¶mExtAck) +{ + if (paramExtAck.param_result != PARAM_ACK_ACCEPTED) { + return; + } + + // This will null terminate the name string + char parameterNameWithNull[MAVLINK_MSG_PARAM_EXT_ACK_FIELD_PARAM_ID_LEN + 1] = {}; + (void) strncpy(parameterNameWithNull, paramExtAck.param_id, MAVLINK_MSG_PARAM_EXT_ACK_FIELD_PARAM_ID_LEN); + + Fact *const fact = _extFact(componentId, QString(parameterNameWithNull)); + if (!fact) { + return; + } + + // Acks are broadcast, so this also picks up writes made from the camera settings UI + const QVariant value = QGCMAVLink::paramExtValueToVariant(paramExtAck.param_value, paramExtAck.param_type); + if (value.isValid()) { + fact->containerSetRawValue(value); + } +} + +void ParameterManager::_extParamTimeout() +{ + bool stillWaiting = false; + + for (const int componentId: _extWaitingReadParamIndexMap.keys()) { + QMap &waitingIndices = _extWaitingReadParamIndexMap[componentId]; + for (const int paramIndex: waitingIndices.keys()) { + if (waitingIndices[paramIndex]++ >= kExtParamReadRetryCount) { + qCWarning(ParameterManagerLog) << _logVehiclePrefix(componentId) << "Ext param never received - index:" << paramIndex; + (void) waitingIndices.remove(paramIndex); + continue; + } + stillWaiting = true; + _mavlinkParamExtRequestRead(componentId, QString(), paramIndex); + } + } + + if (stillWaiting) { + _extParamTimeoutTimer.start(); + } +} + +void ParameterManager::_mavlinkParamExtRequestRead(int componentId, const QString ¶mName, int paramIndex) +{ + const SharedLinkInterfacePtr sharedLink = _vehicle->vehicleLinkManager()->primaryLink().lock(); + if (!sharedLink) { + return; + } + + qCDebug(ParameterManagerLog) << _logVehiclePrefix(componentId) << "Requesting ext param - name:" << paramName << "index:" << paramIndex; + + char paramId[MAVLINK_MSG_PARAM_EXT_REQUEST_READ_FIELD_PARAM_ID_LEN + 1] = {}; + (void) strncpy(paramId, paramName.toLocal8Bit().constData(), MAVLINK_MSG_PARAM_EXT_REQUEST_READ_FIELD_PARAM_ID_LEN); + + mavlink_message_t msg{}; + (void) mavlink_msg_param_ext_request_read_pack_chan(MAVLinkProtocol::instance()->getSystemId(), + MAVLinkProtocol::getComponentId(), + sharedLink->mavlinkChannel(), + &msg, + _vehicle->id(), + static_cast(componentId), + paramId, + paramIndex); + (void) _vehicle->sendMessageOnLinkThreadSafe(sharedLink.get(), msg); +} + +void ParameterManager::_mavlinkParamExtSet(int componentId, const QString ¶mName, MAV_PARAM_EXT_TYPE paramExtType, const QVariant &rawValue) +{ + // Shared between the ack predicate and the states which classify the ack + struct ExtAckResult { + uint8_t result = PARAM_ACK_FAILED; + QVariant value; + }; + auto ackResult = std::make_shared(); + + auto paramExtSetEncoder = [this, componentId, paramName, paramExtType, rawValue](uint8_t /*systemId*/, uint8_t channel, mavlink_message_t *message) -> void { + mavlink_param_ext_set_t paramExtSet{}; + + paramExtSet.target_system = static_cast(_vehicle->id()); + paramExtSet.target_component = static_cast(componentId); + paramExtSet.param_type = paramExtType; + + // param_id is exactly MAVLINK_MSG_PARAM_EXT_SET_FIELD_PARAM_ID_LEN long and not null + // terminated when the name fills it, so stage through a buffer with room for the null + char paramId[MAVLINK_MSG_PARAM_EXT_SET_FIELD_PARAM_ID_LEN + 1] = {}; + (void) strncpy(paramId, paramName.toLocal8Bit().constData(), MAVLINK_MSG_PARAM_EXT_SET_FIELD_PARAM_ID_LEN); + (void) memcpy(paramExtSet.param_id, paramId, MAVLINK_MSG_PARAM_EXT_SET_FIELD_PARAM_ID_LEN); + if (!QGCMAVLink::variantToParamExtValue(rawValue, paramExtType, ¶mExtSet.param_value[0])) { + return; + } + + (void) mavlink_msg_param_ext_set_encode_chan(MAVLinkProtocol::instance()->getSystemId(), + MAVLinkProtocol::getComponentId(), + channel, + message, + ¶mExtSet); + }; + + auto checkForParamExtAck = [componentId, paramName, ackResult](const mavlink_message_t &message) -> bool { + if (message.compid != componentId) { + return false; + } + + mavlink_param_ext_ack_t paramExtAck{}; + mavlink_msg_param_ext_ack_decode(&message, ¶mExtAck); + + char parameterNameWithNull[MAVLINK_MSG_PARAM_EXT_ACK_FIELD_PARAM_ID_LEN + 1] = {}; + (void) strncpy(parameterNameWithNull, paramExtAck.param_id, MAVLINK_MSG_PARAM_EXT_ACK_FIELD_PARAM_ID_LEN); + if (QString(parameterNameWithNull) != paramName) { + return false; + } + + ackResult->result = paramExtAck.param_result; + ackResult->value = QGCMAVLink::paramExtValueToVariant(paramExtAck.param_value, paramExtAck.param_type); + + return true; + }; + + // State Machine: + // Send PARAM_EXT_SET - retries after initial attempt + // Wait for PARAM_EXT_ACK + // PARAM_ACK_IN_PROGRESS: keep waiting + // PARAM_ACK_ACCEPTED: take the acked value, done + // anything else: notify user and restore the vehicle value + + auto stateMachine = new QGCStateMachine(QStringLiteral("ParameterManager PARAM_EXT_SET"), vehicle(), this); + auto sendParamExtSetState = new SendMavlinkMessageState(stateMachine, paramExtSetEncoder, kParamSetRetryCount); + auto incPendingWriteCountState = new FunctionState(QStringLiteral("ParameterManager increment pending write count"), stateMachine, [this]() { + _incrementPendingWriteCount(); + }); + auto waitAckState = new WaitForMavlinkMessageState(stateMachine, MAVLINK_MSG_ID_PARAM_EXT_ACK, _waitForParamValueAckMs, checkForParamExtAck); + auto inProgressState = new ConditionalState(QStringLiteral("ParameterManager ext param in progress"), stateMachine, [ackResult]() { + return ackResult->result == PARAM_ACK_IN_PROGRESS; + }); + auto acceptedState = new ConditionalState(QStringLiteral("ParameterManager ext param accepted"), stateMachine, [ackResult]() { + return ackResult->result == PARAM_ACK_ACCEPTED; + }, [this, componentId, paramName, ackResult]() { + // The component may have clamped the value, take what it acked + if (Fact *const fact = _extFact(componentId, paramName); fact && ackResult->value.isValid()) { + fact->containerSetRawValue(ackResult->value); + } + }); + auto decPendingWriteCountState = new FunctionState(QStringLiteral("ParameterManager decrement pending write count"), stateMachine, [this]() { + _decrementPendingWriteCount(); + }); + auto retryDecPendingWriteCountState = new FunctionState(QStringLiteral("ParameterManager retry decrement pending write count"), stateMachine, [this]() { + _decrementPendingWriteCount(); + }); + auto errorDecPendingWriteCountState = new FunctionState(QStringLiteral("ParameterManager error decrement pending write count"), stateMachine, [this]() { + _decrementPendingWriteCount(); + }); + auto logSuccessState = new FunctionState(QStringLiteral("ParameterManager log success"), stateMachine, [this, componentId, paramName]() { + qCDebug(ParameterManagerLog) << "Ext parameter write succeeded: param:" << paramName << _vehicleAndComponentString(componentId); + emit _paramSetSuccess(componentId, paramName); + }); + auto logFailureState = new FunctionState(QStringLiteral("ParameterManager log failure"), stateMachine, [this, componentId, paramName]() { + qCDebug(ParameterManagerLog) << "Ext parameter write failed: param:" << paramName << _vehicleAndComponentString(componentId); + emit _paramSetFailure(componentId, paramName); + }); + auto userNotifyState = new FunctionState(QStringLiteral("ParameterManager user notify"), stateMachine, [this, componentId, paramName]() { + QGC::showAppMessage(QStringLiteral("Parameter write failed: param: %1 %2").arg(paramName, _vehicleAndComponentString(componentId))); + }); + auto paramRefreshState = new FunctionState(QStringLiteral("ParameterManager ext param refresh"), stateMachine, [this, componentId, paramName]() { + refreshParameter(componentId, paramName); + }); + auto finalState = new QGCFinalState(stateMachine); + + stateMachine->setInitialState(sendParamExtSetState); + sendParamExtSetState->addThisTransition (&QGCState::advance, incPendingWriteCountState); + incPendingWriteCountState->addThisTransition(&QGCState::advance, waitAckState); + waitAckState->addThisTransition (&QGCState::advance, inProgressState); + + // PARAM_ACK_IN_PROGRESS means the component is still working on it, go back to waiting + inProgressState->addThisTransition(&QGCState::advance, waitAckState); + inProgressState->addTransition(inProgressState, &ConditionalState::skipped, acceptedState); + + acceptedState->addThisTransition (&QGCState::advance, decPendingWriteCountState); + decPendingWriteCountState->addThisTransition(&QGCState::advance, logSuccessState); + logSuccessState->addThisTransition (&QGCState::advance, finalState); + + // Rejected by the component + acceptedState->addTransition(acceptedState, &ConditionalState::skipped, errorDecPendingWriteCountState); + errorDecPendingWriteCountState->addThisTransition(&QGCState::advance, logFailureState); + + // No ack at all, retry the send + waitAckState->addTransition(waitAckState, &WaitStateBase::timeout, retryDecPendingWriteCountState); + retryDecPendingWriteCountState->addThisTransition(&QGCState::advance, sendParamExtSetState); + + // Retries exhausted + sendParamExtSetState->addThisTransition(&QGCState::error, logFailureState); + + logFailureState->addThisTransition (&QGCState::advance, userNotifyState); + userNotifyState->addThisTransition (&QGCState::advance, paramRefreshState); + paramRefreshState->addThisTransition(&QGCState::advance, finalState); + + qCDebug(ParameterManagerLog) << "Starting state machine for PARAM_EXT_SET on:" << paramName << _vehicleAndComponentString(componentId); + stateMachine->start(); +} + void ParameterManager::_ftpDownloadComplete(const QString &fileName, const QString &errorMsg) { bool continueWithDefaultParameterdownload = true; @@ -602,6 +936,14 @@ void ParameterManager::refreshAllParameters(uint8_t componentId) _resetHashCheck(); setParameterDownloadSkipped(false); _startParameterDownload(componentId); + + for (const int seenComponentId: _seenComponentIds) { + if ((componentId == MAV_COMP_ID_ALL) || (componentId == seenComponentId)) { + (void) _extParamRequestedCompIds.remove(seenComponentId); + (void) _extWaitingReadParamIndexMap.remove(seenComponentId); + _startExtParameterDownload(seenComponentId); + } + } } void ParameterManager::tryHashCheckCacheLoad() @@ -730,6 +1072,11 @@ void ParameterManager::refreshParameter(int componentId, const QString ¶mNam qCDebug(ParameterManagerLog) << _logVehiclePrefix(componentId) << "refreshParameter - name:" << paramName << ")"; + if (_extFact(componentId, paramName)) { + _mavlinkParamExtRequestRead(componentId, paramName, -1); + return; + } + _mavlinkParamRequestRead(componentId, paramName, -1, true /* notifyFailure */); } @@ -803,7 +1150,12 @@ bool ParameterManager::parameterExists(int componentId, const QString ¶mName ret = _mapCompId2FactMap[componentId].contains(_remapParamNameToVersion(paramName)); } - return ret; + return ret || extParameterExists(componentId, paramName); +} + +bool ParameterManager::extParameterExists(int componentId, const QString ¶mName) const +{ + return _mapCompId2ExtFactMap.value(_actualComponentId(componentId)).contains(paramName); } Fact *ParameterManager::getParameter(int componentId, const QString ¶mName) @@ -812,6 +1164,9 @@ Fact *ParameterManager::getParameter(int componentId, const QString ¶mName) const QString mappedParamName = _remapParamNameToVersion(paramName); if (!_mapCompId2FactMap.contains(componentId) || !_mapCompId2FactMap[componentId].contains(mappedParamName)) { + if (Fact *const extFact = _extFact(componentId, paramName)) { + return extFact; + } qgcApp()->reportMissingParameter(componentId, mappedParamName); return &_defaultFact; } @@ -828,6 +1183,9 @@ QStringList ParameterManager::parameterNames(int componentId) const for (const QString ¶mName: factMap.keys()) { names << paramName; } + for (const QString ¶mName: _mapCompId2ExtFactMap.value(compId).keys()) { + names << paramName; + } return names; } @@ -1235,6 +1593,10 @@ void ParameterManager::writeParametersToStream(QTextStream &stream) const stream << "#\n"; stream << "# Vehicle-Id Component-Id Name Value Type\n"; + // Extended parameters are deliberately left out. Nothing in the format distinguishes them from + // classic ones, so loading such a file before the component has answered PARAM_EXT_REQUEST_LIST + // would send a plain PARAM_SET for a parameter that only accepts PARAM_EXT_SET. Loading does + // write them once they are known, since the editor's diff resolves names against both maps. for (const int componentId: _mapCompId2FactMap.keys()) { for (const QString ¶mName: _mapCompId2FactMap[componentId].keys()) { const Fact *const fact = _mapCompId2FactMap[componentId][paramName]; @@ -1307,6 +1669,70 @@ FactMetaData::ValueType_t ParameterManager::mavTypeToFactType(MAV_PARAM_TYPE mav } } +MAV_PARAM_EXT_TYPE ParameterManager::factTypeToMavExtType(FactMetaData::ValueType_t factType) +{ + switch (factType) { + case FactMetaData::valueTypeUint8: + case FactMetaData::valueTypeBool: + return MAV_PARAM_EXT_TYPE_UINT8; + case FactMetaData::valueTypeInt8: + return MAV_PARAM_EXT_TYPE_INT8; + case FactMetaData::valueTypeUint16: + return MAV_PARAM_EXT_TYPE_UINT16; + case FactMetaData::valueTypeInt16: + return MAV_PARAM_EXT_TYPE_INT16; + case FactMetaData::valueTypeUint32: + return MAV_PARAM_EXT_TYPE_UINT32; + case FactMetaData::valueTypeInt32: + return MAV_PARAM_EXT_TYPE_INT32; + case FactMetaData::valueTypeUint64: + return MAV_PARAM_EXT_TYPE_UINT64; + case FactMetaData::valueTypeInt64: + return MAV_PARAM_EXT_TYPE_INT64; + case FactMetaData::valueTypeFloat: + return MAV_PARAM_EXT_TYPE_REAL32; + case FactMetaData::valueTypeDouble: + return MAV_PARAM_EXT_TYPE_REAL64; + case FactMetaData::valueTypeString: + case FactMetaData::valueTypeCustom: + return MAV_PARAM_EXT_TYPE_CUSTOM; + default: + qCCritical(ParameterManagerLog) << "Internal Error: Unsupported fact value type" << factType; + return MAV_PARAM_EXT_TYPE_INT32; + } +} + +FactMetaData::ValueType_t ParameterManager::mavExtTypeToFactType(MAV_PARAM_EXT_TYPE mavExtType) +{ + switch (mavExtType) { + case MAV_PARAM_EXT_TYPE_UINT8: + return FactMetaData::valueTypeUint8; + case MAV_PARAM_EXT_TYPE_INT8: + return FactMetaData::valueTypeInt8; + case MAV_PARAM_EXT_TYPE_UINT16: + return FactMetaData::valueTypeUint16; + case MAV_PARAM_EXT_TYPE_INT16: + return FactMetaData::valueTypeInt16; + case MAV_PARAM_EXT_TYPE_UINT32: + return FactMetaData::valueTypeUint32; + case MAV_PARAM_EXT_TYPE_INT32: + return FactMetaData::valueTypeInt32; + case MAV_PARAM_EXT_TYPE_UINT64: + return FactMetaData::valueTypeUint64; + case MAV_PARAM_EXT_TYPE_INT64: + return FactMetaData::valueTypeInt64; + case MAV_PARAM_EXT_TYPE_REAL32: + return FactMetaData::valueTypeFloat; + case MAV_PARAM_EXT_TYPE_REAL64: + return FactMetaData::valueTypeDouble; + case MAV_PARAM_EXT_TYPE_CUSTOM: + return FactMetaData::valueTypeString; + default: + qCCritical(ParameterManagerLog) << "Internal Error: Unsupported MAV_PARAM_EXT_TYPE" << mavExtType; + return FactMetaData::valueTypeInt32; + } +} + void ParameterManager::_checkInitialLoadComplete() { if (_initialLoadComplete) { @@ -1342,6 +1768,11 @@ void ParameterManager::_checkInitialLoadComplete() qCDebug(ParameterManagerLog) << _logVehiclePrefix(-1) << "Initial load complete"; + // Held back until now so the ext queries don't compete with the classic parameter download + for (const int componentId: _seenComponentIds) { + _startExtParameterDownload(componentId); + } + // Check for index based load failures QString indexList; bool initialLoadFailures = false; @@ -1572,7 +2003,15 @@ void ParameterManager::setParameterDownloadSkipped(bool skipped) QList ParameterManager::componentIds() const { - return _paramCountMap.keys(); + QList ids = _paramCountMap.keys(); + + for (const int componentId: _mapCompId2ExtFactMap.keys()) { + if (!ids.contains(componentId)) { + ids.append(componentId); + } + } + + return ids; } bool ParameterManager::pendingWrites() const diff --git a/src/FactSystem/ParameterManager.h b/src/FactSystem/ParameterManager.h index f1da8f922d9a..4e3a49ded6f5 100644 --- a/src/FactSystem/ParameterManager.h +++ b/src/FactSystem/ParameterManager.h @@ -2,6 +2,7 @@ #include #include +#include #include #include #include @@ -80,6 +81,11 @@ class ParameterManager : public QObject /// Returns all parameter names QStringList parameterNames(int componentId) const; + /// Returns true if the specified parameter is served by the extended parameter protocol + /// @param componentId: Component id or ParameterManager::defaultComponentId + /// @param name: Parameter name + bool extParameterExists(int componentId, const QString ¶mName) const; + /// Returns the specified Parameter. Returns a default empty fact is parameter does not exists. Also will pop /// a missing parameter error to user if parameter does not exist. /// @param componentId: Component id or ParameterManager::defaultComponentId @@ -102,6 +108,8 @@ class ParameterManager : public QObject static MAV_PARAM_TYPE factTypeToMavType(FactMetaData::ValueType_t factType); static FactMetaData::ValueType_t mavTypeToFactType(MAV_PARAM_TYPE mavType); + static MAV_PARAM_EXT_TYPE factTypeToMavExtType(FactMetaData::ValueType_t factType); + static FactMetaData::ValueType_t mavExtTypeToFactType(MAV_PARAM_EXT_TYPE mavExtType); static constexpr int defaultComponentId = -1; @@ -116,6 +124,8 @@ class ParameterManager : public QObject static constexpr int kTestInitialRequestIntervalMs = 500; ///< Timer interval for initial request in test mode /// Maximum time to wait for initial request retries to exhaust in tests static constexpr int kTestMaxInitialRequestTimeMs = (kMaxInitialRequestListRetry + 1) * kTestInitialRequestIntervalMs + 1000; + static constexpr int kExtParamReadRetryCount = 2; ///< Retries for a missing PARAM_EXT_VALUE index + static constexpr int kExtParamTimeoutMs = 3000; ///< Quiet time after which missing ext params are re-requested signals: void parametersReadyChanged(bool parametersReady); @@ -140,6 +150,16 @@ private slots: private: /// Called whenever a parameter is updated or first seen. void _handleParamValue(int componentId, const QString ¶meterName, int parameterCount, int parameterIndex, MAV_PARAM_TYPE mavParamType, const QVariant ¶meterValue); + /// Called whenever an extended parameter is updated or first seen. + void _handleParamExtValue(int componentId, const mavlink_param_ext_value_t ¶mExtValue); + /// Keeps the local ext parameter value in sync with writes made from anywhere, including the camera UI. + void _handleParamExtAck(int componentId, const mavlink_param_ext_ack_t ¶mExtAck); + /// Sends PARAM_EXT_REQUEST_LIST to a component which has not been queried yet. + void _startExtParameterDownload(int componentId); + void _extParamTimeout(); + void _mavlinkParamExtSet(int componentId, const QString ¶mName, MAV_PARAM_EXT_TYPE paramExtType, const QVariant &rawValue); + void _mavlinkParamExtRequestRead(int componentId, const QString ¶mName, int paramIndex); + Fact *_extFact(int componentId, const QString ¶mName) const; /// Writes the parameter update to mavlink, sets up for write wait void _mavlinkParamSet(int componentId, const QString &name, FactMetaData::ValueType_t valueType, const QVariant &rawValue); void _waitingParamTimeout(); @@ -192,6 +212,15 @@ private slots: QMap> _mapCompId2FactMap; + // Extended parameter protocol (PARAM_EXT_*). Kept separate from the classic protocol facts since + // ext parameters take no part in the initial load/progress/cache machinery and must be written + // with PARAM_EXT_SET. They are merged into the public accessors so the parameter editor shows both. + QMap> _mapCompId2ExtFactMap; + QSet _seenComponentIds; ///< Non-autopilot components heard from via HEARTBEAT + QSet _extParamRequestedCompIds; ///< Components PARAM_EXT_REQUEST_LIST has been sent to + QMap> _extWaitingReadParamIndexMap; + QTimer _extParamTimeoutTimer; + double _loadProgress = 0; ///< Parameter load progess, [0.0,1.0] bool _parametersReady = false; ///< true: parameter load complete bool _parameterDownloadSkipped = false; ///< true: parameter download was intentionally skipped diff --git a/src/MAVLink/QGCMAVLink.cc b/src/MAVLink/QGCMAVLink.cc index f41377d04051..689b4aad59e9 100644 --- a/src/MAVLink/QGCMAVLink.cc +++ b/src/MAVLink/QGCMAVLink.cc @@ -4,6 +4,9 @@ #include +#include +#include + QGC_LOGGING_CATEGORY(QGCMAVLinkLog, "MAVLink.QGCMAVLink") const QHash QGCMAVLink::mavlinkCompIdHash { @@ -551,3 +554,98 @@ QString QGCMAVLink::compIdToString(uint8_t compId) return QStringLiteral("%1 (%2)").arg(compIdStr).arg(static_cast(compId)); } + +QVariant QGCMAVLink::paramExtValueToVariant(const char *value, uint8_t paramExtType) +{ + param_ext_union_t unionValue{}; + (void) memcpy(unionValue.bytes, value, MAVLINK_MSG_PARAM_EXT_SET_FIELD_PARAM_VALUE_LEN); + + switch (paramExtType) { + case MAV_PARAM_EXT_TYPE_UINT8: + return QVariant(unionValue.param_uint8); + case MAV_PARAM_EXT_TYPE_INT8: + return QVariant(unionValue.param_int8); + case MAV_PARAM_EXT_TYPE_UINT16: + return QVariant(unionValue.param_uint16); + case MAV_PARAM_EXT_TYPE_INT16: + return QVariant(unionValue.param_int16); + case MAV_PARAM_EXT_TYPE_UINT32: + return QVariant(unionValue.param_uint32); + case MAV_PARAM_EXT_TYPE_INT32: + return QVariant(unionValue.param_int32); + case MAV_PARAM_EXT_TYPE_UINT64: + return QVariant(static_cast(unionValue.param_uint64)); + case MAV_PARAM_EXT_TYPE_INT64: + return QVariant(static_cast(unionValue.param_int64)); + case MAV_PARAM_EXT_TYPE_REAL32: + return QVariant(unionValue.param_float); + case MAV_PARAM_EXT_TYPE_REAL64: + return QVariant(unionValue.param_double); + case MAV_PARAM_EXT_TYPE_CUSTOM: { + char strValueWithNull[MAVLINK_MSG_PARAM_EXT_SET_FIELD_PARAM_VALUE_LEN + 1] = {}; + (void) strncpy(strValueWithNull, value, MAVLINK_MSG_PARAM_EXT_SET_FIELD_PARAM_VALUE_LEN); + return QVariant(QString(strValueWithNull)); + } + default: + // Comes off the wire, so a component sending a type we don't know is not a coding error + qCWarning(QGCMAVLinkLog) << "Unsupported MAV_PARAM_EXT_TYPE received:" << paramExtType; + return QVariant(); + } +} + +bool QGCMAVLink::variantToParamExtValue(const QVariant &value, uint8_t paramExtType, char *outValue) +{ + param_ext_union_t unionValue{}; + bool ok = true; + + switch (paramExtType) { + case MAV_PARAM_EXT_TYPE_UINT8: + unionValue.param_uint8 = static_cast(value.toUInt(&ok)); + break; + case MAV_PARAM_EXT_TYPE_INT8: + unionValue.param_int8 = static_cast(value.toInt(&ok)); + break; + case MAV_PARAM_EXT_TYPE_UINT16: + unionValue.param_uint16 = static_cast(value.toUInt(&ok)); + break; + case MAV_PARAM_EXT_TYPE_INT16: + unionValue.param_int16 = static_cast(value.toInt(&ok)); + break; + case MAV_PARAM_EXT_TYPE_UINT32: + unionValue.param_uint32 = value.toUInt(&ok); + break; + case MAV_PARAM_EXT_TYPE_INT32: + unionValue.param_int32 = value.toInt(&ok); + break; + case MAV_PARAM_EXT_TYPE_UINT64: + unionValue.param_uint64 = value.toULongLong(&ok); + break; + case MAV_PARAM_EXT_TYPE_INT64: + unionValue.param_int64 = value.toLongLong(&ok); + break; + case MAV_PARAM_EXT_TYPE_REAL32: + unionValue.param_float = value.toFloat(&ok); + break; + case MAV_PARAM_EXT_TYPE_REAL64: + unionValue.param_double = value.toDouble(&ok); + break; + case MAV_PARAM_EXT_TYPE_CUSTOM: { + const QByteArray custom = (value.typeId() == QMetaType::QByteArray) ? value.toByteArray() : value.toString().toUtf8(); + const qsizetype copyLength = std::min(custom.size(), static_cast(MAVLINK_MSG_PARAM_EXT_SET_FIELD_PARAM_VALUE_LEN)); + (void) memcpy(unionValue.bytes, custom.constData(), static_cast(copyLength)); + break; + } + default: + qCCritical(QGCMAVLinkLog) << "Internal Error: Unsupported MAV_PARAM_EXT_TYPE" << paramExtType; + return false; + } + + if (!ok) { + qCWarning(QGCMAVLinkLog) << "Failed to convert value to MAV_PARAM_EXT_TYPE" << paramExtType << "value:" << value; + return false; + } + + (void) memcpy(outValue, unionValue.bytes, MAVLINK_MSG_PARAM_EXT_SET_FIELD_PARAM_VALUE_LEN); + + return true; +} diff --git a/src/MAVLink/QGCMAVLink.h b/src/MAVLink/QGCMAVLink.h index 0b264d89eb0e..c92680c4a32f 100644 --- a/src/MAVLink/QGCMAVLink.h +++ b/src/MAVLink/QGCMAVLink.h @@ -3,6 +3,7 @@ #include #include #include +#include #include #include "MAVLinkEnums.h" @@ -107,6 +108,19 @@ class QGCMAVLink : public QObject, public QGCMAVLinkTypes uint8_t type; }) param_ext_union_t; + /// Decodes the 128 byte PARAM_EXT_VALUE/PARAM_EXT_ACK value blob according to its MAV_PARAM_EXT_TYPE. + /// @param value: Raw value bytes, MAVLINK_MSG_PARAM_EXT_SET_FIELD_PARAM_VALUE_LEN long, not null terminated + /// @param paramExtType: MAV_PARAM_EXT_TYPE of the value + /// @return Decoded value, or an invalid QVariant if the type is not supported + static QVariant paramExtValueToVariant(const char *value, uint8_t paramExtType); + + /// Encodes a value into the 128 byte PARAM_EXT_SET value blob according to its MAV_PARAM_EXT_TYPE. + /// @param value: Value to encode + /// @param paramExtType: MAV_PARAM_EXT_TYPE to encode as + /// @param outValue: Receives the encoded bytes, must be MAVLINK_MSG_PARAM_EXT_SET_FIELD_PARAM_VALUE_LEN long + /// @return true: Value was encoded, false: type not supported or value not convertible + static bool variantToParamExtValue(const QVariant &value, uint8_t paramExtType, char *outValue); + static bool isValidChannel(uint8_t channel) { return (channel < MAVLINK_COMM_NUM_BUFFERS); } static bool isValidChannel(mavlink_channel_t channel) { return isValidChannel(static_cast(channel)); } diff --git a/src/MAVLink/QGCMAVLinkTypes.h b/src/MAVLink/QGCMAVLinkTypes.h index bcdc049b8449..e059f4e9a543 100644 --- a/src/MAVLink/QGCMAVLinkTypes.h +++ b/src/MAVLink/QGCMAVLinkTypes.h @@ -27,4 +27,6 @@ typedef struct __mavlink_camera_information_t mavlink_camera_information_t; typedef struct __mavlink_high_latency2_t mavlink_high_latency2_t; typedef struct __mavlink_event_t mavlink_event_t; typedef struct __mavlink_request_event_t mavlink_request_event_t; +typedef struct __mavlink_param_ext_value_t mavlink_param_ext_value_t; +typedef struct __mavlink_param_ext_ack_t mavlink_param_ext_ack_t; typedef struct param_union mavlink_param_union_t; diff --git a/test/FactSystem/ParameterManagerTest.cc b/test/FactSystem/ParameterManagerTest.cc index 75c8e6c0231e..98f02b72f46a 100644 --- a/test/FactSystem/ParameterManagerTest.cc +++ b/test/FactSystem/ParameterManagerTest.cc @@ -8,12 +8,19 @@ #include #include "BulkRefreshJob.h" +#include "MockLinkCamera.h" #include "MockLinkFTP.h" #include "MultiVehicleManager.h" #include "ParameterManager.h" #include "QGCMath.h" #include "Vehicle.h" +namespace { + /// Int typed ext param served by MockLinkCamera on MAV_COMP_ID_CAMERA + const QString kExtIntParam = QStringLiteral("CAM_EXPMODE"); + const QString kExtCustomParam = QStringLiteral("CAM_MODEL"); +} + // Call from tests that deliberately let PARAM_SET / PARAM_REQUEST_READ waits time out. void ParameterManagerTest::_ignoreParamResponseTimeouts() { @@ -21,6 +28,13 @@ void ParameterManagerTest::_ignoreParamResponseTimeouts() QRegularExpression("Timeout \".*WaitForParamResponseState\"")); } +// Call from tests that deliberately let PARAM_EXT_ACK waits time out. +void ParameterManagerTest::_ignoreExtParamAckTimeouts() +{ + ignoreLogMessage("Utilities.QGCStateMachine", QtWarningMsg, + QRegularExpression("Timeout \".*WaitForMavlinkMessageState.*\"")); +} + void ParameterManagerTest::cleanup() { // Some tests create MockLink directly (not via _connectMockLink), so we need special handling. @@ -634,3 +648,184 @@ void ParameterManagerTest::_bulkRefreshAllRetriesExhausted() _disconnectMockLink(); } + +// MockLinkCamera serves ext params on camera 1 only, so this also covers that components which +// don't answer PARAM_EXT_REQUEST_LIST are simply left out of the parameter view. +void ParameterManagerTest::_extParamsDownloaded() +{ + _connectMockLink(MAV_AUTOPILOT_PX4, MockConfiguration::FailNone, MockConfiguration::OptionEnableCamera); + QVERIFY(_vehicle); + ParameterManager* const paramManager = _vehicle->parameterManager(); + QVERIFY(paramManager); + + QVERIFY_TRUE_WAIT(paramManager->componentIds().contains(MAV_COMP_ID_CAMERA), TestTimeout::longMs()); + QVERIFY(!paramManager->componentIds().contains(MAV_COMP_ID_CAMERA2)); + + const QVector expectedParams = MockLinkCamera::defaultExtParams(); + for (const MockLinkCamera::ExtParam& expectedParam: expectedParams) { + if (expectedParam.type == MAV_PARAM_EXT_TYPE_CUSTOM) { + // Opaque bytes, deliberately not turned into a Fact. The mock serves one so the whole + // download still has to cope with it, including not re-requesting its index. + continue; + } + QVERIFY_TRUE_WAIT(paramManager->extParameterExists(MAV_COMP_ID_CAMERA, expectedParam.name), TestTimeout::mediumMs()); + Fact* const fact = paramManager->getParameter(MAV_COMP_ID_CAMERA, expectedParam.name); + QVERIFY(fact); + QCOMPARE(fact->rawValue().toString(), expectedParam.value.toString()); + } + + QVERIFY(!paramManager->extParameterExists(MAV_COMP_ID_CAMERA, kExtCustomParam)); + + // Ext params must not shadow or disturb the autopilot parameters + QVERIFY(paramManager->parametersReady()); + QVERIFY(!paramManager->extParameterExists(MAV_COMP_ID_AUTOPILOT1, QStringLiteral("CAM_EV"))); + + _disconnectMockLink(); +} + +Fact* ParameterManagerTest::_connectAndWaitForExtParams() +{ + _connectMockLink(MAV_AUTOPILOT_PX4, MockConfiguration::FailNone, MockConfiguration::OptionEnableCamera); + if (!_vehicle || !_vehicle->parameterManager()) { + return nullptr; + } + + ParameterManager* const paramManager = _vehicle->parameterManager(); + if (!UnitTest::waitForCondition([paramManager]() { return paramManager->extParameterExists(MAV_COMP_ID_CAMERA, kExtIntParam); }, + TestTimeout::longMs(), + QStringLiteral("ext params downloaded"))) { + return nullptr; + } + + return paramManager->getParameter(MAV_COMP_ID_CAMERA, kExtIntParam); +} + +void ParameterManagerTest::_extParamWrite() +{ + Fact* const fact = _connectAndWaitForExtParams(); + QVERIFY(fact); + ParameterManager* const paramManager = _vehicle->parameterManager(); + + QSignalSpy successSpy(paramManager, &ParameterManager::_paramSetSuccess); + QVERIFY(successSpy.isValid()); + + fact->setRawValue(3); + QVERIFY_SIGNAL_WAIT(successSpy, TestTimeout::mediumMs()); + QCOMPARE(_mockLink->mockLinkCamera()->extParamValue(kExtIntParam).toInt(), 3); + + // A re-read must go out over the ext protocol and come back with the value the camera stored + fact->containerSetRawValue(0); + paramManager->refreshParameter(MAV_COMP_ID_CAMERA, kExtIntParam); + QCOMPARE_TRUE_WAIT(fact->rawValue().toInt(), 3, TestTimeout::mediumMs()); + + _disconnectMockLink(); +} + +// A camera which refuses the value must leave the vehicle value in place and tell the user, +// rather than leaving the editor showing a value the camera never took. +void ParameterManagerTest::_extParamWriteRejected() +{ + Fact* const fact = _connectAndWaitForExtParams(); + QVERIFY(fact); + ParameterManager* const paramManager = _vehicle->parameterManager(); + + const int originalValue = fact->rawValue().toInt(); + _mockLink->mockLinkCamera()->setExtParamSetFailureMode(MockLinkCamera::FailExtParamSetRejected); + + QSignalSpy failureSpy(paramManager, &ParameterManager::_paramSetFailure); + QVERIFY(failureSpy.isValid()); + + expectAppMessage(QRegularExpression("Parameter write failed")); + fact->setRawValue(originalValue + 1); + QVERIFY_SIGNAL_WAIT(failureSpy, TestTimeout::mediumMs()); + verifyExpectedLogMessage(); + + QCOMPARE(_mockLink->mockLinkCamera()->extParamValue(kExtIntParam).toInt(), originalValue); + QCOMPARE_TRUE_WAIT(fact->rawValue().toInt(), originalValue, TestTimeout::mediumMs()); + + _disconnectMockLink(); +} + +// Retries must not leave the pending write count stranded, which would keep warning the user +// about unsaved parameters on app close. +void ParameterManagerTest::_extParamWriteNoAck() +{ + _ignoreExtParamAckTimeouts(); + + Fact* const fact = _connectAndWaitForExtParams(); + QVERIFY(fact); + ParameterManager* const paramManager = _vehicle->parameterManager(); + + const int originalValue = fact->rawValue().toInt(); + _mockLink->mockLinkCamera()->setExtParamSetFailureMode(MockLinkCamera::FailExtParamSetNoAck); + + QSignalSpy failureSpy(paramManager, &ParameterManager::_paramSetFailure); + QVERIFY(failureSpy.isValid()); + + expectAppMessage(QRegularExpression("Parameter write failed")); + fact->setRawValue(originalValue + 1); + + const int maxWaitMs = ParameterManager::kWaitForParamValueAckMs * (ParameterManager::kParamSetRetryCount + 1) + + TestTimeout::mediumMs(); + QVERIFY_SIGNAL_WAIT(failureSpy, maxWaitMs); + verifyExpectedLogMessage(); + + QVERIFY_TRUE_WAIT(!paramManager->pendingWrites(), TestTimeout::mediumMs()); + + _disconnectMockLink(); +} + +// PARAM_ACK_IN_PROGRESS must not wedge or abandon the write. The delayed second ack a camera +// would send cannot be simulated here - the test ack timeout is shorter than the mock's task +// tick - so this covers the retry that follows instead. +void ParameterManagerTest::_extParamWriteInProgress() +{ + _ignoreExtParamAckTimeouts(); + + Fact* const fact = _connectAndWaitForExtParams(); + QVERIFY(fact); + ParameterManager* const paramManager = _vehicle->parameterManager(); + + const int newValue = fact->rawValue().toInt() + 1; + _mockLink->mockLinkCamera()->setExtParamSetFailureMode(MockLinkCamera::FailExtParamSetInProgress); + + QSignalSpy successSpy(paramManager, &ParameterManager::_paramSetSuccess); + QSignalSpy failureSpy(paramManager, &ParameterManager::_paramSetFailure); + QVERIFY(successSpy.isValid()); + QVERIFY(failureSpy.isValid()); + + fact->setRawValue(newValue); + + const int maxWaitMs = ParameterManager::kWaitForParamValueAckMs * (ParameterManager::kParamSetRetryCount + 1) + + TestTimeout::mediumMs(); + QVERIFY_SIGNAL_WAIT(successSpy, maxWaitMs); + QCOMPARE(failureSpy.count(), 0); + QCOMPARE(_mockLink->mockLinkCamera()->extParamValue(kExtIntParam).toInt(), newValue); + + _disconnectMockLink(); +} + +// A value lost from the PARAM_EXT_REQUEST_LIST stream must be picked up by the indexed re-request, +// otherwise a dropped packet silently hides a parameter from the editor. +void ParameterManagerTest::_extParamMissingIndexRetry() +{ + // The drop has to be configured before the link starts. The list is streamed during initial + // connect, which _connectMockLink waits for, so a drop set from the test body would come too + // late and the parameter would already be present. + _connectMockLink(MAV_AUTOPILOT_PX4, MockConfiguration::FailNone, + MockConfiguration::OptionEnableCamera | MockConfiguration::OptionDropFirstExtParam); + QVERIFY(_vehicle); + ParameterManager* const paramManager = _vehicle->parameterManager(); + QVERIFY(paramManager); + QVERIFY(_mockLink->mockLinkCamera()); + + QCOMPARE(MockLinkCamera::defaultExtParams().at(0).name, kExtIntParam); + + QVERIFY_TRUE_WAIT(paramManager->extParameterExists(MAV_COMP_ID_CAMERA, kExtIntParam), TestTimeout::longMs()); + + // Asserting the parameter was absent first would race the retry timer. The indexed read count + // proves the same thing without a timing window: the value can only have arrived that way. + QVERIFY(_mockLink->mockLinkCamera()->extParamIndexedReadCount() > 0); + + _disconnectMockLink(); +} diff --git a/test/FactSystem/ParameterManagerTest.h b/test/FactSystem/ParameterManagerTest.h index 031eeff6227e..4229057cc8ac 100644 --- a/test/FactSystem/ParameterManagerTest.h +++ b/test/FactSystem/ParameterManagerTest.h @@ -2,6 +2,8 @@ #include "BaseClasses/VehicleTestManualConnect.h" +class Fact; + class ParameterManagerTest : public VehicleTestManualConnect { Q_OBJECT @@ -28,9 +30,19 @@ private slots: void _bulkRefreshUnknownNameSkipped(); void _bulkRefreshRetrySucceeds(); void _bulkRefreshAllRetriesExhausted(); + void _extParamsDownloaded(); + void _extParamWrite(); + void _extParamWriteRejected(); + void _extParamWriteNoAck(); + void _extParamWriteInProgress(); + void _extParamMissingIndexRetry(); private: + /// Connects a camera-enabled MockLink and waits for the ext params to arrive. + /// @return The CAM_EXPMODE fact, nullptr on failure + Fact *_connectAndWaitForExtParams(); void _ignoreParamResponseTimeouts(); + void _ignoreExtParamAckTimeouts(); void _noFailureWorker(MockConfiguration::FailureMode_t failureMode); void _setParamWithFailureMode(MockLink::ParamSetFailureMode_t failureMode, bool expectSuccess, const QString ¶mName, MAV_AUTOPILOT autopilot);