zhaohuwei
/
QG4
1
0
Fork 0
Code Issues Pull Requests Projects Releases Wiki Activity
Browse Source

Flightgear sensor HIL

QGC4.4
Thomas Gubler 12 years ago committed by Thomas Gubler
parent
ea634b841f
commit
b73c911670
8 changed files with 194 additions and 37 deletions
Whitespace
Show all changes Ignore whitespace when comparing lines Ignore changes in amount of whitespace Ignore changes in whitespace at EOL
Split View
Diff Options
Show Stats Download Patch File Download Diff File
  1. 45
      files/flightgear/Protocol/qgroundcontrol-fixed-wing.xml
  2. 142
      src/comm/QGCFlightGearLink.cc
  3. 4
      src/comm/QGCFlightGearLink.h
  4. 3
      src/uas/UAS.cc
  5. 2
      src/uas/UAS.h
  6. 2
      src/ui/QGCHilConfiguration.cc
  7. 2
      src/ui/QGCHilFlightGearConfiguration.cc
  8. 31
      src/ui/QGCHilFlightGearConfiguration.ui

45
files/flightgear/Protocol/qgroundcontrol-fixed-wing.xml
Unescape View File

@ -141,6 +141,49 @@ @@ -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 @@ @@ -184,4 +227,4 @@
</generic>
</PropertyList>
</PropertyList>

142
src/comm/QGCFlightGearLink.cc
Unescape View File

@ -26,6 +26,7 @@ This file is part of the QGROUNDCONTROL project @@ -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 @@ -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 @@ -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() @@ -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() @@ -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() @@ -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)

4
src/comm/QGCFlightGearLink.h
Unescape View File

@ -83,6 +83,10 @@ public: @@ -83,6 +83,10 @@ public:
return _sensorHilEnabled;
}
void sensorHilEnabled(bool sensorHilEnabled) {
_sensorHilEnabled = sensorHilEnabled;
}
void run();
public slots:

3
src/uas/UAS.cc
Unescape View File

@ -2930,7 +2930,7 @@ bool UAS::emergencyKILL() @@ -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) @@ -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();

2
src/uas/UAS.h
Unescape View File

@ -730,7 +730,7 @@ public slots: @@ -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);

2
src/ui/QGCHilConfiguration.cc
Unescape View File

@ -64,7 +64,7 @@ void QGCHilConfiguration::on_simComboBox_currentIndexChanged(int index) @@ -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);

2
src/ui/QGCHilFlightGearConfiguration.cc
Unescape View File

@ -40,7 +40,7 @@ void QGCHilFlightGearConfiguration::on_startButton_clicked() @@ -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()

31
src/ui/QGCHilFlightGearConfiguration.ui
Unescape View File

@ -25,8 +25,17 @@ @@ -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 @@ @@ -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 @@ @@ -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 @@ @@ -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/>

Write Preview
Loading…
Cancel
Save