add emulated sonar output

40 degree beam, hardcoded.

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();

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);
 

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();
 

ground/openpilotgcs/src/plugins/hitlv2/hitlv2optionspage.ui

@@ -424,6 +424,92 @@
           </layout>
          </widget>
         </item>
+        <item>
+         <widget class="QGroupBox" name="sonarAltitude">
+          <property name="title">
+           <string>SonarAltitude</string>
+          </property>
+          <property name="flat">
+           <bool>true</bool>
+          </property>
+          <property name="checkable">
+           <bool>true</bool>
+          </property>
+          <property name="checked">
+           <bool>false</bool>
+          </property>
+          <layout class="QVBoxLayout" name="verticalLayout_6">
+           <property name="spacing">
+            <number>3</number>
+           </property>
+           <property name="leftMargin">
+            <number>6</number>
+           </property>
+           <property name="topMargin">
+            <number>3</number>
+           </property>
+           <property name="rightMargin">
+            <number>0</number>
+           </property>
+           <property name="bottomMargin">
+            <number>0</number>
+           </property>
+           <item>
+            <layout class="QGridLayout" name="gridLayout_2">
+             <item row="0" column="0">
+              <widget class="QLabel" name="label_11">
+               <property name="text">
+                <string>Range detectioon</string>
+               </property>
+              </widget>
+             </item>
+             <item row="0" column="1">
+              <widget class="QSpinBox" name="sonarMaxAlt">
+               <property name="suffix">
+                <string>m</string>
+               </property>
+               <property name="minimum">
+                <number>1</number>
+               </property>
+               <property name="maximum">
+                <number>10</number>
+               </property>
+               <property name="value">
+                <number>5</number>
+               </property>
+              </widget>
+             </item>
+             <item row="1" column="0">
+              <widget class="QLabel" name="label_12">
+               <property name="text">
+                <string>Refresh rate</string>
+               </property>
+              </widget>
+             </item>
+             <item row="1" column="1">
+              <widget class="QSpinBox" name="sonarAltRate">
+               <property name="suffix">
+                <string>ms</string>
+               </property>
+               <property name="minimum">
+                <number>20</number>
+               </property>
+               <property name="maximum">
+                <number>2000</number>
+               </property>
+               <property name="singleStep">
+                <number>10</number>
+               </property>
+               <property name="value">
+                <number>50</number>
+               </property>
+              </widget>
+             </item>
+            </layout>
+           </item>
+          </layout>
+         </widget>
+        </item>
         <item>
          <widget class="QGroupBox" name="inputCommand">
           <property name="title">

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);

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;
+    AttitudeRaw *attRaw;
-    AttitudeActual* attActual;
+    AttitudeActual *attActual;
-    HomeLocation* posHome;
+    HomeLocation *posHome;
-    FlightStatus* flightStatus;
+    FlightStatus *flightStatus;
-    GPSPosition* gpsPosition;
+    GPSPosition *gpsPosition;
     GCSReceiver *gcsReceiver;
     ActuatorCommand *actCommand;
     AttitudeSettings *attSettings;
+    SonarAltitude *sonarAlt;
 
     GCSTelemetryStats* telStats;
     SimulatorSettings settings;