diff --git a/ground/openpilotgcs/src/plugins/hitlv2/aerosimrc.cpp b/ground/openpilotgcs/src/plugins/hitlv2/aerosimrc.cpp
index 1bc5c3b06..25e535134 100644
--- a/ground/openpilotgcs/src/plugins/hitlv2/aerosimrc.cpp
+++ b/ground/openpilotgcs/src/plugins/hitlv2/aerosimrc.cpp
@@ -369,6 +369,27 @@ void AeroSimRCSimulator::processUpdate(const QByteArray &data)
}
}
/**********************************************************************************************/
+ 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;
+ }
+ }
+ /**********************************************************************************************/
/*
BaroAltitude::DataFields altActData;
altActData = altActual->getData();
diff --git a/ground/openpilotgcs/src/plugins/hitlv2/hitlv2configuration.cpp b/ground/openpilotgcs/src/plugins/hitlv2/hitlv2configuration.cpp
index 90c385038..cd6af73ea 100644
--- a/ground/openpilotgcs/src/plugins/hitlv2/hitlv2configuration.cpp
+++ b/ground/openpilotgcs/src/plugins/hitlv2/hitlv2configuration.cpp
@@ -53,6 +53,10 @@ HITLConfiguration::HITLConfiguration(QString classId,
bool attActSim = true;
bool attActCalc = false;
+ bool sonarAltitude = false;
+ float sonarMaxAlt = 5.0;
+ quint16 sonarAltRate = 50;
+
bool gpsPosition = true;
quint16 gpsPosRate = 200;
@@ -84,6 +88,10 @@ HITLConfiguration::HITLConfiguration(QString classId,
settings.attActSim = qSettings->value("attActSim", attActSim).toBool();
settings.attActCalc = qSettings->value("attActCalc", attActCalc).toBool();
+ settings.sonarAltitude = qSettings->value("sonarAltitude", sonarAltitude).toBool();
+ settings.sonarMaxAlt = qSettings->value("sonarMaxAlt", sonarMaxAlt).toFloat();
+ settings.sonarAltRate = qSettings->value("sonarAltRate", sonarAltRate).toInt();
+
settings.gpsPosition = qSettings->value("gpsPosition", gpsPosition).toBool();
settings.gpsPosRate = qSettings->value("gpsPosRate", gpsPosRate).toInt();
@@ -112,6 +120,10 @@ HITLConfiguration::HITLConfiguration(QString classId,
settings.attActSim = attActSim;
settings.attActCalc = attActCalc;
+ settings.sonarAltitude = sonarAltitude;
+ settings.sonarMaxAlt = sonarMaxAlt;
+ settings.sonarAltRate = sonarAltRate;
+
settings.gpsPosition = gpsPosition;
settings.gpsPosRate = gpsPosRate;
@@ -151,6 +163,10 @@ void HITLConfiguration::saveConfig(QSettings* qSettings) const
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("gpsPosRate", settings.gpsPosRate);
diff --git a/ground/openpilotgcs/src/plugins/hitlv2/hitlv2optionspage.cpp b/ground/openpilotgcs/src/plugins/hitlv2/hitlv2optionspage.cpp
index cf89aae18..5b88785ad 100644
--- a/ground/openpilotgcs/src/plugins/hitlv2/hitlv2optionspage.cpp
+++ b/ground/openpilotgcs/src/plugins/hitlv2/hitlv2optionspage.cpp
@@ -82,6 +82,10 @@ QWidget *HITLOptionsPage::createPage(QWidget *parent)
m_optionsPage->attActSim->setChecked(config->Settings().attActSim);
m_optionsPage->attActCalc->setChecked(config->Settings().attActCalc);
+ m_optionsPage->sonarAltitude->setChecked(config->Settings().sonarAltitude);
+ m_optionsPage->sonarMaxAlt->setValue(config->Settings().sonarMaxAlt);
+ m_optionsPage->sonarAltRate->setValue(config->Settings().sonarAltRate);
+
m_optionsPage->gpsPosition->setChecked(config->Settings().gpsPosition);
m_optionsPage->gpsPosRate->setValue(config->Settings().gpsPosRate);
@@ -119,6 +123,10 @@ void HITLOptionsPage::apply()
settings.attActSim = m_optionsPage->attActSim->isChecked();
settings.attActCalc = m_optionsPage->attActCalc->isChecked();
+ settings.sonarAltitude = m_optionsPage->sonarAltitude->isChecked();
+ settings.sonarMaxAlt = m_optionsPage->sonarMaxAlt->value();
+ settings.sonarAltRate = m_optionsPage->sonarAltRate->value();
+
settings.gpsPosition = m_optionsPage->gpsPosition->isChecked();
settings.gpsPosRate = m_optionsPage->gpsPosRate->value();
diff --git a/ground/openpilotgcs/src/plugins/hitlv2/hitlv2optionspage.ui b/ground/openpilotgcs/src/plugins/hitlv2/hitlv2optionspage.ui
index 4bbfe93bb..00a2f60aa 100644
--- a/ground/openpilotgcs/src/plugins/hitlv2/hitlv2optionspage.ui
+++ b/ground/openpilotgcs/src/plugins/hitlv2/hitlv2optionspage.ui
@@ -424,6 +424,92 @@
+ -
+
+
+ SonarAltitude
+
+
+ true
+
+
+ true
+
+
+ false
+
+
+
+ 3
+
+
+ 6
+
+
+ 3
+
+
+ 0
+
+
+ 0
+
+
-
+
+
-
+
+
+ Range detectioon
+
+
+
+ -
+
+
+ m
+
+
+ 1
+
+
+ 10
+
+
+ 5
+
+
+
+ -
+
+
+ Refresh rate
+
+
+
+ -
+
+
+ ms
+
+
+ 20
+
+
+ 2000
+
+
+ 10
+
+
+ 50
+
+
+
+
+
+
+
+
-
diff --git a/ground/openpilotgcs/src/plugins/hitlv2/simulatorv2.cpp b/ground/openpilotgcs/src/plugins/hitlv2/simulatorv2.cpp
index a940c0c4a..912d28ac6 100644
--- a/ground/openpilotgcs/src/plugins/hitlv2/simulatorv2.cpp
+++ b/ground/openpilotgcs/src/plugins/hitlv2/simulatorv2.cpp
@@ -116,6 +116,7 @@ void Simulator::onStart()
gcsReceiver = GCSReceiver::GetInstance(objManager);
actCommand = ActuatorCommand::GetInstance(objManager);
attSettings = AttitudeSettings::GetInstance(objManager);
+ sonarAlt = SonarAltitude::GetInstance(objManager);
telStats = GCSTelemetryStats::GetInstance(objManager);
@@ -204,6 +205,9 @@ void Simulator::setupObjects()
if (settings.homeLocation)
setupOutputObject(posHome);
+ if (settings.sonarAltitude)
+ setupOutputObject(sonarAlt);
+
if (settings.gpsPosition)
setupOutputObject(gpsPosition);
@@ -224,6 +228,7 @@ void Simulator::resetAllObjects()
setupDefaultObject(gpsPosition);
setupDefaultObject(gcsReceiver);
setupDefaultObject(actCommand);
+ setupDefaultObject(sonarAlt);
// setupDefaultObject(manCtrlCommand);
// setupDefaultObject(actDesired);
// setupDefaultObject(camDesired);
diff --git a/ground/openpilotgcs/src/plugins/hitlv2/simulatorv2.h b/ground/openpilotgcs/src/plugins/hitlv2/simulatorv2.h
index 0ddd93a9f..f0471ffd6 100644
--- a/ground/openpilotgcs/src/plugins/hitlv2/simulatorv2.h
+++ b/ground/openpilotgcs/src/plugins/hitlv2/simulatorv2.h
@@ -45,6 +45,7 @@
#include "actuatorcommand.h"
#include "gcstelemetrystats.h"
#include "attitudesettings.h"
+#include "sonaraltitude.h"
//#define DBG_TIMERS
#undef DBG_TIMERS
@@ -74,6 +75,10 @@ typedef struct _CONNECTION
bool attActSim;
bool attActCalc;
+ bool sonarAltitude;
+ float sonarMaxAlt;
+ quint16 sonarAltRate;
+
bool gpsPosition;
quint16 gpsPosRate;
@@ -158,14 +163,15 @@ protected:
// BaroAltitude* altActual;
// CameraDesired *camDesired;
// AccessoryDesired *acsDesired;
- AttitudeRaw* attRaw;
- AttitudeActual* attActual;
- HomeLocation* posHome;
- FlightStatus* flightStatus;
- GPSPosition* gpsPosition;
+ AttitudeRaw *attRaw;
+ AttitudeActual *attActual;
+ HomeLocation *posHome;
+ FlightStatus *flightStatus;
+ GPSPosition *gpsPosition;
GCSReceiver *gcsReceiver;
ActuatorCommand *actCommand;
AttitudeSettings *attSettings;
+ SonarAltitude *sonarAlt;
GCSTelemetryStats* telStats;
SimulatorSettings settings;