diff --git a/ground/openpilotgcs/src/plugins/hitl/hitlconfiguration.cpp b/ground/openpilotgcs/src/plugins/hitl/hitlconfiguration.cpp index 2e69f01d8..3ea4d3bf2 100644 --- a/ground/openpilotgcs/src/plugins/hitl/hitlconfiguration.cpp +++ b/ground/openpilotgcs/src/plugins/hitl/hitlconfiguration.cpp @@ -64,23 +64,30 @@ HITLConfiguration::HITLConfiguration(QString classId, QSettings* qSettings, QObj settings.manualControlEnabled= false; settings.minOutputPeriod = 100; + settings.airspeedActualEnabled= false; + settings.airspeedActualRate = 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(); - settings.inPort = qSettings->value("inPort").toInt(); - settings.remoteAddress = qSettings->value("remoteAddress").toString(); - settings.outPort = qSettings->value("outPort").toInt(); + settings.simulatorId = qSettings->value("simulatorId").toString(); settings.binPath = qSettings->value("binPath").toString(); settings.dataPath = qSettings->value("dataPath").toString(); + settings.hostAddress = qSettings->value("hostAddress").toString(); + settings.remoteAddress = qSettings->value("remoteAddress").toString(); + settings.outPort = qSettings->value("outPort").toInt(); + settings.inPort = qSettings->value("inPort").toInt(); + 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.gcsReceiverEnabled = qSettings->value("gcsReceiverEnabled").toBool(); + settings.manualControlEnabled= qSettings->value("manualControlEnabled").toBool(); + settings.attRawEnabled = qSettings->value("attRawEnabled").toBool(); settings.attRawRate = qSettings->value("attRawRate").toInt(); @@ -99,9 +106,10 @@ HITLConfiguration::HITLConfiguration(QString classId, QSettings* qSettings, QObj settings.groundTruthRate = qSettings->value("groundTruthRate").toInt(); settings.inputCommand = qSettings->value("inputCommand").toBool(); - settings.gcsReceiverEnabled = qSettings->value("gcsReceiverEnabled").toBool(); - settings.manualControlEnabled= qSettings->value("manualControlEnabled").toBool(); settings.minOutputPeriod = qSettings->value("minOutputPeriod").toInt(); + + settings.airspeedActualEnabled=qSettings->value("airspeedActualEnabled").toBool(); + settings.airspeedActualRate = qSettings->value("airspeedActualRate").toInt(); } } @@ -150,6 +158,7 @@ void HITLConfiguration::saveConfig(QSettings* qSettings) const { qSettings->setValue("inputCommand", settings.inputCommand); qSettings->setValue("minOutputPeriod", settings.minOutputPeriod); - + qSettings->setValue("airspeedActualEnabled", settings.airspeedActualEnabled); + qSettings->setValue("airspeedActualRate", settings.airspeedActualRate); } diff --git a/ground/openpilotgcs/src/plugins/hitl/hitlnoisegeneration.cpp b/ground/openpilotgcs/src/plugins/hitl/hitlnoisegeneration.cpp index 1861096c0..4efb72852 100644 --- a/ground/openpilotgcs/src/plugins/hitl/hitlnoisegeneration.cpp +++ b/ground/openpilotgcs/src/plugins/hitl/hitlnoisegeneration.cpp @@ -72,7 +72,7 @@ Noise HitlNoiseGeneration::generateNoise(){ noise.accelData.y=0; noise.accelData.z=0; - noise.baroAirspeed.CalibratedAirspeed=0; + noise.airspeedActual.CalibratedAirspeed=0; return noise; } diff --git a/ground/openpilotgcs/src/plugins/hitl/hitlnoisegeneration.h b/ground/openpilotgcs/src/plugins/hitl/hitlnoisegeneration.h index e18b531d8..0a740707e 100644 --- a/ground/openpilotgcs/src/plugins/hitl/hitlnoisegeneration.h +++ b/ground/openpilotgcs/src/plugins/hitl/hitlnoisegeneration.h @@ -40,7 +40,7 @@ struct Noise{ Accels::DataFields accelData; AttitudeActual::DataFields attActualData; BaroAltitude::DataFields baroAltData; - BaroAirspeed::DataFields baroAirspeed; + AirspeedActual::DataFields airspeedActual; GPSPosition::DataFields gpsPosData; GPSVelocity::DataFields gpsVelData; Gyros::DataFields gyroData; diff --git a/ground/openpilotgcs/src/plugins/hitl/hitloptionspage.cpp b/ground/openpilotgcs/src/plugins/hitl/hitloptionspage.cpp index 05f2f7dc2..9188e66c8 100644 --- a/ground/openpilotgcs/src/plugins/hitl/hitloptionspage.cpp +++ b/ground/openpilotgcs/src/plugins/hitl/hitloptionspage.cpp @@ -113,6 +113,8 @@ QWidget *HITLOptionsPage::createPage(QWidget *parent) m_optionsPage->attActCalc->setChecked(config->Settings().attActCalc); m_optionsPage->attActSim->setChecked(config->Settings().attActSim); + m_optionsPage->airspeedActualCheckbox->setChecked(config->Settings().airspeedActualEnabled); + m_optionsPage->airspeedRateSpinbox->setValue(config->Settings().airspeedActualRate); return optionsPageWidget; } @@ -160,6 +162,10 @@ void HITLOptionsPage::apply() settings.attActSim = m_optionsPage->attActSim->isChecked(); settings.attActCalc = m_optionsPage->attActCalc->isChecked(); + settings.airspeedActualEnabled=m_optionsPage->airspeedActualCheckbox->isChecked(); + settings.airspeedActualRate=m_optionsPage->airspeedRateSpinbox->value(); + + //Write settings to file config->setSimulatorSettings(settings); } diff --git a/ground/openpilotgcs/src/plugins/hitl/hitloptionspage.ui b/ground/openpilotgcs/src/plugins/hitl/hitloptionspage.ui index 92fb7921d..1b8b12997 100644 --- a/ground/openpilotgcs/src/plugins/hitl/hitloptionspage.ui +++ b/ground/openpilotgcs/src/plugins/hitl/hitloptionspage.ui @@ -154,7 +154,7 @@ - + 10 @@ -829,6 +829,50 @@ 0 + + + + + 0 + 0 + + + + AirspeedActual + + + true + + + true + + + + + + Refresh rate: + + + + + + + 0 - update once, or every N seconds + + + ms + + + 5000 + + + 100 + + + + + + @@ -912,7 +956,7 @@ 10 - 50 + 250 diff --git a/ground/openpilotgcs/src/plugins/hitl/simulator.cpp b/ground/openpilotgcs/src/plugins/hitl/simulator.cpp index 778c8e81d..d414049a1 100644 --- a/ground/openpilotgcs/src/plugins/hitl/simulator.cpp +++ b/ground/openpilotgcs/src/plugins/hitl/simulator.cpp @@ -69,6 +69,7 @@ Simulator::Simulator(const SimulatorSettings& params) : gcsRcvrTime = currentTime; attRawTime = currentTime; baroAltTime = currentTime; + airspeedActualTime=currentTime; } @@ -143,7 +144,7 @@ void Simulator::onStart() velActual = VelocityActual::GetInstance(objManager); posActual = PositionActual::GetInstance(objManager); baroAlt = BaroAltitude::GetInstance(objManager); - baroAirspeed = BaroAirspeed::GetInstance(objManager); + airspeedActual = AirspeedActual::GetInstance(objManager); attActual = AttitudeActual::GetInstance(objManager); attSettings = AttitudeSettings::GetInstance(objManager); accels = Accels::GetInstance(objManager); @@ -267,6 +268,9 @@ void Simulator::setupObjects() else setupWatchedObject(attActual, 100); //Hardcoded? Bleh. + if(settings.airspeedActualEnabled) + setupOutputObject(airspeedActual, settings.airspeedActualRate); + if(settings.baroAltitudeEnabled) setupOutputObject(baroAlt, settings.baroAltRate); @@ -674,6 +678,18 @@ void Simulator::updateUAVOs(Output2Hardware out){ } } + // Update AirspeedActual object + if (settings.airspeedActualEnabled){ + if (airspeedActualTime.msecsTo(currentTime) >= settings.airspeedActualRate) { + AirspeedActual::DataFields airspeedActualData; + memset(&airspeedActualData, 0, sizeof(AirspeedActual::DataFields)); + airspeedActualData.CalibratedAirspeed = out.calibratedAirspeed + noise.airspeedActual.CalibratedAirspeed; + airspeedActual->setData(airspeedActualData); + + airspeedActualTime=airspeedActualTime.addMSecs(settings.airspeedActualRate); + } + } + // Update raw attitude sensors if (settings.attRawEnabled) { if (attRawTime.msecsTo(currentTime) >= settings.attRawRate) { diff --git a/ground/openpilotgcs/src/plugins/hitl/simulator.h b/ground/openpilotgcs/src/plugins/hitl/simulator.h index 605259775..41de0937d 100644 --- a/ground/openpilotgcs/src/plugins/hitl/simulator.h +++ b/ground/openpilotgcs/src/plugins/hitl/simulator.h @@ -43,7 +43,7 @@ #include "actuatordesired.h" #include "attitudeactual.h" #include "attitudesettings.h" -#include "baroairspeed.h" +#include "airspeedactual.h" #include "baroaltitude.h" #include "flightstatus.h" #include "gcsreceiver.h" @@ -140,6 +140,9 @@ typedef struct _CONNECTION bool manualControlEnabled; quint16 minOutputPeriod; + bool airspeedActualEnabled; + quint16 airspeedActualRate; + } SimulatorSettings; @@ -260,7 +263,7 @@ protected: ManualControlCommand* manCtrlCommand; FlightStatus* flightStatus; BaroAltitude* baroAlt; - BaroAirspeed* baroAirspeed; + AirspeedActual* airspeedActual; AttitudeActual* attActual; AttitudeSettings* attSettings; VelocityActual* velActual; @@ -297,6 +300,7 @@ private: QTime groundTruthTime; QTime baroAltTime; QTime gcsRcvrTime; + QTime airspeedActualTime; QString name; QString simulatorId;