diff --git a/libraries/AP_Baro/AP_Baro.cpp b/libraries/AP_Baro/AP_Baro.cpp index ef2552dc39..fef5add58a 100644 --- a/libraries/AP_Baro/AP_Baro.cpp +++ b/libraries/AP_Baro/AP_Baro.cpp @@ -27,6 +27,7 @@ #include #include +#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: diff --git a/libraries/AP_Baro/AP_Baro.h b/libraries/AP_Baro/AP_Baro.h index ede2a1458f..e555ac9912 100644 --- a/libraries/AP_Baro/AP_Baro.h +++ b/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); }; diff --git a/libraries/AP_Baro/AP_Baro_SITL.cpp b/libraries/AP_Baro/AP_Baro_SITL.cpp new file mode 100644 index 0000000000..4c12b9f8b4 --- /dev/null +++ b/libraries/AP_Baro/AP_Baro_SITL.cpp @@ -0,0 +1,81 @@ +#include + +#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 diff --git a/libraries/AP_Baro/AP_Baro_SITL.h b/libraries/AP_Baro/AP_Baro_SITL.h new file mode 100644 index 0000000000..e47f59be97 --- /dev/null +++ b/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 +#include + +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 buffer; + +}; +#endif // CONFIG_HAL_BOARD +