zhaohuwei
/
QG4
1
0
Fork 0
Code Issues Pull Requests Projects Releases Wiki Activity
Browse Source

Merge pull request #1348 from Susurrus/cleanup 

Cleanup remnants OpalRT and Kinect
QGC4.4
Don Gagne 10 years ago
parent
c25d1475ac bed84c00fc
commit
7671c26b6d
11 changed files with 2 additions and 1721 deletions
Whitespace
Show all changes Ignore whitespace when comparing lines Ignore changes in amount of whitespace Ignore changes in whitespace at EOL
Split View
Diff Options
Show Stats Download Patch File Download Diff File
  1. 6
      QGCSetup.pri
  2. 227
      data/ParameterList.xml
  3. 37
      data/kinect.cal
  4. 118
      src/comm/Parameter.cc
  5. 92
      src/comm/Parameter.h
  6. 332
      src/comm/ParameterList.cc
  7. 175
      src/comm/ParameterList.h
  8. 51
      src/comm/QGCParamID.cc
  9. 82
      src/comm/QGCParamID.h
  10. 449
      src/input/Freenect.cc
  11. 154
      src/input/Freenect.h

6
QGCSetup.pri
Unescape View File

@ -36,14 +36,12 @@ MacBuild { @@ -36,14 +36,12 @@ MacBuild {
# in the target.
WindowsBuild {
# Make sure to keep both side of this if using the same set of directories
DESTDIR_COPY_RESOURCE_LIST = $$replace(DESTDIR,"/","\\")
BASEDIR_COPY_RESOURCE_LIST = $$replace(BASEDIR,"/","\\")
DESTDIR_COPY_RESOURCE_LIST = $$replace(DESTDIR,"/","\\")
BASEDIR_COPY_RESOURCE_LIST = $$replace(BASEDIR,"/","\\")
QMAKE_POST_LINK += $$escape_expand(\\n) $$QMAKE_COPY_DIR \"$$BASEDIR_COPY_RESOURCE_LIST\\files\" \"$$DESTDIR_COPY_RESOURCE_LIST\\files\"
QMAKE_POST_LINK += $$escape_expand(\\n) $$QMAKE_COPY_DIR \"$$BASEDIR_COPY_RESOURCE_LIST\\data\" \"$$DESTDIR_COPY_RESOURCE_LIST\\data\"
} else {
# Make sure to keep both side of this if using the same set of directories
QMAKE_POST_LINK += && $$QMAKE_COPY_DIR $$BASEDIR/files $$DESTDIR_COPY_RESOURCE_LIST
QMAKE_POST_LINK += && $$QMAKE_COPY_DIR $$BASEDIR/data $$DESTDIR_COPY_RESOURCE_LIST
}
#

227
data/ParameterList.xml
Unescape View File

@ -1,227 +0,0 @@ @@ -1,227 +0,0 @@
<ParameterList version="0.1">
<!-- Parameters in the top level block -->
<Block name="TopLevel">
</Block>
<!--Parameters related to the navigation block -->
<Block name="Navigation">
<Parameter
SimulinkPath="avionics_src/sm_avionics/Navigation_Filter/NAV_FILT_INIT/"
SimulinkParameterName="Value"
QGCParamID="NAV_FILT_INIT"
/>
</Block>
<!--Parameters related to the controller block -->
<Block name="Controller">
<!-- Aileron Control Parameters -->
<Parameter
SimulinkPath="avionics_src/sm_avionics/Attitude_Controller/PID_AIL/kp/"
SimulinkParameterName="Gain"
QGCParamID="AIL_KP"
/>
<Parameter
SimulinkPath="avionics_src/sm_avionics/Attitude_Controller/PID_AIL/ki/"
SimulinkParameterName="Gain"
QGCParamID="AIL_KI"
/>
<Parameter
SimulinkPath="avionics_src/sm_avionics/Attitude_Controller/PID_AIL/kd/"
SimulinkParameterName="Gain"
QGCParamID="AIL_KD"
/>
<Parameter
SimulinkPath="avionics_src/sm_avionics/Attitude_Controller/PID_AIL/Angle_Norm/Max_Min_Angle/"
SimulinkParameterName="Value"
QGCParamID="ANG_AIL_MAX"
/>
<!-- Elevator Control Parameters -->
<Parameter
SimulinkPath="avionics_src/sm_avionics/Attitude_Controller/PID_ELE/kp/"
SimulinkParameterName="Gain"
QGCParamID="ELE_KP"
/>
<Parameter
SimulinkPath="avionics_src/sm_avionics/Attitude_Controller/PID_ELE/ki/"
SimulinkParameterName="Gain"
QGCParamID="ELE_KI"
/>
<Parameter
SimulinkPath="avionics_src/sm_avionics/Attitude_Controller/PID_ELE/kd/"
SimulinkParameterName="Gain"
QGCParamID="ELE_KD"
/>
<Parameter
SimulinkPath="avionics_src/sm_avionics/Attitude_Controller/PID_ELE/Angle_Norm/Max_Min_Angle/"
SimulinkParameterName="Value"
QGCParamID="ANG_ELE_MAX"
/>
</Block>
<!-- Paremters for the Pilot Input/Raw RC block -->
<Block name="ServoInputs">
<!-- Settings for Aileron Servo -->
<Parameter
SimulinkPath="avionics_src/sm_avionics/Pilot_Inputs/AileronInput/"
SimulinkParameterName="Setpoint0"
QGCParamID="AIL_LEFT_IN"
/>
<Parameter
SimulinkPath="avionics_src/sm_avionics/Pilot_Inputs/AileronInput/"
SimulinkParameterName="Setpoint1"
QGCParamID="AIL_CENTER_IN"
/>
<Parameter
SimulinkPath="avionics_src/sm_avionics/Pilot_Inputs/AileronInput/"
SimulinkParameterName="Setpoint2"
QGCParamID="AIL_RIGHT_IN"
/>
<!-- Settings for Elevator Servo -->
<Parameter
SimulinkPath="avionics_src/sm_avionics/Pilot_Inputs/ElevatorInput/"
SimulinkParameterName="Setpoint0"
QGCParamID="ELE_DOWN_IN"
/>
<Parameter
SimulinkPath="avionics_src/sm_avionics/Pilot_Inputs/ElevatorInput/"
SimulinkParameterName="Setpoint1"
QGCParamID="ELE_CENTER_IN"
/>
<Parameter
SimulinkPath="avionics_src/sm_avionics/Pilot_Inputs/ElevatorInput/"
SimulinkParameterName="Setpoint2"
QGCParamID="ELE_UP_IN"
/>
<!-- Settings for Throttle Servo -->
<Parameter
SimulinkPath="avionics_src/sm_avionics/Pilot_Inputs/ThrottleInput/"
SimulinkParameterName="Setpoint0"
QGCParamID="THR_SET0_IN"
/>
<Parameter
SimulinkPath="avionics_src/sm_avionics/Pilot_Inputs/ThrottleInput/"
SimulinkParameterName="Setpoint1"
QGCParamID="THR_SET1_IN"
/>
<Parameter
SimulinkPath="avionics_src/sm_avionics/Pilot_Inputs/ThrottleInput/"
SimulinkParameterName="Setpoint2"
QGCParamID="THR_SET2_IN"
/>
<Parameter
SimulinkPath="avionics_src/sm_avionics/Pilot_Inputs/ThrottleInput/"
SimulinkParameterName="Setpoint3"
QGCParamID="THR_SET3_IN"
/>
<Parameter
SimulinkPath="avionics_src/sm_avionics/Pilot_Inputs/ThrottleInput/"
SimulinkParameterName="Setpoint4"
QGCParamID="THR_SET4_IN"
/>
<!-- Settings for Rudder Servo -->
<Parameter
SimulinkPath="avionics_src/sm_avionics/Pilot_Inputs/RudderInput/"
SimulinkParameterName="Setpoint0"
QGCParamID="RUD_LEFT_IN"
/>
<Parameter
SimulinkPath="avionics_src/sm_avionics/Pilot_Inputs/RudderInput/"
SimulinkParameterName="Setpoint1"
QGCParamID="RUD_CENTER_IN"
/>
<Parameter
SimulinkPath="avionics_src/sm_avionics/Pilot_Inputs/RudderInput/"
SimulinkParameterName="Setpoint2"
QGCParamID="RUD_RIGHT_IN"
/>
<!-- Settings for Gyro Mode/Gain Switch -->
<Parameter
SimulinkPath="avionics_src/sm_avionics/Pilot_Inputs/GyroInput/"
SimulinkParameterName="Setpoint0"
QGCParamID="GYRO_DEF_IN"
/>
<Parameter
SimulinkPath="avionics_src/sm_avionics/Pilot_Inputs/GyroInput/"
SimulinkParameterName="Setpoint1"
QGCParamID="GYRO_TOG_IN"
/>
<!-- Settings for Pitch Servo -->
<Parameter
SimulinkPath="avionics_src/sm_avionics/Pilot_Inputs/PitchInput/"
SimulinkParameterName="Setpoint0"
QGCParamID="PIT_SET0_IN"
/>
<Parameter
SimulinkPath="avionics_src/sm_avionics/Pilot_Inputs/PitchInput/"
SimulinkParameterName="Setpoint1"
QGCParamID="PIT_SET1_IN"
/>
<Parameter
SimulinkPath="avionics_src/sm_avionics/Pilot_Inputs/PitchInput/"
SimulinkParameterName="Setpoint2"
QGCParamID="PIT_SET2_IN"
/>
<Parameter
SimulinkPath="avionics_src/sm_avionics/Pilot_Inputs/PitchInput/"
SimulinkParameterName="Setpoint3"
QGCParamID="PIT_SET3_IN"
/>
<Parameter
SimulinkPath="avionics_src/sm_avionics/Pilot_Inputs/PitchInput/"
SimulinkParameterName="Setpoint4"
QGCParamID="PIT_SET4_IN"
/>
</Block>
<!-- Parameters for the servo output block -->
<Block name="ServoOutputs">
<!-- Settings for Aileron Servo -->
<Parameter
SimulinkPath="avionics_src/sm_avionics/Servo_Outputs/AileronOutput/"
SimulinkParameterName="Setpoint0"
QGCParamID="AIL_LEFT_OUT"
/>
<Parameter
SimulinkPath="avionics_src/sm_avionics/Servo_Outputs/AileronOutput/"
SimulinkParameterName="Setpoint1"
QGCParamID="AIL_CENTER_OUT"
/>
<Parameter
SimulinkPath="avionics_src/sm_avionics/Servo_Outputs/AileronOutput/"
SimulinkParameterName="Setpoint2"
QGCParamID="AIL_RIGHT_OUT"
/>
<Parameter
SimulinkPath="avionics_src/sm_avionics/Servo_Outputs/AileronMix/Controller_Mix/"
SimulinkParameterName="Value"
QGCParamID="MIX_AIL"
/>
<!-- Settings for Elevator Servo -->
<Parameter
SimulinkPath="avionics_src/sm_avionics/Servo_Outputs/ElevatorOutput/"
SimulinkParameterName="Setpoint0"
QGCParamID="ELE_DOWN_OUT"
/>
<Parameter
SimulinkPath="avionics_src/sm_avionics/Servo_Outputs/ElevatorOutput/"
SimulinkParameterName="Setpoint1"
QGCParamID="ELE_CENTER_OUT"
/>
<Parameter
SimulinkPath="avionics_src/sm_avionics/Servo_Outputs/ElevatorOutput/"
SimulinkParameterName="Setpoint2"
QGCParamID="ELE_UP_OUT"
/>
<Parameter
SimulinkPath="avionics_src/sm_avionics/Servo_Outputs/ElevatorMix/Controller_Mix/"
SimulinkParameterName="Value"
QGCParamID="MIX_ELE"
/>
</Block>
</ParameterList>

37
data/kinect.cal
Unescape View File

@ -1,37 +0,0 @@ @@ -1,37 +0,0 @@
[rgb]
principal_point\x=314.70228964
principal_point\y=264.30478827
focal_length\x=527.91246131
focal_length\y=527.91246131
distortion\k1=0.20496745
distortion\k2=-0.36341243
distortion\k3=0.00000000
distortion\k4=0.00000000
distortion\k5=0.00000000
[depth]
principal_point\x=311.88621344
principal_point\y=247.63447078
focal_length\x=593.89813561
focal_length\y=593.89813561
distortion\k1=0.00000000
distortion\k2=0.00000000
distortion\k3=0.00000000
distortion\k4=0.00000000
distortion\k5=0.00000000
[transform]
R11=0.999982
R12=0.000556
R13=0.005927
R21=-0.000563
R22=0.999999
R23=0.001235
R31=-0.005926
R32=-0.001239
R33=0.999982
Tx=-0.024287
Ty=0.001018
Tz=-0.015195
baseline=0.06061
disparity_offset=1092.3403

118
src/comm/Parameter.cc
Unescape View File

@ -1,118 +0,0 @@ @@ -1,118 +0,0 @@
/*=====================================================================
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;
//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(QString simulinkPath, QString 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;
}
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()
{
unsigned short allocatedParams = 1;
unsigned short numParams;
unsigned short numValues = 1;
unsigned short returnedNumValues;
double value;
int returnVal = OpalGetParameters(allocatedParams, &numParams, &opalID,
numValues, &returnedNumValues, &value);
if (returnVal != EOK) {
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);
}

92
src/comm/Parameter.h
Unescape View File

@ -1,92 +0,0 @@ @@ -1,92 +0,0 @@
/*=====================================================================
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 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
{
class Parameter
{
public:
// Parameter(char *simulinkPath = "",
// char *simulinkName = "",
// uint8_t componentID = 0,
// QGCParamID paramID = QGCParamID(),
// unsigned short opalID = 0);
Parameter(QString simulinkPath = QString(),
QString simulinkName = QString(),
uint8_t componentID = 0,
QGCParamID paramID = QGCParamID(),
unsigned short opalID = 0);
Parameter(const Parameter& other);
~Parameter();
const QGCParamID& getParamID() const {
return *paramID;
}
void setOpalID(unsigned short opalID) {
this->opalID = opalID;
}
const QString& getSimulinkPath() const {
return *simulinkPath;
}
const QString& getSimulinkName() const {
return *simulinkName;
}
uint8_t getComponentID() const {
return componentID;
}
float getValue();
void setValue(float value);
bool operator==(const Parameter& other) const;
operator QString() const;
protected:
QString *simulinkPath;
QString *simulinkName;
uint8_t componentID;
QGCParamID *paramID;
unsigned short opalID;
};
}
#endif // PARAMETER_H

332
src/comm/ParameterList.cc
Unescape View File

@ -1,332 +0,0 @@ @@ -1,332 +0,0 @@
/*=====================================================================
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> >),
paramList(new QList<QList<Parameter*> >()),
reqdServoParams(new QStringList())
{
QDir settingsDir = QDir(qApp->applicationDirPath());
if (settingsDir.dirName() == "bin")
settingsDir.cdUp();
settingsDir.cd("data");
// Enforce a list of parameters which are necessary for flight
reqdServoParams->append("AIL_RIGHT_IN");
reqdServoParams->append("AIL_CENTER_IN");
reqdServoParams->append("AIL_LEFT_IN");
reqdServoParams->append("AIL_RIGHT_OUT");
reqdServoParams->append("AIL_CENTER_OUT");
reqdServoParams->append("AIL_LEFT_OUT");
reqdServoParams->append("ELE_DOWN_IN");
reqdServoParams->append("ELE_CENTER_IN");
reqdServoParams->append("ELE_UP_IN");
reqdServoParams->append("ELE_DOWN_OUT");
reqdServoParams->append("ELE_CENTER_OUT");
reqdServoParams->append("ELE_UP_OUT");
reqdServoParams->append("RUD_LEFT_IN");
reqdServoParams->append("RUD_CENTER_IN");
reqdServoParams->append("RUD_RIGHT_IN");
QString filename(settingsDir.path() + "/ParameterList.xml");
if ((QFile::exists(filename)) && open(filename)) {
/* 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) {
paramList->append(QList<Parameter*>());
for (paramIter = (*componentIter).begin(); paramIter != (*componentIter).end(); ++paramIter) {
paramList->last().append(paramIter.operator ->());
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;
delete paramList;
}
/**
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) {
// OpalRT::setLastErrorMsg();
OpalRT::OpalErrorMsg::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) {
// OpalRT::setLastErrorMsg();
OpalRT::OpalErrorMsg::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();
}
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+=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;
}
/* Functions related to reading the xml config file */
bool ParameterList::open(QString filename)
{
QFile paramFile(filename);
if (!paramFile.exists()) {
/// \todo open dialog box (maybe: that could also go in comm config window)
return false;
}
if (!paramFile.open(QIODevice::ReadOnly)) {
return false;
}
read(&paramFile);
paramFile.close();
return true;
}
bool ParameterList::read(QIODevice *device)
{
QDomDocument *paramConfig = new QDomDocument();
QString errorStr;
int errorLine;
int errorColumn;
if (!paramConfig->setContent(device, true, &errorStr, &errorLine,
&errorColumn)) {
qDebug() << "Error reading XML Parameter File on line: " << errorLine << errorStr;
return false;
}
QDomElement root = paramConfig->documentElement();
if (root.tagName() != "ParameterList") {
qDebug() << __FILE__ << __LINE__ << "This is not a parameter list xml file";
return false;
}
QDomElement child = root.firstChildElement("Block");
while (!child.isNull()) {
parseBlock(child);
child = child.nextSiblingElement("Block");
}
if (!reqdServoParams->empty()) {
qDebug() << __FILE__ << __LINE__ << "Missing the following required servo parameters";
foreach(QString s, *reqdServoParams) {
qDebug() << s;
}
}
delete paramConfig;
return true;
}
void ParameterList::parseBlock(const QDomElement &block)
{
QDomNodeList paramList;
QDomElement e;
Parameter *p;
SubsystemIds id;
if (block.attribute("name") == "Navigation")
id = OpalRT::NAV;
else if (block.attribute("name") == "Controller")
id = OpalRT::CONTROLLER;
else if (block.attribute("name") == "ServoOutputs")
id = OpalRT::SERVO_OUTPUTS;
else if (block.attribute("name") == "ServoInputs")
id = OpalRT::SERVO_INPUTS;
paramList = block.elementsByTagName("Parameter");
for (int i=0; i < paramList.size(); ++i) {
e = paramList.item(i).toElement();
if (e.hasAttribute("SimulinkPath") &&
e.hasAttribute("SimulinkParameterName") &&
e.hasAttribute("QGCParamID")) {
p = new Parameter(e.attribute("SimulinkPath"),
e.attribute("SimulinkParameterName"),
static_cast<uint8_t>(id),
QGCParamID(e.attribute("QGCParamID")));
(*params)[id].insert(p->getParamID(), *p);
if (reqdServoParams->contains((QString)p->getParamID()))
reqdServoParams->removeAt(reqdServoParams->indexOf((QString)p->getParamID()));
delete p;
} else {
qDebug() << __FILE__ << ":" << __LINE__ << ": error in xml doc in block" << block.attribute("name");
}
}
}

175
src/comm/ParameterList.h
Unescape View File

@ -1,175 +0,0 @@ @@ -1,175 +0,0 @@
/*=====================================================================
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 <QVector>
#include <QIODevice>
#include <QFile>
#include <QDir>
#include <QApplication>
#include <QtXml>
#include <QStringList>
#include "mavlink_types.h"
#include "QGCParamID.h"
#include "Parameter.h"
#include "OpalRT.h"
namespace OpalRT
{
class ParameterList
{
public:
class const_iterator
{
friend class ParameterList;
public:
inline const_iterator() {}
const_iterator(const const_iterator& other);
const_iterator& operator+=(int i) {
index += i;
return *this;
}
bool operator<(const const_iterator& other) const {
return (this->paramList == other.paramList)
&&(this->index<other.index);
}
bool operator==(const const_iterator& other) const {
return (this->paramList == other.paramList)
&&(this->index==other.index);
}
bool operator!=(const const_iterator& other) const {
return !((*this) == other);
}
const Parameter& operator*() const {
return paramList[index];
}
const Parameter* operator->() const {
return &paramList[index];
}
const_iterator& operator++() {
++index;
return *this;
}
private:
const_iterator(QList<Parameter>);
QList<Parameter> paramList;
int index;
};
ParameterList();
~ParameterList();
/** Count the number of parameters in the list.
\return Total number of parameters
*/
int count();
/** 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;
const_iterator end() const;
protected:
/** Store the parameters mapped by componentid, and paramid.
\code
// Look up a parameter
int compid = 1;
QGCParamID paramid("PID_GAIN");
Parameter p = params[compid][paramid];
\endcode
*/
QMap<int, QMap<QGCParamID, Parameter> > *params;
/**
Store pointers to the parameters to allow fast lookup by index.
This variable may be changed to const pointers to ensure all changes
are made through the map container.
*/
QList<QList<Parameter*> > *paramList;
/**
List of parameters which are necessary to control the servos.
*/
QStringList *reqdServoParams;
/**
Get the list of available parameters from Opal-RT.
\param[out] opalParams Map of parameter paths/names to ids which are valid in Opal-RT
*/
void getParameterList(QMap<QString, unsigned short>* opalParams);
/**
Open a file for reading in the xml config data
*/
bool open(QString filename=QString());
/**
Attempt to read XML configuration data from device
\param[in] the device to read the xml data from
\return true if the configuration was read successfully, false otherwise
*/
bool read(QIODevice *device);
void parseBlock(const QDomElement &block);
};
}
#endif // PARAMETERLIST_H

