ground/gcs/src/plugins/hitl/simulator.cpp

   1 /**
   2  ******************************************************************************
   3  *
   4  * @file       simulator.cpp
   5  * @author     The OpenPilot Team, http://www.openpilot.org Copyright (C) 2010.
   6  * @addtogroup GCSPlugins GCS Plugins
   7  * @{
   8  * @addtogroup HITLPlugin HITL Plugin
   9  * @{
  10  * @brief The Hardware In The Loop plugin
  11  *****************************************************************************/
  12 /*
  13  * This program is free software; you can redistribute it and/or modify
  14  * it under the terms of the GNU General Public License as published by
  15  * the Free Software Foundation; either version 3 of the License, or
  16  * (at your option) any later version.
  17  *
  18  * This program is distributed in the hope that it will be useful, but
  19  * WITHOUT ANY WARRANTY; without even the implied warranty of MERCHANTABILITY
  20  * or FITNESS FOR A PARTICULAR PURPOSE. See the GNU General Public License
  21  * for more details.
  22  *
  23  * You should have received a copy of the GNU General Public License along
  24  * with this program; if not, write to the Free Software Foundation, Inc.,
  25  * 59 Temple Place, Suite 330, Boston, MA 02111-1307 USA
  26  */
  27
  28
  29 #include "simulator.h"
  30 #include "hitlnoisegeneration.h"
  31
  32 #include <extensionsystem/pluginmanager.h>
  33 #include <coreplugin/icore.h>
  34 #include <coreplugin/threadmanager.h>
  35 #include <uavtalk/telemetrymanager.h>
  36
  37 volatile bool Simulator::isStarted = false;
  38
  39 const float Simulator::GEE      = 9.81;
  40 const float Simulator::FT2M     = 0.3048;
  41 const float Simulator::KT2MPS   = 0.514444444;
  42 const float Simulator::INHG2KPA = 3.386;
  43 const float Simulator::FPS2CMPS = 30.48;
  44 const float Simulator::DEG2RAD  = (M_PI / 180.0);
  45 const float Simulator::RAD2DEG  = (180.0 / M_PI);
  46
  47
  48 Simulator::Simulator(const SimulatorSettings & params) :
  49     simProcess(NULL),
  50     time(NULL),
  51     inSocket(NULL),
  52     outSocket(NULL),
  53     settings(params),
  54     updatePeriod(50),
  55     simTimeout(8000),
  56     autopilotConnectionStatus(false),
  57     simConnectionStatus(false),
  58     txTimer(NULL),
  59     simTimer(NULL),
  60     name("")
  61 {
  62     // move to thread
  63     moveToThread(Core::ICore::instance()->threadManager()->getRealTimeThread());
  64     connect(this, SIGNAL(myStart()), this, SLOT(onStart()), Qt::QueuedConnection);
  65     emit myStart();
  66
  67     QTime currentTime = QTime::currentTime();
  68     gpsPosTime        = currentTime;
  69     groundTruthTime   = currentTime;
  70     gcsRcvrTime       = currentTime;
  71     attRawTime        = currentTime;
  72     baroAltTime       = currentTime;
  73     battTime = currentTime;
  74     airspeedStateTime = currentTime;
  75
  76     // Define standard atmospheric constants
  77     airParameters.univGasConstant  = 8.31447; // [J/(mol·K)]
  78     airParameters.dryAirConstant   = 287.058;  // [J/(kg*K)]
  79     airParameters.groundDensity    = 1.225;     // [kg/m^3]
  80     airParameters.groundTemp = 15 + 273.15; // [K]
  81     airParameters.tempLapseRate    = 0.0065;    // [deg/m]
  82     airParameters.M = 0.0289644; // [kg/mol]
  83     airParameters.relativeHumidity = 20; // [%]
  84     airParameters.seaLevelPress    = 101.325;   // [kPa]
  85 }
  86
  87 Simulator::~Simulator()
  88 {
  89     if (inSocket) {
  90         delete inSocket;
  91         inSocket = NULL;
  92     }
  93
  94     if (outSocket) {
  95         delete outSocket;
  96         outSocket = NULL;
  97     }
  98
  99     if (txTimer) {
 100         delete txTimer;
 101         txTimer = NULL;
 102     }
 103
 104     if (simTimer) {
 105         delete simTimer;
 106         simTimer = NULL;
 107     }
 108     // NOTE: Does not currently work, may need to send control+c to through the terminal
 109     if (simProcess != NULL) {
 110         // connect(simProcess,SIGNAL(finished(int, QProcess::ExitStatus)),this,SLOT(onFinished(int, QProcess::ExitStatus)));
 111
 112         simProcess->disconnect();
 113         if (simProcess->state() == QProcess::Running) {
 114             simProcess->kill();
 115         }
 116         // if(simProcess->waitForFinished())
 117         // emit deleteSimProcess();
 118         delete simProcess;
 119         simProcess = NULL;
 120     }
 121 }
 122
 123 void Simulator::onDeleteSimulator(void)
 124 {
 125     // [1]
 126     Simulator::setStarted(false);
 127     // [2]
 128     Simulator::Instances().removeOne(simulatorId);
 129
 130     disconnect(this);
 131     delete this;
 132 }
 133
 134 void Simulator::onStart()
 135 {
 136     QMutexLocker locker(&lock);
 137
 138     QThread *mainThread = QThread::currentThread();
 139
 140     qDebug() << "Simulator Thread: " << mainThread;
 141
 142     // Get required UAVObjects
 143     ExtensionSystem::PluginManager *pm = ExtensionSystem::PluginManager::instance();
 144     UAVObjectManager *objManager = pm->getObject<UAVObjectManager>();
 145     actDesired     = ActuatorDesired::GetInstance(objManager);
 146     actCommand     = ActuatorCommand::GetInstance(objManager);
 147     manCtrlCommand = ManualControlCommand::GetInstance(objManager);
 148     gcsReceiver    = GCSReceiver::GetInstance(objManager);
 149     flightStatus   = FlightStatus::GetInstance(objManager);
 150     posHome       = HomeLocation::GetInstance(objManager);
 151     velState      = VelocityState::GetInstance(objManager);
 152     posState      = PositionState::GetInstance(objManager);
 153     baroAlt       = BaroSensor::GetInstance(objManager);
 154     flightBatt    = FlightBatteryState::GetInstance(objManager);
 155     airspeedState = AirspeedState::GetInstance(objManager);
 156     attState      = AttitudeState::GetInstance(objManager);
 157     attSettings   = AttitudeSettings::GetInstance(objManager);
 158     accelState    = AccelState::GetInstance(objManager);
 159     gyroState     = GyroState::GetInstance(objManager);
 160     gpsPos = GPSPositionSensor::GetInstance(objManager);
 161     gpsVel = GPSVelocitySensor::GetInstance(objManager);
 162     telStats      = GCSTelemetryStats::GetInstance(objManager);
 163     groundTruth   = GroundTruth::GetInstance(objManager);
 164
 165     // Listen to autopilot connection events
 166     TelemetryManager *telMngr = pm->getObject<TelemetryManager>();
 167     connect(telMngr, SIGNAL(connected()), this, SLOT(onAutopilotConnect()));
 168     connect(telMngr, SIGNAL(disconnected()), this, SLOT(onAutopilotDisconnect()));
 169     // connect(telStats, SIGNAL(objectUpdated(UAVObject*)), this, SLOT(telStatsUpdated(UAVObject*)));
 170
 171     // If already connect setup autopilot
 172     GCSTelemetryStats::DataFields stats = telStats->getData();
 173     if (stats.Status == GCSTelemetryStats::STATUS_CONNECTED) {
 174         onAutopilotConnect();
 175     }
 176
 177     inSocket  = new QUdpSocket();
 178     outSocket = new QUdpSocket();
 179     setupUdpPorts(settings.hostAddress, settings.inPort, settings.outPort);
 180
 181     emit processOutput("\nLocal interface: " + settings.hostAddress + "\n" + \
 182                        "Remote interface: " + settings.remoteAddress + "\n" + \
 183                        "inputPort: " + QString::number(settings.inPort) + "\n" + \
 184                        "outputPort: " + QString::number(settings.outPort) + "\n");
 185
 186     qDebug() << ("\nLocal interface: " + settings.hostAddress + "\n" + \
 187                  "Remote interface: " + settings.remoteAddress + "\n" + \
 188                  "inputPort: " + QString::number(settings.inPort) + "\n" + \
 189                  "outputPort: " + QString::number(settings.outPort) + "\n");
 190
 191 // if(!inSocket->waitForConnected(5000))
 192 // emit processOutput(QString("Can't connect to %1 on %2 port!").arg(settings.hostAddress).arg(settings.inPort));
 193 // outSocket->connectToHost(settings.hostAddress,settings.outPort); // FG
 194 // if(!outSocket->waitForConnected(5000))
 195 // emit processOutput(QString("Can't connect to %1 on %2 port!").arg(settings.hostAddress).arg(settings.outPort));
 196
 197
 198     connect(inSocket, SIGNAL(readyRead()), this, SLOT(receiveUpdate()), Qt::DirectConnection);
 199
 200     // Setup transmit timer
 201     txTimer = new QTimer();
 202     connect(txTimer, SIGNAL(timeout()), this, SLOT(transmitUpdate()), Qt::DirectConnection);
 203     txTimer->setInterval(updatePeriod);
 204     txTimer->start();
 205     // Setup simulator connection timer
 206     simTimer = new QTimer();
 207     connect(simTimer, SIGNAL(timeout()), this, SLOT(onSimulatorConnectionTimeout()), Qt::DirectConnection);
 208     simTimer->setInterval(simTimeout);
 209     simTimer->start();
 210
 211     // setup time
 212     time = new QTime();
 213     time->start();
 214     current.T = 0;
 215     current.i = 0;
 216 }
 217
 218 void Simulator::receiveUpdate()
 219 {
 220     // Update connection timer and status
 221     simTimer->setInterval(simTimeout);
 222     simTimer->stop();
 223     simTimer->start();
 224     if (!simConnectionStatus) {
 225         simConnectionStatus = true;
 226         emit simulatorConnected();
 227     }
 228
 229     // Process data
 230     while (inSocket->hasPendingDatagrams()) {
 231         // Receive datagram
 232         QByteArray datagram;
 233         datagram.resize(inSocket->pendingDatagramSize());
 234         QHostAddress sender;
 235         quint16 senderPort;
 236         inSocket->readDatagram(datagram.data(), datagram.size(),
 237                                &sender, &senderPort);
 238         // QString datastr(datagram);
 239         // Process incomming data
 240         processUpdate(datagram);
 241     }
 242 }
 243
 244 void Simulator::setupObjects()
 245 {
 246     if (settings.gcsReceiverEnabled) {
 247         setupInputObject(actCommand, settings.minOutputPeriod); // Input to the simulator
 248         setupOutputObject(gcsReceiver, settings.minOutputPeriod);
 249     } else if (settings.manualControlEnabled) {
 250         setupInputObject(actDesired, settings.minOutputPeriod); // Input to the simulator
 251     }
 252
 253     setupOutputObject(posHome, 10000); // Hardcoded? Bleh.
 254
 255     if (settings.gpsPositionEnabled) {
 256         setupOutputObject(gpsPos, settings.gpsPosRate);
 257         setupOutputObject(gpsVel, settings.gpsPosRate);
 258     }
 259
 260     if (settings.groundTruthEnabled) {
 261         setupOutputObject(posState, settings.groundTruthRate);
 262         setupOutputObject(velState, settings.groundTruthRate);
 263     }
 264
 265     if (settings.attRawEnabled) {
 266         setupOutputObject(accelState, settings.attRawRate);
 267         setupOutputObject(gyroState, settings.attRawRate);
 268     }
 269
 270     if (settings.attStateEnabled && settings.attActHW) {
 271         setupOutputObject(accelState, settings.attRawRate);
 272         setupOutputObject(gyroState, settings.attRawRate);
 273     }
 274
 275     if (settings.attStateEnabled && !settings.attActHW) {
 276         setupOutputObject(attState, 20); // Hardcoded? Bleh.
 277     } else {
 278         setupWatchedObject(attState, 100); // Hardcoded? Bleh.
 279     }
 280     if (settings.airspeedStateEnabled) {
 281         setupOutputObject(airspeedState, settings.airspeedStateRate);
 282     }
 283
 284     if (settings.baroSensorEnabled) {
 285         setupOutputObject(baroAlt, settings.baroAltRate);
 286         setupOutputObject(flightBatt, settings.baroAltRate);
 287     }
 288 }
 289
 290
 291 void Simulator::setupInputObject(UAVObject *obj, quint32 updatePeriod)
 292 {
 293     UAVObject::Metadata mdata;
 294
 295     mdata = obj->getDefaultMetadata();
 296
 297     UAVObject::SetGcsAccess(mdata, UAVObject::ACCESS_READONLY);
 298     UAVObject::SetGcsTelemetryAcked(mdata, false);
 299     UAVObject::SetGcsTelemetryUpdateMode(mdata, UAVObject::UPDATEMODE_MANUAL);
 300     mdata.gcsTelemetryUpdatePeriod = 0;
 301
 302     UAVObject::SetFlightAccess(mdata, UAVObject::ACCESS_READWRITE);
 303     UAVObject::SetFlightTelemetryAcked(mdata, false);
 304
 305     UAVObject::SetFlightTelemetryUpdateMode(mdata, UAVObject::UPDATEMODE_PERIODIC);
 306     mdata.flightTelemetryUpdatePeriod = updatePeriod;
 307
 308     obj->setMetadata(mdata);
 309 }
 310
 311
 312 void Simulator::setupWatchedObject(UAVObject *obj, quint32 updatePeriod)
 313 {
 314     UAVObject::Metadata mdata;
 315
 316     mdata = obj->getDefaultMetadata();
 317
 318     UAVObject::SetGcsAccess(mdata, UAVObject::ACCESS_READONLY);
 319     UAVObject::SetGcsTelemetryAcked(mdata, false);
 320     UAVObject::SetGcsTelemetryUpdateMode(mdata, UAVObject::UPDATEMODE_MANUAL);
 321     mdata.gcsTelemetryUpdatePeriod = 0;
 322
 323     UAVObject::SetFlightAccess(mdata, UAVObject::ACCESS_READWRITE);
 324     UAVObject::SetFlightTelemetryAcked(mdata, false);
 325     UAVObject::SetFlightTelemetryUpdateMode(mdata, UAVObject::UPDATEMODE_PERIODIC);
 326     mdata.flightTelemetryUpdatePeriod = updatePeriod;
 327
 328     obj->setMetadata(mdata);
 329 }
 330
 331
 332 void Simulator::setupOutputObject(UAVObject *obj, quint32 updatePeriod)
 333 {
 334     UAVObject::Metadata mdata;
 335
 336     mdata = obj->getDefaultMetadata();
 337
 338     UAVObject::SetGcsAccess(mdata, UAVObject::ACCESS_READWRITE);
 339     UAVObject::SetGcsTelemetryAcked(mdata, false);
 340     UAVObject::SetGcsTelemetryUpdateMode(mdata, UAVObject::UPDATEMODE_PERIODIC);
 341     mdata.gcsTelemetryUpdatePeriod = updatePeriod;
 342
 343     UAVObject::SetFlightAccess(mdata, UAVObject::ACCESS_READONLY);
 344     UAVObject::SetFlightTelemetryUpdateMode(mdata, UAVObject::UPDATEMODE_MANUAL);
 345
 346     obj->setMetadata(mdata);
 347 }
 348
 349 void Simulator::onAutopilotConnect()
 350 {
 351     autopilotConnectionStatus = true;
 352     setupObjects();
 353     emit autopilotConnected();
 354 }
 355
 356 void Simulator::onAutopilotDisconnect()
 357 {
 358     autopilotConnectionStatus = false;
 359     emit autopilotDisconnected();
 360 }
 361
 362 void Simulator::onSimulatorConnectionTimeout()
 363 {
 364     if (simConnectionStatus) {
 365         simConnectionStatus = false;
 366         emit simulatorDisconnected();
 367     }
 368 }
 369
 370
 371 void Simulator::telStatsUpdated(UAVObject *obj)
 372 {
 373     Q_UNUSED(obj);
 374
 375     GCSTelemetryStats::DataFields stats = telStats->getData();
 376     if (!autopilotConnectionStatus && stats.Status == GCSTelemetryStats::STATUS_CONNECTED) {
 377         onAutopilotConnect();
 378     } else if (autopilotConnectionStatus && stats.Status != GCSTelemetryStats::STATUS_CONNECTED) {
 379         onAutopilotDisconnect();
 380     }
 381 }
 382
 383
 384 void Simulator::resetInitialHomePosition()
 385 {
 386     once = false;
 387 }
 388
 389
 390 void Simulator::updateUAVOs(Output2Hardware out)
 391 {
 392     QTime currentTime = QTime::currentTime();
 393
 394     Noise noise;
 395     HitlNoiseGeneration noiseSource;
 396
 397     if (settings.addNoise) {
 398         noise = noiseSource.generateNoise();
 399     } else {
 400         memset(&noise, 0, sizeof(Noise));
 401     }
 402
 403     /*******************************/
 404     HomeLocation::DataFields homeData = posHome->getData();
 405     if (!once) {
 406         // Upon startup, we reset the HomeLocation object to
 407         // the plane's location:
 408         memset(&homeData, 0, sizeof(HomeLocation::DataFields));
 409         // Update homelocation
 410         homeData.Latitude  = out.latitude;   // Already in *10^7 integer format
 411         homeData.Longitude = out.longitude; // Already in *10^7 integer format
 412         homeData.Altitude  = out.agl;
 413
 414         homeData.Be[0]     = 0;
 415         homeData.Be[1]     = 0;
 416         homeData.Be[2]     = 0;
 417         posHome->setData(homeData);
 418         posHome->updated();
 419
 420         // Compute initial distance
 421         initN = out.dstN;
 422         initE = out.dstE;
 423         initD = out.dstD;
 424
 425         once  = true;
 426     }
 427
 428     /*******************************/
 429     // Copy everything to the ground truth object. GroundTruth is Noise-free.
 430     GroundTruth::DataFields groundTruthData;
 431     groundTruthData = groundTruth->getData();
 432
 433     groundTruthData.AccelerationXYZ[0] = out.accX;
 434     groundTruthData.AccelerationXYZ[1] = out.accY;
 435     groundTruthData.AccelerationXYZ[2] = out.accZ;
 436
 437     groundTruthData.AngularRates[0]    = out.rollRate;
 438     groundTruthData.AngularRates[1]    = out.pitchRate;
 439     groundTruthData.AngularRates[2]    = out.yawRate;
 440
 441     groundTruthData.CalibratedAirspeed = out.calibratedAirspeed;
 442     groundTruthData.TrueAirspeed   = out.trueAirspeed;
 443     groundTruthData.AngleOfAttack  = out.angleOfAttack;
 444     groundTruthData.AngleOfSlip    = out.angleOfSlip;
 445
 446     groundTruthData.PositionNED[0] = out.dstN - initN;
 447     groundTruthData.PositionNED[1] = out.dstE - initD;
 448     groundTruthData.PositionNED[2] = out.dstD - initD;
 449
 450     groundTruthData.VelocityNED[0] = out.velNorth;
 451     groundTruthData.VelocityNED[1] = out.velEast;
 452     groundTruthData.VelocityNED[2] = out.velDown;
 453
 454     groundTruthData.RPY[0] = out.roll;
 455     groundTruthData.RPY[0] = out.pitch;
 456     groundTruthData.RPY[0] = out.heading;
 457
 458     // Set UAVO
 459     groundTruth->setData(groundTruthData);
 460
 461 /*******************************/
 462     // Update attState object
 463     AttitudeState::DataFields attStateData;
 464     attStateData = attState->getData();
 465
 466     if (settings.attActHW) {
 467         // do nothing
 468         /*****************************************/
 469     } else if (settings.attActSim) {
 470         // take all data from simulator
 471         attStateData.Roll  = out.roll + noise.attStateData.Roll;   // roll;
 472         attStateData.Pitch = out.pitch + noise.attStateData.Pitch; // pitch
 473         attStateData.Yaw   = out.heading + noise.attStateData.Yaw; // Yaw
 474         float rpy[3];
 475         float quat[4];
 476         rpy[0] = attStateData.Roll;
 477         rpy[1] = attStateData.Pitch;
 478         rpy[2] = attStateData.Yaw;
 479         Utils::CoordinateConversions().RPY2Quaternion(rpy, quat);
 480         attStateData.q1 = quat[0];
 481         attStateData.q2 = quat[1];
 482         attStateData.q3 = quat[2];
 483         attStateData.q4 = quat[3];
 484
 485         // Set UAVO
 486         attState->setData(attStateData);
 487         /*****************************************/
 488     } else if (settings.attActCalc) {
 489         // calculate RPY with code from Attitude module
 490         static float q[4] = { 1, 0, 0, 0 };
 491         static float gyro_correct_int2 = 0;
 492
 493         float dT = out.delT;
 494
 495         AttitudeSettings::DataFields attSettData = attSettings->getData();
 496         float accelKp     = attSettData.AccelKp * 0.1666666666666667;
 497         // float accelKi     = attSettData.AccelKp * 0.1666666666666667;
 498         float yawBiasRate = attSettData.YawBiasRate;
 499
 500         // calibrate sensors on arming
 501         if (flightStatus->getData().Armed == FlightStatus::ARMED_ARMING) {
 502             accelKp = 2.0;
 503             // accelKi = 0.9;
 504         }
 505
 506         float gyro[3] = { out.rollRate, out.pitchRate, out.yawRate };
 507         float attRawAcc[3] = { out.accX, out.accY, out.accZ };
 508
 509         // code from Attitude module begin ///////////////////////////////
 510         float *accels = attRawAcc;
 511         float grot[3];
 512         float accel_err[3];
 513
 514         // Rotate gravity to body frame and cross with accels
 515         grot[0] = -(2 * (q[1] * q[3] - q[0] * q[2]));
 516         grot[1] = -(2 * (q[2] * q[3] + q[0] * q[1]));
 517         grot[2] = -(q[0] * q[0] - q[1] * q[1] - q[2] * q[2] + q[3] * q[3]);
 518
 519         // CrossProduct
 520         {
 521             accel_err[0] = accels[1] * grot[2] - grot[1] * accels[2];
 522             accel_err[1] = grot[0] * accels[2] - accels[0] * grot[2];
 523             accel_err[2] = accels[0] * grot[1] - grot[0] * accels[1];
 524         }
 525
 526         // Account for accel magnitude
 527         float accel_mag = sqrt(accels[0] * accels[0] + accels[1] * accels[1] + accels[2] * accels[2]);
 528         accel_err[0] /= accel_mag;
 529         accel_err[1] /= accel_mag;
 530         accel_err[2] /= accel_mag;
 531
 532         // Accumulate integral of error.  Scale here so that units are (deg/s) but Ki has units of s
 533         gyro_correct_int2 += -gyro[2] * yawBiasRate;
 534
 535         // Correct rates based on error, integral component dealt with in updateSensors
 536         gyro[0] += accel_err[0] * accelKp / dT;
 537         gyro[1] += accel_err[1] * accelKp / dT;
 538         gyro[2] += accel_err[2] * accelKp / dT + gyro_correct_int2;
 539
 540         // Work out time derivative from INSAlgo writeup
 541         // Also accounts for the fact that gyros are in deg/s
 542         float qdot[4];
 543         qdot[0] = (-q[1] * gyro[0] - q[2] * gyro[1] - q[3] * gyro[2]) * dT * M_PI / 180 / 2;
 544         qdot[1] = (+q[0] * gyro[0] - q[3] * gyro[1] + q[2] * gyro[2]) * dT * M_PI / 180 / 2;
 545         qdot[2] = (+q[3] * gyro[0] + q[0] * gyro[1] - q[1] * gyro[2]) * dT * M_PI / 180 / 2;
 546         qdot[3] = (-q[2] * gyro[0] + q[1] * gyro[1] + q[0] * gyro[2]) * dT * M_PI / 180 / 2;
 547
 548         // Take a time step
 549         q[0]   += qdot[0];
 550         q[1]   += qdot[1];
 551         q[2]   += qdot[2];
 552         q[3]   += qdot[3];
 553
 554         if (q[0] < 0) {
 555             q[0] = -q[0];
 556             q[1] = -q[1];
 557             q[2] = -q[2];
 558             q[3] = -q[3];
 559         }
 560
 561         // Renomalize
 562         float qmag = sqrt((q[0] * q[0]) + (q[1] * q[1]) + (q[2] * q[2]) + (q[3] * q[3]));
 563         q[0] /= qmag;
 564         q[1] /= qmag;
 565         q[2] /= qmag;
 566         q[3] /= qmag;
 567
 568         // If quaternion has become inappropriately short or is nan reinit.
 569         // THIS SHOULD NEVER ACTUALLY HAPPEN
 570         if ((fabs(qmag) < 1e-3) || (qmag != qmag)) {
 571             q[0] = 1;
 572             q[1] = 0;
 573             q[2] = 0;
 574             q[3] = 0;
 575         }
 576
 577         float rpy2[3];
 578         // Quaternion2RPY
 579         {
 580             float q0s, q1s, q2s, q3s;
 581             q0s     = q[0] * q[0];
 582             q1s     = q[1] * q[1];
 583             q2s     = q[2] * q[2];
 584             q3s     = q[3] * q[3];
 585
 586             float R13, R11, R12, R23, R33;
 587             R13     = 2 * (q[1] * q[3] - q[0] * q[2]);
 588             R11     = q0s + q1s - q2s - q3s;
 589             R12     = 2 * (q[1] * q[2] + q[0] * q[3]);
 590             R23     = 2 * (q[2] * q[3] + q[0] * q[1]);
 591             R33     = q0s - q1s - q2s + q3s;
 592
 593             rpy2[1] = RAD2DEG * asinf(-R13); // pitch always between -pi/2 to pi/2
 594             rpy2[2] = RAD2DEG * atan2f(R12, R11);
 595             rpy2[0] = RAD2DEG * atan2f(R23, R33);
 596         }
 597
 598         attStateData.Roll  = rpy2[0];
 599         attStateData.Pitch = rpy2[1];
 600         attStateData.Yaw   = rpy2[2];
 601         attStateData.q1    = q[0];
 602         attStateData.q2    = q[1];
 603         attStateData.q3    = q[2];
 604         attStateData.q4    = q[3];
 605
 606         // Set UAVO
 607         attState->setData(attStateData);
 608         /*****************************************/
 609     }
 610
 611     /*******************************/
 612     if (settings.gcsReceiverEnabled) {
 613         if (gcsRcvrTime.msecsTo(currentTime) >= settings.minOutputPeriod) {
 614             GCSReceiver::DataFields gcsRcvrData;
 615             memset(&gcsRcvrData, 0, sizeof(GCSReceiver::DataFields));
 616
 617             for (quint16 i = 0; i < GCSReceiver::CHANNEL_NUMELEM; i++) {
 618                 gcsRcvrData.Channel[i] = 1500 + (out.rc_channel[i] * 500); // Elements in rc_channel are between -1 and 1
 619             }
 620
 621             gcsReceiver->setData(gcsRcvrData);
 622
 623             gcsRcvrTime = gcsRcvrTime.addMSecs(settings.minOutputPeriod);
 624         }
 625     }
 626
 627
 628     /*******************************/
 629     if (settings.gpsPositionEnabled) {
 630         if (gpsPosTime.msecsTo(currentTime) >= settings.gpsPosRate) {
 631             qDebug() << " GPS time:" << gpsPosTime << ", currentTime: " << currentTime << ", difference: " << gpsPosTime.msecsTo(currentTime);
 632             // Update GPS Position objects
 633             GPSPositionSensor::DataFields gpsPosData;
 634             memset(&gpsPosData, 0, sizeof(GPSPositionSensor::DataFields));
 635             gpsPosData.Altitude        = out.altitude + noise.gpsPosData.Altitude;
 636             gpsPosData.Heading         = out.heading + noise.gpsPosData.Heading;
 637             gpsPosData.Groundspeed     = out.groundspeed + noise.gpsPosData.Groundspeed;
 638             gpsPosData.Latitude        = out.latitude + noise.gpsPosData.Latitude;    // Already in *10^7 integer format
 639             gpsPosData.Longitude       = out.longitude + noise.gpsPosData.Longitude; // Already in *10^7 integer format
 640             gpsPosData.GeoidSeparation = 0.0;
 641             gpsPosData.PDOP = 3.0;
 642             gpsPosData.VDOP = gpsPosData.PDOP * 1.5;
 643             gpsPosData.Satellites      = 10;
 644             gpsPosData.Status = GPSPositionSensor::STATUS_FIX3D;
 645
 646             gpsPos->setData(gpsPosData);
 647
 648             // Update GPS Velocity.{North,East,Down}
 649             GPSVelocitySensor::DataFields gpsVelData;
 650             memset(&gpsVelData, 0, sizeof(GPSVelocitySensor::DataFields));
 651             gpsVelData.North = out.velNorth + noise.gpsVelData.North;
 652             gpsVelData.East  = out.velEast + noise.gpsVelData.East;
 653             gpsVelData.Down  = out.velDown + noise.gpsVelData.Down;
 654
 655             gpsVel->setData(gpsVelData);
 656
 657             gpsPosTime = gpsPosTime.addMSecs(settings.gpsPosRate);
 658         }
 659     }
 660
 661     /*******************************/
 662     // Update VelocityState.{North,East,Down}
 663     if (settings.groundTruthEnabled) {
 664         if (groundTruthTime.msecsTo(currentTime) >= settings.groundTruthRate) {
 665             VelocityState::DataFields velocityStateData;
 666             memset(&velocityStateData, 0, sizeof(VelocityState::DataFields));
 667             velocityStateData.North = out.velNorth + noise.velocityStateData.North;
 668             velocityStateData.East  = out.velEast + noise.velocityStateData.East;
 669             velocityStateData.Down  = out.velDown + noise.velocityStateData.Down;
 670             velState->setData(velocityStateData);
 671
 672             // Update PositionState.{Nort,East,Down}
 673             PositionState::DataFields positionStateData;
 674             memset(&positionStateData, 0, sizeof(PositionState::DataFields));
 675             positionStateData.North = (out.dstN - initN) + noise.positionStateData.North;
 676             positionStateData.East  = (out.dstE - initE) + noise.positionStateData.East;
 677             positionStateData.Down  = (out.dstD /*-initD*/) + noise.positionStateData.Down;
 678             posState->setData(positionStateData);
 679
 680             groundTruthTime = groundTruthTime.addMSecs(settings.groundTruthRate);
 681         }
 682     }
 683
 684 ///*******************************/
 685 // if (settings.sonarAltitude) {
 686 // static QTime sonarAltTime = currentTime;
 687 // if (sonarAltTime.msecsTo(currentTime) >= settings.sonarAltRate) {
 688 // SonarAltitude::DataFields sonarAltData;
 689 // sonarAltData = sonarAlt->getData();
 690
 691 // float sAlt = settings.sonarMaxAlt;
 692 //// 0.35 rad ~= 20 degree
 693 // if ((agl < (sAlt * 2.0)) && (roll < 0.35) && (pitch < 0.35)) {
 694 // float x = agl * qTan(roll);
 695 // float y = agl * qTan(pitch);
 696 // float h = qSqrt(x*x + y*y + agl*agl);
 697 // sAlt = qMin(h, sAlt);
 698 // }
 699
 700 // sonarAltData.Altitude = sAlt;
 701 // sonarAlt->setData(sonarAltData);
 702 // sonarAltTime = currentTime;
 703 // }
 704 // }
 705
 706     /*******************************/
 707     // Update BaroSensor object
 708     if (settings.baroSensorEnabled) {
 709         if (baroAltTime.msecsTo(currentTime) >= settings.baroAltRate) {
 710             BaroSensor::DataFields baroAltData;
 711             memset(&baroAltData, 0, sizeof(BaroSensor::DataFields));
 712             baroAltData.Altitude    = out.altitude + noise.baroAltData.Altitude;
 713             baroAltData.Temperature = out.temperature + noise.baroAltData.Temperature;
 714             baroAltData.Pressure    = out.pressure + noise.baroAltData.Pressure;
 715             baroAlt->setData(baroAltData);
 716
 717             baroAltTime = baroAltTime.addMSecs(settings.baroAltRate);
 718         }
 719     }
 720
 721     /*******************************/
 722     // Update FlightBatteryState object
 723     if (settings.baroSensorEnabled) {
 724         if (battTime.msecsTo(currentTime) >= settings.baroAltRate) {
 725             FlightBatteryState::DataFields batteryData;
 726             memset(&batteryData, 0, sizeof(FlightBatteryState::DataFields));
 727             batteryData.Voltage = out.voltage;
 728             batteryData.Current = out.current;
 729             batteryData.ConsumedEnergy = out.consumption;
 730             flightBatt->setData(batteryData);
 731
 732             battTime = battTime.addMSecs(settings.baroAltRate);
 733         }
 734     }
 735
 736     /*******************************/
 737     // Update AirspeedState object
 738     if (settings.airspeedStateEnabled) {
 739         if (airspeedStateTime.msecsTo(currentTime) >= settings.airspeedStateRate) {
 740             AirspeedState::DataFields airspeedStateData;
 741             memset(&airspeedStateData, 0, sizeof(AirspeedState::DataFields));
 742             airspeedStateData.CalibratedAirspeed = out.calibratedAirspeed + noise.airspeedState.CalibratedAirspeed;
 743             airspeedStateData.TrueAirspeed = out.trueAirspeed + noise.airspeedState.TrueAirspeed;
 744             // airspeedStateData.alpha=out.angleOfAttack; // to be implemented
 745             // airspeedStateData.beta=out.angleOfSlip;
 746             airspeedState->setData(airspeedStateData);
 747
 748             airspeedStateTime = airspeedStateTime.addMSecs(settings.airspeedStateRate);
 749         }
 750     }
 751
 752     /*******************************/
 753     // Update raw attitude sensors
 754     if (settings.attRawEnabled) {
 755         if (attRawTime.msecsTo(currentTime) >= settings.attRawRate) {
 756             // Update gyroscope sensor data
 757             GyroState::DataFields gyroStateData;
 758             memset(&gyroStateData, 0, sizeof(GyroState::DataFields));
 759             gyroStateData.x = out.rollRate + noise.gyroStateData.x;
 760             gyroStateData.y = out.pitchRate + noise.gyroStateData.y;
 761             gyroStateData.z = out.yawRate + noise.gyroStateData.z;
 762             gyroState->setData(gyroStateData);
 763
 764             // Update accelerometer sensor data
 765             AccelState::DataFields accelStateData;
 766             memset(&accelStateData, 0, sizeof(AccelState::DataFields));
 767             accelStateData.x = out.accX + noise.accelStateData.x;
 768             accelStateData.y = out.accY + noise.accelStateData.y;
 769             accelStateData.z = out.accZ + noise.accelStateData.z;
 770             accelState->setData(accelStateData);
 771
 772             attRawTime = attRawTime.addMSecs(settings.attRawRate);
 773         }
 774     }
 775 }
 776
 777 /**
 778  * calculate air density from altitude. http://en.wikipedia.org/wiki/Density_of_air
 779  */
 780 float Simulator::airDensityFromAltitude(float alt, AirParameters air, float gravity)
 781 {
 782     float p   = airPressureFromAltitude(alt, air, gravity);
 783     float rho = p * air.M / (air.univGasConstant * (air.groundTemp - air.tempLapseRate * alt));
 784
 785     return rho;
 786 }
 787
 788 /**
 789  * @brief Simulator::airPressureFromAltitude Get air pressure from altitude and atmospheric conditions.
 790  * @param alt altitude
 791  * @param air atmospheric conditions
 792  * @param gravity
 793  * @return
 794  */
 795 float Simulator::airPressureFromAltitude(float alt, AirParameters air, float gravity)
 796 {
 797     return air.seaLevelPress * pow(1 - air.tempLapseRate * alt / air.groundTemp, gravity * air.M / (air.univGasConstant * air.tempLapseRate));
 798 }
 799
 800 /**
 801  * @brief Simulator::cas2tas calculate TAS from CAS and altitude. http://en.wikipedia.org/wiki/Airspeed
 802  * @param CAS Calibrated airspeed
 803  * @param alt altitude
 804  * @param air atmospheric conditions
 805  * @param gravity
 806  * @return True airspeed
 807  */
 808 float Simulator::cas2tas(float CAS, float alt, AirParameters air, float gravity)
 809 {
 810     float rho = airDensityFromAltitude(alt, air, gravity);
 811
 812     return CAS * sqrt(air.groundDensity / rho);
 813 }
 814
 815 /**
 816  * @brief Simulator::tas2cas calculate CAS from TAS and altitude. http://en.wikipedia.org/wiki/Airspeed
 817  * @param TAS True airspeed
 818  * @param alt altitude
 819  * @param air atmospheric conditions
 820  * @param gravity
 821  * @return Calibrated airspeed
 822  */
 823 float Simulator::tas2cas(float TAS, float alt, AirParameters air, float gravity)
 824 {
 825     float rho = airDensityFromAltitude(alt, air, gravity);
 826
 827     return TAS / sqrt(air.groundDensity / rho);
 828 }
 829
 830 /**
 831  * @brief Simulator::getAirParameters get air parameters
 832  * @return airParameters
 833  */
 834 AirParameters Simulator::getAirParameters()
 835 {
 836     return airParameters;
 837 }
 838
 839 /**
 840  * @brief Simulator::setAirParameters set air parameters
 841  * @param airParameters
 842  */
 843 void Simulator::setAirParameters(AirParameters airParameters)
 844 {
 845     this->airParameters = airParameters;
 846 }