successfully reading parameters from model

src/comm/OpalLink.cc

@@ -82,16 +82,31 @@ 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";
+//                char paramName[] = "NAV_FILT_INIT";
-                mavlink_msg_param_value_pack(systemID, componentID, &param,
+//                mavlink_msg_param_value_pack(systemID, componentID, &param,
-                                             (int8_t*)(paramName),
+//                                             (int8_t*)(paramName),
-                                             0,
+//                                             0,
-                                             1,
+//                                             1,
-                                             0);
+//                                             0);
+//                receiveMessage(param);
+
+                OpalRT::ParameterList::const_iterator paramIter;
+                for (paramIter = params->begin(); paramIter != params->end(); ++paramIter)
+                {
+                    mavlink_msg_param_value_pack(systemID,
+                                                 (*paramIter).getComponentID(),
+                                                 &param,
+                                                 (*paramIter).getParamID().toInt8_t(),
+                                                 (static_cast<OpalRT::Parameter>(*paramIter)).getValue(),
+                                                 params->count(),
+                                                 params->indexOf(*paramIter));
                     receiveMessage(param);
                 }
+
+
+            }
         case MAVLINK_MSG_ID_PARAM_SET:
             {
                 qDebug() << "OpalLink::writeBytes(): Attempt to set a parameter";
@@ -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);

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"
 

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,
+        LOG_ID = 1,
-        CONTROLLER_ID
+        CONTROLLER_ID = 1
     };
 }
 #endif // OPALRT_H

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<float>(value);
+}

src/comm/Parameter.h

@@ -27,6 +27,8 @@ This file is part of the QGROUNDCONTROL project
 #include <QString>
 #include "mavlink_types.h"
 #include "QGCParamID.h"
+#include "OpalApi.h"
+#include <cfloat>
 
 namespace OpalRT
 {
@@ -38,14 +40,18 @@ public:
                   uint8_t componentID = 0,
                   QGCParamID paramID = QGCParamID(),
                   unsigned short opalID = 0);
-    /// \todo Implement copy constructor
         Parameter(const Parameter& other);
         ~Parameter();
 
-    QGCParamID getParamID() {return *paramID;}
+        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;

src/comm/ParameterList.cc

@@ -2,8 +2,10 @@
 using namespace OpalRT;
 
 ParameterList::ParameterList()
+    :params(new QMap<int, QMap<QGCParamID, Parameter> >),
+    paramVector(new QVector<Parameter>)
 {
-    params = new QMap<int, QMap<QGCParamID, Parameter> >;
+//    params = new QMap<int, QMap<QGCParamID, Parameter> >;
 
     /* 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<Parameter> paramList)
 {
+    this->paramList = QList<Parameter>(paramList);
+    index = 0;
+}
 
+ParameterList::const_iterator::const_iterator(const const_iterator &other)
+{
+    paramList = QList<Parameter>(other.paramList);
+    index = other.index;
 }
 
-const_iterator ParameterList::begin()
+ParameterList::const_iterator ParameterList::begin() const
 {
+    QList<QMap<QGCParamID, Parameter> > compList = params->values();
+    QList<Parameter> paramList;
+    QList<QMap<QGCParamID, Parameter> >::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();
 }
 
 /**
@@ -168,3 +190,9 @@ void ParameterList::getParameterList(QMap<QString, unsigned short> *opalParams)
 
 
 }
+
+int ParameterList::count()
+{
+    const_iterator iter = begin();
+    return iter.paramList.count();
+}

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();
+            inline const_iterator() {}
-            const_iterator(const_iterator& other);
+            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->index<other.index);}
+            bool operator==(const const_iterator& other) {return (this->paramList == 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 &paramList[index];}
+
+            const_iterator& operator++() {++index; return *this;}
         private:
-            int componentID;
+            const_iterator(QList<Parameter>);
-            QGCParamID paramID;
+            QList<Parameter> 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<int, QMap<QGCParamID, Parameter> > *params;
-
+        QVector<Parameter> *paramVector;
         void getParameterList(QMap<QString, unsigned short>*);
 
 

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)
 {
 
 }
 
-/*
+QGCParamID::QGCParamID(const QGCParamID &other):data(other.data)
-bool QGCParamID::operator<(const QGCParamID& other)
 {
-    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)
+//
+//}

src/comm/QGCParamID.h

@@ -25,18 +25,27 @@ This file is part of the QGROUNDCONTROL project
 #define QGCPARAMID_H
 
 #include <QString>
+#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<other.data;}
+        bool operator==(const QGCParamID& other) const {return data == other.data;}
+
+        const QString getParamString() const {return static_cast<const QString>(data);}
+        int8_t* toInt8_t() const {return (int8_t*)data.toAscii().data();}
+
+    protected:
+        QString data;
     };
 }
 #endif // QGCPARAMID_H