Skip to content
GitLab
Commit e30960c5 authored by Lorenz Meier's avatar Lorenz Meier
Browse files

Merge pull request #387 from thomasgubler/hil_flightgear 

Sensor Hil for flightgear
parents ea634b84 b73c9116
Hide whitespace changes
Inline Side-by-side
files/flightgear/Protocol/qgroundcontrol-fixed-wing.xml
View file @ e30960c5
......@@ -141,6 +141,49 @@
<node>/velocities/airspeed-kt</node>
<factor>0.514444444444444</factor> <!-- knots to mps -->
</chunk>
<chunk>
<name>airspeed-indicated-mps</name>
<type>float</type>
<format>%.8f</format>
<node>/instrumentation/indicated-speed-kt</node>
<factor>0.514444444444444</factor> <!-- knots to mps -->
</chunk>
<!-- Magnetometer -->
<chunk>
<name>Magnetic Variation (rad)</name>
<type>float</type>
<format>%.8f</format>
<node>/environment/magnetic-variation-deg</node>
<factor>0.01745329251994329576</factor> <!-- degrees to radians -->
</chunk>
<chunk>
<name>Magnetic Dip (rad)</name>
<type>float</type>
<format>%.8f</format>
<node>/environment/magnetic-dip-deg</node>
<factor>0.01745329251994329576</factor> <!-- degrees to radians -->
</chunk>
<!-- Temperature -->
<chunk>
<name>Temperature (deg C)</name>
<type>float</type>
<format>%.8f</format>
<node>/environment/temperature-degc</node>
<factor>1</factor>
</chunk>
<!-- Pressure -->
<chunk>
<name>Pressure (hPa)</name>
<type>float</type>
<format>%.8f</format>
<node>/environment/pressure-inhg</node>
<factor>33.86389</factor> <!-- inhg to hpa -->
</chunk>
</output>
......@@ -184,4 +227,4 @@
</generic>
</PropertyList>
\ No newline at end of file
</PropertyList>
src/comm/QGCFlightGearLink.cc
View file @ e30960c5
......@@ -26,6 +26,7 @@ This file is part of the QGROUNDCONTROL project
* @brief Definition of UDP connection (server) for unmanned vehicles
* @see Flightgear Manual http://mapserver.flightgear.org/getstart.pdf
* @author Lorenz Meier <mavteam@student.ethz.ch>
* @author Thomas Gubler <thomasgubler@student.ethz.ch>
*
*/
......@@ -44,7 +45,8 @@ QGCFlightGearLink::QGCFlightGearLink(UASInterface* mav, QString startupArguments
process(NULL),
terraSync(NULL),
flightGearVersion(0),
startupArguments(startupArguments)
startupArguments(startupArguments),
_sensorHilEnabled(true)
{
this->host = host;
this->port = port+mav->getUASID();
......@@ -172,6 +174,7 @@ void QGCFlightGearLink::updateControls(uint64_t time, float rollAilerons, float
QString state("%1\t%2\t%3\t%4\t%5\n");
state = state.arg(rollAilerons).arg(pitchElevator).arg(yawRudder).arg(true).arg(throttle);
writeBytes(state.toAscii().constData(), state.length());
// qDebug() << "updated controls" << rollAilerons << pitchElevator << yawRudder << throttle;
}
else
{
......@@ -227,49 +230,133 @@ void QGCFlightGearLink::readBytes()
// Print string
QString state(b);
//qDebug() << "FG LINK GOT:" << state;
// qDebug() << "FG LINK GOT:" << state;
QStringList values = state.split("\t");
// Check length
if (values.size() != 17)
if (values.size() != 22)
{
qDebug() << "RETURN LENGTH MISMATCHING EXPECTED" << 17 << "BUT GOT" << values.size();
qDebug() << "RETURN LENGTH MISMATCHING EXPECTED" << 22 << "BUT GOT" << values.size();
qDebug() << state;
return;
}
// Parse string
float roll, pitch, yaw, rollspeed, pitchspeed, yawspeed;
double lat, lon, alt;
double lat, lon, alt;
float ind_airspeed;
float true_airspeed;
float vx, vy, vz, xacc, yacc, zacc;
float diff_pressure;
float temperature;
float abs_pressure;
float mag_variation, mag_dip, xmag_ned, ymag_ned, zmag_ned, xmag_body, ymag_body, zmag_body;
// XXX add
float ind_airspeed = 0.0f;
float true_airspeed = 0.0f;
double vx, vy, vz, xacc, yacc, zacc;
lat = values.at(1).toDouble();
lon = values.at(2).toDouble();
alt = values.at(3).toDouble();
roll = values.at(4).toDouble();
pitch = values.at(5).toDouble();
yaw = values.at(6).toDouble();
rollspeed = values.at(7).toDouble();
pitchspeed = values.at(8).toDouble();
yawspeed = values.at(9).toDouble();
roll = values.at(4).toFloat();
pitch = values.at(5).toFloat();
yaw = values.at(6).toFloat();
rollspeed = values.at(7).toFloat();
pitchspeed = values.at(8).toFloat();
yawspeed = values.at(9).toFloat();
xacc = values.at(10).toDouble();
yacc = values.at(11).toDouble();
zacc = values.at(12).toDouble();
xacc = values.at(10).toFloat();
yacc = values.at(11).toFloat();
zacc = values.at(12).toFloat();
vx = values.at(13).toDouble();
vy = values.at(14).toDouble();
vz = values.at(15).toDouble();
vx = values.at(13).toFloat();
vy = values.at(14).toFloat();
vz = values.at(15).toFloat();
true_airspeed = values.at(16).toFloat();
ind_airspeed = values.at(17).toFloat();
mag_variation = values.at(18).toFloat();
mag_dip = values.at(19).toFloat();
temperature = values.at(20).toFloat();
abs_pressure = values.at(21).toFloat();
// Send updated state
emit hilStateChanged(QGC::groundTimeUsecs(), roll, pitch, yaw, rollspeed,
//qDebug() << "sensorHilEnabled: " << sensorHilEnabled;
if (_sensorHilEnabled)
{
quint16 fields_changed = 0xFFF; //set all 12 used bits
//calculate differential pressure
const float air_gas_constant = 287.1f; // J/(kg * K)
const float absolute_null_celsius = -273.15f; // °C
float density = abs_pressure / (air_gas_constant * (temperature - absolute_null_celsius));
diff_pressure = true_airspeed * true_airspeed * density / 2.0f;
float pressure_alt = alt;
xmag_ned = cosf(mag_variation);
ymag_ned = sinf(mag_variation);
zmag_ned = sinf(mag_dip);
float tempMagLength = sqrtf(xmag_ned*xmag_ned + ymag_ned*ymag_ned + zmag_ned*zmag_ned);
xmag_ned = xmag_ned / tempMagLength;
ymag_ned = ymag_ned / tempMagLength;
zmag_ned = zmag_ned / tempMagLength;
//transform magnetic measurement to body frame coordinates
double cosPhi = cos(roll);
double sinPhi = sin(roll);
double cosThe = cos(pitch);
double sinThe = sin(pitch);
double cosPsi = cos(yaw);
double sinPsi = sin(yaw);
float R_B_N[3][3];
R_B_N[0][0] = cosThe * cosPsi;
R_B_N[0][1] = -cosPhi * sinPsi + sinPhi * sinThe * cosPsi;
R_B_N[0][2] = sinPhi * sinPsi + cosPhi * sinThe * cosPsi;
R_B_N[1][0] = cosThe * sinPsi;
R_B_N[1][1] = cosPhi * cosPsi + sinPhi * sinThe * sinPsi;
R_B_N[1][2] = -sinPhi * cosPsi + cosPhi * sinThe * sinPsi;
R_B_N[2][0] = -sinThe;
R_B_N[2][1] = sinPhi * cosThe;
R_B_N[2][2] = cosPhi * cosThe;
Eigen::Matrix3f R_B_N_M = Eigen::Map<Eigen::Matrix3f>((float*)R_B_N).eval();
Eigen::Vector3f mag_ned(xmag_ned, ymag_ned, zmag_ned);
Eigen::Vector3f mag_body = R_B_N_M * mag_ned;
xmag_body = mag_body(0);
ymag_body = mag_body(1);
zmag_body = mag_body(2);
emit sensorHilRawImuChanged(QGC::groundTimeUsecs(), xacc, yacc, zacc, rollspeed, pitchspeed, yawspeed,
xmag_body, ymag_body, zmag_body, abs_pressure, diff_pressure, pressure_alt, temperature, fields_changed);
// qDebug() << "sensorHilRawImuChanged " << xacc << yacc << zacc << rollspeed << pitchspeed << yawspeed << xmag << ymag << zmag << abs_pressure << diff_pressure << pressure_alt << temperature;
int gps_fix_type = 3;
float eph = 0.3;
float epv = 0.6;
float vel = sqrt(vx*vx + vy*vy + vz*vz);
float cog = yaw;
int satellites = 8;
emit sensorHilGpsChanged(QGC::groundTimeUsecs(), lat, lon, alt, gps_fix_type, eph, epv, vel, vx, vy, vz, cog, satellites);
// qDebug() << "sensorHilGpsChanged " << lat << lon << alt << vel;
} else {
emit hilStateChanged(QGC::groundTimeUsecs(), roll, pitch, yaw, rollspeed,
pitchspeed, yawspeed, lat, lon, alt,
vx, vy, vz, ind_airspeed, true_airspeed, xacc, yacc, zacc);
vx, vy, vz,
ind_airspeed, true_airspeed,
xacc, yacc, zacc);
//qDebug() << "hilStateChanged " << (int32_t)lat << (int32_t)lon << (int32_t)alt;
}
// // Echo data for debugging purposes
// std::cerr << __FILE__ << __LINE__ << "Received datagram:" << std::endl;
......@@ -303,7 +390,9 @@ bool QGCFlightGearLink::disconnectSimulation()
disconnect(process, SIGNAL(error(QProcess::ProcessError)),
this, SLOT(processError(QProcess::ProcessError)));
disconnect(mav, SIGNAL(hilControlsChanged(uint64_t, float, float, float, float, uint8_t, uint8_t)), this, SLOT(updateControls(uint64_t,float,float,float,float,uint8_t,uint8_t)));
disconnect(this, SIGNAL(hilStateChanged(quint64,float,float,float,float,float,float,double,double,double,float,float,float,float,float,float,float,float)), mav, SLOT(sendHilState(quint64,float,float,float,float,float,float,double,double,double,float,float,float,float,float,float,float,float)));
disconnect(this, SIGNAL(hilStateChanged(quint64, float, float, float, float,float, float, double, double, double, float, float, float, float, float, float, float, float)), mav, SLOT(sendHilState(quint64, float, float, float, float,float, float, double, double, double, float, float, float, float, float, float, float, float)));
disconnect(this, SIGNAL(sensorHilGpsChanged(quint64, double, double, double, int, float, float, float, float, float, float, float, int)), mav, SLOT(sendHilGps(quint64, double, double, double, int, float, float, float, float, float, float, float, int)));
disconnect(this, SIGNAL(sensorHilRawImuChanged(quint64,float,float,float,float,float,float,float,float,float,float,float,float,float,quint32)), mav, SLOT(sendHilSensors(quint64,float,float,float,float,float,float,float,float,float,float,float,float,float,quint32)));
if (process)
{
......@@ -350,8 +439,9 @@ bool QGCFlightGearLink::connectSimulation()
terraSync = new QProcess(this);
connect(mav, SIGNAL(hilControlsChanged(uint64_t, float, float, float, float, uint8_t, uint8_t)), this, SLOT(updateControls(uint64_t,float,float,float,float,uint8_t,uint8_t)));
connect(this, SIGNAL(hilStateChanged(quint64,float,float,float,float,float,float,double,double,double,float,float,float,float,float,float,float,float)), mav, SLOT(sendHilState(quint64,float,float,float,float,float,float,double,double,double,float,float,float,float,float,float,float,float)));
connect(this, SIGNAL(hilStateChanged(quint64, float, float, float, float,float, float, double, double, double, float, float, float, float, float, float, float, float)), mav, SLOT(sendHilState(quint64, float, float, float, float,float, float, double, double, double, float, float, float, float, float, float, float, float)));
connect(this, SIGNAL(sensorHilGpsChanged(quint64, double, double, double, int, float, float, float, float, float, float, float, int)), mav, SLOT(sendHilGps(quint64, double, double, double, int, float, float, float, float, float, float, float, int)));
connect(this, SIGNAL(sensorHilRawImuChanged(quint64,float,float,float,float,float,float,float,float,float,float,float,float,float,quint32)), mav, SLOT(sendHilSensors(quint64,float,float,float,float,float,float,float,float,float,float,float,float,float,quint32)));
UAS* uas = dynamic_cast<UAS*>(mav);
if (uas)
......
src/comm/QGCFlightGearLink.h
View file @ e30960c5
......@@ -83,6 +83,10 @@ public:
return _sensorHilEnabled;
}
void sensorHilEnabled(bool sensorHilEnabled) {
_sensorHilEnabled = sensorHilEnabled;
}
void run();
public slots:
......
src/uas/UAS.cc
View file @ e30960c5
......@@ -2930,7 +2930,7 @@ bool UAS::emergencyKILL()
/**
* If enabled, connect the flight gear link.
*/
void UAS::enableHilFlightGear(bool enable, QString options)
void UAS::enableHilFlightGear(bool enable, QString options, bool sensorHil)
{
QGCFlightGearLink* link = dynamic_cast<QGCFlightGearLink*>(simulation);
if (!link || !simulation) {
......@@ -2944,6 +2944,7 @@ void UAS::enableHilFlightGear(bool enable, QString options)
// Connect Flight Gear Link
link = dynamic_cast<QGCFlightGearLink*>(simulation);
link->setStartupArguments(options);
link->sensorHilEnabled(sensorHil);
if (enable)
{
startHil();
......
src/uas/UAS.h
View file @ e30960c5
......@@ -730,7 +730,7 @@ public slots:
void go();
/** @brief Enable / disable HIL */
void enableHilFlightGear(bool enable, QString options);
void enableHilFlightGear(bool enable, QString options, bool sensorHil);
void enableHilJSBSim(bool enable, QString options);
void enableHilXPlane(bool enable);
......
src/ui/QGCHilConfiguration.cc
View file @ e30960c5
......@@ -64,7 +64,7 @@ void QGCHilConfiguration::on_simComboBox_currentIndexChanged(int index)
if(1 == index)
{
// Ensure the sim exists and is disabled
mav->enableHilFlightGear(false, "");
mav->enableHilFlightGear(false, "", true);
QGCHilFlightGearConfiguration* hfgconf = new QGCHilFlightGearConfiguration(mav, this);
hfgconf->show();
ui->simulatorConfigurationLayout->addWidget(hfgconf);
......
src/ui/QGCHilFlightGearConfiguration.cc
View file @ e30960c5
......@@ -40,7 +40,7 @@ void QGCHilFlightGearConfiguration::on_startButton_clicked()
//XXX check validity of inputs
QString options = ui->optionsPlainTextEdit->toPlainText();
options.append(" --aircraft=" + ui->aircraftComboBox->currentText());
mav->enableHilFlightGear(true, options);
mav->enableHilFlightGear(true, options, ui->sensorHilCheckBox->isChecked());
}
void QGCHilFlightGearConfiguration::on_stopButton_clicked()
......
src/ui/QGCHilFlightGearConfiguration.ui
View file @ e30960c5
......@@ -25,8 +25,17 @@
<property name="autoFillBackground">
<bool>false</bool>
</property>
<layout class="QGridLayout" name="gridLayout" rowminimumheight="0,0,0,0,0,0,0,0">
<property name="margin">
<layout class="QGridLayout" name="gridLayout">
<property name="leftMargin">
<number>0</number>
</property>
<property name="topMargin">
<number>0</number>
</property>
<property name="rightMargin">
<number>0</number>
</property>
<property name="bottomMargin">
<number>0</number>
</property>
<property name="spacing">
......@@ -52,14 +61,14 @@
</property>
</widget>
</item>
<item row="2" column="0">
<item row="3" column="0">
<widget class="QLabel" name="optionsLabel">
<property name="text">
<string>&lt;html&gt;&lt;head/&gt;&lt;body&gt;&lt;p&gt;Additional Options:&lt;/p&gt;&lt;/body&gt;&lt;/html&gt;</string>
</property>
</widget>
</item>
<item row="3" column="0" colspan="2">
<item row="4" column="0" colspan="2">
<widget class="QPlainTextEdit" name="optionsPlainTextEdit">
<property name="sizePolicy">
<sizepolicy hsizetype="Preferred" vsizetype="Preferred">
......@@ -72,7 +81,7 @@
</property>
</widget>
</item>
<item row="5" column="0">
<item row="6" column="0">
<widget class="QPushButton" name="startButton">
<property name="sizePolicy">
<sizepolicy hsizetype="Minimum" vsizetype="Fixed">
......@@ -85,13 +94,23 @@
</property>
</widget>
</item>
<item row="5" column="1">
<item row="6" column="1">
<widget class="QPushButton" name="stopButton">
<property name="text">
<string>Stop</string>
</property>
</widget>
</item>
<item row="2" column="0">
<widget class="QCheckBox" name="sensorHilCheckBox">
<property name="text">
<string>Sensor HIL</string>
</property>
<property name="checked">
<bool>true</bool>
</property>
</widget>
</item>
</layout>
</widget>
<resources/>
......
Supports Markdown
0% or .
You are about to add 0 people to the discussion. Proceed with caution.
Finish editing this message first!
Please register or to comment