rooty/LibrePilot
1
0
Fork 0
mirror of https://bitbucket.org/librepilot/librepilot.git synced 2025-02-20 10:54:14 +01:00
Code Issues Releases Activity

Refactored HiTL to add functionality created by hrrrrr in HiTLv2.

Browse Source
This commit is contained in:
Kenz Dale 2012-08-23 17:11:20 +02:00
parent cf9b04d0ac
commit 05f131fa46
12 changed files with 531 additions and 259 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

48
ground/openpilotgcs/src/plugins/coreplugin/OpenPilotGCS.xml
View File

@ -905,13 +905,32 @@
<version>0.0.0</version>
</configInfo>
<data>
<addNoise>false</addNoise>
<attActCalc>false</attActCalc>
<attActHW>true</attActHW>
<attActSim>true</attActSim>
<attActualEnabled>false</attActualEnabled>
<attRawEnabled>false</attRawEnabled>
<attRawRate>20</attRawRate>
<baroAltRate>50</baroAltRate>
<baroAltitudeEnabled>false</baroAltitudeEnabled>
<binPath>\usr\games\fgfs</binPath>
<dataPath>\usr\share\games\FlightGear</dataPath>
<gcsReciever>true</gcsReciever>
<gpsPosRate>100</gpsPosRate>
<gpsPositionEnabled>false</gpsPositionEnabled>
<groundTruthEnabled>true</groundTruthEnabled>
<groundTruthRate>100</groundTruthRate>
<hostAddress>127.0.0.1</hostAddress>
<inPort>9009</inPort>
<manual>false</manual>
<inputCommand>true</inputCommand>
<latitude></latitude>
<longitude></longitude>
<manualControl>false</manualControl>
<manualOutput>false</manualOutput>
<minOutputPeriod>100</minOutputPeriod>
<outPort>9010</outPort>
<remoteHostAddress>127.0.0.1</remoteHostAddress>
<remoteAddress>127.0.0.1</remoteAddress>
<simulatorId>FG</simulatorId>
<startSim>true</startSim>
</data>
@ -922,13 +941,32 @@
<version>0.0.0</version>
</configInfo>
<data>
<addNoise>false</addNoise>
<attActCalc>false</attActCalc>
<attActHW>true</attActHW>
<attActSim>true</attActSim>
<attActualEnabled>false</attActualEnabled>
<attRawEnabled>false</attRawEnabled>
<attRawRate>20</attRawRate>
<baroAltRate>50</baroAltRate>
<baroAltitudeEnabled>false</baroAltitudeEnabled>
<binPath>\home\lafargue\X-Plane 9\X-Plane-i686</binPath>
<dataPath>\usr\share\games\FlightGear</dataPath>
<hostAddress>127.0.0.3</hostAddress>
<gcsReciever>true</gcsReciever>
<gpsPosRate>100</gpsPosRate>
<gpsPositionEnabled>false</gpsPositionEnabled>
<groundTruthEnabled>true</groundTruthEnabled>
<groundTruthRate>100</groundTruthRate>
<hostAddress>0.0.0.0</hostAddress>
<inPort>6756</inPort>
<manual>false</manual>
<inputCommand>true</inputCommand>
<latitude></latitude>
<longitude></longitude>
<manualControl>false</manualControl>
<manualOutput>false</manualOutput>
<minOutputPeriod>100</minOutputPeriod>
<outPort>49000</outPort>
<remoteHostAddress>127.0.0.1</remoteHostAddress>
<remoteAddress>127.0.0.1</remoteAddress>
<simulatorId>X-Plane</simulatorId>
<startSim>false</startSim>
</data>

14
ground/openpilotgcs/src/plugins/hitlnew/fgsimulator.cpp
View File

@ -332,13 +332,13 @@ void FGSimulator::processUpdate(const QByteArray& inp)
positionActualData.Down = altitude ; //Multiply by 1 because positionActual expects input in meters.
posActual->setData(positionActualData);
// Update AltitudeActual object
BaroAltitude::DataFields altActualData;
memset(&altActualData, 0, sizeof(BaroAltitude::DataFields));
altActualData.Altitude = altitudeAGL;
altActualData.Temperature = temperature;
altActualData.Pressure = pressure;
altActual->setData(altActualData);
// Update AltitudeActual object
BaroAltitude::DataFields baroAltData;
memset(&baroAltData, 0, sizeof(BaroAltitude::DataFields));
baroAltData.Altitude = altitudeAGL;
baroAltData.Temperature = temperature;
baroAltData.Pressure = pressure;
baroAlt->setData(baroAltData);
// Update attActual object
AttitudeActual::DataFields attActualData;

105
ground/openpilotgcs/src/plugins/hitlnew/hitlconfiguration.cpp
View File

