Skip to content

Q
qgroundcontrol
Commit 062fddaf authored by Bryan Godbolt's avatar Bryan Godbolt
Browse files
Options

First version of OpalLink working. Signals can be read, and parameters can be set via qgc

parent bb350243
Hide whitespace changes
Inline Side-by-side
Showing with 323 additions and 233 deletions
qgroundcontrol.pro
View file @ 062fddaf
......@@ -225,21 +225,20 @@ SOURCES += src/main.cc \
RESOURCES = mavground.qrc
# Include RT-LAB Library
win32 {
exists(src/lib/opalrt/OpalApi.h){
win32:exists(src/lib/opalrt/OpalApi.h) {
message("Building support for Opal-RT")
LIBS += -LC:\OPAL-RT\RT-LAB7.2.4\Common\bin \
-lOpalApi
INCLUDEPATH += src/lib/opalrt
SOURCES += src/comm/OpalLink.cc \
src/comm/Parameter.cc \
src/comm/QGCParamID.cc \
src/comm/ParameterList.cc
HEADERS += src/comm/OpalLink.h \
src/comm/OpalRT.h \
HEADERS += src/comm/OpalRT.h \
src/comm/OpalLink.h \
src/comm/Parameter.h \
src/comm/QGCParamID.h \
src/comm/ParameterList.h
SOURCES += src/comm/OpalRT.cc \
src/comm/OpalLink.cc \
src/comm/Parameter.cc \
src/comm/QGCParamID.cc \
src/comm/ParameterList.cc
DEFINES += OPAL_RT
}
}
src/comm/OpalLink.cc
View file @ 062fddaf
......@@ -81,16 +81,9 @@ void OpalLink::writeBytes(const char *bytes, qint64 length)
case MAVLINK_MSG_ID_PARAM_REQUEST_LIST:
{
qDebug() << "OpalLink::writeBytes(): request params";
// getParameterList();
mavlink_message_t param;
// char paramName[] = "NAV_FILT_INIT";
// mavlink_msg_param_value_pack(systemID, componentID, &param,
// (int8_t*)(paramName),
// 0,
// 1,
// 0);
// receiveMessage(param);
OpalRT::ParameterList::const_iterator paramIter;
for (paramIter = params->begin(); paramIter != params->end(); ++paramIter)
......@@ -109,23 +102,32 @@ void OpalLink::writeBytes(const char *bytes, qint64 length)
}
case MAVLINK_MSG_ID_PARAM_SET:
{
qDebug() << "OpalLink::writeBytes(): Attempt to set a parameter";
// qDebug() << "OpalLink::writeBytes(): Attempt to set a parameter";
mavlink_param_set_t param;
mavlink_msg_param_set_decode(&msg, &param);
QString paramName((char*)param.param_id);
qDebug() << "OpalLink::writeBytes():paramName: " << paramName;
if (paramName == "NAV_FILT_INIT")
OpalRT::QGCParamID paramName((char*)param.param_id);
// qDebug() << "OpalLink::writeBytes():paramName: " << paramName;
if ((*params).contains(param.target_component, paramName))
{
if (param.param_value == 1 || param.param_value == 0)
{
double values[2] = {};
values[0] = param.param_value;
setSignals(values);
}
else
{
qDebug() << "OpalLink::writeBytes(): Param NAV_FILT_INIT must be 1 or 0";
}
OpalRT::Parameter p = (*params)(param.target_component, paramName);
// qDebug() << __FILE__ << ":" << __LINE__ << ": " << p;
// Set the param value in Opal-RT
p.setValue(param.param_value);
// Get the param value from Opal-RT to make sure it was set properly
mavlink_message_t paramMsg;
mavlink_msg_param_value_pack(systemID,
p.getComponentID(),
&paramMsg,
p.getParamID().toInt8_t(),
p.getValue(),
params->count(),
params->indexOf(p));
receiveMessage(paramMsg);
}
}
break;
......@@ -141,10 +143,6 @@ void OpalLink::writeBytes(const char *bytes, qint64 length)
void OpalLink::readBytes()
{
receiveDataMutex.lock();
// qDebug() << "OpalLink::readBytes(): Reading a message. size of buffer: " << receiveBuffer->count();
// QByteArray message = receiveBuffer->dequeue();
emit bytesReceived(this, receiveBuffer->dequeue());
receiveDataMutex.unlock();
......@@ -173,7 +171,6 @@ void OpalLink::heartbeat()
if (m_heartbeatsEnabled)
{
// qDebug() << "OpalLink::heartbeat(): Generate a heartbeat";
mavlink_message_t beat;
mavlink_msg_heartbeat_pack(systemID, componentID,&beat, MAV_HELICOPTER, MAV_AUTOPILOT_GENERIC);
receiveMessage(beat);
......@@ -185,30 +182,17 @@ void OpalLink::setSignals(double *values)
unsigned short numSignals = 2;
unsigned short logicalId = 1;
unsigned short signalIndex[] = {0,1};
// double values[] = {0.5, // ch 1
// 0.5, // ch2
// 0.5, // ch3
// 0.5, // ch4
// 0.5, // ch5
// 0.5, // ch6
// 0.5, // ch7
// 0.5, // ch8
// 0.5}; // ch9
int returnValue;
returnValue = OpalSetSignals( numSignals, logicalId, signalIndex, values);
if (returnValue != EOK)
{
setLastErrorMsg();
displayLastErrorMsg();
// OpalRT::setLastErrorMsg();
OpalRT::OpalErrorMsg::displayLastErrorMsg();
}
}
void OpalLink::getSignals()
{
// getSignalsMutex.lock();
// qDebug() << "OpalLink::getSignals(): Attempting to acquire signals";
unsigned long timeout = 0;
unsigned short acqGroup = 0; //this is actually group 1 in the model
unsigned short allocatedSignals = NUM_OUTPUT_SIGNALS;
......@@ -224,8 +208,7 @@ void OpalLink::getSignals()
values, lastValues, decimation);
if (returnVal == EOK )
{
// qDebug() << "OpalLink::getSignals: Timestep=" << *timestep;// << ", Last? " << (bool)(*lastValues);
{
/* Send position info to qgroundcontrol */
mavlink_message_t local_position;
mavlink_msg_local_position_pack(systemID, componentID, &local_position,
......@@ -263,16 +246,11 @@ void OpalLink::getSignals()
values[OpalRT::B_W_2]
);
receiveMessage(bias);
}
// else if (returnVal == EAGAIN)
// {
// qDebug() << "OpalLink::getSignals: Data was not ready";
// }
// if returnVal == EAGAIN => data just wasn't ready
else if (returnVal != EAGAIN)
}
else if (returnVal != EAGAIN) // if returnVal == EAGAIN => data just wasn't ready
{
getSignalsTimer->stop();
displayLastErrorMsg();
OpalRT::OpalErrorMsg::displayLastErrorMsg();
}
}
......@@ -282,82 +260,10 @@ void OpalLink::getSignals()
delete timestep;
delete lastValues;
delete decimation;
// getSignalsMutex.unlock();
}
void OpalLink::getParameterList()
{
/* 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 **path=NULL;
unsigned short maxNameLen;
char **name=NULL;
unsigned short maxVarLen;
char **var=NULL;
int returnValue;
returnValue = OpalGetParameterList(allocatedParams, &numParams, idParam,
allocatedPathLen, &maxPathLen, path,
allocatedNameLen, &maxNameLen, name,
allocatedVarLen, &maxVarLen, var);
if (returnValue!=E2BIG)
{
setLastErrorMsg();
displayLastErrorMsg();
return;
}
// allocate memory for parameter list
idParam = new unsigned short[numParams];
allocatedParams = numParams;
path = new char*[numParams];
for (int i=0; i<numParams; i++)
path[i]=new char[maxPathLen];
allocatedPathLen = maxPathLen;
name = new char*[numParams];
for (int i=0; i<numParams; i++)
name[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, path,
allocatedNameLen, &maxNameLen, name,
allocatedVarLen, &maxVarLen, var);
if (returnValue != EOK)
{
setLastErrorMsg();
displayLastErrorMsg();
return;
}
qDebug() << "Num params: " << numParams << endl;
qDebug() << "Name\tPath\tVar" << endl;
for (int i=0; i<numParams; i++)
qDebug() << qSetFieldWidth(20) << name[i] << qSetFieldWidth(5) << idParam[i]
<< qSetFieldWidth(50) << path[i];
}
/*
*
Administrative
......@@ -384,8 +290,7 @@ void OpalLink::setName(QString name)
emit nameChanged(this->name);
}
bool OpalLink::isConnected() {
//qDebug() << "OpalLink::isConnected:: connectState: " << connectState;
bool OpalLink::isConnected() {
return connectState;
}
......@@ -397,7 +302,9 @@ bool OpalLink::connect()
short modelState;
/// \todo allow configuration of instid in window
if ((OpalConnect(101, false, &modelState) == EOK) && (OpalGetSignalControl(0, true) == EOK))
if ((OpalConnect(101, false, &modelState) == EOK)
&& (OpalGetSignalControl(0, true) == EOK)
&& (OpalGetParameterControl(true) == EOK))
{
connectState = true;
/// \todo try/catch a delete in case params has already been allocated
......@@ -409,7 +316,7 @@ bool OpalLink::connect()
else
{
connectState = false;
displayLastErrorMsg();
OpalRT::OpalErrorMsg::displayLastErrorMsg();
}
emit connected(connectState);
......@@ -421,25 +328,7 @@ bool OpalLink::disconnect()
return false;
}
void OpalLink::displayLastErrorMsg()
{
static QString lastErrorMsg;
setLastErrorMsg();
QMessageBox msgBox;
msgBox.setIcon(QMessageBox::Critical);
msgBox.setText(lastErrorMsg);
msgBox.exec();
}
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
View file @ 062fddaf
......@@ -33,7 +33,6 @@ This file is part of the QGROUNDCONTROL project
#include <QMutex>
#include <QDebug>
#include <QTextStreamManipulator>
#include <QMessageBox>
#include <QTimer>
#include <QQueue>
#include <QByteArray>
......@@ -50,16 +49,9 @@ This file is part of the QGROUNDCONTROL project
#include "OpalRT.h"
#include "ParameterList.h"
#include "Parameter.h"
#include "errno.h"
#ifdef OPAL_RT
#include "QGCParamID.h"
#include "OpalApi.h"
#endif
#include "errno.h"
#include "string.h"
/*
......@@ -110,8 +102,7 @@ public:
qint64 bytesAvailable();
void run();
static void setLastErrorMsg();
static void displayLastErrorMsg();
public slots:
void writeBytes(const char *bytes, qint64 length);
......@@ -142,8 +133,6 @@ protected:
QMutex statisticsMutex;
QMutex receiveDataMutex;
// QMutex getSignalsMutex;
static QString lastErrorMsg;
void setName(QString name);
......@@ -164,5 +153,4 @@ protected:
OpalRT::ParameterList *params;
};
//QString OpalLink::lastErrorMsg = QString();
#endif // OPALLINK_H
src/comm/OpalRT.h
View file @ 062fddaf
......@@ -21,9 +21,20 @@ This file is part of the QGROUNDCONTROL project
======================================================================*/
/**
* @file
* @brief Types used for Opal-RT interface configuration
* @author Bryan Godbolt <godbolt@ualberta.ca>
*/
#ifndef OPALRT_H
#define OPALRT_H
#include <QString>
#include <QMessageBox>
#include "OpalApi.h"
namespace OpalRT
{
/* ------------------------------ Outputs ------------------------------
......@@ -78,5 +89,13 @@ namespace OpalRT
LOG_ID = 1,
CONTROLLER_ID = 1
};
class OpalErrorMsg
{
static QString lastErrorMsg;
public:
static void displayLastErrorMsg();
static void setLastErrorMsg();
};
}
#endif // OPALRT_H
src/comm/Parameter.cc
View file @ 062fddaf
/*=====================================================================
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/>.
======================================================================*/
/**
* @file
* @brief Implementation of class OpalRT::Parameter
* @author Bryan Godbolt <godbolt@ualberta.ca>
*/
#include "Parameter.h"
using namespace OpalRT;
......@@ -38,11 +66,10 @@ bool Parameter::operator ==(const Parameter& other) const
}
float Parameter::getValue() //const
float Parameter::getValue()
{
unsigned short allocatedParams = 1;
unsigned short numParams;
// unsigned short allocatedValues;
unsigned short numValues = 1;
unsigned short returnedNumValues;
double value;
......@@ -52,8 +79,33 @@ float Parameter::getValue() //const
if (returnVal != EOK)
{
// OpalRT::setLastErrorMsg();
OpalRT::OpalErrorMsg::displayLastErrorMsg();
return FLT_MAX;
}
return static_cast<float>(value);
}
void Parameter::setValue(float val)
{
unsigned short allocatedParams = 1;
unsigned short numParams;
unsigned short numValues = 1;
unsigned short returnedNumValues;
double value = static_cast<double>(val);
int returnVal = OpalSetParameters(allocatedParams, &numParams, &opalID,
numValues, &returnedNumValues, &value);
if (returnVal != EOK)
{
qDebug() << __FILE__ << ":" << __LINE__ << ": Error numer: " << QString::number(returnVal);
OpalErrorMsg::displayLastErrorMsg();
}
}
Parameter::operator QString() const
{
return *simulinkPath + *simulinkName + " " + QString::number(componentID)
+ " " + *paramID + " " + QString::number(opalID);
}
src/comm/Parameter.h
View file @ 062fddaf
......@@ -21,13 +21,24 @@ This file is part of the QGROUNDCONTROL project
======================================================================*/
/**
* @file
* @brief Parameter Object used to intefrace with an OpalRT Simulink Parameter
\see OpalLink
\see OpalRT::ParameterList
* @author Bryan Godbolt <godbolt@ualberta.ca>
*/
#ifndef PARAMETER_H
#define PARAMETER_H
#include <QString>
#include <QDebug>
#include "mavlink_types.h"
#include "QGCParamID.h"
#include "OpalApi.h"
#include "OpalRT.h"
#include <cfloat>
namespace OpalRT
......@@ -48,9 +59,11 @@ namespace OpalRT
const QString& getSimulinkPath() {return *simulinkPath;}
const QString& getSimulinkName() {return *simulinkName;}
uint8_t getComponentID() const {return componentID;}
float getValue();// const;
float getValue();
void setValue(float value);
bool operator==(const Parameter& other) const;
operator QString() const;
protected:
QString *simulinkPath;
......
src/comm/ParameterList.cc
View file @ 062fddaf
/*=====================================================================
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/>.
======================================================================*/
/**
* @file
* @brief Implementation of class OpalRT::ParameterList
* @author Bryan Godbolt <godbolt@ualberta.ca>
*/
#include "ParameterList.h"
using namespace OpalRT;
ParameterList::ParameterList()
:params(new QMap<int, QMap<QGCParamID, Parameter> >),
paramVector(new QVector<Parameter>)
paramList(new QList<QList<Parameter*> >())
{
// params = new QMap<int, QMap<QGCParamID, Parameter> >;
......@@ -49,18 +78,19 @@ ParameterList::ParameterList()
QString s;
for (componentIter = params->begin(); componentIter != params->end(); ++componentIter)
{
paramList->append(QList<Parameter*>());
for (paramIter = (*componentIter).begin(); paramIter != (*componentIter).end(); ++paramIter)
{
paramList->last().append(paramIter.operator ->());
s = (*paramIter).getSimulinkPath() + (*paramIter).getSimulinkName();
paramVector->append((*paramIter));
if (opalParams->contains(s))
{
(*paramIter).setOpalID(opalParams->value(s));
qDebug() << __FILE__ << " Line:" << __LINE__ << ": Successfully added " << s;
// qDebug() << __FILE__ << " Line:" << __LINE__ << ": Successfully added " << s;
}
else
{
qDebug() << __FILE__ << " Line:" << __LINE__ << ": " << s << " was not found in param list";
qWarning() << __FILE__ << " Line:" << __LINE__ << ": " << s << " was not found in param list";
}
}
}
......@@ -71,35 +101,7 @@ ParameterList::ParameterList()
ParameterList::~ParameterList()
{
delete params;
delete paramVector;
}
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;
}
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();
delete paramList;
}
/**
......@@ -134,8 +136,8 @@ void ParameterList::getParameterList(QMap<QString, unsigned short> *opalParams)
allocatedVarLen, &maxVarLen, var);
if (returnValue!=E2BIG)
{
OpalLink::setLastErrorMsg();
OpalLink::displayLastErrorMsg();
// OpalRT::setLastErrorMsg();
OpalRT::OpalErrorMsg::displayLastErrorMsg();
return;
}
......@@ -166,8 +168,8 @@ void ParameterList::getParameterList(QMap<QString, unsigned short> *opalParams)
if (returnValue != EOK)
{
OpalLink::setLastErrorMsg();
OpalLink::displayLastErrorMsg();
// OpalRT::setLastErrorMsg();
OpalRT::OpalErrorMsg::displayLastErrorMsg();
return;
}
......@@ -191,8 +193,55 @@ void ParameterList::getParameterList(QMap<QString, unsigned short> *opalParams)
}
int ParameterList::count()
int ParameterList::indexOf(const Parameter &p)
{
// incase p is a copy of the actual parameter we want (i.e., addresses differ)
Parameter *pPtr = &((*params)[p.getComponentID()][p.getParamID()]);
QList<QList<Parameter*> >::const_iterator iter;
int index = -1;
for (iter = paramList->begin(); iter != paramList->end(); ++iter)
{
if ((index = (*iter).indexOf(pPtr)) != -1)
return index;
}
return index;
}
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;
}
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.paramList.count();
return iter+=iter.paramList.size();
}
int ParameterList::count()
{
int count = 0;
QList<QList<Parameter*> >::const_iterator iter;
for (iter = paramList->begin(); iter != paramList->end(); ++iter)
count += (*iter).count();
return count;
}
src/comm/ParameterList.h
View file @ 062fddaf
......@@ -25,19 +25,13 @@ This file is part of the QGROUNDCONTROL project
#define PARAMETERLIST_H
#include <QMap>
#include <QVector>
#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
namespace OpalRT
{
class ParameterList;
}
#include "OpalLink.h"
namespace OpalRT
{
class ParameterList
......@@ -71,21 +65,61 @@ namespace OpalRT
ParameterList();
~ParameterList();
int setValue(int compid, QGCParamID paramid, float value);
float getValue(int compid, QGCParamID paramid);
/** Count the number of parameters in the list.
\return Total number of parameters
*/
int count();
int indexOf(const Parameter& p) {return paramVector->indexOf(p);}
/** Find p in the list and return its index.
\note In order to use this index to look up p, the component is also needed.
\return the index of p or -1 if p is not found
\example
\code
int compid = OpalRT::CONTROLLER_ID;
Parameter p("simulinkpath", "simulinkparamname", compid, QGCParamID("PID_GAIN"));
ParameterList pList;
if ((int index=pList.indexOf(p)) != -1)
qDebug() << "PID_GAIN is at index " << index;
\endcode
*/
int indexOf(const Parameter& p);
bool contains(int compid, QGCParamID paramid) const {return (*params)[compid].contains(paramid);}
/// Get a parameter from the list
const Parameter getParameter(int compid, QGCParamID paramid) const {return (*params)[compid][paramid];}
Parameter& getParameter(int compid, QGCParamID paramid) {return (*params)[compid][paramid];}
const Parameter getParameter(int compid, int index) const {return *((*paramList)[compid][index]);}
/** Convenient syntax for calling OpalRT::Parameter::getParameter() */
Parameter& operator()(int compid, QGCParamID paramid) {return getParameter(compid, paramid);}
Parameter& operator()(uint8_t compid, QGCParamID paramid) {return getParameter(static_cast<int>(compid), paramid);}
const_iterator begin() const;