Skip to content

Q
qgroundcontrol
Commit d5e2f29c authored by Don Gagne's avatar Don Gagne
Browse files
Options

Update to new ParameterLoader and component id support

parent 4bfb3701
Show whitespace changes
Inline Side-by-side
Showing with 214 additions and 150 deletions
src/AutoPilotPlugins/AutoPilotPlugin.cc
View file @ d5e2f29c
......@@ -34,42 +34,22 @@ AutoPilotPlugin::AutoPilotPlugin(UASInterface* uas, QObject* parent) :
Q_ASSERT(_uas);
}
void AutoPilotPlugin::refreshAllParameters(void)
bool AutoPilotPlugin::factExists(FactSystem::Provider_t provider, int componentId, const QString& name)
{
Q_ASSERT(_uas);
QGCUASParamManagerInterface* paramMgr = _uas->getParamManager();
Q_ASSERT(paramMgr);
paramMgr->requestParameterList();
}
void AutoPilotPlugin::refreshParameter(const QString& param)
{
Q_ASSERT(_uas);
QGCUASParamManagerInterface* paramMgr = _uas->getParamManager();
Q_ASSERT(paramMgr);
QList<int> compIdList = paramMgr->getComponentForParam(param);
Q_ASSERT(compIdList.count() > 0);
paramMgr->requestParameterUpdate(compIdList[0], param);
}
switch (provider) {
case FactSystem::ParameterProvider:
return getParameterLoader()->factExists(componentId, name);
void AutoPilotPlugin::refreshParametersPrefix(const QString& paramPrefix)
{
foreach(QVariant varFact, parameters()) {
Fact* fact = qvariant_cast<Fact*>(varFact);
Q_ASSERT(fact);
if (fact->name().startsWith(paramPrefix)) {
refreshParameter(fact->name());
}
// Other providers will go here once they come online
}
}
bool AutoPilotPlugin::factExists(const QString& param)
Fact* AutoPilotPlugin::getFact(FactSystem::Provider_t provider, int componentId, const QString& name)
{
return parameters().contains(param);
}
switch (provider) {
case FactSystem::ParameterProvider:
return getParameterLoader()->getFact(componentId, name);
Fact* AutoPilotPlugin::getFact(const QString& name)
{
return parameters()[name].value<Fact*>();
// Other providers will go here once they come online
}
}
src/AutoPilotPlugins/AutoPilotPlugin.h
View file @ d5e2f29c
......@@ -35,6 +35,7 @@
#include "UASInterface.h"
#include "VehicleComponent.h"
#include "FactSystem.h"
#include "ParameterLoader.h"
/// This is the base class for AutoPilot plugins
///
......@@ -52,26 +53,43 @@ public:
Q_PROPERTY(bool pluginReady READ pluginReady NOTIFY pluginReadyChanged)
Q_PROPERTY(QVariantList components READ components CONSTANT)
Q_PROPERTY(QUrl setupBackgroundImage READ setupBackgroundImage CONSTANT)
/// List of VehicleComponent objects
Q_PROPERTY(QVariantList vehicleComponents READ vehicleComponents CONSTANT)
/// Re-request the full set of parameters from the autopilot
Q_INVOKABLE void refreshAllParameters(void);
Q_INVOKABLE void refreshAllParameters(void) { getParameterLoader()->refreshAllParameters(); }
/// Request a refresh on the specific parameter
Q_INVOKABLE void refreshParameter(const QString& param);
Q_INVOKABLE void refreshParameter(int componentId, const QString& name) { getParameterLoader()->refreshParameter(componentId, name); }
// Request a refresh on all parameters that begin with the specified prefix
Q_INVOKABLE void refreshParametersPrefix(const QString& paramPrefix);
/// Request a refresh on all parameters that begin with the specified prefix
Q_INVOKABLE void refreshParametersPrefix(int componentId, const QString& namePrefix) { getParameterLoader()->refreshParametersPrefix(componentId, namePrefix); }
Q_INVOKABLE bool factExists(const QString& param);
/// Returns true if the specifed fact exists
Q_INVOKABLE bool factExists(FactSystem::Provider_t provider, ///< fact provider
int componentId, ///< fact component, -1=default component
const QString& name); ///< fact name
Fact* getFact(const QString& name);
/// Returns the specified Fact.
/// WARNING: Will assert if fact does not exists. If that possibility exists, check for existince first with
/// factExists.
Fact* getFact(FactSystem::Provider_t provider, ///< fact provider
int componentId, ///< fact component, -1=default component
const QString& name); ///< fact name
// Property accessors
virtual const QVariantList& components(void) = 0;
virtual const QVariantMap& parameters(void) = 0;
virtual QUrl setupBackgroundImage(void) = 0;
/// Returns true if the specifed parameter exists from the default component
Q_INVOKABLE bool parameterExists(const QString& name) { return getParameterLoader()->factExists(FactSystem::defaultComponentId, name); }
/// Returns the specified parameter Fact from the default component
/// WARNING: Will assert if fact does not exists. If that possibility exists, check for existince first with
/// factExists.
Fact* getParameterFact(const QString& name) { return getParameterLoader()->getFact(FactSystem::defaultComponentId, name); }
// Must be implemented by derived class
virtual const QVariantList& vehicleComponents(void) = 0;
/// Returns the ParameterLoader
virtual ParameterLoader* getParameterLoader(void) = 0;
/// FIXME: Kind of hacky
static void clearStaticData(void);
......
src/AutoPilotPlugins/PX4/FlightModesComponentController.cc
View file @ d5e2f29c
......@@ -69,7 +69,9 @@ void FlightModesComponentController::_validateConfiguration(void)
{
_validConfiguration = true;
_channelCount = _autoPilotPlugin->factExists("RC_CHAN_CNT") ? _autoPilotPlugin->getFact("RC_CHAN_CNT")->value().toInt() : _chanMax;
_channelCount = _autoPilotPlugin->parameterExists("RC_CHAN_CNT") ?
_autoPilotPlugin->getParameterFact("RC_CHAN_CNT")->value().toInt() :
_chanMax;
if (_channelCount <= 0 || _channelCount > _chanMax) {
// Parameter exists, but has not yet been set or is invalid. Use default
_channelCount = _chanMax;
......@@ -84,7 +86,7 @@ void FlightModesComponentController::_validateConfiguration(void)
switchNames << "Mode Switch" << "Return Switch" << "Loiter Switch" << "PosCtl Switch";
for(int i=0; i<switchParams.count(); i++) {
int map = _autoPilotPlugin->getFact(switchParams[i])->value().toInt();
int map = _autoPilotPlugin->getParameterFact(switchParams[i])->value().toInt();
switchMappings << map;
if (map < 0 || map > _channelCount) {
......@@ -101,7 +103,7 @@ void FlightModesComponentController::_validateConfiguration(void)
attitudeNames << "Throttle" << "Yaw" << "Pitch" << "Roll" << "Flaps" << "Aux1" << "Aux2";
for (int i=0; i<attitudeParams.count(); i++) {
int map = _autoPilotPlugin->getFact(attitudeParams[i])->value().toInt();
int map = _autoPilotPlugin->getParameterFact(attitudeParams[i])->value().toInt();
for (int j=0; j<switchParams.count(); j++) {
if (map != 0 && map == switchMappings[j]) {
......@@ -126,10 +128,10 @@ void FlightModesComponentController::setSendLiveRCSwitchRanges(bool start)
QVariant value;
_rgRCMin[i] = _autoPilotPlugin->getFact(rcMinParam)->value().toInt();
_rgRCMax[i] = _autoPilotPlugin->getFact(rcMaxParam)->value().toInt();
_rgRCMin[i] = _autoPilotPlugin->getParameterFact(rcMinParam)->value().toInt();
_rgRCMax[i] = _autoPilotPlugin->getParameterFact(rcMaxParam)->value().toInt();
float floatReversed = _autoPilotPlugin->getFact(rcRevParam)->value().toFloat();
float floatReversed = _autoPilotPlugin->getParameterFact(rcRevParam)->value().toFloat();
_rgRCReversed[i] = floatReversed == -1.0f;
}
......@@ -170,7 +172,7 @@ double FlightModesComponentController::_switchLiveRange(const QString& param)
{
QVariant value;
int channel = _autoPilotPlugin->getFact(param)->value().toInt();
int channel = _autoPilotPlugin->getParameterFact(param)->value().toInt();
if (channel == 0) {
return 1.0;
} else {
......
src/AutoPilotPlugins/PX4/SensorsComponentController.cc
View file @ d5e2f29c
......@@ -268,15 +268,15 @@ void SensorsComponentController::_handleUASTextMessage(int uasId, int compId, in
void SensorsComponentController::_refreshParams(void)
{
// Pull specified params first so red/green indicators update quickly
_autopilot->refreshParameter("CAL_MAG0_ID");
_autopilot->refreshParameter("CAL_GYRO0_ID");
_autopilot->refreshParameter("CAL_ACC0_ID");
_autopilot->refreshParameter("SENS_DPRES_OFF");
_autopilot->refreshParameter("CAL_MAG0_ROT");
_autopilot->refreshParameter("CAL_MAG1_ROT");
_autopilot->refreshParameter("CAL_MAG2_ROT");
_autopilot->refreshParameter("SENS_BOARD_ROT");
_autopilot->refreshParameter(FactSystem::defaultComponentId, "CAL_MAG0_ID");
_autopilot->refreshParameter(FactSystem::defaultComponentId, "CAL_GYRO0_ID");
_autopilot->refreshParameter(FactSystem::defaultComponentId, "CAL_ACC0_ID");
_autopilot->refreshParameter(FactSystem::defaultComponentId, "SENS_DPRES_OFF");
_autopilot->refreshParameter(FactSystem::defaultComponentId, "CAL_MAG0_ROT");
_autopilot->refreshParameter(FactSystem::defaultComponentId, "CAL_MAG1_ROT");
_autopilot->refreshParameter(FactSystem::defaultComponentId, "CAL_MAG2_ROT");
_autopilot->refreshParameter(FactSystem::defaultComponentId, "SENS_BOARD_ROT");
// Pull full set in order to get all cal values back
_autopilot->refreshAllParameters();
......
src/FactSystem/Fact.cc
View file @ d5e2f29c
......@@ -28,9 +28,10 @@
#include <QtQml>
Fact::Fact(QString name, FactMetaData::ValueType_t type, QObject* parent) :
Fact::Fact(int componentId, QString name, FactMetaData::ValueType_t type, QObject* parent) :
QObject(parent),
_name(name),
_componentId(componentId),
_type(type),
_metaData(NULL)
{
......@@ -75,6 +76,11 @@ QString Fact::name(void) const
return _name;
}
int Fact::componentId(void) const
{
return _componentId;
}
QVariant Fact::value(void) const
{
return _value;
......
src/FactSystem/Fact.h
View file @ d5e2f29c
......@@ -44,6 +44,7 @@ class Fact : public QObject
Q_OBJECT
Q_PROPERTY(QString name READ name CONSTANT)
Q_PROPERTY(int componentId READ componentId CONSTANT)
Q_PROPERTY(QVariant value READ value WRITE setValue NOTIFY valueChanged USER true)
Q_PROPERTY(QVariant valueString READ valueString NOTIFY valueChanged)
Q_PROPERTY(QVariant defaultValue READ defaultValue CONSTANT)
......@@ -57,13 +58,17 @@ class Fact : public QObject
Q_ENUMS(FactMetaData::ValueType_t)
public:
Fact(QString name = "", FactMetaData::ValueType_t type = FactMetaData::valueTypeInt32, QObject* parent = NULL);
//Fact(int componentId, QString name = "", FactMetaData::ValueType_t type = FactMetaData::valueTypeInt32, QObject* parent = NULL);
Fact(int componentId, QString name, FactMetaData::ValueType_t type, QObject* parent = NULL);
// Property system methods
/// Read accessor or name property
/// Read accessor for name property
QString name(void) const;
/// Read accessor for componentId property
int componentId(void) const;
/// Read accessor for value property
QVariant value(void) const;
......@@ -112,6 +117,7 @@ signals:
private:
QString _name;
int _componentId;
QVariant _value;
FactMetaData::ValueType_t _type;
FactMetaData* _metaData;
......
src/FactSystem/FactBinder.cc
View file @ d5e2f29c
......@@ -31,7 +31,8 @@
FactBinder::FactBinder(void) :
_autopilotPlugin(NULL),
_fact(NULL)
_fact(NULL),
_componentId(FactSystem::defaultComponentId)
{
UASInterface* uas = UASManager::instance()->getActiveUAS();
Q_ASSERT(uas);
......@@ -58,8 +59,8 @@ void FactBinder::setName(const QString& name)
}
if (!name.isEmpty()) {
if (_autopilotPlugin->factExists(name)) {
_fact = _autopilotPlugin->getFact(name);
if (_autopilotPlugin->factExists(FactSystem::ParameterProvider, _componentId, name)) {
_fact = _autopilotPlugin->getFact(FactSystem::ParameterProvider, _componentId, name);
connect(_fact, &Fact::valueChanged, this, &FactBinder::valueChanged);
emit valueChanged();
......
src/FactSystem/FactBinder.h
View file @ d5e2f29c
......@@ -38,6 +38,7 @@ class FactBinder : public QObject
{
Q_OBJECT
Q_PROPERTY(int componentId MEMBER _componentId NOTIFY componentIdChanged)
Q_PROPERTY(QString name READ name WRITE setName NOTIFY nameChanged)
Q_PROPERTY(QVariant value READ value WRITE setValue NOTIFY valueChanged USER true)
Q_PROPERTY(QVariant valueString READ valueString NOTIFY valueChanged)
......@@ -46,6 +47,9 @@ class FactBinder : public QObject
public:
FactBinder(void);
int componentId(void) const;
void setComponentId(int componentId);
QString name(void) const;
void setName(const QString& name);
......@@ -58,12 +62,14 @@ public:
QString units(void) const;
signals:
void componentIdChanged(void);
void nameChanged(void);
void valueChanged(void);
private:
AutoPilotPlugin* _autopilotPlugin;
Fact* _fact;
int _componentId;
};
#endif
\ No newline at end of file
src/FactSystem/FactSystem.cc
View file @ d5e2f29c
......@@ -40,8 +40,6 @@ FactSystem::FactSystem(QObject* parent) :
QGCSingleton(parent)
{
qmlRegisterType<FactBinder>(_factSystemQmlUri, 1, 0, "Fact");
// FIXME: Where should these go?
qmlRegisterUncreatableType<VehicleComponent>(_factSystemQmlUri, 1, 0, "VehicleComponent", "Can only reference VehicleComponent");
}
......
src/FactSystem/FactSystem.h
View file @ d5e2f29c
......@@ -24,15 +24,12 @@
/// @file
/// @author Don Gagne <don@thegagnes.com>
#ifndef FactSystem_h
#define FactSystem_h
#ifndef FACTSYSTEM_H
#define FACTSYSTEM_H
#include "Fact.h"
#include "FactLoader.h"
#include "FactMetaData.h"
#include "UASInterface.h"
#include "QGCSingleton.h"
#include "FactValidator.h"
/// FactSystem is a singleton which provides access to the Facts in the system
///
......@@ -42,12 +39,20 @@
/// settings. Client code can then use this system to expose sets of Facts to QML code. An example
/// of this is the PX4ParameterFacts onbject which is part of the PX4 AutoPilot plugin. It exposes
/// the firmware parameters to QML such that you can bind QML ui elements directly to parameters.
class FactSystem : public QGCSingleton
{
Q_OBJECT
DECLARE_QGC_SINGLETON(FactSystem, FactSystem)
public:
typedef enum {
ParameterProvider,
} Provider_t;
static const int defaultComponentId = -1;
private:
/// All access to FactSystem is through FactSystem::instance, so constructor is private
FactSystem(QObject* parent = NULL);
......
src/FactSystem/FactSystemTest.qml
View file @ d5e2f29c
......@@ -26,14 +26,19 @@ import QtQuick.Controls 1.2
import QGroundControl.FactSystem 1.0
Item {
// Use default component id
TextInput {
objectName: "testControl"
Fact { id: fact; name: "RC_MAP_THROTTLE" }
text: fact.value
font.family: "Helvetica"
font.pointSize: 24;
color: "red"
focus: true
onAccepted: { fact.value = text; }
Fact { id: fact1; name: "RC_MAP_THROTTLE" }
text: fact1.value
onAccepted: { fact1.value = text; }
}
// Use specific component id
TextInput {
objectName: "testControl"
Fact { id: fact2; name: "COMPONENT_51"; componentId: 51 }
text: fact2.value
onAccepted: { fact2.value = text; }
}
}
src/FactSystem/FactSystemTestBase.cc
View file @ d5e2f29c
......@@ -85,21 +85,45 @@ void FactSystemTestBase::_cleanup(void)
}
/// Basic test of parameter values in Fact System
void FactSystemTestBase::_parameter_test(void)
void FactSystemTestBase::_parameter_default_component_id_test(void)
{
// Get the parameter facts from the AutoPilot
// Compare the value in the Parameter Manager with the value from the FactSystem.
const QVariantMap& parameterFacts = _plugin->parameters();
QVERIFY(_plugin->factExists(FactSystem::ParameterProvider, FactSystem::defaultComponentId, "RC_MAP_THROTTLE"));
Fact* fact = _plugin->getFact(FactSystem::ParameterProvider, FactSystem::defaultComponentId, "RC_MAP_THROTTLE");
QVERIFY(fact != NULL);
QVariant factValue = fact->value();
QCOMPARE(factValue.isValid(), true);
QVariant paramValue;
Q_ASSERT(_paramMgr->getParameterValue(_paramMgr->getDefaultComponentId(), "RC_MAP_THROTTLE", paramValue));
// Compare the value in the Parameter Manager with the value from the FactSystem
QCOMPARE(factValue.toInt(), paramValue.toInt());
}
void FactSystemTestBase::_parameter_specific_component_id_test(void)
{
// Compare the value in the Parameter Manager with the value from the FactSystem.
Fact* fact = parameterFacts["RC_MAP_THROTTLE"].value<Fact*>();
QVERIFY(_plugin->factExists(FactSystem::ParameterProvider, 50, "RC_MAP_THROTTLE"));
Fact* fact = _plugin->getFact(FactSystem::ParameterProvider, 50, "RC_MAP_THROTTLE");
QVERIFY(fact != NULL);
QVariant factValue = fact->value();
QCOMPARE(factValue.isValid(), true);
QVariant paramValue;
Q_ASSERT(_paramMgr->getParameterValue(_paramMgr->getDefaultComponentId(), "RC_MAP_THROTTLE", paramValue));
Q_ASSERT(_paramMgr->getParameterValue(50, "RC_MAP_THROTTLE", paramValue));
QCOMPARE(factValue.toInt(), paramValue.toInt());
// Test another component id
QVERIFY(_plugin->factExists(FactSystem::ParameterProvider, 51, "COMPONENT_51"));
fact = _plugin->getFact(FactSystem::ParameterProvider, 51, "COMPONENT_51");
QVERIFY(fact != NULL);
factValue = fact->value();
QCOMPARE(factValue.isValid(), true);
Q_ASSERT(_paramMgr->getParameterValue(51, "COMPONENT_51", paramValue));
QCOMPARE(factValue.toInt(), paramValue.toInt());
}
......@@ -129,9 +153,7 @@ void FactSystemTestBase::_paramMgrSignal_test(void)
{
// Get the parameter Fact from the AutoPilot
const QVariantMap& parameterFacts = _plugin->parameters();
Fact* fact = parameterFacts["RC_MAP_THROTTLE"].value<Fact*>();
Fact* fact = _plugin->getFact(FactSystem::ParameterProvider, -1, "RC_MAP_THROTTLE");
QVERIFY(fact != NULL);
// Setting a new value into the parameter should trigger a valueChanged signal on the Fact
......
src/FactSystem/FactSystemTestBase.h
View file @ d5e2f29c
......@@ -43,7 +43,8 @@ protected:
void _init(MAV_AUTOPILOT autopilot);
void _cleanup(void);
void _parameter_test(void);
void _parameter_default_component_id_test(void);
void _parameter_specific_component_id_test(void);
void _qml_test(void);
void _paramMgrSignal_test(void);
void _qmlUpdate_test(void);
......
src/FactSystem/FactSystemTestGeneric.h
View file @ d5e2f29c
......@@ -43,7 +43,8 @@ private slots:
void init(void);
void cleanup(void) { _cleanup(); }
void parameter_test(void) { _parameter_test(); }
void parameter_default_component_id_test(void) { _parameter_default_component_id_test(); }
void parameter_specific_component_id_test(void) { _parameter_specific_component_id_test(); }
void qml_test(void) { _qml_test(); }
void paramMgrSignal_test(void) { _paramMgrSignal_test(); }
void qmlUpdate_test(void) { _qmlUpdate_test(); }
......
src/FactSystem/FactSystemTestPX4.h
View file @ d5e2f29c
......@@ -43,7 +43,8 @@ private slots:
void init(void);
void cleanup(void) { _cleanup(); }
void parameter_test(void) { _parameter_test(); }
void parameter_default_component_id_test(void) { _parameter_default_component_id_test(); }
void parameter_specific_component_id_test(void) { _parameter_specific_component_id_test(); }
void qml_test(void) { _qml_test(); }
void paramMgrSignal_test(void) { _paramMgrSignal_test(); }
void qmlUpdate_test(void) { _qmlUpdate_test(); }
......
src/VehicleSetup/SetupViewButtonsConnected.qml
View file @ d5e2f29c
......@@ -39,7 +39,7 @@ Rectangle {
}
Repeater {
model: autopilot ? autopilot.components : 0
model: autopilot ? autopilot.vehicleComponents : 0
SubMenuButton {
width: parent.width
......
src/VehicleSetup/VehicleSummary.qml
View file @ d5e2f29c
......@@ -72,7 +72,7 @@ Rectangle {
spacing: 10
Repeater {
model: autopilot.components
model: autopilot.vehicleComponents
// Outer summary item rectangle
Rectangle {
......
src/qgcunittest/MockLink.cc
View file @ d5e2f29c
......@@ -184,6 +184,7 @@ void MockLink::_loadParams(void)
QStringList paramData = line.split("\t");
Q_ASSERT(paramData.count() == 5);
int componentId = paramData.at(1).toInt();
QString paramName = paramData.at(2);
QString valStr = paramData.at(3);
uint paramType = paramData.at(4).toUInt();
......@@ -209,7 +210,7 @@ void MockLink::_loadParams(void)
qCDebug(MockLinkLog) << "Loading param" << paramName << paramValue;
_mapParamName2Value[paramName] = paramValue;
_mapParamName2Value[componentId][paramName] = paramValue;
_mapParamName2MavParamType[paramName] = static_cast<MAV_PARAM_TYPE>(paramType);
}
}
......@@ -368,11 +369,12 @@ void MockLink::_handleSetMode(const mavlink_message_t& msg)
_mavCustomMode = request.custom_mode;
}
void MockLink::_setParamFloatUnionIntoMap(const QString& paramName, float paramFloat)
void MockLink::_setParamFloatUnionIntoMap(int componentId, const QString& paramName, float paramFloat)
{
mavlink_param_union_t valueUnion;
Q_ASSERT(_mapParamName2Value.contains(paramName));
Q_ASSERT(_mapParamName2Value.contains(componentId));
Q_ASSERT(_mapParamName2Value[componentId].contains(paramName));
Q_ASSERT(_mapParamName2MavParamType.contains(paramName));
valueUnion.param_float = paramFloat;
......@@ -403,19 +405,20 @@ void MockLink::_setParamFloatUnionIntoMap(const QString& paramName, float paramF
}
qCDebug(MockLinkLog) << "_setParamFloatUnionIntoMap" << paramName << paramVariant;
_mapParamName2Value[paramName] = paramVariant;
_mapParamName2Value[componentId][paramName] = paramVariant;
}
/// Convert from a parameter variant to the float value from mavlink_param_union_t
float MockLink::_floatUnionForParam(const QString& paramName)
float MockLink::_floatUnionForParam(int componentId, const QString& paramName)
{
mavlink_param_union_t valueUnion;
Q_ASSERT(_mapParamName2Value.contains(paramName));
Q_ASSERT(_mapParamName2Value.contains(componentId));
Q_ASSERT(_mapParamName2Value[componentId].contains(paramName));
Q_ASSERT(_mapParamName2MavParamType.contains(paramName));
MAV_PARAM_TYPE paramType = _mapParamName2MavParamType[paramName];
QVariant paramVar = _mapParamName2Value[paramName];
QVariant paramVar = _mapParamName2Value[componentId][paramName];
switch (paramType) {
case MAV_PARAM_TYPE_INT8:
......@@ -443,20 +446,22 @@ float MockLink::_floatUnionForParam(const QString& paramName)
void MockLink::_handleParamRequestList(const mavlink_message_t& msg)
{
uint16_t paramIndex = 0;
mavlink_param_request_list_t request;
mavlink_msg_param_request_list_decode(&msg, &request);
int cParameters = _mapParamName2Value.count();
Q_ASSERT(request.target_system == _vehicleSystemId);
Q_ASSERT(request.target_component == MAV_COMP_ID_ALL);
foreach(QString paramName, _mapParamName2Value.keys()) {
foreach (int componentId, _mapParamName2Value.keys()) {
uint16_t paramIndex = 0;
int cParameters = _mapParamName2Value[componentId].count();
foreach(QString paramName, _mapParamName2Value[componentId].keys()) {
char paramId[MAVLINK_MSG_ID_PARAM_VALUE_LEN];
mavlink_message_t responseMsg;
Q_ASSERT(_mapParamName2Value.contains(paramName));
Q_ASSERT(_mapParamName2Value[componentId].contains(paramName));
Q_ASSERT(_mapParamName2MavParamType.contains(paramName));
MAV_PARAM_TYPE paramType = _mapParamName2MavParamType[paramName];
......@@ -467,15 +472,16 @@ void MockLink::_handleParamRequestList(const mavlink_message_t& msg)
qCDebug(MockLinkLog) << "Sending msg_param_value" << paramId << paramType;
mavlink_msg_param_value_pack(_vehicleSystemId,
_vehicleComponentId,
componentId, // component id
&responseMsg, // Outgoing message
paramId, // Parameter name
_floatUnionForParam(paramName), // Parameter value
_floatUnionForParam(componentId, paramName), // Parameter value
paramType, // MAV_PARAM_TYPE
cParameters, // Total number of parameters
paramIndex++); // Index of this parameter
_emitMavlinkMessage(responseMsg);
}
}
}
void MockLink::_handleParamSet(const mavlink_message_t& msg)
......@@ -484,27 +490,29 @@ void MockLink::_handleParamSet(const mavlink_message_t& msg)
mavlink_msg_param_set_decode(&msg, &request);
Q_ASSERT(request.target_system == _vehicleSystemId);
int componentId = request.target_component;
// Param may not be null terminated if exactly fits
char paramId[MAVLINK_MSG_PARAM_SET_FIELD_PARAM_ID_LEN + 1];
strncpy(paramId, request.param_id, MAVLINK_MSG_PARAM_SET_FIELD_PARAM_ID_LEN);
Q_ASSERT(_mapParamName2Value.contains(paramId));
Q_ASSERT(_mapParamName2Value.contains(componentId));
Q_ASSERT(_mapParamName2Value[componentId].contains(paramId));
Q_ASSERT(request.param_type == _mapParamName2MavParamType[paramId]);
// Save the new value
_setParamFloatUnionIntoMap(paramId, request.param_value);
_setParamFloatUnionIntoMap(componentId, paramId, request.param_value);
// Respond with a param_value to ack
mavlink_message_t responseMsg;
mavlink_msg_param_value_pack(_vehicleSystemId,
_vehicleComponentId,
componentId, // component id
&responseMsg, // Outgoing message
paramId, // Parameter name
request.param_value, // Send same value back
request.param_type, // Send same type back
_mapParamName2Value.count(), // Total number of parameters
_mapParamName2Value.keys().indexOf(paramId)); // Index of this parameter
_mapParamName2Value[componentId].count(), // Total number of parameters
_mapParamName2Value[componentId].keys().indexOf(paramId)); // Index of this parameter
_emitMavlinkMessage(responseMsg);
}
......@@ -513,6 +521,9 @@ void MockLink::_handleParamRequestRead(const mavlink_message_t& msg)
mavlink_param_request_read_t request;
mavlink_msg_param_request_read_decode(&msg, &request);
int componentId = request.target_component;
Q_ASSERT(_mapParamName2Value.contains(componentId));
char paramId[MAVLINK_MSG_PARAM_REQUEST_READ_FIELD_PARAM_ID_LEN + 1];
paramId[0] = 0;
......@@ -526,24 +537,24 @@ void MockLink::_handleParamRequestRead(const mavlink_message_t& msg)
Q_ASSERT(request.param_index >= 0 && request.param_index < _mapParamName2Value.count());
QString key = _mapParamName2Value.keys().at(request.param_index);
QString key = _mapParamName2Value[componentId].keys().at(request.param_index);
Q_ASSERT(key.length() <= MAVLINK_MSG_PARAM_REQUEST_READ_FIELD_PARAM_ID_LEN);
strcpy(paramId, key.toLocal8Bit().constData());
}
Q_ASSERT(_mapParamName2Value.contains(paramId));
Q_ASSERT(_mapParamName2Value[componentId].contains(paramId));
Q_ASSERT(_mapParamName2MavParamType.contains(paramId));
mavlink_message_t responseMsg;
mavlink_msg_param_value_pack(_vehicleSystemId,
_vehicleComponentId,
componentId, // component id
&responseMsg, // Outgoing message
paramId, // Parameter name
_floatUnionForParam(paramId), // Parameter value
_floatUnionForParam(componentId, paramId), // Parameter value
_mapParamName2MavParamType[paramId], // Parameter type
_mapParamName2Value.count(), // Total number of parameters
_mapParamName2Value.keys().indexOf(paramId)); // Index of this parameter
_mapParamName2Value[componentId].count(), // Total number of parameters
_mapParamName2Value[componentId].keys().indexOf(paramId)); // Index of this parameter
_emitMavlinkMessage(responseMsg);
}
......
src/qgcunittest/MockLink.h
View file @ d5e2f29c
......@@ -117,8 +117,8 @@ private:
void _handleMissionRequestList(const mavlink_message_t& msg);
void _handleMissionRequest(const mavlink_message_t& msg);
void _handleMissionItem(const mavlink_message_t& msg);
float _floatUnionForParam(const QString& paramName);
void _setParamFloatUnionIntoMap(const QString& paramName, float paramFloat);
float _floatUnionForParam(int componentId, const QString& paramName);
void _setParamFloatUnionIntoMap(int componentId, const QString& paramName, float paramFloat);
MockLinkMissionItemHandler* _missionItemHandler;
......@@ -131,7 +131,7 @@ private:
bool _inNSH;
bool _mavlinkStarted;
QMap<QString, QVariant> _mapParamName2Value;
QMap<int, QMap<QString, QVariant> > _mapParamName2Value;
QMap<QString, MAV_PARAM_TYPE> _mapParamName2MavParamType;
typedef QMap<uint16_t, mavlink_mission_item_t> MissionList_t;
......
src/qgcunittest/MockLink.param
View file @ d5e2f29c
......@@ -510,3 +510,4 @@
1 50 VT_MOT_COUNT 0 6
1 50 VT_POWER_MAX 120 9
1 50 VT_PROP_EFF 0 9
1 51 COMPONENT_51 51 6
Markdown is supported
0% or
You are about to add 0 people to the discussion. Proceed with caution.
Finish editing this message first!
Please register or to comment