Add PARAM_EXT to parameter tab

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<uint8_t>(_fact->rawValue().toUInt(&ok));
-            break;
-        case FactMetaData::valueTypeInt8:
-            union_value.param_int8 = static_cast<int8_t>(_fact->rawValue().toInt(&ok));
-            break;
-        case FactMetaData::valueTypeUint16:
-            union_value.param_uint16 = static_cast<uint16_t>(_fact->rawValue().toUInt(&ok));
-            break;
-        case FactMetaData::valueTypeInt16:
-            union_value.param_int16 = static_cast<int16_t>(_fact->rawValue().toInt(&ok));
-            break;
-        case FactMetaData::valueTypeUint32:
-            union_value.param_uint32 = static_cast<uint32_t>(_fact->rawValue().toUInt(&ok));
-            break;
-        case FactMetaData::valueTypeInt64:
-            union_value.param_int64 = static_cast<int64_t>(_fact->rawValue().toLongLong(&ok));
-            break;
-        case FactMetaData::valueTypeUint64:
-            union_value.param_uint64 = static_cast<uint64_t>(_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<size_t>(std::max(custom.size(), static_cast<qsizetype>(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<int32_t>(_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<uint8_t>(_vehicle->id());
         p.target_component = static_cast<uint8_t>(_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<quint64>(u.param_uint64));
-            break;
-        case MAV_PARAM_EXT_TYPE_INT64:
-            var = QVariant(static_cast<qint64>(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)

src/Camera/VehicleCameraControl.cc

@@ -1295,7 +1295,8 @@ void VehicleCameraControl::handleParamExtAck(const mavlink_param_ext_ack_t& para
         << "\n\tType:" << static_cast<int>(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<int>(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]) {

src/Comms/MockLink/MockLink.cc

@@ -3259,6 +3259,11 @@ MockLinkFTP *MockLink::mockLinkFTP() const
     return _mockLinkFTP;
 }
 
+MockLinkCamera *MockLink::mockLinkCamera() const
+{
+    return _mockLinkCamera;
+}
+
 void MockLink::_sendAvailableMode(uint8_t modeIndexOneBased)
 {
     if (modeIndexOneBased < 1 || modeIndexOneBased > _availableModesCount()) {

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; }

src/Comms/MockLink/MockLinkCamera.cc

@@ -3,6 +3,7 @@
 #include "MockLink.h"
 #include "MissionManager/MissionCommandTree.h"
 #include "QGCLoggingCategory.h"
+#include "QGCMAVLink.h"
 
 #include <QtCore/QDateTime>
 #include <QtCore/QLoggingCategory>
@@ -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::ExtParam> 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,154 @@ 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) {
+        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<uint16_t>(_extParams.count());
+    paramExtValue.param_index = static_cast<uint16_t>(index);
+    (void) QGCMAVLink::variantToParamExtValue(extParam.value, extParam.type, &paramExtValue.param_value[0]);
+
+    mavlink_message_t msg{};
+    (void) mavlink_msg_param_ext_value_encode_chan(
+        _mockLink->vehicleId(),
+        kExtParamCompId,
+        _mockLink->outgoingMavlinkChannel(),
+        &msg,
+        &paramExtValue);
+    _mockLink->respondWithMavlinkMessage(msg);
+}