able to read opal parameters into a map. ready to rewrite parameter sending

qgroundcontrol.pro

@@ -42,8 +42,8 @@ DEPENDPATH += . \
     plugins
 INCLUDEPATH += . \
     lib/QMapControl \
-    $$BASEDIR/../mavlink/contrib/slugs/include \
-    $$BASEDIR/../mavlink/include
+    $$BASEDIR/../mavlink/include \
+    $$BASEDIR/../mavlink/contrib/slugs/include
 
 # ../mavlink/include \
 # MAVLink/include \
@@ -230,11 +230,13 @@ win32 {
         -lOpalApi
     INCLUDEPATH += src/lib/opalrt
     SOURCES += src/comm/OpalLink.cc \
-               src/comm/Parameter.cc \
-               src/comm/QGCParamID.cc
+        src/comm/Parameter.cc \
+        src/comm/QGCParamID.cc \
+        src/comm/ParameterList.cc
     HEADERS += src/comm/OpalLink.h \
         src/comm/OpalRT.h \
         src/comm/Parameter.h \
-        src/comm/QGCParamID.h
+        src/comm/QGCParamID.h \
+        src/comm/ParameterList.h
     DEFINES += OPAL_RT
 }

src/comm/OpalLink.cc

@@ -81,7 +81,8 @@ void OpalLink::writeBytes(const char *bytes, qint64 length)
         case MAVLINK_MSG_ID_PARAM_REQUEST_LIST:
             {
                 qDebug() << "OpalLink::writeBytes(): request params";
-                getParameterList();
+//                getParameterList();
+                params = new OpalRT::ParameterList();
                 mavlink_message_t param;
                 char paramName[] = "NAV_FILT_INIT";
                 mavlink_msg_param_value_pack(systemID, componentID, &param,
@@ -270,6 +271,7 @@ void OpalLink::getSignals()
 
 }
 
+
 void OpalLink::getParameterList()
 {
     /* inputs */
@@ -404,6 +406,7 @@ bool OpalLink::disconnect()
 
 void OpalLink::displayLastErrorMsg()
 {
+    static QString lastErrorMsg;
     setLastErrorMsg();
     QMessageBox msgBox;
     msgBox.setIcon(QMessageBox::Critical);
@@ -415,6 +418,7 @@ void OpalLink::setLastErrorMsg()
 {
     char buf[512];
     unsigned short len;
+    static QString lastErrorMsg;
     OpalGetLastErrMsg(buf, sizeof(buf), &len);
     lastErrorMsg.clear();
     lastErrorMsg.append(buf);

src/comm/OpalLink.h

@@ -48,6 +48,7 @@ This file is part of the QGROUNDCONTROL project
 #include "mavlink_types.h"
 #include "configuration.h"
 #include "OpalRT.h"
+#include "ParameterList.h"
 
 #include "errno.h"
 
@@ -101,23 +102,19 @@ public:
     qint64 getBitsSent();
     qint64 getBitsReceived();
 
-
     bool connect();
 
-
     bool disconnect();
 
-
     qint64 bytesAvailable();
 
     void run();
-
+    static void setLastErrorMsg();
+    static void displayLastErrorMsg();
 public slots:
 
-
     void writeBytes(const char *bytes, qint64 length);
 
-
     void readBytes();
 
     void heartbeat();
@@ -129,8 +126,6 @@ protected slots:
     void receiveMessage(mavlink_message_t message);
     void setSignals(double *values);
 
-
-
 protected:
     QString name;
     int id;
@@ -147,9 +142,7 @@ protected:
     QMutex statisticsMutex;
     QMutex receiveDataMutex;
 //    QMutex getSignalsMutex;
-    QString lastErrorMsg;
-    void setLastErrorMsg();
-    void displayLastErrorMsg();
+    static QString lastErrorMsg;
 
     void setName(QString name);
 
@@ -167,8 +160,8 @@ protected:
     const int componentID;
 
     void getParameterList();
-
-
+    OpalRT::ParameterList *params;
 };
 
+//QString OpalLink::lastErrorMsg = QString();
 #endif // OPALLINK_H

src/comm/OpalRT.h

+/*=====================================================================
+
+QGroundControl Open Source Ground Control Station
+
+(c) 2009, 2010 QGROUNDCONTROL PROJECT <http://www.qgroundcontrol.org>
+
+This file is part of the QGROUNDCONTROL project
+
+    QGROUNDCONTROL is free software: you can redistribute it and/or modify
+    it under the terms of the GNU General Public License as published by
+    the Free Software Foundation, either version 3 of the License, or
+    (at your option) any later version.
+
+    QGROUNDCONTROL is distributed in the hope that it will be useful,
+    but WITHOUT ANY WARRANTY; without even the implied warranty of
+    MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
+    GNU General Public License for more details.
+
+    You should have received a copy of the GNU General Public License
+    along with QGROUNDCONTROL. If not, see <http://www.gnu.org/licenses/>.
+
+======================================================================*/
+
 #ifndef OPALRT_H
 #define OPALRT_H
 
@@ -44,5 +67,12 @@ namespace OpalRT
         B_W_1,
         B_W_2
     };       
+
+    enum SubsystemIds
+    {
+        NAV_ID = 1,
+        LOG_ID,
+        CONTROLLER_ID
+    };
 }
 #endif // OPALRT_H

src/comm/Parameter.cc

 #include "Parameter.h"
 using namespace OpalRT;
 
-Parameter::Parameter()
+Parameter::Parameter(char *simulinkPath, char *simulinkName, uint8_t componentID,
+                     QGCParamID paramID, unsigned short opalID)
+                         : simulinkPath(new QString(simulinkPath)),
+                         simulinkName(new QString(simulinkName)),
+                         componentID(componentID),
+                         paramID(new QGCParamID(paramID)),
+                         opalID(opalID)
+
+{
+}
+Parameter::Parameter(const Parameter &other)
+    : componentID(other.componentID),
+    opalID(other.opalID)
+{
+    simulinkPath = new QString(*other.simulinkPath);
+    simulinkName = new QString(*other.simulinkName);
+    paramID = new QGCParamID(*other.paramID);
+}
+
+Parameter::~Parameter()
 {
+    delete simulinkPath;
+    delete simulinkName;
+    delete paramID;
 }

src/comm/Parameter.h

+/*=====================================================================
+
+QGroundControl Open Source Ground Control Station
+
+(c) 2009, 2010 QGROUNDCONTROL PROJECT <http://www.qgroundcontrol.org>
+
+This file is part of the QGROUNDCONTROL project
+
+    QGROUNDCONTROL is free software: you can redistribute it and/or modify
+    it under the terms of the GNU General Public License as published by
+    the Free Software Foundation, either version 3 of the License, or
+    (at your option) any later version.
+
+    QGROUNDCONTROL is distributed in the hope that it will be useful,
+    but WITHOUT ANY WARRANTY; without even the implied warranty of
+    MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
+    GNU General Public License for more details.
+
+    You should have received a copy of the GNU General Public License
+    along with QGROUNDCONTROL. If not, see <http://www.gnu.org/licenses/>.
+
+======================================================================*/
+
 #ifndef PARAMETER_H
 #define PARAMETER_H
 
@@ -10,15 +33,25 @@ namespace OpalRT
 class Parameter
 {
 public:
-    Parameter();
+    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 *simulinkName;
     QString *simulinkPath;
-    unsigned short opalID;
-
-    QGCParamID *paramID;
+    QString *simulinkName;
     uint8_t componentID;
+    QGCParamID *paramID;
+    unsigned short opalID;
 };
 }
 

