Commit e402c344 authored by Bryan Godbolt's avatar Bryan Godbolt

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

parent b647bb9b
......@@ -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
}
......@@ -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);
......
......@@ -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
/*=====================================================================
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
#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;
}
/*=====================================================================
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;
};
}
......
#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();
}
/*=====================================================================
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
#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;
}
*/
/*=====================================================================
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
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