From 4d66ea3d58585b563446b91c045cf09248111918 Mon Sep 17 00:00:00 2001 From: Gus Grubba Date: Tue, 19 Sep 2017 15:57:20 -0400 Subject: [PATCH] Adding support for custom value. --- src/Camera/QGCCameraControl.cc | 4 +++ src/Camera/QGCCameraIO.cc | 25 +++++++++++++------ src/Camera/QGCCameraIO.h | 15 +++++++++++ src/FactSystem/FactMetaData.cc | 25 +++++++++++++++++-- src/FactSystem/FactMetaData.h | 1 + .../PX4/PX4ParameterMetaData.cc | 10 ++++++-- 6 files changed, 68 insertions(+), 12 deletions(-) diff --git a/src/Camera/QGCCameraControl.cc b/src/Camera/QGCCameraControl.cc index 627fbefff..e56bea933 100644 --- a/src/Camera/QGCCameraControl.cc +++ b/src/Camera/QGCCameraControl.cc @@ -603,6 +603,10 @@ QGCCameraControl::_loadSettings(const QDomNodeList nodeList) qCritical() << QString("Unknown type for parameter %1").arg(factName); return false; } + //-- By definition, custom types do not have control + if(factType == FactMetaData::valueTypeCustom) { + control = false; + } //-- Description QString description; if(!read_value(parameterNode, kDescription, description)) { diff --git a/src/Camera/QGCCameraIO.cc b/src/Camera/QGCCameraIO.cc index 1b52fa203..16e40eab8 100644 --- a/src/Camera/QGCCameraIO.cc +++ b/src/Camera/QGCCameraIO.cc @@ -55,6 +55,9 @@ QGCCameraParamIO::QGCCameraParamIO(QGCCameraControl *control, Fact* fact, Vehicl case FactMetaData::valueTypeFloat: _mavParamType = MAV_PARAM_TYPE_REAL32; break; + case FactMetaData::valueTypeCustom: + _mavParamType = (MAV_PARAM_TYPE)11; // MAV_PARAM_TYPE_CUSTOM; + break; default: qWarning() << "Unsupported fact type" << _fact->type() << "for" << _fact->name(); // Fall through @@ -111,11 +114,10 @@ QGCCameraParamIO::sendParameter(bool updateUI) void QGCCameraParamIO::_sendParameter() { - //-- TODO: We should use something other than mavlink_param_union_t for PARAM_EXT_SET mavlink_param_ext_set_t p; memset(&p, 0, sizeof(mavlink_param_ext_set_t)); - mavlink_param_union_t union_value; - mavlink_message_t msg; + param_ext_union_t union_value; + mavlink_message_t msg; FactMetaData::ValueType_t factType = _fact->type(); p.param_type = _mavParamType; switch (factType) { @@ -138,6 +140,12 @@ QGCCameraParamIO::_sendParameter() case FactMetaData::valueTypeFloat: union_value.param_float = _fact->rawValue().toFloat(); break; + case FactMetaData::valueTypeCustom: + { + QByteArray custom = _fact->rawValue().toByteArray(); + memcpy(union_value.bytes, custom.data(), std::max(custom.size(), MAVLINK_MSG_PARAM_EXT_SET_FIELD_PARAM_VALUE_LEN)); + } + break; default: qCritical() << "Unsupported fact type" << factType << "for" << _fact->name(); // fall through @@ -145,7 +153,7 @@ QGCCameraParamIO::_sendParameter() union_value.param_int32 = (int32_t)_fact->rawValue().toInt(); break; } - memcpy(&p.param_value[0], &union_value.param_float, sizeof(float)); + memcpy(&p.param_value[0], &union_value.bytes[0], MAVLINK_MSG_PARAM_EXT_SET_FIELD_PARAM_VALUE_LEN); p.target_system = (uint8_t)_vehicle->id(); p.target_component = (uint8_t)_control->compID(); strncpy(p.param_id, _fact->name().toStdString().c_str(), MAVLINK_MSG_PARAM_EXT_SET_FIELD_PARAM_ID_LEN); @@ -239,11 +247,9 @@ QGCCameraParamIO::handleParamValue(const mavlink_param_ext_value_t& value) QVariant QGCCameraParamIO::_valueFromMessage(const char* value, uint8_t param_type) { - //-- TODO: Even though we don't use anything larger than 32-bit, this should - // probably be updated. QVariant var; - mavlink_param_union_t u; - memcpy(&u.param_float, value, sizeof(float)); + param_ext_union_t u; + memcpy(u.bytes, value, MAVLINK_MSG_PARAM_EXT_SET_FIELD_PARAM_VALUE_LEN); switch (param_type) { case MAV_PARAM_TYPE_REAL32: var = QVariant(u.param_float); @@ -266,6 +272,9 @@ QGCCameraParamIO::_valueFromMessage(const char* value, uint8_t param_type) case MAV_PARAM_TYPE_INT32: var = QVariant(u.param_int32); break; + case 11: //MAV_PARAM_TYPE_CUSTOM: + var = QVariant(QByteArray(value, MAVLINK_MSG_PARAM_EXT_SET_FIELD_PARAM_VALUE_LEN)); + break; default: var = QVariant(0); qCritical() << "Invalid param_type used for camera setting:" << param_type; diff --git a/src/Camera/QGCCameraIO.h b/src/Camera/QGCCameraIO.h index 94cdc2717..a3f4d57a2 100644 --- a/src/Camera/QGCCameraIO.h +++ b/src/Camera/QGCCameraIO.h @@ -15,6 +15,21 @@ class QGCCameraControl; Q_DECLARE_LOGGING_CATEGORY(CameraIOLog) Q_DECLARE_LOGGING_CATEGORY(CameraIOLogVerbose) +MAVPACKED( +typedef struct { + union { + float param_float; + int32_t param_int32; + uint32_t param_uint32; + int16_t param_int16; + uint16_t param_uint16; + int8_t param_int8; + uint8_t param_uint8; + uint8_t bytes[MAVLINK_MSG_PARAM_EXT_SET_FIELD_PARAM_VALUE_LEN]; + }; + uint8_t type; +}) param_ext_union_t; + //----------------------------------------------------------------------------- class QGCCameraParamIO : public QObject { diff --git a/src/FactSystem/FactMetaData.cc b/src/FactSystem/FactMetaData.cc index a29bc6387..0c42c137a 100644 --- a/src/FactSystem/FactMetaData.cc +++ b/src/FactSystem/FactMetaData.cc @@ -240,6 +240,8 @@ QVariant FactMetaData::_minForType(void) const return QVariant(0); case valueTypeElapsedTimeInSeconds: return QVariant(0.0); + case valueTypeCustom: + return QVariant(); } // Make windows compiler happy, even switch is full cased @@ -270,6 +272,8 @@ QVariant FactMetaData::_maxForType(void) const return QVariant(); case valueTypeBool: return QVariant(1); + case valueTypeCustom: + return QVariant(); } // Make windows compiler happy, even switch is full cased @@ -328,6 +332,10 @@ bool FactMetaData::convertAndValidateRaw(const QVariant& rawValue, bool convertO convertOk = true; typedValue = QVariant(rawValue.toBool()); break; + case FactMetaData::valueTypeCustom: + convertOk = true; + typedValue = QVariant(rawValue.toByteArray()); + break; } if (!convertOk) { @@ -389,6 +397,10 @@ bool FactMetaData::convertAndValidateCooked(const QVariant& cookedValue, bool co convertOk = true; typedValue = QVariant(cookedValue.toBool()); break; + case FactMetaData::valueTypeCustom: + convertOk = true; + typedValue = QVariant(cookedValue.toByteArray()); + break; } if (!convertOk) { @@ -455,6 +467,10 @@ bool FactMetaData::clampValue(const QVariant& cookedValue, QVariant& typedValue) convertOk = true; typedValue = QVariant(cookedValue.toBool()); break; + case FactMetaData::valueTypeCustom: + convertOk = true; + typedValue = QVariant(cookedValue.toByteArray()); + break; } return convertOk; } @@ -693,7 +709,8 @@ FactMetaData::ValueType_t FactMetaData::stringToType(const QString& typeString, << QStringLiteral("Double") << QStringLiteral("String") << QStringLiteral("Bool") - << QStringLiteral("ElapsedSeconds"); + << QStringLiteral("ElapsedSeconds") + << QStringLiteral("Custom"); knownTypes << valueTypeUint8 << valueTypeInt8 @@ -705,7 +722,8 @@ FactMetaData::ValueType_t FactMetaData::stringToType(const QString& typeString, << valueTypeDouble << valueTypeString << valueTypeBool - << valueTypeElapsedTimeInSeconds; + << valueTypeElapsedTimeInSeconds + << valueTypeCustom; for (int i=0; i #include +static const char* kInvalidConverstion = "Internal Error: No support for string parameters"; + QGC_LOGGING_CATEGORY(PX4ParameterMetaDataLog, "PX4ParameterMetaDataLog") PX4ParameterMetaData::PX4ParameterMetaData(void) @@ -57,13 +59,17 @@ QVariant PX4ParameterMetaData::_stringToTypedVariant(const QString& string, Fact convertTo = QVariant::Double; break; case FactMetaData::valueTypeString: - qWarning() << "Internal Error: No support for string parameters"; + qWarning() << kInvalidConverstion; convertTo = QVariant::String; break; case FactMetaData::valueTypeBool: - qWarning() << "Internal Error: No support for string parameters"; + qWarning() << kInvalidConverstion; convertTo = QVariant::Bool; break; + case FactMetaData::valueTypeCustom: + qWarning() << kInvalidConverstion; + convertTo = QVariant::ByteArray; + break; } *convertOk = var.convert(convertTo); -- 2.22.0