@ -31,6 +31,7 @@ HITLConfiguration::HITLConfiguration(QString classId, QSettings* qSettings, QObj
IUAVGadgetConfiguration(classId, parent)
{
//Default settings values
settings.simulatorId = "";
settings.binPath = "";
settings.dataPath = "";
@ -44,7 +45,28 @@ HITLConfiguration::HITLConfiguration(QString classId, QSettings* qSettings, QObj
settings.latitude = "";
settings.longitude = "";
// if a saved configuration exists load it
settings.attRawEnabled = false;
settings.attRawRate = 20;
settings.attActualEnabled = true;
settings.attActHW = false;
settings.attActSim = true;
settings.attActCalc = false;
settings.gpsPositionEnabled = false;
settings.gpsPosRate = 100;
settings.groundTruthEnabled = false;
settings.groundTruthRate = 100;
settings.inputCommand = false;
settings.gcsReciever = false;
settings.manualControl = false;
settings.manualOutput = false;
settings.minOutputPeriod = 100;
// if a saved configuration exists load it, and overwrite defaults
if (qSettings != 0) {
settings.simulatorId = qSettings->value("simulatorId").toString();
settings.hostAddress = qSettings->value("hostAddress").toString();
@ -55,70 +77,34 @@ HITLConfiguration::HITLConfiguration(QString classId, QSettings* qSettings, QObj
settings.binPath = qSettings->value("binPath").toString();
settings.dataPath = qSettings->value("dataPath").toString();
settings.latitude = qSettings->value("latitude").toString();
settings.longitude = qSettings->value("longitude").toString();
settings.startSim = qSettings->value("startSim").toBool();
settings.addNoise = qSettings->value("noiseCheckBox").toBool();
settings.latitude = qSettings->value("latitude").toString();
settings.longitude = qSettings->value("longitude").toString();
settings.startSim = qSettings->value("startSim").toBool();
settings.addNoise = qSettings->value("noiseCheckBox").toBool();
settings.homeLocation = qSettings->value("homeLocation").toBool();
settings.homeLocRate = qSettings->value("homeLocRate").toInt();
settings.attRaw = qSettings->value("attRaw").toBool();
settings.attRawEnabled = qSettings->value("attRawEnabled").toBool();
settings.attRawRate = qSettings->value("attRawRate").toInt();
settings.attActual = qSettings->value("attActual").toBool();
settings.attActualEnabled = qSettings->value("attActualEnabled").toBool();
settings.attActHW = qSettings->value("attActHW").toBool();
settings.attActSim = qSettings->value("attActSim").toBool();
settings.attActCalc = qSettings->value("attActCalc").toBool();
settings.sonarAltitude = qSettings->value("sonarAltitude").toBool();
settings.sonarMaxAlt = qSettings->value("sonarMaxAlt").toFloat();
settings.sonarAltRate = qSettings->value("sonarAltRate").toInt();
settings.baroAltitudeEnabled= qSettings->value("baroAltitudeEnabled").toBool();
settings.baroAltRate = qSettings->value("baroAltRate").toInt();
settings.gpsPosition = qSettings->value("gpsPosition").toBool();
settings.gpsPositionEnabled = qSettings->value("gpsPositionEnabled").toBool();
settings.gpsPosRate = qSettings->value("gpsPosRate").toInt();
settings.groundTruthEnabled = qSettings->value("groundTruthEnabled").toBool();
settings.groundTruthRate = qSettings->value("groundTruthRate").toInt();
settings.inputCommand = qSettings->value("inputCommand").toBool();
settings.gcsReciever = qSettings->value("gcsReciever").toBool();
settings.manualControl = qSettings->value("manualControl").toBool();
settings.manualOutput = qSettings->value("manualOutput").toBool();
settings.outputRate = qSettings->value("outputRate").toInt();
settings.minOutputPeriod = qSettings->value("minOutputPeriod").toInt();
}
#ifdef HHRRRRRRRRRR
else {
settings.simulatorId = simulatorId;
settings.hostAddress = hostAddress;
settings.inPort = inPort;
settings.remoteAddress = remoteAddress;
settings.outPort = outPort;
settings.binPath = binPath;
settings.dataPath = dataPath;
settings.homeLocation = homeLocation;
settings.homeLocRate = homeLocRate;
settings.attRaw = attRaw;
settings.attRawRate = attRawRate;
settings.attActual = attActual;
settings.attActHW = attActHW;
settings.attActSim = attActSim;
settings.attActCalc = attActCalc;
settings.sonarAltitude = sonarAltitude;
settings.sonarMaxAlt = sonarMaxAlt;
settings.sonarAltRate = sonarAltRate;
settings.gpsPosition = gpsPosition;
settings.gpsPosRate = gpsPosRate;
settings.inputCommand = inputCommand;
settings.gcsReciever = gcsReciever;
settings.manualControl = manualControl;
settings.manualOutput = manualOutput;
settings.outputRate = outputRate;
}
#endif
}
IUAVGadgetConfiguration *HITLConfiguration::clone()
@ -149,23 +135,24 @@ void HITLConfiguration::saveConfig(QSettings* qSettings) const {
qSettings->setValue("startSim", settings.startSim);
qSettings->setValue("manualControl", settings.manualControl);
qSettings->setValue("homeLocation", settings.homeLocation);
qSettings->setValue("homeLocRate", settings.homeLocRate);
qSettings->setValue("attRaw", settings.attRaw);
qSettings->setValue("attRawEnabled", settings.attRawEnabled);
qSettings->setValue("attRawRate", settings.attRawRate);
qSettings->setValue("attActual", settings.attActual);
qSettings->setValue("attActualEnabled", settings.attActualEnabled);
qSettings->setValue("attActHW", settings.attActHW);
qSettings->setValue("attActSim", settings.attActSim);
qSettings->setValue("attActCalc", settings.attActCalc);
qSettings->setValue("sonarAltitude", settings.sonarAltitude);
qSettings->setValue("sonarMaxAlt", settings.sonarMaxAlt);
qSettings->setValue("sonarAltRate", settings.sonarAltRate);
qSettings->setValue("gpsPosition", settings.gpsPosition);
qSettings->setValue("baroAltitudeEnabled", settings.baroAltitudeEnabled);
qSettings->setValue("baroAltRate", settings.baroAltRate);
qSettings->setValue("gpsPositionEnabled", settings.gpsPositionEnabled);
qSettings->setValue("gpsPosRate", settings.gpsPosRate);
qSettings->setValue("groundTruthEnabled", settings.groundTruthEnabled);
qSettings->setValue("groundTruthRate", settings.groundTruthRate);
qSettings->setValue("inputCommand", settings.inputCommand);
qSettings->setValue("gcsReciever", settings.gcsReciever);
qSettings->setValue("manualControl", settings.manualControl);
qSettings->setValue("manualOutput", settings.manualOutput);
qSettings->setValue("outputRate", settings.outputRate);
qSettings->setValue("minOutputPeriod", settings.minOutputPeriod);
}

78
ground/openpilotgcs/src/plugins/hitlnew/hitlnoisegeneration.cpp Normal file
View File

@ -0,0 +1,78 @@
/**
******************************************************************************
*
* @file hitlnoisegeneration.cpp
* @author The OpenPilot Team, http://www.openpilot.org Copyright (C) 2012.
* @brief
* @see The GNU Public License (GPL) Version 3
* @defgroup hitlplugin
* @{
*
*****************************************************************************/
/*
* This program 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.
*
* This program 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 this program; if not, write to the Free Software Foundation, Inc.,
* 59 Temple Place, Suite 330, Boston, MA 02111-1307 USA
*/
#include "hitlnoisegeneration.h"
HitlNoiseGeneration::HitlNoiseGeneration()
{
memset(&noise, 0, sizeof(Noise));
}
HitlNoiseGeneration::~HitlNoiseGeneration()
{
}
Noise HitlNoiseGeneration::getNoise(){
return noise;
}
Noise HitlNoiseGeneration::generateNoise(){
noise.accelData.x=0;
noise.accelData.y=0;
noise.accelData.z=0;
noise.gpsPosData.Latitude=0;
noise.gpsPosData.Longitude=0;
noise.gpsPosData.Groundspeed=0;
noise.gpsPosData.Heading=0;
noise.gpsPosData.Altitude=0;
noise.gpsVelData.North=0;
noise.gpsVelData.East=0;
noise.gpsVelData.Down=0;
noise.baroAltData.Altitude=0;
noise.attActualData.Roll=0;
noise.attActualData.Pitch=0;
noise.attActualData.Yaw=0;
noise.gyroData.x=0;
noise.gyroData.y=0;
noise.gyroData.z=0;
noise.accelData.x=0;
noise.accelData.y=0;
noise.accelData.z=0;
noise.baroAirspeed.CalibratedAirspeed=0;
return noise;
}

66
ground/openpilotgcs/src/plugins/hitlnew/hitlnoisegeneration.h Normal file
View File

@ -0,0 +1,66 @@
/**
******************************************************************************
*
* @file hitlnoisegeneration.h
* @author The OpenPilot Team, http://www.openpilot.org Copyright (C) 2012.
* @brief
* @see The GNU Public License (GPL) Version 3
* @defgroup hitlplugin
* @{
*
*****************************************************************************/
/*
* This program 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.
*
* This program 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 this program; if not, write to the Free Software Foundation, Inc.,
* 59 Temple Place, Suite 330, Boston, MA 02111-1307 USA
*/
#ifndef HITLNOISEGENERATION_H
#define HITLNOISEGENERATION_H
//#include <QObject>
//#include <simulator.h>
#include "xplanesimulator.h"
#include "hitlnoisegeneration.h"
#include "extensionsystem/pluginmanager.h"
#include <coreplugin/icore.h>
#include <coreplugin/threadmanager.h>
struct Noise{
Accels::DataFields accelData;
AttitudeActual::DataFields attActualData;
BaroAltitude::DataFields baroAltData;
BaroAirspeed::DataFields baroAirspeed;
GPSPosition::DataFields gpsPosData;
GPSVelocity::DataFields gpsVelData;
Gyros::DataFields gyroData;
HomeLocation::DataFields homeData;
PositionActual::DataFields positionActualData;
VelocityActual::DataFields velocityActualData;
};
class HitlNoiseGeneration
{
// Q_OBJECT
public:
HitlNoiseGeneration();
~HitlNoiseGeneration();
Noise getNoise();
Noise generateNoise();
private slots:
private:
Noise noise;
};
#endif // HITLNOISEGENERATION_H

37
ground/openpilotgcs/src/plugins/hitlnew/hitloptionspage.cpp
View File

@ -45,6 +45,8 @@ HITLOptionsPage::HITLOptionsPage(HITLConfiguration *conf, QObject *parent) :
QWidget *HITLOptionsPage::createPage(QWidget *parent)
{
Q_UNUSED(parent);
// Create page
m_optionsPage = new Ui::HITLOptionsPage();
QWidget* optionsPageWidget = new QWidget;
@ -87,6 +89,22 @@ QWidget *HITLOptionsPage::createPage(QWidget *parent)
m_optionsPage->latitude->setText(config->Settings().latitude);
m_optionsPage->longitude->setText(config->Settings().longitude);
m_optionsPage->groundTruthCheckbox->setChecked(config->Settings().groundTruthEnabled);
m_optionsPage->gpsPositionCheckbox->setChecked(config->Settings().gpsPositionEnabled);
m_optionsPage->attActualCheckbox->setChecked(config->Settings().attActualEnabled);
m_optionsPage->attRawCheckbox->setChecked(config->Settings().attRawEnabled);
m_optionsPage->attRawRateSpinbox->setValue(config->Settings().attRawRate);
m_optionsPage->gpsPosRateSpinbox->setValue(config->Settings().gpsPosRate);
m_optionsPage->groundTruthRateSpinbox->setValue(config->Settings().groundTruthRate);
// m_optionsPage->attActualRate->setValue(config->Settings().attActualRate);
m_optionsPage->baroAltitudeCheckbox->setChecked(config->Settings().baroAltitudeEnabled);
m_optionsPage->baroAltRateSpinbox->setValue(config->Settings().baroAltRate);
m_optionsPage->minOutputPeriodSpinbox->setValue(config->Settings().minOutputPeriod);
return optionsPageWidget;
}
@ -109,6 +127,25 @@ void HITLOptionsPage::apply()
settings.longitude = m_optionsPage->longitude->text();
settings.latitude = m_optionsPage->latitude->text();
settings.addNoise = m_optionsPage->noiseCheckBox->isChecked();
settings.attRawEnabled = m_optionsPage->attRawCheckbox->isChecked();
settings.attRawRate = m_optionsPage->attRawRateSpinbox->value();
settings.attActualEnabled = m_optionsPage->attActualCheckbox->isChecked();
settings.gpsPositionEnabled = m_optionsPage->gpsPositionCheckbox->isChecked();
settings.gpsPosRate = m_optionsPage->gpsPosRateSpinbox->value();
settings.groundTruthEnabled = m_optionsPage->groundTruthCheckbox->isChecked();
settings.groundTruthRate = m_optionsPage->groundTruthRateSpinbox->value();
settings.baroAltitudeEnabled = m_optionsPage->baroAltitudeCheckbox->isChecked();
settings.baroAltRate = m_optionsPage->baroAltRateSpinbox->value();
settings.minOutputPeriod = m_optionsPage->minOutputPeriodSpinbox->value();
config->setSimulatorSettings(settings);
}

68
ground/openpilotgcs/src/plugins/hitlnew/hitloptionspage.ui
View File

@ -224,8 +224,6 @@
</item>
</layout>
</widget>
<zorder>layoutWidget</zorder>
<zorder>layoutWidget</zorder>
</widget>
</item>
<item>
@ -492,6 +490,9 @@
</property>
<item>
<widget class="QGroupBox" name="attRawCheckbox">
<property name="enabled">
<bool>false</bool>
</property>
<property name="title">
<string>AttitudeRaw (gyro, accels)</string>
</property>
@ -525,10 +526,13 @@
</widget>
</item>
<item>
<widget class="QSpinBox" name="attRawRate">
<widget class="QSpinBox" name="attRawRateSpinbox">
<property name="enabled">
<bool>false</bool>
</property>
<property name="toolTip">
<string>0 - update once, or every N seconds</string>
</property>
<property name="suffix">
<string>ms</string>
</property>
@ -548,6 +552,9 @@
</item>
<item>
<widget class="QGroupBox" name="attActualCheckbox">
<property name="enabled">
<bool>false</bool>
</property>
<property name="title">
<string>AttitudeActual</string>
</property>
@ -558,7 +565,7 @@
<bool>true</bool>
</property>
<property name="checked">
<bool>false</bool>
<bool>true</bool>
</property>
<layout class="QVBoxLayout" name="verticalLayout_4">
<property name="spacing">
@ -667,7 +674,7 @@
<bool>true</bool>
</property>
<property name="checked">
<bool>false</bool>
<bool>true</bool>
</property>
<layout class="QHBoxLayout" name="horizontalLayout_2">
<property name="spacing">
@ -692,16 +699,25 @@
<item>
<widget class="QSpinBox" name="groundTruthRateSpinbox">
<property name="enabled">
<bool>false</bool>
<bool>true</bool>
</property>
<property name="toolTip">
<string>0 - update once, or every N seconds</string>
</property>
<property name="suffix">
<string>sec</string>
<string>ms</string>
</property>
<property name="minimum">
<number>1</number>
</property>
<property name="maximum">
<number>10</number>
<number>5000</number>
</property>
<property name="singleStep">
<number>100</number>
</property>
<property name="value">
<number>100</number>
</property>
</widget>
</item>
@ -709,7 +725,7 @@
</widget>
</item>
<item>
<widget class="QGroupBox" name="gpsPosition">
<widget class="QGroupBox" name="gpsPositionCheckbox">
<property name="title">
<string>GPS data</string>
</property>
@ -745,19 +761,25 @@
<item>
<widget class="QSpinBox" name="gpsPosRateSpinbox">
<property name="enabled">
<bool>false</bool>
<bool>true</bool>
</property>
<property name="toolTip">
<string>0 - update once, or every N seconds</string>
</property>
<property name="suffix">
<string>ms</string>
</property>
<property name="minimum">
<number>100</number>
<number>1</number>
</property>
<property name="maximum">
<number>2000</number>
<number>5000</number>
</property>
<property name="singleStep">
<number>100</number>
</property>
<property name="value">
<number>500</number>
<number>100</number>
</property>
</widget>
</item>
@ -765,12 +787,12 @@
</widget>
</item>
<item>
<widget class="QGroupBox" name="baroAltitude">
<widget class="QGroupBox" name="baroAltitudeCheckbox">
<property name="enabled">
<bool>false</bool>
</property>
<property name="title">
<string>SonarAltitude</string>
<string>BaroAltitude</string>
</property>
<property name="flat">
<bool>true</bool>
@ -830,7 +852,10 @@
</widget>
</item>
<item row="1" column="1">
<widget class="QSpinBox" name="sonarAltRate">
<widget class="QSpinBox" name="baroAltRateSpinbox">
<property name="toolTip">
<string>0 - update once, or every N seconds</string>
</property>
<property name="suffix">
<string>ms</string>
</property>
@ -854,7 +879,7 @@
</widget>
</item>
<item>
<widget class="QGroupBox" name="inputCommand">
<widget class="QGroupBox" name="inputCommandCheckbox">
<property name="enabled">
<bool>false</bool>
</property>
@ -928,10 +953,13 @@
</widget>
</item>
<item>
<widget class="QSpinBox" name="outputRate">
<widget class="QSpinBox" name="minOutputPeriodSpinbox">
<property name="enabled">
<bool>false</bool>
</property>
<property name="toolTip">
<string>0 - update once, or every N seconds</string>
</property>
<property name="suffix">
<string>ms</string>
</property>
@ -939,13 +967,13 @@
<number>10</number>
</property>
<property name="maximum">
<number>100</number>
<number>500</number>
</property>
<property name="singleStep">
<number>5</number>
</property>
<property name="value">
<number>15</number>
<number>100</number>
</property>
</widget>
</item>

14
ground/openpilotgcs/src/plugins/hitlnew/il2simulator.cpp
View File

@ -237,11 +237,11 @@ void IL2Simulator::processUpdate(const QByteArray& inp)
current.dPitch = (angleDifference(current.pitch,old.pitch)/current.dT + TURN_FILTER * (old.dPitch)) / (TURN_FILTER+1);
current.dRoll = (angleDifference(current.roll,old.roll)/current.dT + TURN_FILTER * (old.dRoll)) / (TURN_FILTER+1);
// Update AltitudeActual object
BaroAltitude::DataFields altActualData;
memset(&altActualData, 0, sizeof(BaroAltitude::DataFields));
altActualData.Altitude = current.Z;
altActualData.Temperature = TEMP_GROUND + (current.Z * TEMP_LAPSE_RATE) - 273.0;
altActualData.Pressure = PRESSURE(current.Z)/1000.0; // kpa
BaroAltitude::DataFields baroAltData;
memset(&baroAltData, 0, sizeof(BaroAltitude::DataFields));
baroAltData.Altitude = current.Z;
baroAltData.Temperature = TEMP_GROUND + (current.Z * TEMP_LAPSE_RATE) - 273.0;
baroAltData.Pressure = PRESSURE(current.Z)/1000.0; // kpa
// Update attActual object
AttitudeActual::DataFields attActualData;
@ -351,8 +351,8 @@ void IL2Simulator::processUpdate(const QByteArray& inp)
//velActual->updated();
posActual->setData(posData);
//posActual->updated();
altActual->setData(altActualData);
//altActual->updated();
baroAlt->setData(baroAltData);
//baroAlt->updated();
gpsPos->setData(gpsData);
//gpsPos->updated();
posHome->setData(homeData);

206
ground/openpilotgcs/src/plugins/hitlnew/simulator.cpp
View File

@ -31,6 +31,7 @@
#include "extensionsystem/pluginmanager.h"
#include "coreplugin/icore.h"
#include "coreplugin/threadmanager.h"
#include "hitlnoisegeneration.h"
volatile bool Simulator::isStarted = false;
@ -61,6 +62,11 @@ Simulator::Simulator(const SimulatorSettings& params) :
moveToThread(Core::ICore::instance()->threadManager()->getRealTimeThread());
connect(this, SIGNAL(myStart()), this, SLOT(onStart()),Qt::QueuedConnection);
emit myStart();
QTime currentTime=QTime::currentTime();
gpsPosTime = currentTime;
groundTruthTime = currentTime;
}
Simulator::~Simulator()
@ -223,8 +229,8 @@ void Simulator::receiveUpdate()
void Simulator::setupObjects()
{
setupInputObject(actDesired, 100);
setupOutputObject(altActual, 250);
setupInputObject(actDesired, settings.minOutputPeriod);
setupOutputObject(baroAlt, 250);
setupOutputObject(attActual, 10);
//setupOutputObject(attActual, 100);
setupOutputObject(gpsPos, 250);
@ -239,20 +245,33 @@ void Simulator::setupObjects()
}
void Simulator::setupInputObject(UAVObject* obj, int updatePeriod)
void Simulator::setupInputObject(UAVObject* obj, quint32 updatePeriod)
{
UAVObject::Metadata mdata;
mdata = obj->getDefaultMetadata();
UAVObject::SetFlightAccess(mdata, UAVObject::ACCESS_READWRITE);
UAVObject::SetGcsAccess(mdata, UAVObject::ACCESS_READWRITE);
UAVObject::SetFlightTelemetryAcked(mdata, false);
UAVObject::SetFlightTelemetryUpdateMode(mdata, UAVObject::UPDATEMODE_PERIODIC);
mdata.flightTelemetryUpdatePeriod = updatePeriod;
UAVObject::SetGcsTelemetryUpdateMode(mdata, UAVObject::UPDATEMODE_MANUAL);
obj->setMetadata(mdata);
UAVObject::Metadata mdata;
mdata = obj->getDefaultMetadata();
UAVObject::SetGcsAccess(mdata, UAVObject::ACCESS_READONLY);
UAVObject::SetGcsTelemetryAcked(mdata, false);
UAVObject::SetGcsTelemetryUpdateMode(mdata, UAVObject::UPDATEMODE_MANUAL);
mdata.gcsTelemetryUpdatePeriod = 0;
UAVObject::SetFlightAccess(mdata, UAVObject::ACCESS_READWRITE);
UAVObject::SetFlightTelemetryAcked(mdata, false);
if (settings.manualOutput) {
UAVObject::SetFlightTelemetryUpdateMode(mdata, UAVObject::UPDATEMODE_PERIODIC);
mdata.flightTelemetryUpdatePeriod = updatePeriod;
} else {
UAVObject::SetFlightTelemetryUpdateMode(mdata, UAVObject::UPDATEMODE_ONCHANGE);
mdata.flightTelemetryUpdatePeriod = 0;
}
obj->setMetadata(mdata);
}
void Simulator::setupOutputObject(UAVObject* obj, int updatePeriod)
void Simulator::setupOutputObject(UAVObject* obj, quint32 updatePeriod)
{
UAVObject::Metadata mdata;
mdata = obj->getDefaultMetadata();
@ -290,7 +309,9 @@ void Simulator::onSimulatorConnectionTimeout()
void Simulator::telStatsUpdated(UAVObject* obj)
{
GCSTelemetryStats::DataFields stats = telStats->getData();
Q_UNUSED(obj);
GCSTelemetryStats::DataFields stats = telStats->getData();
if ( !autopilotConnectionStatus && stats.Status == GCSTelemetryStats::STATUS_CONNECTED )
{
onAutopilotConnect();
@ -304,11 +325,11 @@ void Simulator::telStatsUpdated(UAVObject* obj)
void Simulator::updateUAVOs(Output2OP out){
// QTime currentTime = QTime::currentTime();
QTime currentTime = QTime::currentTime();
Noise noise;
HitlNoiseGeneration noiseSource;
if(settings.addNoise){
noise = noiseSource.generateNoise();
}
@ -377,8 +398,6 @@ void Simulator::updateUAVOs(Output2OP out){
/*****************************************/
} else if (settings.attActCalc) {
// calculate RPY with code from Attitude module
AttitudeActual::DataFields attActData;
static float q[4] = {1, 0, 0, 0};
static float gyro_correct_int2 = 0;
@ -518,60 +537,60 @@ void Simulator::updateUAVOs(Output2OP out){
// not implemented yet
}
if (settings.gpsPositionEnabled) {
if (gpsPosTime.msecsTo(currentTime) >= settings.gpsPosRate) {
// Update GPS Position objects
GPSPosition::DataFields gpsPosData;
memset(&gpsPosData, 0, sizeof(GPSPosition::DataFields));
gpsPosData.Altitude = out.altitude + noise.gpsPosData.Altitude;
gpsPosData.Heading = out.heading + noise.gpsPosData.Heading;
gpsPosData.Groundspeed = out.groundspeed + noise.gpsPosData.Groundspeed;
gpsPosData.Latitude = out.latitude + noise.gpsPosData.Latitude; //Already in *10^7 integer format
gpsPosData.Longitude = out.longitude + noise.gpsPosData.Longitude; //Already in *10^7 integer format
gpsPosData.GeoidSeparation = 0.0;
gpsPosData.PDOP = 3.0;
gpsPosData.VDOP = gpsPosData.PDOP*1.5;
gpsPosData.Satellites = 10;
gpsPosData.Status = GPSPosition::STATUS_FIX3D;
if (settings.gpsPosition) {
// if (gpsPosTime.msecsTo(currentTime) >= settings.gpsPosRate) {
// Update GPS Position objects
GPSPosition::DataFields gpsPosData;
memset(&gpsPosData, 0, sizeof(GPSPosition::DataFields));
gpsPosData.Altitude = out.altitude + noise.gpsPosData.Altitude;
gpsPosData.Heading = out.heading + noise.gpsPosData.Heading;
gpsPosData.Groundspeed = out.groundspeed + noise.gpsPosData.Groundspeed;
gpsPosData.Latitude = out.latitude + noise.gpsPosData.Latitude; //Already in *10^7 integer format
gpsPosData.Longitude = out.longitude + noise.gpsPosData.Longitude; //Already in *10^7 integer format
gpsPosData.GeoidSeparation = 0.0;
gpsPosData.PDOP = 3.0;
gpsPosData.VDOP = gpsPosData.PDOP*1.5;
gpsPosData.Satellites = 10;
gpsPosData.Status = GPSPosition::STATUS_FIX3D;
gpsPos->setData(gpsPosData);
gpsPos->setData(gpsPosData);
// Update GPS Velocity.{North,East,Down}
GPSVelocity::DataFields gpsVelData;
memset(&gpsVelData, 0, sizeof(GPSVelocity::DataFields));
gpsVelData.North = out.velNorth + noise.gpsVelData.North;
gpsVelData.East = out.velEast + noise.gpsVelData.East;
gpsVelData.Down = out.velDown + noise.gpsVelData.Down;
// Update GPS Velocity.{North,East,Down}
GPSVelocity::DataFields gpsVelData;
memset(&gpsVelData, 0, sizeof(GPSVelocity::DataFields));
gpsVelData.North = out.velNorth + noise.gpsVelData.North;
gpsVelData.East = out.velEast + noise.gpsVelData.East;
gpsVelData.Down = out.velDown + noise.gpsVelData.Down;
gpsVel->setData(gpsVelData);
// static QTime gpsPosTime = currentTime;
// gpsPosTime = currentTime;
// }
gpsVel->setData(gpsVelData);
gpsPosTime.addMSecs(settings.gpsPosRate);
}
}
// Update VelocityActual.{North,East,Down}
if (settings.groundTruth) {
VelocityActual::DataFields velocityActualData;
memset(&velocityActualData, 0, sizeof(VelocityActual::DataFields));
velocityActualData.North = out.velNorth + noise.velocityActualData.North;
velocityActualData.East = out.velEast + noise.velocityActualData.East;
velocityActualData.Down = out.velDown + noise.velocityActualData.Down;
velActual->setData(velocityActualData);
if (settings.groundTruthEnabled) {
if (groundTruthTime.msecsTo(currentTime) >= settings.groundTruthRate) {
VelocityActual::DataFields velocityActualData;
memset(&velocityActualData, 0, sizeof(VelocityActual::DataFields));
velocityActualData.North = out.velNorth + noise.velocityActualData.North;
velocityActualData.East = out.velEast + noise.velocityActualData.East;
velocityActualData.Down = out.velDown + noise.velocityActualData.Down;
velActual->setData(velocityActualData);
// Update PositionActual.{Nort,East,Down}
PositionActual::DataFields positionActualData;
memset(&positionActualData, 0, sizeof(PositionActual::DataFields));
positionActualData.North = (out.dstN-initN) + noise.positionActualData.North;
positionActualData.East = (out.dstE-initE) + noise.positionActualData.East;
positionActualData.Down = (out.dstD/*-initD*/) + noise.positionActualData.Down;
posActual->setData(positionActualData);
// Update PositionActual.{Nort,East,Down}
PositionActual::DataFields positionActualData;
memset(&positionActualData, 0, sizeof(PositionActual::DataFields));
positionActualData.North = (out.dstN-initN) + noise.positionActualData.North;
positionActualData.East = (out.dstE-initE) + noise.positionActualData.East;
positionActualData.Down = (out.dstD/*-initD*/) + noise.positionActualData.Down;
posActual->setData(positionActualData);
groundTruthTime.addMSecs(settings.groundTruthRate);
}
}
if (settings.sonarAltitude) {
// if (settings.sonarAltitude) {
// static QTime sonarAltTime = currentTime;
// if (sonarAltTime.msecsTo(currentTime) >= settings.sonarAltRate) {
// SonarAltitude::DataFields sonarAltData;
@ -590,37 +609,46 @@ void Simulator::updateUAVOs(Output2OP out){
// sonarAlt->setData(sonarAltData);
// sonarAltTime = currentTime;
// }
}
// }
// Update BaroAltitude object
BaroAltitude::DataFields baroAltData;
memset(&baroAltData, 0, sizeof(BaroAltitude::DataFields));
baroAltData.Altitude = out.altitude + noise.baroAltData.Altitude;
baroAltData.Temperature = out.temperature + noise.baroAltData.Temperature;
baroAltData.Pressure = out.pressure + noise.baroAltData.Pressure;
baroAlt->setData(baroAltData);
if (settings.baroAltitudeEnabled){
if (baroAltTime.msecsTo(currentTime) >= settings.baroAltRate) {
BaroAltitude::DataFields baroAltData;
memset(&baroAltData, 0, sizeof(BaroAltitude::DataFields));
baroAltData.Altitude = out.altitude + noise.baroAltData.Altitude;
baroAltData.Temperature = out.temperature + noise.baroAltData.Temperature;
baroAltData.Pressure = out.pressure + noise.baroAltData.Pressure;
baroAlt->setData(baroAltData);
// Update BaroAirspeed object
BaroAirspeed::DataFields baroAirspeedData;
memset(&baroAirspeedData, 0, sizeof(BaroAirspeed::DataFields));
baroAirspeedData.CalibratedAirspeed = out.calibratedAirspeed + noise.baroAirspeed.CalibratedAirspeed;
baroAirspeed->setData(baroAirspeedData);
// Update BaroAirspeed object
BaroAirspeed::DataFields baroAirspeedData;
memset(&baroAirspeedData, 0, sizeof(BaroAirspeed::DataFields));
baroAirspeedData.CalibratedAirspeed = out.calibratedAirspeed + noise.baroAirspeed.CalibratedAirspeed;
baroAirspeed->setData(baroAirspeedData);
}
}
if (settings.attRaw) {
//Update gyroscope sensor data
Gyros::DataFields gyroData;
memset(&gyroData, 0, sizeof(Gyros::DataFields));
gyroData.x = out.rollRate + noise.gyroData.x;
gyroData.y = out.pitchRate + noise.gyroData.y;
gyroData.z = out.yawRate + noise.gyroData.z;
gyros->setData(gyroData);
// Update raw attitude sensors
if (settings.attRawEnabled) {
if (attRawTime.msecsTo(currentTime) >= settings.attRawRate) {
//Update gyroscope sensor data
Gyros::DataFields gyroData;
memset(&gyroData, 0, sizeof(Gyros::DataFields));
gyroData.x = out.rollRate + noise.gyroData.x;
gyroData.y = out.pitchRate + noise.gyroData.y;
gyroData.z = out.yawRate + noise.gyroData.z;
gyros->setData(gyroData);
//Update accelerometer sensor data
Accels::DataFields accelData;
memset(&accelData, 0, sizeof(Accels::DataFields));
accelData.x = out.accX + noise.accelData.x;
accelData.y = out.accY + noise.accelData.y;
accelData.z = out.accZ + noise.accelData.z;
accels->setData(accelData);
//Update accelerometer sensor data
Accels::DataFields accelData;
memset(&accelData, 0, sizeof(Accels::DataFields));
accelData.x = out.accX + noise.accelData.x;
accelData.y = out.accY + noise.accelData.y;
accelData.z = out.accZ + noise.accelData.z;
accels->setData(accelData);
attRawTime.addMSecs(settings.attRawRate);
}
}
}

136
ground/openpilotgcs/src/plugins/hitlnew/simulator.h
View File

@ -39,18 +39,18 @@
#include "uavobjectmanager.h"
#include "accels.h"
#include "actuatordesired.h"
#include "actuatorcommand.h"
#include "actuatordesired.h"
#include "attitudeactual.h"
#include "attitudesettings.h"
#include "baroaltitude.h"
#include "baroairspeed.h"
#include "baroaltitude.h"
#include "flightstatus.h"
#include "gcsreceiver.h"
#include "gcstelemetrystats.h"
#include "gpsposition.h"
#include "gpsvelocity.h"
#include "gyros.h"
#include "flightstatus.h"
#include "homelocation.h"
#include "manualcontrolcommand.h"
#include "positionactual.h"
@ -62,42 +62,43 @@
/**
* just imagine this was a class without methods and all public properties
*/
typedef struct _FLIGHT_PARAM {
typedef struct _FLIGHT_PARAM {
// time
float T;
float dT;
unsigned int i;
// time
float T;
float dT;
unsigned int i;
// speed (relative)
float ias;
float tas;
float groundspeed;
// speed (relative)
float ias;
float cas;
float tas;
float groundspeed;
// position (absolute)
float X;
float Y;
float Z;
// position (absolute)
float X;
float Y;
float Z;
// speed (absolute)
float dX;
float dY;
float dZ;
// speed (absolute)
float dX;
float dY;
float dZ;
// acceleration (absolute)
float ddX;
float ddY;
float ddZ;
// acceleration (absolute)
float ddX;
float ddY;
float ddZ;
//angle
float azimuth;
float pitch;
float roll;
//rotation speed
float dAzimuth;
float dPitch;
float dRoll;
//angle
float azimuth;
float pitch;
float roll;
//rotation speed
float dAzimuth;
float dPitch;
float dRoll;
} FLIGHT_PARAM;
@ -111,37 +112,34 @@ typedef struct _CONNECTION
int outPort;
int inPort;
bool startSim;
bool addNoise;
QString latitude;
QString longitude;
// bool homeLocation;
//Added by Hhrrrr
bool homeLocation;
quint16 homeLocRate;
bool attRaw;
bool attRawEnabled;
quint8 attRawRate;
bool attActual;
bool attActualEnabled;
bool attActHW;
bool attActSim;
bool attActCalc;
bool sonarAltitude;
float sonarMaxAlt;
quint16 sonarAltRate;
bool baroAltitudeEnabled;
quint16 baroAltRate;
bool groundTruth;
bool gpsPosition;
bool groundTruthEnabled;
quint16 groundTruthRate;
bool gpsPositionEnabled;
quint16 gpsPosRate;
bool inputCommand;
bool gcsReciever;
bool manualControl;
bool manualOutput;
quint8 outputRate;
quint16 minOutputPeriod;
} SimulatorSettings;
@ -195,9 +193,10 @@ public:
static void setStarted(bool val) { isStarted = val; }
static QStringList& Instances() { return Simulator::instances; }
static void setInstance(const QString& str) { Simulator::instances.append(str); }
virtual void setupUdpPorts(const QString& host, int inPort, int outPort) { Q_UNUSED(host) Q_UNUSED(inPort) Q_UNUSED(outPort)}
virtual void stopProcess() {}
virtual void setupUdpPorts(const QString& host, int inPort, int outPort) { Q_UNUSED(host) Q_UNUSED(inPort) Q_UNUSED(outPort)}
void resetInitialHomePosition();
void updateUAVOs(Output2OP out);
signals:
void autopilotConnected();
@ -223,7 +222,6 @@ private slots:
virtual void processUpdate(const QByteArray& data) = 0;
protected:
static const float GEE;
static const float FT2M;
static const float KT2MPS;
@ -261,21 +259,31 @@ protected:
QMutex lock;
private:
bool once;
float initN;
float initE;
float initD;
int updatePeriod;
int simTimeout;
volatile bool autopilotConnectionStatus;
volatile bool simConnectionStatus;
QTimer* txTimer;
QTimer* simTimer;
QString name;
QString simulatorId;
volatile static bool isStarted;
static QStringList instances;
//QList<QScopedPointer<UAVDataObject> > requiredUAVObjects;
void setupOutputObject(UAVObject* obj, int updatePeriod);
void setupInputObject(UAVObject* obj, int updatePeriod);
void setupObjects();
int updatePeriod;
int simTimeout;
volatile bool autopilotConnectionStatus;
volatile bool simConnectionStatus;
QTimer* txTimer;
QTimer* simTimer;
QTime attRawTime;
QTime gpsPosTime;
QTime groundTruthTime;
QTime baroAltTime;
QString name;
QString simulatorId;
volatile static bool isStarted;
static QStringList instances;
//QList<QScopedPointer<UAVDataObject> > requiredUAVObjects;
void setupOutputObject(UAVObject* obj, quint32 updatePeriod);
void setupInputObject(UAVObject* obj, quint32 updatePeriod);
void setupObjects();
};

16
ground/openpilotgcs/src/plugins/hitlnew/xplanesimulator.cpp
View File

@ -321,13 +321,13 @@ void XplaneSimulator::processUpdate(const QByteArray& dataBuf)
}
// Update AltitudeActual object
BaroAltitude::DataFields altActualData;
memset(&altActualData, 0, sizeof(BaroAltitude::DataFields));
altActualData.Altitude = altitude;
altActualData.Temperature = temperature;
altActualData.Pressure = pressure;
altActual->setData(altActualData);
// Update AltitudeActual object
BaroAltitude::DataFields baroAltData;
memset(&baroAltData, 0, sizeof(BaroAltitude::DataFields));
baroAltData.Altitude = altitude;
baroAltData.Temperature = temperature;
baroAltData.Pressure = pressure;
baroAlt->setData(baroAltData);
// Update attActual object
AttitudeActual::DataFields attActualData;
@ -406,7 +406,7 @@ void XplaneSimulator::processUpdate(const QByteArray& dataBuf)
}
// issue manual update
//attActual->updated();
//altActual->updated();
//baroAlt->updated();
//posActual->updated();
}

2
ground/openpilotgcs/src/plugins/uavobjects/uavobjects.pro
View File

@ -24,6 +24,7 @@ OTHER_FILES += UAVObjects.pluginspec
# Add in all of the synthetic/generated uavobject files
HEADERS += $$UAVOBJECT_SYNTHETICS/accessorydesired.h \
$$UAVOBJECT_SYNTHETICS/baroairspeed.h \
$$UAVOBJECT_SYNTHETICS/baroaltitude.h \
$$UAVOBJECT_SYNTHETICS/attitudeactual.h \
$$UAVOBJECT_SYNTHETICS/altholdsmoothed.h \
@ -89,6 +90,7 @@ HEADERS += $$UAVOBJECT_SYNTHETICS/accessorydesired.h \
$$UAVOBJECT_SYNTHETICS/pathaction.h
SOURCES += $$UAVOBJECT_SYNTHETICS/accessorydesired.cpp \
$$UAVOBJECT_SYNTHETICS/baroairspeed.cpp \
$$UAVOBJECT_SYNTHETICS/baroaltitude.cpp \
$$UAVOBJECT_SYNTHETICS/attitudeactual.cpp \
$$UAVOBJECT_SYNTHETICS/altholdsmoothed.cpp \