From 45c61a29bb7ba71c74ecf148a7b4cf190751ce49 Mon Sep 17 00:00:00 2001 From: Thomas Gubler Date: Sun, 20 Oct 2013 11:26:45 +0200 Subject: [PATCH 1/2] flightgear hil: correct scaling of pressure for mavlink message and remove indicated airspeed from flightgear protocol, commit requires flightgear protocol update --- .../Protocol/qgroundcontrol-fixed-wing.xml | 8 ------ src/comm/QGCFlightGearLink.cc | 29 ++++++++++++++++------ src/uas/UAS.cc | 4 +++ 3 files changed, 25 insertions(+), 16 deletions(-) diff --git a/files/flightgear/Protocol/qgroundcontrol-fixed-wing.xml b/files/flightgear/Protocol/qgroundcontrol-fixed-wing.xml index 5fe0ad2..0ecbf20 100644 --- a/files/flightgear/Protocol/qgroundcontrol-fixed-wing.xml +++ b/files/flightgear/Protocol/qgroundcontrol-fixed-wing.xml @@ -142,14 +142,6 @@ 0.514444444444444 - - airspeed-indicated-mps - float - %.8f - /instrumentation/indicated-speed-kt - 0.514444444444444 - - Magnetic Variation (rad) diff --git a/src/comm/QGCFlightGearLink.cc b/src/comm/QGCFlightGearLink.cc index d8bc240..108f8d1 100644 --- a/src/comm/QGCFlightGearLink.cc +++ b/src/comm/QGCFlightGearLink.cc @@ -235,9 +235,10 @@ void QGCFlightGearLink::readBytes() QStringList values = state.split("\t"); // Check length - if (values.size() != 22) + const int nValues = 21; + if (values.size() != nValues) { - qDebug() << "RETURN LENGTH MISMATCHING EXPECTED" << 22 << "BUT GOT" << values.size(); + qDebug() << "RETURN LENGTH MISMATCHING EXPECTED" << nValues << "BUT GOT" << values.size(); qDebug() << state; return; } @@ -273,13 +274,12 @@ void QGCFlightGearLink::readBytes() 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(); + mag_variation = values.at(17).toFloat(); + mag_dip = values.at(18).toFloat(); - temperature = values.at(20).toFloat(); - abs_pressure = values.at(21).toFloat(); + temperature = values.at(19).toFloat(); + abs_pressure = values.at(20).toFloat()*1e2; //convert to Pa from hPa // Send updated state //qDebug() << "sensorHilEnabled: " << sensorHilEnabled; @@ -292,6 +292,19 @@ void QGCFlightGearLink::readBytes() 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; + //qDebug() << "diff_pressure: " << diff_pressure << "abs_pressure: " << abs_pressure; + + /* Calculate indicated airspeed */ + const float air_density_sea_level_15C = 1.225f; //kg/m^3 + if (diff_pressure > 0) + { + ind_airspeed = sqrtf((2.0f*diff_pressure) / air_density_sea_level_15C); + } else + { + ind_airspeed = -sqrtf((2.0f*fabsf(diff_pressure)) / air_density_sea_level_15C); + } + + //qDebug() << "ind_airspeed: " << ind_airspeed << "true_airspeed: " << true_airspeed; float pressure_alt = alt; @@ -336,7 +349,7 @@ void QGCFlightGearLink::readBytes() 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); + xmag_body, ymag_body, zmag_body, abs_pressure*1e-2f, diff_pressure*1e-2f, pressure_alt, temperature, fields_changed); //Pressure in hPa for mavlink // qDebug() << "sensorHilRawImuChanged " << xacc << yacc << zacc << rollspeed << pitchspeed << yawspeed << xmag << ymag << zmag << abs_pressure << diff_pressure << pressure_alt << temperature; int gps_fix_type = 3; diff --git a/src/uas/UAS.cc b/src/uas/UAS.cc index 94d73f1..e50e1dc 100644 --- a/src/uas/UAS.cc +++ b/src/uas/UAS.cc @@ -3124,6 +3124,10 @@ void UAS::sendHilState(quint64 time_us, float roll, float pitch, float yaw, floa } } +/* +* @param abs_pressure Absolute Pressure (hPa) +* @param diff_pressure Differential Pressure (hPa) +*/ void UAS::sendHilSensors(quint64 time_us, float xacc, float yacc, float zacc, float rollspeed, float pitchspeed, float yawspeed, float xmag, float ymag, float zmag, float abs_pressure, float diff_pressure, float pressure_alt, float temperature, quint32 fields_changed) { From 126b51bed5765279b0743a82a9add69658dcfae7 Mon Sep 17 00:00:00 2001 From: Thomas Gubler Date: Wed, 23 Oct 2013 11:45:43 +0200 Subject: [PATCH 2/2] fw hil: fixed indentation --- src/comm/QGCFlightGearLink.cc | 10 +++++----- 1 file changed, 5 insertions(+), 5 deletions(-) diff --git a/src/comm/QGCFlightGearLink.cc b/src/comm/QGCFlightGearLink.cc index 108f8d1..7926fa2 100644 --- a/src/comm/QGCFlightGearLink.cc +++ b/src/comm/QGCFlightGearLink.cc @@ -298,11 +298,11 @@ void QGCFlightGearLink::readBytes() const float air_density_sea_level_15C = 1.225f; //kg/m^3 if (diff_pressure > 0) { - ind_airspeed = sqrtf((2.0f*diff_pressure) / air_density_sea_level_15C); - } else - { - ind_airspeed = -sqrtf((2.0f*fabsf(diff_pressure)) / air_density_sea_level_15C); - } + ind_airspeed = sqrtf((2.0f*diff_pressure) / air_density_sea_level_15C); + } else + { + ind_airspeed = -sqrtf((2.0f*fabsf(diff_pressure)) / air_density_sea_level_15C); + } //qDebug() << "ind_airspeed: " << ind_airspeed << "true_airspeed: " << true_airspeed;