mirror of https://github.com/ArduPilot/ardupilot
127 lines
3.8 KiB
C++
127 lines
3.8 KiB
C++
#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) :
|
|
_has_sample(false),
|
|
AP_Baro_Backend(baro)
|
|
{
|
|
sitl = (SITL::SITL *)AP_Param::find_object("SIM_");
|
|
if (sitl != nullptr) {
|
|
instance = _frontend.register_sensor();
|
|
hal.scheduler->register_timer_process(FUNCTOR_BIND(this, &AP_Baro_SITL::_timer, void));
|
|
}
|
|
}
|
|
|
|
// adjust for board temperature
|
|
void AP_Baro_SITL::temperature_adjustment(float &p, float &T)
|
|
{
|
|
float tsec = AP_HAL::millis() * 0.001;
|
|
const float T0 = sitl->temp_start;
|
|
const float T1 = sitl->temp_flight;
|
|
const float tconst = sitl->temp_tconst;
|
|
const float baro_factor = sitl->temp_baro_factor;
|
|
const float Tzero = 30; // start baro adjustment at 30C
|
|
T = T1 - (T1 - T0)*expf(-tsec / tconst);
|
|
if (baro_factor > 0) {
|
|
// this produces a pressure change with temperature that
|
|
// closely matches what has been observed with a ICM-20789
|
|
// barometer. A typical factor is 1.2.
|
|
p -= powf(MAX(T - Tzero, 0), baro_factor);
|
|
}
|
|
}
|
|
|
|
void AP_Baro_SITL::_timer()
|
|
{
|
|
|
|
// 100Hz
|
|
uint32_t now = AP_HAL::millis();
|
|
if ((now - _last_sample_time) < 10) {
|
|
return;
|
|
}
|
|
_last_sample_time = now;
|
|
|
|
float sim_alt = sitl->state.altitude;
|
|
|
|
if (sitl->baro_disable) {
|
|
// barometer is disabled
|
|
return;
|
|
}
|
|
|
|
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
|
|
|
|
temperature_adjustment(p, T);
|
|
|
|
_recent_press = p;
|
|
_recent_temp = T;
|
|
_has_sample = true;
|
|
}
|
|
|
|
// Read the sensor
|
|
void AP_Baro_SITL::update(void)
|
|
{
|
|
if (_sem->take_nonblocking()) {
|
|
if (!_has_sample) {
|
|
_sem->give();
|
|
return;
|
|
}
|
|
|
|
_copy_to_frontend(instance, _recent_press, _recent_temp);
|
|
_has_sample = false;
|
|
_sem->give();
|
|
}
|
|
}
|
|
|
|
#endif // CONFIG_HAL_BOARD
|