src/comm/ParameterList.cc

+#include "ParameterList.h"
+using namespace OpalRT;
+
+ParameterList::ParameterList()
+{
+    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.
+       Note: This function is written in a way that calls a lot of copy constructors and is
+           therefore not particularly efficient.  However since it is only called once memory
+           and computation time are sacrificed for code clarity when adding and modifying
+           parameters.
+           When defining the path, the trailing slash is necessary
+       */
+    Parameter *p;
+    /* Component: Navigation Filter */
+    p = new Parameter("avionics_src/sm_ampro/NAV_FILT_INIT/",
+                            "Value",
+                            OpalRT::NAV_ID,
+                            QGCParamID("NAV_FILT_INIT"));
+    (*params)[OpalRT::NAV_ID].insert(p->getParamID(), *p);
+    delete p;
+
+    p = new Parameter("avionics_src/sm_ampro/Gain/",
+                            "Gain",
+                            OpalRT::NAV_ID,
+                            QGCParamID("TEST_OUTP_GAIN"));
+    (*params)[OpalRT::NAV_ID].insert(p->getParamID(), *p);
+    delete p;
+
+    /* Component: Log Facility */
+    p = new Parameter("avionics_src/sm_ampro/LOG_FILE_ON/",
+                            "Value",
+                            OpalRT::LOG_ID,
+                            QGCParamID("LOG_FILE_ON"));
+    (*params)[OpalRT::LOG_ID].insert(p->getParamID(), *p);
+    delete p;
+
+    /* Get a list of the available parameters from opal-rt */
+    QMap<QString, unsigned short> *opalParams = new QMap<QString, unsigned short>;
+    getParameterList(opalParams);
+
+    /* Iterate over the parameters we want to use in qgc and populate their ids */
+    QMap<int, QMap<QGCParamID, Parameter> >::iterator componentIter;
+    QMap<QGCParamID, Parameter>::iterator paramIter;
+    QString s;
+    for (componentIter = params->begin(); componentIter != params->end(); ++componentIter)
+    {
+        for (paramIter = (*componentIter).begin(); paramIter != (*componentIter).end(); ++paramIter)
+        {
+            s = (*paramIter).getSimulinkPath() + (*paramIter).getSimulinkName();
+            if (opalParams->contains(s))
+            {
+                (*paramIter).setOpalID(opalParams->value(s));
+                qDebug() << __FILE__ << " Line:" << __LINE__ << ": Successfully added " << s;
+            }
+            else
+            {
+                qDebug() << __FILE__ << " Line:" << __LINE__ << ": " << s << " was not found in param list";
+            }
+        }
+    }
+    delete opalParams;
+
+}
+
+ParameterList::~ParameterList()
+{
+    delete params;
+}
+/**
+  Get the list of parameters in the simulink model.  This function does not require
+  any prior knowlege of the parameters.  It works by first calling OpalGetParameterList to
+  get the number of paramters, then allocates the required amount of memory and then gets
+  the paramter list using a second call to OpalGetParameterList.
+  */
+void ParameterList::getParameterList(QMap<QString, unsigned short> *opalParams)
+{
+    /* inputs */
+    unsigned short allocatedParams=0;
+    unsigned short allocatedPathLen=0;
+    unsigned short allocatedNameLen=0;
+    unsigned short allocatedVarLen=0;
+
+    /* outputs */
+    unsigned short numParams;
+    unsigned short *idParam=NULL;
+    unsigned short maxPathLen;
+    char **paths=NULL;
+    unsigned short maxNameLen;
+    char **names=NULL;
+    unsigned short maxVarLen;
+    char **var=NULL;
+
+    int returnValue;
+
+    returnValue = OpalGetParameterList(allocatedParams, &numParams, idParam,
+                             allocatedPathLen, &maxPathLen, paths,
+                             allocatedNameLen, &maxNameLen, names,
+                             allocatedVarLen, &maxVarLen, var);
+    if (returnValue!=E2BIG)
+    {
+        OpalLink::setLastErrorMsg();
+        OpalLink::displayLastErrorMsg();
+        return;
+    }
+
+    // allocate memory for parameter list
+
+    idParam = new unsigned short[numParams];
+    allocatedParams = numParams;
+
+    paths = new char*[numParams];
+    for (int i=0; i<numParams; i++)
+        paths[i]=new char[maxPathLen];
+    allocatedPathLen = maxPathLen;
+
+    names = new char*[numParams];
+    for (int i=0; i<numParams; i++)
+        names[i] = new char[maxNameLen];
+    allocatedNameLen = maxNameLen;
+
+    var = new char*[numParams];
+    for (int i=0; i<numParams; i++)
+        var[i] = new char[maxVarLen];
+    allocatedVarLen = maxVarLen;
+
+    returnValue = OpalGetParameterList(allocatedParams, &numParams, idParam,
+                             allocatedPathLen, &maxPathLen, paths,
+                             allocatedNameLen, &maxNameLen, names,
+                             allocatedVarLen, &maxVarLen, var);
+
+    if (returnValue != EOK)
+    {
+        OpalLink::setLastErrorMsg();
+        OpalLink::displayLastErrorMsg();
+        return;
+    }
+
+    QString path, name;
+    for (int i=0; i<numParams; ++i)
+    {
+        path.clear();
+        path.append(paths[i]);
+        name.clear();
+        name.append(names[i]);
+        if (path[path.size()-1] == '/')
+            opalParams->insert(path+name, idParam[i]);
+        else
+            opalParams->insert(path+'/'+name, idParam[i]);
+    }
+    // Dump out the list of parameters
+//    QMap<QString, unsigned short>::const_iterator paramPrint;
+//    for (paramPrint = opalParams->begin(); paramPrint != opalParams->end(); ++paramPrint)
+//        qDebug() << paramPrint.key();
+
+
+}