51
src/comm/QGCParamID.cc
Unescape View File

@ -1,51 +0,0 @@ @@ -1,51 +0,0 @@
/*=====================================================================
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::QGCParamID
* @author Bryan Godbolt <godbolt@ualberta.ca>
*/
#include "QGCParamID.h"
using namespace OpalRT;
QGCParamID::QGCParamID(const char paramid[]):data(paramid)
{
}
QGCParamID::QGCParamID(const QString s):data(s)
{
}
QGCParamID::QGCParamID(const QGCParamID &other):data(other.data)
{
}
//
//QDataStream& operator<<(QDataStream& stream, const QGCParamID& paramid)
//{
// return stream << paramid.data;
//}

82
src/comm/QGCParamID.h
Unescape View File

@ -1,82 +0,0 @@ @@ -1,82 +0,0 @@
/*=====================================================================
QGroundControl Open Source Ground Control Station
(c) 2009 - 2011 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 Stores the paramid used for mavlink
* @author Bryan Godbolt <godbolt@ualberta.ca>
*/
#ifndef QGCPARAMID_H
#define QGCPARAMID_H
#include <QString>
#include "mavlink_types.h"
//namespace OpalRT
//{
// class QGCParamID;
//}
//
//QDataStream& operator<<(QDataStream&, const OpalRT::QGCParamID&);
namespace OpalRT
{
/** Stores a param_id for the mavlink parameter packets. This class adds the convenience
of storing the id as a string (e.g., easy comparison).
\todo Fix: warning: deprecated conversion from string constant to 'char*'
*/
class QGCParamID
{
// friend QDataStream& operator<<(QDataStream& stream, const QGCParamID& paramid);
public:
QGCParamID(const char[]);
QGCParamID(const QString);
QGCParamID() {}
QGCParamID(const QGCParamID& other);
bool operator<(const QGCParamID& other) const {
return data<other.data;
}
bool operator==(const QGCParamID& other) const {
return data == other.data;
}
operator QString() const {
return data;
}
const QString getParamString() const {
return static_cast<const QString>(data);
}
int8_t* toInt8_t() const {
return (int8_t*)data.toLatin1().data();
}
protected:
QString data;
};
}
#endif // QGCPARAMID_H

