rooty/LibrePilot
1
0
Fork 0
mirror of https://bitbucket.org/librepilot/librepilot.git synced 2025-01-18 03:52:11 +01:00
Code Issues Releases Activity

add emulated sonar output

Browse Source 
40 degree beam, hardcoded.
This commit is contained in:
Dmitry Zaitsev 2012-04-04 01:22:34 +04:00
parent d45e88515b
commit 34b8564864
6 changed files with 147 additions and 5 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

21
ground/openpilotgcs/src/plugins/hitlv2/aerosimrc.cpp
View File

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

16
ground/openpilotgcs/src/plugins/hitlv2/hitlv2configuration.cpp
View File

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

8
ground/openpilotgcs/src/plugins/hitlv2/hitlv2optionspage.cpp
View File

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

86
ground/openpilotgcs/src/plugins/hitlv2/hitlv2optionspage.ui
View File

@ -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">

5
ground/openpilotgcs/src/plugins/hitlv2/simulatorv2.cpp
View File

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

16
ground/openpilotgcs/src/plugins/hitlv2/simulatorv2.h
View File

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