1
0
mirror of https://bitbucket.org/librepilot/librepilot.git synced 2024-12-13 20:48:42 +01:00
LibrePilot/ground/openpilotgcs/src/plugins/hitl/simulator.cpp

850 lines
30 KiB
C++
Raw Normal View History

2013-04-07 08:49:13 +02:00
/**
******************************************************************************
*
* @file simulator.cpp
* @author The OpenPilot Team, http://www.openpilot.org Copyright (C) 2010.
* @addtogroup GCSPlugins GCS Plugins
* @{
* @addtogroup HITLPlugin HITL Plugin
* @{
* @brief The Hardware In The Loop plugin
*****************************************************************************/
/*
* 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 "simulator.h"
#include "qxtlogger.h"
#include "extensionsystem/pluginmanager.h"
#include "coreplugin/icore.h"
#include "coreplugin/threadmanager.h"
#include "hitlnoisegeneration.h"
volatile bool Simulator::isStarted = false;
const float Simulator::GEE = 9.81;
const float Simulator::FT2M = 0.3048;
const float Simulator::KT2MPS = 0.514444444;
2013-04-07 08:49:13 +02:00
const float Simulator::INHG2KPA = 3.386;
const float Simulator::FPS2CMPS = 30.48;
const float Simulator::DEG2RAD = (M_PI / 180.0);
const float Simulator::RAD2DEG = (180.0 / M_PI);
Simulator::Simulator(const SimulatorSettings & params) :
simProcess(NULL),
time(NULL),
inSocket(NULL),
outSocket(NULL),
settings(params),
updatePeriod(50),
simTimeout(8000),
autopilotConnectionStatus(false),
simConnectionStatus(false),
txTimer(NULL),
simTimer(NULL),
name("")
2013-04-07 08:49:13 +02:00
{
// move to thread
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;
gcsRcvrTime = currentTime;
attRawTime = currentTime;
baroAltTime = currentTime;
2013-04-07 08:49:13 +02:00
battTime = currentTime;
airspeedStateTime = currentTime;
// Define standard atmospheric constants
airParameters.univGasConstant = 8.31447; // [J/(mol·K)]
airParameters.dryAirConstant = 287.058; // [J/(kg*K)]
airParameters.groundDensity = 1.225; // [kg/m^3]
airParameters.groundTemp = 15 + 273.15; // [K]
airParameters.tempLapseRate = 0.0065; // [deg/m]
airParameters.M = 0.0289644; // [kg/mol]
airParameters.relativeHumidity = 20; // [%]
airParameters.seaLevelPress = 101.325; // [kPa]
2013-04-07 08:49:13 +02:00
}
Simulator::~Simulator()
{
if (inSocket) {
delete inSocket;
inSocket = NULL;
}
if (outSocket) {
delete outSocket;
outSocket = NULL;
}
if (txTimer) {
delete txTimer;
txTimer = NULL;
}
if (simTimer) {
delete simTimer;
simTimer = NULL;
}
// NOTE: Does not currently work, may need to send control+c to through the terminal
if (simProcess != NULL) {
// connect(simProcess,SIGNAL(finished(int, QProcess::ExitStatus)),this,SLOT(onFinished(int, QProcess::ExitStatus)));
simProcess->disconnect();
if (simProcess->state() == QProcess::Running) {
simProcess->kill();
}
// if(simProcess->waitForFinished())
// emit deleteSimProcess();
delete simProcess;
simProcess = NULL;
}
2013-04-07 08:49:13 +02:00
}
void Simulator::onDeleteSimulator(void)
{
// [1]
Simulator::setStarted(false);
// [2]
Simulator::Instances().removeOne(simulatorId);
2013-04-07 08:49:13 +02:00
disconnect(this);
delete this;
2013-04-07 08:49:13 +02:00
}
void Simulator::onStart()
{
QMutexLocker locker(&lock);
QThread *mainThread = QThread::currentThread();
2013-04-07 08:49:13 +02:00
qDebug() << "Simulator Thread: " << mainThread;
2013-04-07 08:49:13 +02:00
// Get required UAVObjects
ExtensionSystem::PluginManager *pm = ExtensionSystem::PluginManager::instance();
UAVObjectManager *objManager = pm->getObject<UAVObjectManager>();
actDesired = ActuatorDesired::GetInstance(objManager);
actCommand = ActuatorCommand::GetInstance(objManager);
2013-04-07 08:49:13 +02:00
manCtrlCommand = ManualControlCommand::GetInstance(objManager);
gcsReceiver = GCSReceiver::GetInstance(objManager);
flightStatus = FlightStatus::GetInstance(objManager);
posHome = HomeLocation::GetInstance(objManager);
velState = VelocityState::GetInstance(objManager);
posState = PositionState::GetInstance(objManager);
baroAlt = BaroSensor::GetInstance(objManager);
flightBatt = FlightBatteryState::GetInstance(objManager);
airspeedState = AirspeedState::GetInstance(objManager);
attState = AttitudeState::GetInstance(objManager);
attSettings = AttitudeSettings::GetInstance(objManager);
accelState = AccelState::GetInstance(objManager);
gyroState = GyroState::GetInstance(objManager);
gpsPos = GPSPosition::GetInstance(objManager);
gpsVel = GPSVelocity::GetInstance(objManager);
telStats = GCSTelemetryStats::GetInstance(objManager);
groundTruth = GroundTruth::GetInstance(objManager);
2013-04-07 08:49:13 +02:00
// Listen to autopilot connection events
TelemetryManager *telMngr = pm->getObject<TelemetryManager>();
2013-04-07 08:49:13 +02:00
connect(telMngr, SIGNAL(connected()), this, SLOT(onAutopilotConnect()));
connect(telMngr, SIGNAL(disconnected()), this, SLOT(onAutopilotDisconnect()));
// connect(telStats, SIGNAL(objectUpdated(UAVObject*)), this, SLOT(telStatsUpdated(UAVObject*)));
2013-04-07 08:49:13 +02:00
// If already connect setup autopilot
GCSTelemetryStats::DataFields stats = telStats->getData();
if (stats.Status == GCSTelemetryStats::STATUS_CONNECTED) {
2013-04-07 08:49:13 +02:00
onAutopilotConnect();
}
2013-04-07 08:49:13 +02:00
inSocket = new QUdpSocket();
2013-04-07 08:49:13 +02:00
outSocket = new QUdpSocket();
setupUdpPorts(settings.hostAddress, settings.inPort, settings.outPort);
emit processOutput("\nLocal interface: " + settings.hostAddress + "\n" + \
"Remote interface: " + settings.remoteAddress + "\n" + \
"inputPort: " + QString::number(settings.inPort) + "\n" + \
"outputPort: " + QString::number(settings.outPort) + "\n");
qxtLog->info("\nLocal interface: " + settings.hostAddress + "\n" + \
"Remote interface: " + settings.remoteAddress + "\n" + \
"inputPort: " + QString::number(settings.inPort) + "\n" + \
"outputPort: " + QString::number(settings.outPort) + "\n");
// if(!inSocket->waitForConnected(5000))
// emit processOutput(QString("Can't connect to %1 on %2 port!").arg(settings.hostAddress).arg(settings.inPort));
// outSocket->connectToHost(settings.hostAddress,settings.outPort); // FG
// if(!outSocket->waitForConnected(5000))
// emit processOutput(QString("Can't connect to %1 on %2 port!").arg(settings.hostAddress).arg(settings.outPort));
connect(inSocket, SIGNAL(readyRead()), this, SLOT(receiveUpdate()), Qt::DirectConnection);
// Setup transmit timer
txTimer = new QTimer();
connect(txTimer, SIGNAL(timeout()), this, SLOT(transmitUpdate()), Qt::DirectConnection);
txTimer->setInterval(updatePeriod);
txTimer->start();
// Setup simulator connection timer
simTimer = new QTimer();
connect(simTimer, SIGNAL(timeout()), this, SLOT(onSimulatorConnectionTimeout()), Qt::DirectConnection);
simTimer->setInterval(simTimeout);
simTimer->start();
// setup time
time = new QTime();
time->start();
current.T = 0;
current.i = 0;
2013-04-07 08:49:13 +02:00
}
void Simulator::receiveUpdate()
{
// Update connection timer and status
simTimer->setInterval(simTimeout);
simTimer->stop();
simTimer->start();
if (!simConnectionStatus) {
simConnectionStatus = true;
emit simulatorConnected();
}
// Process data
while (inSocket->hasPendingDatagrams()) {
// Receive datagram
QByteArray datagram;
datagram.resize(inSocket->pendingDatagramSize());
QHostAddress sender;
quint16 senderPort;
inSocket->readDatagram(datagram.data(), datagram.size(),
&sender, &senderPort);
// QString datastr(datagram);
// Process incomming data
processUpdate(datagram);
}
2013-04-07 08:49:13 +02:00
}
void Simulator::setupObjects()
{
if (settings.gcsReceiverEnabled) {
setupInputObject(actCommand, settings.minOutputPeriod); // Input to the simulator
2013-04-07 08:49:13 +02:00
setupOutputObject(gcsReceiver, settings.minOutputPeriod);
} else if (settings.manualControlEnabled) {
setupInputObject(actDesired, settings.minOutputPeriod); // Input to the simulator
2013-04-07 08:49:13 +02:00
}
setupOutputObject(posHome, 10000); // Hardcoded? Bleh.
2013-04-07 08:49:13 +02:00
if (settings.gpsPositionEnabled) {
2013-04-07 08:49:13 +02:00
setupOutputObject(gpsPos, settings.gpsPosRate);
setupOutputObject(gpsVel, settings.gpsPosRate);
}
if (settings.groundTruthEnabled) {
setupOutputObject(posState, settings.groundTruthRate);
setupOutputObject(velState, settings.groundTruthRate);
2013-04-07 08:49:13 +02:00
}
if (settings.attRawEnabled) {
setupOutputObject(accelState, settings.attRawRate);
setupOutputObject(gyroState, settings.attRawRate);
2013-04-07 08:49:13 +02:00
}
if (settings.attStateEnabled && settings.attActHW) {
setupOutputObject(accelState, settings.attRawRate);
setupOutputObject(gyroState, settings.attRawRate);
2013-04-07 08:49:13 +02:00
}
if (settings.attStateEnabled && !settings.attActHW) {
setupOutputObject(attState, 20); // Hardcoded? Bleh.
} else {
setupWatchedObject(attState, 100); // Hardcoded? Bleh.
}
if (settings.airspeedStateEnabled) {
setupOutputObject(airspeedState, settings.airspeedStateRate);
}
2013-04-07 08:49:13 +02:00
if (settings.baroSensorEnabled) {
2013-04-07 08:49:13 +02:00
setupOutputObject(baroAlt, settings.baroAltRate);
setupOutputObject(flightBatt, settings.baroAltRate);
}
}
void Simulator::setupInputObject(UAVObject *obj, quint32 updatePeriod)
2013-04-07 08:49:13 +02:00
{
UAVObject::Metadata mdata;
2013-04-07 08:49:13 +02:00
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);
UAVObject::SetFlightTelemetryUpdateMode(mdata, UAVObject::UPDATEMODE_PERIODIC);
mdata.flightTelemetryUpdatePeriod = updatePeriod;
obj->setMetadata(mdata);
}
void Simulator::setupWatchedObject(UAVObject *obj, quint32 updatePeriod)
{
UAVObject::Metadata mdata;
2013-04-07 08:49:13 +02:00
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);
UAVObject::SetFlightTelemetryUpdateMode(mdata, UAVObject::UPDATEMODE_PERIODIC);
mdata.flightTelemetryUpdatePeriod = updatePeriod;
obj->setMetadata(mdata);
}
void Simulator::setupOutputObject(UAVObject *obj, quint32 updatePeriod)
2013-04-07 08:49:13 +02:00
{
UAVObject::Metadata mdata;
mdata = obj->getDefaultMetadata();
2013-04-07 08:49:13 +02:00
UAVObject::SetGcsAccess(mdata, UAVObject::ACCESS_READWRITE);
UAVObject::SetGcsTelemetryAcked(mdata, false);
UAVObject::SetGcsTelemetryUpdateMode(mdata, UAVObject::UPDATEMODE_PERIODIC);
2013-04-07 08:49:13 +02:00
mdata.gcsTelemetryUpdatePeriod = updatePeriod;
UAVObject::SetFlightAccess(mdata, UAVObject::ACCESS_READONLY);
UAVObject::SetFlightTelemetryUpdateMode(mdata, UAVObject::UPDATEMODE_MANUAL);
2013-04-07 08:49:13 +02:00
obj->setMetadata(mdata);
}
void Simulator::onAutopilotConnect()
{
autopilotConnectionStatus = true;
setupObjects();
emit autopilotConnected();
2013-04-07 08:49:13 +02:00
}
void Simulator::onAutopilotDisconnect()
{
autopilotConnectionStatus = false;
emit autopilotDisconnected();
2013-04-07 08:49:13 +02:00
}
void Simulator::onSimulatorConnectionTimeout()
{
if (simConnectionStatus) {
simConnectionStatus = false;
emit simulatorDisconnected();
}
2013-04-07 08:49:13 +02:00
}
void Simulator::telStatsUpdated(UAVObject *obj)
2013-04-07 08:49:13 +02:00
{
Q_UNUSED(obj);
GCSTelemetryStats::DataFields stats = telStats->getData();
if (!autopilotConnectionStatus && stats.Status == GCSTelemetryStats::STATUS_CONNECTED) {
onAutopilotConnect();
} else if (autopilotConnectionStatus && stats.Status != GCSTelemetryStats::STATUS_CONNECTED) {
onAutopilotDisconnect();
}
2013-04-07 08:49:13 +02:00
}
void Simulator::resetInitialHomePosition()
{
once = false;
2013-04-07 08:49:13 +02:00
}
void Simulator::updateUAVOs(Output2Hardware out)
{
2013-04-07 08:49:13 +02:00
QTime currentTime = QTime::currentTime();
Noise noise;
HitlNoiseGeneration noiseSource;
if (settings.addNoise) {
2013-04-07 08:49:13 +02:00
noise = noiseSource.generateNoise();
} else {
2013-04-07 08:49:13 +02:00
memset(&noise, 0, sizeof(Noise));
}
/*******************************/
HomeLocation::DataFields homeData = posHome->getData();
if (!once) {
2013-04-07 08:49:13 +02:00
// Upon startup, we reset the HomeLocation object to
// the plane's location:
memset(&homeData, 0, sizeof(HomeLocation::DataFields));
// Update homelocation
homeData.Latitude = out.latitude; // Already in *10^7 integer format
homeData.Longitude = out.longitude; // Already in *10^7 integer format
homeData.Altitude = out.agl;
2013-04-07 08:49:13 +02:00
double LLA[3];
LLA[0] = out.latitude;
LLA[1] = out.longitude;
LLA[2] = out.altitude;
2013-04-07 08:49:13 +02:00
homeData.Be[0] = 0;
homeData.Be[1] = 0;
homeData.Be[2] = 0;
2013-04-07 08:49:13 +02:00
posHome->setData(homeData);
posHome->updated();
// Compute initial distance
initN = out.dstN;
initE = out.dstE;
initD = out.dstD;
once = true;
2013-04-07 08:49:13 +02:00
}
/*******************************/
// Copy everything to the ground truth object. GroundTruth is Noise-free.
2013-04-07 08:49:13 +02:00
GroundTruth::DataFields groundTruthData;
groundTruthData = groundTruth->getData();
groundTruthData.AccelerationXYZ[0] = out.accX;
groundTruthData.AccelerationXYZ[1] = out.accY;
groundTruthData.AccelerationXYZ[2] = out.accZ;
2013-04-07 08:49:13 +02:00
groundTruthData.AngularRates[0] = out.rollRate;
groundTruthData.AngularRates[1] = out.pitchRate;
groundTruthData.AngularRates[2] = out.yawRate;
2013-04-07 08:49:13 +02:00
groundTruthData.CalibratedAirspeed = out.calibratedAirspeed;
groundTruthData.TrueAirspeed = out.trueAirspeed;
groundTruthData.AngleOfAttack = out.angleOfAttack;
groundTruthData.AngleOfSlip = out.angleOfSlip;
2013-04-07 08:49:13 +02:00
groundTruthData.PositionNED[0] = out.dstN - initN;
groundTruthData.PositionNED[1] = out.dstE - initD;
groundTruthData.PositionNED[2] = out.dstD - initD;
2013-04-07 08:49:13 +02:00
groundTruthData.VelocityNED[0] = out.velNorth;
groundTruthData.VelocityNED[1] = out.velEast;
groundTruthData.VelocityNED[2] = out.velDown;
2013-04-07 08:49:13 +02:00
groundTruthData.RPY[0] = out.roll;
groundTruthData.RPY[0] = out.pitch;
groundTruthData.RPY[0] = out.heading;
2013-04-07 08:49:13 +02:00
// Set UAVO
2013-04-07 08:49:13 +02:00
groundTruth->setData(groundTruthData);
/*******************************/
// Update attState object
AttitudeState::DataFields attStateData;
attStateData = attState->getData();
2013-04-07 08:49:13 +02:00
if (settings.attActHW) {
// do nothing
/*****************************************/
} else if (settings.attActSim) {
// take all data from simulator
attStateData.Roll = out.roll + noise.attStateData.Roll; // roll;
attStateData.Pitch = out.pitch + noise.attStateData.Pitch; // pitch
attStateData.Yaw = out.heading + noise.attStateData.Yaw; // Yaw
2013-04-07 08:49:13 +02:00
float rpy[3];
float quat[4];
rpy[0] = attStateData.Roll;
rpy[1] = attStateData.Pitch;
rpy[2] = attStateData.Yaw;
Utils::CoordinateConversions().RPY2Quaternion(rpy, quat);
attStateData.q1 = quat[0];
attStateData.q2 = quat[1];
attStateData.q3 = quat[2];
attStateData.q4 = quat[3];
2013-04-07 08:49:13 +02:00
// Set UAVO
attState->setData(attStateData);
2013-04-07 08:49:13 +02:00
/*****************************************/
} else if (settings.attActCalc) {
// calculate RPY with code from Attitude module
static float q[4] = { 1, 0, 0, 0 };
2013-04-07 08:49:13 +02:00
static float gyro_correct_int2 = 0;
float dT = out.delT;
AttitudeSettings::DataFields attSettData = attSettings->getData();
float accelKp = attSettData.AccelKp * 0.1666666666666667;
float accelKi = attSettData.AccelKp * 0.1666666666666667;
2013-04-07 08:49:13 +02:00
float yawBiasRate = attSettData.YawBiasRate;
// calibrate sensors on arming
if (flightStatus->getData().Armed == FlightStatus::ARMED_ARMING) {
accelKp = 2.0;
accelKi = 0.9;
}
float gyro[3] = { out.rollRate, out.pitchRate, out.yawRate };
float attRawAcc[3] = { out.accX, out.accY, out.accZ };
2013-04-07 08:49:13 +02:00
// code from Attitude module begin ///////////////////////////////
float *accels = attRawAcc;
float grot[3];
float accel_err[3];
// Rotate gravity to body frame and cross with accels
grot[0] = -(2 * (q[1] * q[3] - q[0] * q[2]));
grot[1] = -(2 * (q[2] * q[3] + q[0] * q[1]));
grot[2] = -(q[0] * q[0] - q[1] * q[1] - q[2] * q[2] + q[3] * q[3]);
2013-04-07 08:49:13 +02:00
// CrossProduct
{
accel_err[0] = accels[1] * grot[2] - grot[1] * accels[2];
accel_err[1] = grot[0] * accels[2] - accels[0] * grot[2];
accel_err[2] = accels[0] * grot[1] - grot[0] * accels[1];
2013-04-07 08:49:13 +02:00
}
// Account for accel magnitude
float accel_mag = sqrt(accels[0] * accels[0] + accels[1] * accels[1] + accels[2] * accels[2]);
accel_err[0] /= accel_mag;
accel_err[1] /= accel_mag;
accel_err[2] /= accel_mag;
// Accumulate integral of error. Scale here so that units are (deg/s) but Ki has units of s
gyro_correct_int2 += -gyro[2] * yawBiasRate;
// Correct rates based on error, integral component dealt with in updateSensors
gyro[0] += accel_err[0] * accelKp / dT;
gyro[1] += accel_err[1] * accelKp / dT;
gyro[2] += accel_err[2] * accelKp / dT + gyro_correct_int2;
// Work out time derivative from INSAlgo writeup
// Also accounts for the fact that gyros are in deg/s
float qdot[4];
qdot[0] = (-q[1] * gyro[0] - q[2] * gyro[1] - q[3] * gyro[2]) * dT * M_PI / 180 / 2;
qdot[1] = (+q[0] * gyro[0] - q[3] * gyro[1] + q[2] * gyro[2]) * dT * M_PI / 180 / 2;
qdot[2] = (+q[3] * gyro[0] + q[0] * gyro[1] - q[1] * gyro[2]) * dT * M_PI / 180 / 2;
qdot[3] = (-q[2] * gyro[0] + q[1] * gyro[1] + q[0] * gyro[2]) * dT * M_PI / 180 / 2;
// Take a time step
q[0] += qdot[0];
q[1] += qdot[1];
q[2] += qdot[2];
q[3] += qdot[3];
2013-04-07 08:49:13 +02:00
if (q[0] < 0) {
2013-04-07 08:49:13 +02:00
q[0] = -q[0];
q[1] = -q[1];
q[2] = -q[2];
q[3] = -q[3];
}
// Renomalize
float qmag = sqrt((q[0] * q[0]) + (q[1] * q[1]) + (q[2] * q[2]) + (q[3] * q[3]));
q[0] /= qmag;
q[1] /= qmag;
q[2] /= qmag;
q[3] /= qmag;
// If quaternion has become inappropriately short or is nan reinit.
// THIS SHOULD NEVER ACTUALLY HAPPEN
if ((fabs(qmag) < 1e-3) || (qmag != qmag)) {
2013-04-07 08:49:13 +02:00
q[0] = 1;
q[1] = 0;
q[2] = 0;
q[3] = 0;
}
float rpy2[3];
// Quaternion2RPY
{
float q0s, q1s, q2s, q3s;
q0s = q[0] * q[0];
q1s = q[1] * q[1];
q2s = q[2] * q[2];
q3s = q[3] * q[3];
2013-04-07 08:49:13 +02:00
float R13, R11, R12, R23, R33;
R13 = 2 * (q[1] * q[3] - q[0] * q[2]);
R11 = q0s + q1s - q2s - q3s;
R12 = 2 * (q[1] * q[2] + q[0] * q[3]);
R23 = 2 * (q[2] * q[3] + q[0] * q[1]);
R33 = q0s - q1s - q2s + q3s;
2013-04-07 08:49:13 +02:00
rpy2[1] = RAD2DEG * asinf(-R13); // pitch always between -pi/2 to pi/2
2013-04-07 08:49:13 +02:00
rpy2[2] = RAD2DEG * atan2f(R12, R11);
rpy2[0] = RAD2DEG * atan2f(R23, R33);
}
attStateData.Roll = rpy2[0];
attStateData.Pitch = rpy2[1];
attStateData.Yaw = rpy2[2];
attStateData.q1 = q[0];
attStateData.q2 = q[1];
attStateData.q3 = q[2];
attStateData.q4 = q[3];
2013-04-07 08:49:13 +02:00
// Set UAVO
attState->setData(attStateData);
2013-04-07 08:49:13 +02:00
/*****************************************/
}
/*******************************/
if (settings.gcsReceiverEnabled) {
if (gcsRcvrTime.msecsTo(currentTime) >= settings.minOutputPeriod) {
GCSReceiver::DataFields gcsRcvrData;
memset(&gcsRcvrData, 0, sizeof(GCSReceiver::DataFields));
for (quint16 i = 0; i < GCSReceiver::CHANNEL_NUMELEM; i++) {
gcsRcvrData.Channel[i] = 1500 + (out.rc_channel[i] * 500); // Elements in rc_channel are between -1 and 1
2013-04-07 08:49:13 +02:00
}
gcsReceiver->setData(gcsRcvrData);
gcsRcvrTime = gcsRcvrTime.addMSecs(settings.minOutputPeriod);
2013-04-07 08:49:13 +02:00
}
}
/*******************************/
if (settings.gpsPositionEnabled) {
if (gpsPosTime.msecsTo(currentTime) >= settings.gpsPosRate) {
qDebug() << " GPS time:" << gpsPosTime << ", currentTime: " << currentTime << ", difference: " << gpsPosTime.msecsTo(currentTime);
2013-04-07 08:49:13 +02:00
// 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
2013-04-07 08:49:13 +02:00
gpsPosData.GeoidSeparation = 0.0;
gpsPosData.PDOP = 3.0;
gpsPosData.VDOP = gpsPosData.PDOP * 1.5;
gpsPosData.Satellites = 10;
2013-04-07 08:49:13 +02:00
gpsPosData.Status = GPSPosition::STATUS_FIX3D;
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;
2013-04-07 08:49:13 +02:00
gpsVel->setData(gpsVelData);
gpsPosTime = gpsPosTime.addMSecs(settings.gpsPosRate);
2013-04-07 08:49:13 +02:00
}
}
/*******************************/
// Update VelocityState.{North,East,Down}
2013-04-07 08:49:13 +02:00
if (settings.groundTruthEnabled) {
if (groundTruthTime.msecsTo(currentTime) >= settings.groundTruthRate) {
VelocityState::DataFields velocityStateData;
memset(&velocityStateData, 0, sizeof(VelocityState::DataFields));
velocityStateData.North = out.velNorth + noise.velocityStateData.North;
velocityStateData.East = out.velEast + noise.velocityStateData.East;
velocityStateData.Down = out.velDown + noise.velocityStateData.Down;
velState->setData(velocityStateData);
// Update PositionState.{Nort,East,Down}
PositionState::DataFields positionStateData;
memset(&positionStateData, 0, sizeof(PositionState::DataFields));
positionStateData.North = (out.dstN - initN) + noise.positionStateData.North;
positionStateData.East = (out.dstE - initE) + noise.positionStateData.East;
positionStateData.Down = (out.dstD /*-initD*/) + noise.positionStateData.Down;
posState->setData(positionStateData);
2013-04-07 08:49:13 +02:00
groundTruthTime = groundTruthTime.addMSecs(settings.groundTruthRate);
2013-04-07 08:49:13 +02:00
}
}
///*******************************/
// if (settings.sonarAltitude) {
// static QTime sonarAltTime = currentTime;
// if (sonarAltTime.msecsTo(currentTime) >= settings.sonarAltRate) {
// SonarAltitude::DataFields sonarAltData;
// sonarAltData = sonarAlt->getData();
// float sAlt = settings.sonarMaxAlt;
//// 0.35 rad ~= 20 degree
// if ((agl < (sAlt * 2.0)) && (roll < 0.35) && (pitch < 0.35)) {
// float x = agl * qTan(roll);
// float y = agl * qTan(pitch);
// float h = qSqrt(x*x + y*y + agl*agl);
// sAlt = qMin(h, sAlt);
// }
// sonarAltData.Altitude = sAlt;
// sonarAlt->setData(sonarAltData);
// sonarAltTime = currentTime;
// }
// }
2013-04-07 08:49:13 +02:00
/*******************************/
// Update BaroSensor object
if (settings.baroSensorEnabled) {
2013-04-07 08:49:13 +02:00
if (baroAltTime.msecsTo(currentTime) >= settings.baroAltRate) {
BaroSensor::DataFields baroAltData;
memset(&baroAltData, 0, sizeof(BaroSensor::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);
baroAltTime = baroAltTime.addMSecs(settings.baroAltRate);
2013-04-07 08:49:13 +02:00
}
}
/*******************************/
// Update FlightBatteryState object
if (settings.baroSensorEnabled) {
2013-04-07 08:49:13 +02:00
if (battTime.msecsTo(currentTime) >= settings.baroAltRate) {
FlightBatteryState::DataFields batteryData;
memset(&batteryData, 0, sizeof(FlightBatteryState::DataFields));
batteryData.Voltage = out.voltage;
batteryData.Current = out.current;
batteryData.ConsumedEnergy = out.consumption;
flightBatt->setData(batteryData);
battTime = battTime.addMSecs(settings.baroAltRate);
2013-04-07 08:49:13 +02:00
}
}
/*******************************/
// Update AirspeedState object
if (settings.airspeedStateEnabled) {
if (airspeedStateTime.msecsTo(currentTime) >= settings.airspeedStateRate) {
AirspeedState::DataFields airspeedStateData;
memset(&airspeedStateData, 0, sizeof(AirspeedState::DataFields));
airspeedStateData.CalibratedAirspeed = out.calibratedAirspeed + noise.airspeedState.CalibratedAirspeed;
airspeedStateData.TrueAirspeed = out.trueAirspeed + noise.airspeedState.TrueAirspeed;
// airspeedStateData.alpha=out.angleOfAttack; // to be implemented
// airspeedStateData.beta=out.angleOfSlip;
airspeedState->setData(airspeedStateData);
airspeedStateTime = airspeedStateTime.addMSecs(settings.airspeedStateRate);
2013-04-07 08:49:13 +02:00
}
}
/*******************************/
// Update raw attitude sensors
if (settings.attRawEnabled) {
if (attRawTime.msecsTo(currentTime) >= settings.attRawRate) {
// Update gyroscope sensor data
GyroState::DataFields gyroStateData;
memset(&gyroStateData, 0, sizeof(GyroState::DataFields));
gyroStateData.x = out.rollRate + noise.gyroStateData.x;
gyroStateData.y = out.pitchRate + noise.gyroStateData.y;
gyroStateData.z = out.yawRate + noise.gyroStateData.z;
gyroState->setData(gyroStateData);
2013-04-07 08:49:13 +02:00
// Update accelerometer sensor data
AccelState::DataFields accelStateData;
memset(&accelStateData, 0, sizeof(AccelState::DataFields));
accelStateData.x = out.accX + noise.accelStateData.x;
accelStateData.y = out.accY + noise.accelStateData.y;
accelStateData.z = out.accZ + noise.accelStateData.z;
accelState->setData(accelStateData);
attRawTime = attRawTime.addMSecs(settings.attRawRate);
2013-04-07 08:49:13 +02:00
}
}
}
/**
* calculate air density from altitude. http://en.wikipedia.org/wiki/Density_of_air
*/
float Simulator::airDensityFromAltitude(float alt, AirParameters air, float gravity)
{
float p = airPressureFromAltitude(alt, air, gravity);
float rho = p * air.M / (air.univGasConstant * (air.groundTemp - air.tempLapseRate * alt));
2013-04-07 08:49:13 +02:00
return rho;
}
/**
* @brief Simulator::airPressureFromAltitude Get air pressure from altitude and atmospheric conditions.
* @param alt altitude
* @param air atmospheric conditions
* @param gravity
* @return
*/
float Simulator::airPressureFromAltitude(float alt, AirParameters air, float gravity)
{
return air.seaLevelPress * pow(1 - air.tempLapseRate * alt / air.groundTemp, gravity * air.M / (air.univGasConstant * air.tempLapseRate));
2013-04-07 08:49:13 +02:00
}
/**
* @brief Simulator::cas2tas calculate TAS from CAS and altitude. http://en.wikipedia.org/wiki/Airspeed
* @param CAS Calibrated airspeed
* @param alt altitude
* @param air atmospheric conditions
* @param gravity
* @return True airspeed
*/
float Simulator::cas2tas(float CAS, float alt, AirParameters air, float gravity)
{
float rho = airDensityFromAltitude(alt, air, gravity);
2013-04-07 08:49:13 +02:00
return CAS * sqrt(air.groundDensity / rho);
2013-04-07 08:49:13 +02:00
}
/**
* @brief Simulator::tas2cas calculate CAS from TAS and altitude. http://en.wikipedia.org/wiki/Airspeed
* @param TAS True airspeed
* @param alt altitude
* @param air atmospheric conditions
* @param gravity
* @return Calibrated airspeed
*/
float Simulator::tas2cas(float TAS, float alt, AirParameters air, float gravity)
{
float rho = airDensityFromAltitude(alt, air, gravity);
2013-04-07 08:49:13 +02:00
return TAS / sqrt(air.groundDensity / rho);
2013-04-07 08:49:13 +02:00
}
/**
* @brief Simulator::getAirParameters get air parameters
* @return airParameters
*/
AirParameters Simulator::getAirParameters()
{
2013-04-07 08:49:13 +02:00
return airParameters;
}
/**
* @brief Simulator::setAirParameters set air parameters
* @param airParameters
*/
void Simulator::setAirParameters(AirParameters airParameters)
{
this->airParameters = airParameters;
2013-04-07 08:49:13 +02:00
}