449
src/input/Freenect.cc
Unescape View File

@ -1,449 +0,0 @@ @@ -1,449 +0,0 @@
/*=====================================================================
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 Definition of the class Freenect.
*
* @author Lionel Heng <hengli@student.ethz.ch>
*
*/
#include "Freenect.h"
#include <cmath>
#include <string.h>
#include <QSettings>
Freenect::Freenect()
: context(NULL)
, device(NULL)
, tiltAngle(0)
, rgbData(new QByteArray)
, rawDepthData(new QByteArray)
, coloredDepthData(new QByteArray)
, pointCloud3D(new QVector<QVector3D>)
, pointCloud6D(new QVector<Vector6D>)
{
}
Freenect::~Freenect()
{
if (device != NULL) {
freenect_stop_depth(device);
freenect_stop_video(device);
}
freenect_close_device(device);
freenect_shutdown(context);
}
bool
Freenect::init(int userDeviceNumber)
{
// read in settings
readConfigFile();
// populate gamma lookup table
for (int i = 0; i < 2048; ++i) {
float v = static_cast<float>(i) / 2048.0f;
v = powf(v, 3.0f) * 6.0f;
gammaTable[i] = static_cast<unsigned short>(v * 6.0f * 256.0f);
}
// populate depth projection matrix
for (int i = 0; i < FREENECT_FRAME_H; ++i) {
for (int j = 0; j < FREENECT_FRAME_W; ++j) {
QVector2D originalPoint(j, i);
QVector2D rectifiedPoint;
rectifyPoint(originalPoint, rectifiedPoint, depthCameraParameters);
QVector3D rectifiedRay;
projectPixelTo3DRay(rectifiedPoint, rectifiedRay, depthCameraParameters);
depthProjectionMatrix[i * FREENECT_FRAME_W + j] = rectifiedRay;
rectifyPoint(originalPoint, rectifiedPoint, rgbCameraParameters);
rgbRectificationMap[i * FREENECT_FRAME_W + j] = rectifiedPoint;
}
}
if (freenect_init(&context, NULL) < 0) {
return false;
}
freenect_set_log_level(context, FREENECT_LOG_DEBUG);
if (freenect_num_devices(context) < 1) {
return false;
}
if (freenect_open_device(context, &device, userDeviceNumber) < 0) {
return false;
}
freenect_set_user(device, this);
memset(rgb, 0, FREENECT_VIDEO_RGB_SIZE);
memset(depth, 0, FREENECT_DEPTH_11BIT_SIZE);
// set Kinect parameters
if (freenect_set_tilt_degs(device, tiltAngle) != 0) {
return false;
}
if (freenect_set_led(device, LED_RED) != 0) {
return false;
}
if (freenect_set_video_format(device, FREENECT_VIDEO_RGB) != 0) {
return false;
}
if (freenect_set_depth_format(device, FREENECT_DEPTH_11BIT) != 0) {
return false;
}
freenect_set_video_callback(device, videoCallback);
freenect_set_depth_callback(device, depthCallback);
if (freenect_start_depth(device) != 0) {
return false;
}
if (freenect_start_video(device) != 0) {
return false;
}
thread.reset(new FreenectThread(device));
thread->start();
return true;
}
bool
Freenect::process(void)
{
if (freenect_process_events(context) < 0) {
return false;
}
freenect_raw_tilt_state* state;
freenect_update_tilt_state(device);
state = freenect_get_tilt_state(device);
freenect_get_mks_accel(state, &ax, &ay, &az);
return true;
}
QSharedPointer<QByteArray>
Freenect::getRgbData(void)
{
QMutexLocker locker(&rgbMutex);
rgbData->clear();
rgbData->append(rgb, FREENECT_VIDEO_RGB_SIZE);
return rgbData;
}
QSharedPointer<QByteArray>
Freenect::getRawDepthData(void)
{
QMutexLocker locker(&depthMutex);
rawDepthData->clear();
rawDepthData->append(depth, FREENECT_DEPTH_11BIT_SIZE);
return rawDepthData;
}
QSharedPointer<QByteArray>
Freenect::getColoredDepthData(void)
{
QMutexLocker locker(&coloredDepthMutex);
coloredDepthData->clear();
coloredDepthData->append(coloredDepth, FREENECT_VIDEO_RGB_SIZE);
return coloredDepthData;
}
QSharedPointer< QVector<QVector3D> >
Freenect::get3DPointCloudData(void)
{
QMutexLocker locker(&depthMutex);
pointCloud3D->clear();
unsigned short* data = reinterpret_cast<unsigned short*>(depth);
for (int i = 0; i < FREENECT_FRAME_PIX; ++i) {
if (data[i] > 0 && data[i] <= 2048) {
double range = baseline * depthCameraParameters.fx
/ (1.0 / 8.0 * (disparityOffset
- static_cast<double>(data[i])));
if (range > 0.0f) {
QVector3D ray = depthProjectionMatrix[i];
ray *= range;
pointCloud3D->push_back(QVector3D(ray.x(), ray.y(), ray.z()));
}
}
}
return pointCloud3D;
}
QSharedPointer< QVector<Freenect::Vector6D> >
Freenect::get6DPointCloudData(void)
{
get3DPointCloudData();
pointCloud6D->clear();
for (int i = 0; i < pointCloud3D->size(); ++i) {
Vector6D point;
point.x = pointCloud3D->at(i).x();
point.y = pointCloud3D->at(i).y();
point.z = pointCloud3D->at(i).z();
QVector4D transformedPoint = transformMatrix * QVector4D(point.x, point.y, point.z, 1.0);
float iz = 1.0 / transformedPoint.z();
QVector2D rectifiedPoint(transformedPoint.x() * iz * rgbCameraParameters.fx + rgbCameraParameters.cx,
transformedPoint.y() * iz * rgbCameraParameters.fy + rgbCameraParameters.cy);
QVector2D originalPoint;
unrectifyPoint(rectifiedPoint, originalPoint, rgbCameraParameters);
if (originalPoint.x() >= 0.0 && originalPoint.x() < FREENECT_FRAME_W &&
originalPoint.y() >= 0.0 && originalPoint.y() < FREENECT_FRAME_H) {
int x = static_cast<int>(originalPoint.x());
int y = static_cast<int>(originalPoint.y());
unsigned char* pixel = reinterpret_cast<unsigned char*>(rgb)
+ (y * FREENECT_FRAME_W + x) * 3;
point.r = pixel[0];
point.g = pixel[1];
point.b = pixel[2];
pointCloud6D->push_back(point);
}
}
return pointCloud6D;
}
int
Freenect::getTiltAngle(void) const
{
return tiltAngle;
}
void
Freenect::setTiltAngle(int angle)
{
if (angle > 30) {
angle = 30;
}
if (angle < -30) {
angle = -30;
}
tiltAngle = angle;
}
Freenect::FreenectThread::FreenectThread(freenect_device* _device)
{
device = _device;
}
void
Freenect::FreenectThread::run(void)
{
Freenect* freenect = static_cast<Freenect *>(freenect_get_user(device));
while (1) {
freenect->process();
}
}
void
Freenect::readConfigFile(void)
{
QSettings settings("src/data/kinect.cal", QSettings::IniFormat, 0);
rgbCameraParameters.cx = settings.value("rgb/principal_point/x").toDouble();
rgbCameraParameters.cy = settings.value("rgb/principal_point/y").toDouble();
rgbCameraParameters.fx = settings.value("rgb/focal_length/x").toDouble();
rgbCameraParameters.fy = settings.value("rgb/focal_length/y").toDouble();
rgbCameraParameters.k[0] = settings.value("rgb/distortion/k1").toDouble();
rgbCameraParameters.k[1] = settings.value("rgb/distortion/k2").toDouble();
rgbCameraParameters.k[2] = settings.value("rgb/distortion/k3").toDouble();
rgbCameraParameters.k[3] = settings.value("rgb/distortion/k4").toDouble();
rgbCameraParameters.k[4] = settings.value("rgb/distortion/k5").toDouble();
depthCameraParameters.cx = settings.value("depth/principal_point/x").toDouble();
depthCameraParameters.cy = settings.value("depth/principal_point/y").toDouble();
depthCameraParameters.fx = settings.value("depth/focal_length/x").toDouble();
depthCameraParameters.fy = settings.value("depth/focal_length/y").toDouble();
depthCameraParameters.k[0] = settings.value("depth/distortion/k1").toDouble();
depthCameraParameters.k[1] = settings.value("depth/distortion/k2").toDouble();
depthCameraParameters.k[2] = settings.value("depth/distortion/k3").toDouble();
depthCameraParameters.k[3] = settings.value("depth/distortion/k4").toDouble();
depthCameraParameters.k[4] = settings.value("depth/distortion/k5").toDouble();
transformMatrix = QMatrix4x4(settings.value("transform/R11").toDouble(),
settings.value("transform/R12").toDouble(),
settings.value("transform/R13").toDouble(),
settings.value("transform/Tx").toDouble(),
settings.value("transform/R21").toDouble(),
settings.value("transform/R22").toDouble(),
settings.value("transform/R23").toDouble(),
settings.value("transform/Ty").toDouble(),
settings.value("transform/R31").toDouble(),
settings.value("transform/R32").toDouble(),
settings.value("transform/R33").toDouble(),
settings.value("transform/Tz").toDouble(),
0.0, 0.0, 0.0, 1.0);
transformMatrix = transformMatrix.inverted();
baseline = settings.value("transform/baseline").toDouble();
disparityOffset = settings.value("transform/disparity_offset").toDouble();
}
void
Freenect::rectifyPoint(const QVector2D& originalPoint,
QVector2D& rectifiedPoint,
const IntrinsicCameraParameters& params)
{
double x = (originalPoint.x() - params.cx) / params.fx;
double y = (originalPoint.y() - params.cy) / params.fy;
double x0 = x;
double y0 = y;
// eliminate lens distortion iteratively
for (int i = 0; i < 4; ++i) {
double r2 = x * x + y * y;
// tangential distortion vector [dx dy]
double dx = 2 * params.k[2] * x * y + params.k[3] * (r2 + 2 * x * x);
double dy = params.k[2] * (r2 + 2 * y * y) + 2 * params.k[3] * x * y;
double icdist = 1.0 / (1.0 + r2 * (params.k[0] + r2 * (params.k[1] + r2 * params.k[4])));
x = (x0 - dx) * icdist;
y = (y0 - dy) * icdist;
}
rectifiedPoint.setX(x * params.fx + params.cx);
rectifiedPoint.setY(y * params.fy + params.cy);
}
void
Freenect::unrectifyPoint(const QVector2D& rectifiedPoint,
QVector2D& originalPoint,
const IntrinsicCameraParameters& params)
{
double x = (rectifiedPoint.x() - params.cx) / params.fx;
double y = (rectifiedPoint.y() - params.cy) / params.fy;
double r2 = x * x + y * y;
// tangential distortion vector [dx dy]
double dx = 2 * params.k[2] * x * y + params.k[3] * (r2 + 2 * x * x);
double dy = params.k[2] * (r2 + 2 * y * y) + 2 * params.k[3] * x * y;
double cdist = 1.0 + r2 * (params.k[0] + r2 * (params.k[1] + r2 * params.k[4]));
x = x * cdist + dx;
y = y * cdist + dy;
originalPoint.setX(x * params.fx + params.cx);
originalPoint.setY(y * params.fy + params.cy);
}
void
Freenect::projectPixelTo3DRay(const QVector2D& pixel, QVector3D& ray,
const IntrinsicCameraParameters& params)
{
ray.setX((pixel.x() - params.cx) / params.fx);
ray.setY((pixel.y() - params.cy) / params.fy);
ray.setZ(1.0);
}
void
Freenect::videoCallback(freenect_device* device, void* video, uint32_t timestamp)
{
Freenect* freenect = static_cast<Freenect *>(freenect_get_user(device));
QMutexLocker locker(&freenect->rgbMutex);
memcpy(freenect->rgb, video, FREENECT_VIDEO_RGB_SIZE);
}
void
Freenect::depthCallback(freenect_device* device, void* depth, uint32_t timestamp)
{
Freenect* freenect = static_cast<Freenect *>(freenect_get_user(device));
uint16_t* data = reinterpret_cast<uint16_t *>(depth);
QMutexLocker depthLocker(&freenect->depthMutex);
memcpy(freenect->depth, data, FREENECT_DEPTH_11BIT_SIZE);
QMutexLocker coloredDepthLocker(&freenect->coloredDepthMutex);
unsigned short* src = reinterpret_cast<unsigned short *>(data);
unsigned char* dst = reinterpret_cast<unsigned char *>(freenect->coloredDepth);
for (int i = 0; i < FREENECT_FRAME_PIX; ++i) {
unsigned short pval = freenect->gammaTable[src[i]];
unsigned short lb = pval & 0xFF;
switch (pval >> 8) {
case 0:
dst[3 * i] = 255;
dst[3 * i + 1] = 255 - lb;
dst[3 * i + 2] = 255 - lb;
break;
case 1:
dst[3 * i] = 255;
dst[3 * i + 1] = lb;
dst[3 * i + 2] = 0;
break;
case 2:
dst[3 * i] = 255 - lb;
dst[3 * i + 1] = 255;
dst[3 * i + 2] = 0;
break;
case 3:
dst[3 * i] = 0;
dst[3 * i + 1] = 255;
dst[3 * i + 2] = lb;
break;
case 4:
dst[3 * i] = 0;
dst[3 * i + 1] = 255 - lb;
dst[3 * i + 2] = 255;
break;
case 5:
dst[3 * i] = 0;
dst[3 * i + 1] = 0;
dst[3 * i + 2] = 255 - lb;
break;
default:
dst[3 * i] = 0;
dst[3 * i + 1] = 0;
dst[3 * i + 2] = 0;
break;
}
}
}

