AP_Baro: moved SITL baro to standard sensor backend model

libraries/AP_Baro/AP_Baro.cpp

@@ -27,6 +27,7 @@
 #include <AP_BoardConfig/AP_BoardConfig.h>
 #include <AP_Vehicle/AP_Vehicle_Type.h>
 
+#include "AP_Baro_SITL.h"
 #include "AP_Baro_BMP085.h"
 #include "AP_Baro_BMP280.h"
 #include "AP_Baro_HIL.h"
@@ -388,6 +389,11 @@ void AP_Baro::init(void)
         return;
     }
 
+#if CONFIG_HAL_BOARD == HAL_BOARD_SITL
+    ADD_BACKEND(new AP_Baro_SITL(*this));
+    return;
+#endif
+    
 #if HAL_BARO_DEFAULT == HAL_BARO_PX4 || HAL_BARO_DEFAULT == HAL_BARO_VRBRAIN
     switch (AP_BoardConfig::get_board_type()) {
     case AP_BoardConfig::PX4_BOARD_PX4V1:

libraries/AP_Baro/AP_Baro.h

@@ -149,6 +149,9 @@ public:
     // get baro drift amount
     float get_baro_drift_offset(void) { return _alt_offset_active; }
 
+    // simple atmospheric model
+    static void SimpleAtmosphere(const float alt, float &sigma, float &delta, float &theta);
+
 private:
     // how many drivers do we have?
     uint8_t _num_drivers;
@@ -189,6 +192,5 @@ private:
     // when did we last notify the GCS of new pressure reference?
     uint32_t                            _last_notify_ms;
 
-    void SimpleAtmosphere(const float alt, float &sigma, float &delta, float &theta);
     bool _add_backend(AP_Baro_Backend *backend);
 };

libraries/AP_Baro/AP_Baro_SITL.cpp

@@ -0,0 +1,81 @@
+#include <AP_HAL/AP_HAL.h>
+
+#if CONFIG_HAL_BOARD == HAL_BOARD_SITL
+
+#include "AP_Baro_SITL.h"
+
+extern const AP_HAL::HAL& hal;
+
+/*
+  constructor - registers instance at top Baro driver
+ */
+AP_Baro_SITL::AP_Baro_SITL(AP_Baro &baro) :
+    AP_Baro_Backend(baro)
+{
+    sitl = (SITL::SITL *)AP_Param::find_object("SIM_");
+    if (sitl != nullptr) {
+        instance = _frontend.register_sensor();
+    }
+}
+
+// Read the sensor
+void AP_Baro_SITL::update(void)
+{
+    float sim_alt = sitl->state.altitude;
+
+    if (sitl->baro_disable) {
+        // barometer is disabled
+        return;
+    }
+
+    uint32_t now = AP_HAL::millis();
+    sim_alt += sitl->baro_drift * now / 1000;
+    sim_alt += sitl->baro_noise * rand_float();
+
+    // add baro glitch
+    sim_alt += sitl->baro_glitch;
+
+    // add delay
+    uint32_t best_time_delta = 200; // initialise large time representing buffer entry closest to current time - delay.
+    uint8_t best_index = 0; // initialise number representing the index of the entry in buffer closest to delay.
+
+    // storing data from sensor to buffer
+    if (now - last_store_time >= 10) { // store data every 10 ms.
+        last_store_time = now;
+        if (store_index > buffer_length-1) { // reset buffer index if index greater than size of buffer
+            store_index = 0;
+        }
+        buffer[store_index].data = sim_alt; // add data to current index
+        buffer[store_index].time = last_store_time; // add time_stamp to current index
+        store_index = store_index + 1; // increment index
+    }
+
+    // return delayed measurement
+    uint32_t delayed_time = now - sitl->baro_delay; // get time corresponding to delay
+
+    // find data corresponding to delayed time in buffer
+    for (uint8_t i=0; i<=buffer_length-1; i++) {
+        // find difference between delayed time and time stamp in buffer
+        uint32_t time_delta = abs(
+                (int32_t)(delayed_time - buffer[i].time));
+        // if this difference is smaller than last delta, store this time
+        if (time_delta < best_time_delta) {
+            best_index = i;
+            best_time_delta = time_delta;
+        }
+    }
+    if (best_time_delta < 200) { // only output stored state if < 200 msec retrieval error
+        sim_alt = buffer[best_index].data;
+    }
+
+    float sigma, delta, theta;
+    const float p0 = 101325;
+
+    AP_Baro::SimpleAtmosphere(sim_alt*0.001f, sigma, delta, theta);
+    float p = p0 * delta;
+    float T = 303.16f * theta - 273.16f; // Assume 30 degrees at sea level - converted to degrees Kelvin
+
+    _copy_to_frontend(instance, p, T);
+}
+
+#endif // CONFIG_HAL_BOARD

libraries/AP_Baro/AP_Baro_SITL.h

@@ -0,0 +1,31 @@
+#pragma once
+
+#include "AP_Baro_Backend.h"
+
+#if CONFIG_HAL_BOARD == HAL_BOARD_SITL
+#include <SITL/SITL.h>
+#include <AP_Math/vectorN.h>
+
+class AP_Baro_SITL : public AP_Baro_Backend {
+public:
+    AP_Baro_SITL(AP_Baro &);
+
+    void update() override;
+
+private:
+    uint8_t instance;
+    SITL::SITL *sitl;
+
+    // barometer delay buffer variables
+    struct readings_baro {
+        uint32_t time;
+        float data;
+    };
+    uint8_t store_index;
+    uint32_t last_store_time;
+    static const uint8_t buffer_length = 50;
+    VectorN<readings_baro,buffer_length> buffer;
+
+};
+#endif // CONFIG_HAL_BOARD
+