Skip to content
Projects
Groups
Snippets
Help
Loading...
Help
Support
Submit feedback
Contribute to GitLab
Sign in
Toggle navigation
Q
qgroundcontrol
Project
Project
Details
Activity
Releases
Cycle Analytics
Repository
Repository
Files
Commits
Branches
Tags
Contributors
Graph
Compare
Charts
Issues
0
Issues
0
List
Boards
Labels
Milestones
Merge Requests
0
Merge Requests
0
CI / CD
CI / CD
Pipelines
Jobs
Schedules
Charts
Wiki
Wiki
Snippets
Snippets
Members
Members
Collapse sidebar
Close sidebar
Activity
Graph
Charts
Create a new issue
Jobs
Commits
Issue Boards
Open sidebar
Valentin Platzgummer
qgroundcontrol
Commits
062fddaf
Commit
062fddaf
authored
Aug 31, 2010
by
Bryan Godbolt
Browse files
Options
Browse Files
Download
Email Patches
Plain Diff
First version of OpalLink working. Signals can be read, and parameters can be set via qgc
parent
bb350243
Changes
10
Hide whitespace changes
Inline
Side-by-side
Showing
10 changed files
with
323 additions
and
233 deletions
+323
-233
qgroundcontrol.pro
qgroundcontrol.pro
+8
-9
OpalLink.cc
src/comm/OpalLink.cc
+35
-146
OpalLink.h
src/comm/OpalLink.h
+3
-15
OpalRT.h
src/comm/OpalRT.h
+19
-0
Parameter.cc
src/comm/Parameter.cc
+54
-2
Parameter.h
src/comm/Parameter.h
+14
-1
ParameterList.cc
src/comm/ParameterList.cc
+88
-39
ParameterList.h
src/comm/ParameterList.h
+49
-15
QGCParamID.cc
src/comm/QGCParamID.cc
+33
-5
QGCParamID.h
src/comm/QGCParamID.h
+20
-1
No files found.
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, ¶m,
// (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
\t
Path
\t
Var"
<<
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
>
>
),
param
Vector
(
new
QVector
<
Parameter
>
)
param
List
(
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
{
q
Debu
g
()
<<
__FILE__
<<
" Line:"
<<
__LINE__
<<
": "
<<
s
<<
" was not found in param list"
;
q
Warnin
g
()
<<
__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
();
Opal
Link
::
displayLastErrorMsg
();
// OpalRT
::setLastErrorMsg();
Opal
RT
::
OpalErrorMsg
::
displayLastErrorMsg
();
return
;
}
...
...
@@ -166,8 +168,8 @@ void ParameterList::getParameterList(QMap<QString, unsigned short> *opalParams)
if
(
returnValue
!=
EOK
)
{
OpalLink
::
setLastErrorMsg
();
Opal
Link
::
displayLastErrorMsg
();
// OpalRT
::setLastErrorMsg();
Opal
RT
::
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
;