154
src/input/Freenect.h
Unescape View File

@ -1,154 +0,0 @@ @@ -1,154 +0,0 @@
/*=====================================================================
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 Definition of the class Freenect.
*
* @author Lionel Heng <hengli@student.ethz.ch>
*
*/
#ifndef FREENECT_H
#define FREENECT_H
#include <libfreenect/libfreenect.h>
#include <QMatrix4x4>
#include <QMutex>
#include <QScopedPointer>
#include <QSharedPointer>
#include <QThread>
#include <QVector>
#include <QVector2D>
#include <QVector3D>
class Freenect
{
public:
Freenect();
~Freenect();
bool init(int userDeviceNumber = 0);
bool process(void);
QSharedPointer<QByteArray> getRgbData(void);
QSharedPointer<QByteArray> getRawDepthData(void);
QSharedPointer<QByteArray> getColoredDepthData(void);
QSharedPointer< QVector<QVector3D> > get3DPointCloudData(void);
typedef struct {
double x;
double y;
double z;
unsigned char r;
unsigned char g;
unsigned char b;
} Vector6D;
QSharedPointer< QVector<Vector6D> > get6DPointCloudData();
int getTiltAngle(void) const;
void setTiltAngle(int angle);
private:
typedef struct {
// coordinates of principal point
double cx;
double cy;
// focal length in pixels
double fx;
double fy;
// distortion parameters
double k[5];
} IntrinsicCameraParameters;
void readConfigFile(void);
void rectifyPoint(const QVector2D& originalPoint,
QVector2D& rectifiedPoint,
const IntrinsicCameraParameters& params);
void unrectifyPoint(const QVector2D& rectifiedPoint,
QVector2D& originalPoint,
const IntrinsicCameraParameters& params);
void projectPixelTo3DRay(const QVector2D& pixel, QVector3D& ray,
const IntrinsicCameraParameters& params);
static void videoCallback(freenect_device* device, void* video, uint32_t timestamp);
static void depthCallback(freenect_device* device, void* depth, uint32_t timestamp);
freenect_context* context;
freenect_device* device;
class FreenectThread : public QThread
{
public:
explicit FreenectThread(freenect_device* _device);
protected:
virtual void run(void);
freenect_device* device;
};
QScopedPointer<FreenectThread> thread;
IntrinsicCameraParameters rgbCameraParameters;
IntrinsicCameraParameters depthCameraParameters;
QMatrix4x4 transformMatrix;
double baseline;
double disparityOffset;
// tilt angle of Kinect camera
int tiltAngle;
// rgbd data
char rgb[FREENECT_VIDEO_RGB_SIZE];
QMutex rgbMutex;
char depth[FREENECT_DEPTH_11BIT_SIZE];
QMutex depthMutex;
char coloredDepth[FREENECT_VIDEO_RGB_SIZE];
QMutex coloredDepthMutex;
// accelerometer data
double ax, ay, az;
double dx, dy, dz;
// gamma map
unsigned short gammaTable[2048];
QVector3D depthProjectionMatrix[FREENECT_FRAME_PIX];
QVector2D rgbRectificationMap[FREENECT_FRAME_PIX];
// variables for use outside class
QSharedPointer<QByteArray> rgbData;
QSharedPointer<QByteArray> rawDepthData;
QSharedPointer<QByteArray> coloredDepthData;
QSharedPointer< QVector<QVector3D> > pointCloud3D;
QSharedPointer< QVector<Vector6D> > pointCloud6D;
};
#endif // FREENECT_H
Write Preview
Loading…
Cancel
Save