AP_Baro: average SITL baro measurements using update()

libraries/AP_Baro/AP_Baro.h

@@ -7,6 +7,7 @@
 #include <AP_Param/AP_Param.h>
 #include <Filter/Filter.h>
 #include <Filter/DerivativeFilter.h>
+#include <AP_Buffer/AP_Buffer.h>
 
 // maximum number of sensor instances
 #define BARO_MAX_INSTANCES 3
@@ -109,6 +110,12 @@ public:
     // HIL (and SITL) interface, setting pressure and temperature
     void setHIL(uint8_t instance, float pressure, float temperature);
 
+    // HIL variables
+    struct {
+        AP_Buffer<float,10> press_buffer;
+        AP_Buffer<float,10> temp_buffer;
+    } _hil;
+
     // register a new sensor, claiming a sensor slot. If we are out of
     // slots it will panic
     uint8_t register_sensor(void);

libraries/AP_Baro/AP_Baro_HIL.cpp

@@ -66,7 +66,30 @@ void AP_Baro::setHIL(uint8_t instance, float pressure, float temperature)
         // invalid
         return;
     }
-    sensors[instance].pressure = pressure;
-    sensors[instance].temperature = temperature;
-    sensors[instance].last_update_ms = hal.scheduler->millis();
+    _hil.press_buffer.push_back(pressure);
+    _hil.temp_buffer.push_back(temperature);
+}
+
+// Read the sensor
+void AP_Baro_HIL::update(void)
+{
+    float pressure = 0.0;
+    float temperature = 0.0;
+    float pressure_sum = 0.0;
+    float temperature_sum = 0.0;
+    uint32_t sum_count = 0;
+
+    while (_frontend._hil.press_buffer.is_empty() == false){
+        _frontend._hil.press_buffer.pop_front(pressure);
+        pressure_sum += pressure; // Pressure in Pascals
+        _frontend._hil.temp_buffer.pop_front(temperature);
+        temperature_sum += temperature; // degrees celcius
+        sum_count++;
+    }
+
+    if (sum_count > 0) {
+        pressure_sum /= (float)sum_count;
+        temperature_sum /= (float)sum_count;
+        _copy_to_frontend(0, pressure_sum, temperature_sum);
+    }
 }

libraries/AP_Baro/AP_Baro_HIL.h

@@ -13,7 +13,7 @@ class AP_Baro_HIL : public AP_Baro_Backend
 {
 public:
     AP_Baro_HIL(AP_Baro &baro);
-    void update() {}
+    void update(void);
 
 private:
     uint8_t _instance;