From 34b85648646d3e67d93ff189401eaff69310a5a8 Mon Sep 17 00:00:00 2001 From: Dmitry Zaitsev Date: Wed, 4 Apr 2012 01:22:34 +0400 Subject: [PATCH] add emulated sonar output 40 degree beam, hardcoded. --- .../src/plugins/hitlv2/aerosimrc.cpp | 21 +++++ .../plugins/hitlv2/hitlv2configuration.cpp | 16 ++++ .../src/plugins/hitlv2/hitlv2optionspage.cpp | 8 ++ .../src/plugins/hitlv2/hitlv2optionspage.ui | 86 +++++++++++++++++++ .../src/plugins/hitlv2/simulatorv2.cpp | 5 ++ .../src/plugins/hitlv2/simulatorv2.h | 16 ++-- 6 files changed, 147 insertions(+), 5 deletions(-) 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;