src/comm/ParameterList.h

+/*=====================================================================
+
+QGroundControl Open Source Ground Control Station
+
+(c) 2009, 2010 QGROUNDCONTROL PROJECT <http://www.qgroundcontrol.org>
+
+This file is part of the QGROUNDCONTROL project
+
+    QGROUNDCONTROL is free software: you can redistribute it and/or modify
+    it under the terms of the GNU General Public License as published by
+    the Free Software Foundation, either version 3 of the License, or
+    (at your option) any later version.
+
+    QGROUNDCONTROL is distributed in the hope that it will be useful,
+    but WITHOUT ANY WARRANTY; without even the implied warranty of
+    MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
+    GNU General Public License for more details.
+
+    You should have received a copy of the GNU General Public License
+    along with QGROUNDCONTROL. If not, see <http://www.gnu.org/licenses/>.
+
+======================================================================*/
+
+#ifndef PARAMETERLIST_H
+#define PARAMETERLIST_H
+
+#include <QMap>
+
+#include "mavlink_types.h"
+#include "QGCParamID.h"
+#include "Parameter.h"
+#include "OpalRT.h"
+
+// Forward declare ParameterList before including OpalLink.h because a member of type ParameterList is used in OpalLink
+namespace OpalRT
+{
+        class ParameterList;
+}
+#include "OpalLink.h"
+namespace OpalRT{
+    class ParameterList
+    {
+    public:
+        ParameterList();
+        ~ParameterList();
+        int setValue(int compid, QGCParamID paramid, float value);
+        float getValue(int compid, QGCParamID paramid);
+    protected:
+        QMap<int, QMap<QGCParamID, Parameter> > *params;
+
+        void getParameterList(QMap<QString, unsigned short>*);
+
+    };
+}
+#endif // PARAMETERLIST_H

