From 0b71652afc26ab7573fdcbac31f12f7e9f2dc839 Mon Sep 17 00:00:00 2001 From: Andrew Tridgell Date: Thu, 5 May 2016 10:10:04 +1000 Subject: [PATCH] AP_Baro; allow for exact replay of baro data --- libraries/AP_Baro/AP_Baro.cpp | 6 ++++++ libraries/AP_Baro/AP_Baro.h | 9 +++++++-- libraries/AP_Baro/AP_Baro_HIL.cpp | 10 ++++++++-- 3 files changed, 21 insertions(+), 4 deletions(-) diff --git a/libraries/AP_Baro/AP_Baro.cpp b/libraries/AP_Baro/AP_Baro.cpp index fa9bff025f..c2db07c5f8 100644 --- a/libraries/AP_Baro/AP_Baro.cpp +++ b/libraries/AP_Baro/AP_Baro.cpp @@ -246,6 +246,9 @@ float AP_Baro::get_air_density_ratio(void) // note that this relies on read() being called regularly to get new data float AP_Baro::get_climb_rate(void) { + if (_hil.have_crate) { + return _hil.climb_rate; + } // we use a 7 point derivative filter on the climb rate. This seems // to produce somewhat reasonable results on real hardware return _climb_rate_filter.slope() * 1.0e3f; @@ -378,6 +381,9 @@ void AP_Baro::update(void) sensors[i].altitude = altitude + _alt_offset_active; } } + if (_hil.have_alt) { + sensors[0].altitude = _hil.altitude; + } } // ensure the climb rate filter is updated diff --git a/libraries/AP_Baro/AP_Baro.h b/libraries/AP_Baro/AP_Baro.h index 19e74c3eed..a2d1a1894e 100644 --- a/libraries/AP_Baro/AP_Baro.h +++ b/libraries/AP_Baro/AP_Baro.h @@ -104,14 +104,19 @@ public: // HIL (and SITL) interface, setting altitude void setHIL(float altitude_msl); - // HIL (and SITL) interface, setting pressure and temperature - void setHIL(uint8_t instance, float pressure, float temperature); + // HIL (and SITL) interface, setting pressure, temperature, altitude and climb_rate + // used by Replay + void setHIL(uint8_t instance, float pressure, float temperature, float altitude, float climb_rate); // HIL variables struct { float pressure; float temperature; + float altitude; + float climb_rate; bool updated:1; + bool have_alt:1; + bool have_crate:1; } _hil; // register a new sensor, claiming a sensor slot. If we are out of diff --git a/libraries/AP_Baro/AP_Baro_HIL.cpp b/libraries/AP_Baro/AP_Baro_HIL.cpp index 971833eadd..7d37479e52 100644 --- a/libraries/AP_Baro/AP_Baro_HIL.cpp +++ b/libraries/AP_Baro/AP_Baro_HIL.cpp @@ -54,13 +54,15 @@ void AP_Baro::setHIL(float altitude_msl) float p = p0 * delta; float T = 303.16f * theta - 273.16f; // Assume 30 degrees at sea level - converted to degrees Kelvin - setHIL(0, p, T); + _hil.pressure = p; + _hil.temperature = T; + _hil.updated = true; } /* set HIL pressure and temperature for an instance */ -void AP_Baro::setHIL(uint8_t instance, float pressure, float temperature) +void AP_Baro::setHIL(uint8_t instance, float pressure, float temperature, float altitude, float climb_rate) { if (instance >= _num_sensors) { // invalid @@ -68,7 +70,11 @@ void AP_Baro::setHIL(uint8_t instance, float pressure, float temperature) } _hil.pressure = pressure; _hil.temperature = temperature; + _hil.altitude = altitude; + _hil.climb_rate = climb_rate; _hil.updated = true; + _hil.have_alt = true; + _hil.have_crate = true; } // Read the sensor