diff --git a/libraries/AP_Baro/AP_Baro.cpp b/libraries/AP_Baro/AP_Baro.cpp index c2db07c5f8..115387b98e 100644 --- a/libraries/AP_Baro/AP_Baro.cpp +++ b/libraries/AP_Baro/AP_Baro.cpp @@ -246,7 +246,7 @@ 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) { + if (_hil.have_alt) { return _hil.climb_rate; } // we use a 7 point derivative filter on the climb rate. This seems @@ -384,6 +384,9 @@ void AP_Baro::update(void) if (_hil.have_alt) { sensors[0].altitude = _hil.altitude; } + if (_hil.have_last_update) { + sensors[0].last_update_ms = _hil.last_update_ms; + } } // ensure the climb rate filter is updated diff --git a/libraries/AP_Baro/AP_Baro.h b/libraries/AP_Baro/AP_Baro.h index a2d1a1894e..a110f6ab9d 100644 --- a/libraries/AP_Baro/AP_Baro.h +++ b/libraries/AP_Baro/AP_Baro.h @@ -106,7 +106,7 @@ public: // 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); + void setHIL(uint8_t instance, float pressure, float temperature, float altitude, float climb_rate, uint32_t last_update_ms); // HIL variables struct { @@ -114,9 +114,10 @@ public: float temperature; float altitude; float climb_rate; + uint32_t last_update_ms; bool updated:1; bool have_alt:1; - bool have_crate:1; + bool have_last_update: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 7d37479e52..a93da1cf5f 100644 --- a/libraries/AP_Baro/AP_Baro_HIL.cpp +++ b/libraries/AP_Baro/AP_Baro_HIL.cpp @@ -62,7 +62,7 @@ void AP_Baro::setHIL(float altitude_msl) /* set HIL pressure and temperature for an instance */ -void AP_Baro::setHIL(uint8_t instance, float pressure, float temperature, float altitude, float climb_rate) +void AP_Baro::setHIL(uint8_t instance, float pressure, float temperature, float altitude, float climb_rate, uint32_t last_update_ms) { if (instance >= _num_sensors) { // invalid @@ -74,7 +74,11 @@ void AP_Baro::setHIL(uint8_t instance, float pressure, float temperature, float _hil.climb_rate = climb_rate; _hil.updated = true; _hil.have_alt = true; - _hil.have_crate = true; + + if (last_update_ms != 0) { + _hil.last_update_ms = last_update_ms; + _hil.have_last_update = true; + } } // Read the sensor