src/comm/QGCParamID.cc

 #include "QGCParamID.h"
 using namespace OpalRT;
 
-QGCParamID::QGCParamID()
+QGCParamID::QGCParamID(const char *paramid):QString(paramid)
 {
 }
+
+QGCParamID::QGCParamID(const QGCParamID &other):QString(other)
+{
+
+}
+
+/*
+bool QGCParamID::operator<(const QGCParamID& other)
+{
+    const QString *lefthand, *righthand;
+    lefthand = this;
+    righthand = &other;
+    return lefthand < righthand;
+}
+*/

src/comm/QGCParamID.h

+/*=====================================================================
+
+QGroundControl Open Source Ground Control Station
+
+(c) 2009, 2010 QGROUNDCONTROL PROJECT <http://www.qgroundcontrol.org>
+
+This file is part of the QGROUNDCONTROL project
+
+    QGROUNDCONTROL is free software: you can redistribute it and/or modify
+    it under the terms of the GNU General Public License as published by
+    the Free Software Foundation, either version 3 of the License, or
+    (at your option) any later version.
+
+    QGROUNDCONTROL is distributed in the hope that it will be useful,
+    but WITHOUT ANY WARRANTY; without even the implied warranty of
+    MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
+    GNU General Public License for more details.
+
+    You should have received a copy of the GNU General Public License
+    along with QGROUNDCONTROL. If not, see <http://www.gnu.org/licenses/>.
+
+======================================================================*/
+
 #ifndef QGCPARAMID_H
 #define QGCPARAMID_H
 
@@ -5,10 +28,15 @@
 
 namespace OpalRT
 {
-    class QGCParamID : protected QString
+    class QGCParamID : public QString
     {
     public:
-        QGCParamID();
+
+        QGCParamID(const char *paramid);
+        QGCParamID() {}
+        QGCParamID(const QGCParamID& other);
+
+//       bool operator<(const QGCParamID& other);
     };
 }
 #endif // QGCPARAMID_H