From b974f3501163b4041942aea244317b6100faa9ed Mon Sep 17 00:00:00 2001 From: Don Gagne Date: Wed, 7 Nov 2018 12:43:32 -0800 Subject: [PATCH] Support parameters from multiple components --- src/comm/MockLink.cc | 24 +++++++++++++----------- src/comm/MockLink.h | 4 ++-- src/comm/PX4MockLink.params | 1 + 3 files changed, 16 insertions(+), 13 deletions(-) diff --git a/src/comm/MockLink.cc b/src/comm/MockLink.cc index 53a381b81..f1c600c27 100644 --- a/src/comm/MockLink.cc +++ b/src/comm/MockLink.cc @@ -257,6 +257,7 @@ void MockLink::_loadParams(void) QStringList paramData = line.split("\t"); Q_ASSERT(paramData.count() == 5); + int compId = paramData.at(1).toInt(); QString paramName = paramData.at(2); QString valStr = paramData.at(3); uint paramType = paramData.at(4).toUInt(); @@ -292,8 +293,8 @@ void MockLink::_loadParams(void) qCDebug(MockLinkVerboseLog) << "Loading param" << paramName << paramValue; - _mapParamName2Value[_vehicleComponentId][paramName] = paramValue; - _mapParamName2MavParamType[paramName] = static_cast(paramType); + _mapParamName2Value[compId][paramName] = paramValue; + _mapParamName2MavParamType[compId][paramName] = static_cast(paramType); } } @@ -515,11 +516,11 @@ void MockLink::_setParamFloatUnionIntoMap(int componentId, const QString& paramN Q_ASSERT(_mapParamName2Value.contains(componentId)); Q_ASSERT(_mapParamName2Value[componentId].contains(paramName)); - Q_ASSERT(_mapParamName2MavParamType.contains(paramName)); + Q_ASSERT(_mapParamName2MavParamType[componentId].contains(paramName)); valueUnion.param_float = paramFloat; - MAV_PARAM_TYPE paramType = _mapParamName2MavParamType[paramName]; + MAV_PARAM_TYPE paramType = _mapParamName2MavParamType[componentId][paramName]; QVariant paramVariant; @@ -569,9 +570,9 @@ float MockLink::_floatUnionForParam(int componentId, const QString& paramName) Q_ASSERT(_mapParamName2Value.contains(componentId)); Q_ASSERT(_mapParamName2Value[componentId].contains(paramName)); - Q_ASSERT(_mapParamName2MavParamType.contains(paramName)); + Q_ASSERT(_mapParamName2MavParamType[componentId].contains(paramName)); - MAV_PARAM_TYPE paramType = _mapParamName2MavParamType[paramName]; + MAV_PARAM_TYPE paramType = _mapParamName2MavParamType[componentId][paramName]; QVariant paramVar = _mapParamName2Value[componentId][paramName]; switch (paramType) { @@ -677,9 +678,9 @@ void MockLink::_paramRequestListWorker(void) mavlink_message_t responseMsg; Q_ASSERT(_mapParamName2Value[componentId].contains(paramName)); - Q_ASSERT(_mapParamName2MavParamType.contains(paramName)); + Q_ASSERT(_mapParamName2MavParamType[componentId].contains(paramName)); - MAV_PARAM_TYPE paramType = _mapParamName2MavParamType[paramName]; + MAV_PARAM_TYPE paramType = _mapParamName2MavParamType[componentId][paramName]; Q_ASSERT(paramName.length() <= MAVLINK_MSG_ID_PARAM_VALUE_LEN); strncpy(paramId, paramName.toLocal8Bit().constData(), MAVLINK_MSG_ID_PARAM_VALUE_LEN); @@ -726,8 +727,9 @@ void MockLink::_handleParamSet(const mavlink_message_t& msg) qCDebug(MockLinkLog) << "_handleParamSet" << componentId << paramId << request.param_type; Q_ASSERT(_mapParamName2Value.contains(componentId)); + Q_ASSERT(_mapParamName2MavParamType.contains(componentId)); Q_ASSERT(_mapParamName2Value[componentId].contains(paramId)); - Q_ASSERT(request.param_type == _mapParamName2MavParamType[paramId]); + Q_ASSERT(request.param_type == _mapParamName2MavParamType[componentId][paramId]); // Save the new value _setParamFloatUnionIntoMap(componentId, paramId, request.param_value); @@ -795,7 +797,7 @@ void MockLink::_handleParamRequestRead(const mavlink_message_t& msg) } Q_ASSERT(_mapParamName2Value[componentId].contains(paramId)); - Q_ASSERT(_mapParamName2MavParamType.contains(paramId)); + Q_ASSERT(_mapParamName2MavParamType[componentId].contains(paramId)); if (_failureMode == MockConfiguration::FailMissingParamOnAllRequests && strcmp(paramId, _failParam) == 0) { qCDebug(MockLinkLog) << "Ignoring request read for " << _failParam; @@ -809,7 +811,7 @@ void MockLink::_handleParamRequestRead(const mavlink_message_t& msg) &responseMsg, // Outgoing message paramId, // Parameter name _floatUnionForParam(componentId, paramId), // Parameter value - _mapParamName2MavParamType[paramId], // Parameter type + _mapParamName2MavParamType[componentId][paramId], // Parameter type _mapParamName2Value[componentId].count(), // Total number of parameters _mapParamName2Value[componentId].keys().indexOf(paramId)); // Index of this parameter respondWithMavlinkMessage(responseMsg); diff --git a/src/comm/MockLink.h b/src/comm/MockLink.h index 4c6de8308..3800e5fac 100644 --- a/src/comm/MockLink.h +++ b/src/comm/MockLink.h @@ -216,8 +216,8 @@ private: bool _inNSH; bool _mavlinkStarted; - QMap > _mapParamName2Value; - QMap _mapParamName2MavParamType; + QMap> _mapParamName2Value; + QMap> _mapParamName2MavParamType; uint8_t _mavBaseMode; uint32_t _mavCustomMode; diff --git a/src/comm/PX4MockLink.params b/src/comm/PX4MockLink.params index 39592dac7..871d08b17 100644 --- a/src/comm/PX4MockLink.params +++ b/src/comm/PX4MockLink.params @@ -737,3 +737,4 @@ 1 1 TRIG_MODE 4 6 1 1 VT_B_DEC_MSS 2.000000000000000000 9 1 1 VT_B_REV_DEL 0.000000000000000000 9 +1 2 VT_B_REV_DEL 0.000000000000000000 9 -- 2.22.0