From 0b71652afc26ab7573fdcbac31f12f7e9f2dc839 Mon Sep 17 00:00:00 2001
From: Andrew Tridgell <andrew@tridgell.net>
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