From 7cee3ba2483981aeb9635244435fad4ce4e4394b Mon Sep 17 00:00:00 2001 From: Bryan Godbolt Date: Tue, 31 Aug 2010 08:53:07 -0600 Subject: [PATCH] successfully reading parameters from model --- src/comm/OpalLink.cc | 33 +++++++++++++++++++------ src/comm/OpalLink.h | 1 + src/comm/OpalRT.h | 8 ++++-- src/comm/Parameter.cc | 31 +++++++++++++++++++++++ src/comm/Parameter.h | 52 ++++++++++++++++++++++----------------- src/comm/ParameterList.cc | 36 ++++++++++++++++++++++++--- src/comm/ParameterList.h | 34 +++++++++++++++++++------ src/comm/QGCParamID.cc | 20 ++++++++------- src/comm/QGCParamID.h | 13 ++++++++-- 9 files changed, 173 insertions(+), 55 deletions(-) diff --git a/src/comm/OpalLink.cc b/src/comm/OpalLink.cc index 128f9ccd0..d6928f46c 100644 --- a/src/comm/OpalLink.cc +++ b/src/comm/OpalLink.cc @@ -82,15 +82,30 @@ void OpalLink::writeBytes(const char *bytes, qint64 length) { qDebug() << "OpalLink::writeBytes(): request params"; // getParameterList(); - params = new OpalRT::ParameterList(); + mavlink_message_t param; - char paramName[] = "NAV_FILT_INIT"; - mavlink_msg_param_value_pack(systemID, componentID, ¶m, - (int8_t*)(paramName), - 0, - 1, - 0); - receiveMessage(param); +// char paramName[] = "NAV_FILT_INIT"; +// mavlink_msg_param_value_pack(systemID, componentID, ¶m, +// (int8_t*)(paramName), +// 0, +// 1, +// 0); +// receiveMessage(param); + + OpalRT::ParameterList::const_iterator paramIter; + for (paramIter = params->begin(); paramIter != params->end(); ++paramIter) + { + mavlink_msg_param_value_pack(systemID, + (*paramIter).getComponentID(), + ¶m, + (*paramIter).getParamID().toInt8_t(), + (static_cast(*paramIter)).getValue(), + params->count(), + params->indexOf(*paramIter)); + receiveMessage(param); + } + + } case MAVLINK_MSG_ID_PARAM_SET: { @@ -385,6 +400,8 @@ bool OpalLink::connect() if ((OpalConnect(101, false, &modelState) == EOK) && (OpalGetSignalControl(0, true) == EOK)) { connectState = true; + /// \todo try/catch a delete in case params has already been allocated + params = new OpalRT::ParameterList(); emit connected(); heartbeatTimer->start(1000/heartbeatRate); getSignalsTimer->start(getSignalsPeriod); diff --git a/src/comm/OpalLink.h b/src/comm/OpalLink.h index 7ea358edb..29c9b84c7 100644 --- a/src/comm/OpalLink.h +++ b/src/comm/OpalLink.h @@ -49,6 +49,7 @@ This file is part of the QGROUNDCONTROL project #include "configuration.h" #include "OpalRT.h" #include "ParameterList.h" +#include "Parameter.h" #include "errno.h" diff --git a/src/comm/OpalRT.h b/src/comm/OpalRT.h index f2d938fe2..48d0a3cae 100644 --- a/src/comm/OpalRT.h +++ b/src/comm/OpalRT.h @@ -68,11 +68,15 @@ namespace OpalRT B_W_2 }; + /* Component IDs of the parameters. Currently they are all 1 becuase there is no advantage + to dividing them between component ids. However this syntax is used so that this can + easily be changed in the future. + */ enum SubsystemIds { NAV_ID = 1, - LOG_ID, - CONTROLLER_ID + LOG_ID = 1, + CONTROLLER_ID = 1 }; } #endif // OPALRT_H diff --git a/src/comm/Parameter.cc b/src/comm/Parameter.cc index 30e211ddc..9d8c3093e 100644 --- a/src/comm/Parameter.cc +++ b/src/comm/Parameter.cc @@ -26,3 +26,34 @@ Parameter::~Parameter() delete simulinkName; delete paramID; } + +bool Parameter::operator ==(const Parameter& other) const +{ + return + (*simulinkPath) == *(other.simulinkPath) + && *simulinkName == *(other.simulinkName) + && componentID == other.componentID + && *paramID == *(other.paramID) + && opalID == other.opalID; + +} + +float Parameter::getValue() //const +{ + unsigned short allocatedParams = 1; + unsigned short numParams; +// unsigned short allocatedValues; + unsigned short numValues = 1; + unsigned short returnedNumValues; + double value; + + int returnVal = OpalGetParameters(allocatedParams, &numParams, &opalID, + numValues, &returnedNumValues, &value); + + if (returnVal != EOK) + { + return FLT_MAX; + } + + return static_cast(value); +} diff --git a/src/comm/Parameter.h b/src/comm/Parameter.h index 0306b6e89..28d2afeef 100644 --- a/src/comm/Parameter.h +++ b/src/comm/Parameter.h @@ -27,32 +27,38 @@ This file is part of the QGROUNDCONTROL project #include #include "mavlink_types.h" #include "QGCParamID.h" +#include "OpalApi.h" +#include namespace OpalRT { -class Parameter -{ -public: - Parameter(char *simulinkPath = "", - char *simulinkName = "", - uint8_t componentID = 0, - QGCParamID paramID = QGCParamID(), - unsigned short opalID = 0); - /// \todo Implement copy constructor - Parameter(const Parameter& other); - ~Parameter(); - - QGCParamID getParamID() {return *paramID;} - void setOpalID(unsigned short opalID) {this->opalID = opalID;} - const QString& getSimulinkPath() {return *simulinkPath;} - const QString& getSimulinkName() {return *simulinkName;} -protected: - QString *simulinkPath; - QString *simulinkName; - uint8_t componentID; - QGCParamID *paramID; - unsigned short opalID; -}; + class Parameter + { + public: + Parameter(char *simulinkPath = "", + char *simulinkName = "", + uint8_t componentID = 0, + QGCParamID paramID = QGCParamID(), + unsigned short opalID = 0); + Parameter(const Parameter& other); + ~Parameter(); + + const QGCParamID& getParamID() const {return *paramID;} + void setOpalID(unsigned short opalID) {this->opalID = opalID;} + const QString& getSimulinkPath() {return *simulinkPath;} + const QString& getSimulinkName() {return *simulinkName;} + uint8_t getComponentID() const {return componentID;} + float getValue();// const; + + bool operator==(const Parameter& other) const; + + protected: + QString *simulinkPath; + QString *simulinkName; + uint8_t componentID; + QGCParamID *paramID; + unsigned short opalID; + }; } #endif // PARAMETER_H diff --git a/src/comm/ParameterList.cc b/src/comm/ParameterList.cc index 6956f83c1..60cdff1b8 100644 --- a/src/comm/ParameterList.cc +++ b/src/comm/ParameterList.cc @@ -2,8 +2,10 @@ using namespace OpalRT; ParameterList::ParameterList() + :params(new QMap >), + paramVector(new QVector) { - params = new QMap >; +// params = new QMap >; /* Populate the map with parameter names. There is no elegant way of doing this so all parameter paths and names must be known at compile time and defined here. @@ -50,6 +52,7 @@ ParameterList::ParameterList() for (paramIter = (*componentIter).begin(); paramIter != (*componentIter).end(); ++paramIter) { s = (*paramIter).getSimulinkPath() + (*paramIter).getSimulinkName(); + paramVector->append((*paramIter)); if (opalParams->contains(s)) { (*paramIter).setOpalID(opalParams->value(s)); @@ -68,16 +71,35 @@ ParameterList::ParameterList() ParameterList::~ParameterList() { delete params; + delete paramVector; } -ParameterList::const_iterator::const_iterator() +ParameterList::const_iterator::const_iterator(QList paramList) { + this->paramList = QList(paramList); + index = 0; +} +ParameterList::const_iterator::const_iterator(const const_iterator &other) +{ + paramList = QList(other.paramList); + index = other.index; } -const_iterator ParameterList::begin() +ParameterList::const_iterator ParameterList::begin() const { + QList > compList = params->values(); + QList paramList; + QList >::const_iterator compIter; + for (compIter = compList.begin(); compIter != compList.end(); ++compIter) + paramList.append((*compIter).values()); + return const_iterator(paramList); +} +ParameterList::const_iterator ParameterList::end() const +{ + const_iterator iter = begin(); + return iter+=iter.paramList.size(); } /** @@ -161,10 +183,16 @@ void ParameterList::getParameterList(QMap *opalParams) else opalParams->insert(path+'/'+name, idParam[i]); } - // Dump out the list of parameters +// Dump out the list of parameters // QMap::const_iterator paramPrint; // for (paramPrint = opalParams->begin(); paramPrint != opalParams->end(); ++paramPrint) // qDebug() << paramPrint.key(); } + +int ParameterList::count() +{ + const_iterator iter = begin(); + return iter.paramList.count(); +} diff --git a/src/comm/ParameterList.h b/src/comm/ParameterList.h index 2be6485f5..2f8596c0d 100644 --- a/src/comm/ParameterList.h +++ b/src/comm/ParameterList.h @@ -29,6 +29,7 @@ This file is part of the QGROUNDCONTROL project #include "mavlink_types.h" #include "QGCParamID.h" #include "Parameter.h" +#include "QVector" #include "OpalRT.h" // Forward declare ParameterList before including OpalLink.h because a member of type ParameterList is used in OpalLink @@ -45,23 +46,42 @@ namespace OpalRT class const_iterator { + friend class ParameterList; + public: - const_iterator(); - const_iterator(const_iterator& other); + inline const_iterator() {} + const_iterator(const const_iterator& other); + + const_iterator& operator+=(int i) {index += i; return *this;} + bool operator<(const const_iterator& other) {return (this->paramList == other.paramList) + &&(this->indexparamList == other.paramList) + &&(this->index==other.index);} + bool operator!=(const const_iterator& other) {return !((*this) == other);} + const Parameter& operator*() const {return paramList[index];} + const Parameter* operator->() const {return ¶mList[index];} + + const_iterator& operator++() {++index; return *this;} private: - int componentID; - QGCParamID paramID; + const_iterator(QList); + QList paramList; + int index; }; + + ParameterList(); ~ParameterList(); int setValue(int compid, QGCParamID paramid, float value); float getValue(int compid, QGCParamID paramid); + int count(); + int indexOf(const Parameter& p) {return paramVector->indexOf(p);} + + const_iterator begin() const; + const_iterator end() const; -// const_iterator begin() const; -// const_iterator end() const; protected: QMap > *params; - + QVector *paramVector; void getParameterList(QMap*); diff --git a/src/comm/QGCParamID.cc b/src/comm/QGCParamID.cc index 75d248882..e486d0957 100644 --- a/src/comm/QGCParamID.cc +++ b/src/comm/QGCParamID.cc @@ -1,21 +1,23 @@ #include "QGCParamID.h" using namespace OpalRT; -QGCParamID::QGCParamID(const char *paramid):QString(paramid) +QGCParamID::QGCParamID(const char *paramid):data(paramid) { } -QGCParamID::QGCParamID(const QGCParamID &other):QString(other) +QGCParamID::QGCParamID(const QString s):data(s) { } -/* -bool QGCParamID::operator<(const QGCParamID& other) +QGCParamID::QGCParamID(const QGCParamID &other):data(other.data) { - const QString *lefthand, *righthand; - lefthand = this; - righthand = &other; - return lefthand < righthand; + } -*/ + +//int8_t* QGCParamID::toInt8_t() +//{ +// int8_t +// for (int i=0; ((i < data.size()) && (i < 15)); ++i) +// +//} diff --git a/src/comm/QGCParamID.h b/src/comm/QGCParamID.h index c822161c5..280e3cd54 100644 --- a/src/comm/QGCParamID.h +++ b/src/comm/QGCParamID.h @@ -25,18 +25,27 @@ This file is part of the QGROUNDCONTROL project #define QGCPARAMID_H #include +#include "mavlink_types.h" namespace OpalRT { - class QGCParamID : public QString + class QGCParamID { public: QGCParamID(const char *paramid); + QGCParamID(const QString); QGCParamID() {} QGCParamID(const QGCParamID& other); -// bool operator<(const QGCParamID& other); + bool operator<(const QGCParamID& other) const {return data(data);} + int8_t* toInt8_t() const {return (int8_t*)data.toAscii().data();} + + protected: + QString data; }; } #endif // QGCPARAMID_H -- 2.22.0