|
|
|
@ -13,8 +13,8 @@ AP_Baro_SITL::AP_Baro_SITL(AP_Baro &baro) :
@@ -13,8 +13,8 @@ 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) { |
|
|
|
|
_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)); |
|
|
|
|
} |
|
|
|
@ -24,10 +24,10 @@ AP_Baro_SITL::AP_Baro_SITL(AP_Baro &baro) :
@@ -24,10 +24,10 @@ AP_Baro_SITL::AP_Baro_SITL(AP_Baro &baro) :
|
|
|
|
|
void AP_Baro_SITL::temperature_adjustment(float &p, float &T) |
|
|
|
|
{ |
|
|
|
|
const float tsec = AP_HAL::millis() * 0.001f; |
|
|
|
|
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 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.0f; // start baro adjustment at 30C
|
|
|
|
|
T = T1 - (T1 - T0) * expf(-tsec / tconst); |
|
|
|
|
if (is_positive(baro_factor)) { |
|
|
|
@ -48,42 +48,42 @@ void AP_Baro_SITL::_timer()
@@ -48,42 +48,42 @@ void AP_Baro_SITL::_timer()
|
|
|
|
|
} |
|
|
|
|
_last_sample_time = now; |
|
|
|
|
|
|
|
|
|
float sim_alt = sitl->state.altitude; |
|
|
|
|
float sim_alt = _sitl->state.altitude; |
|
|
|
|
|
|
|
|
|
if (sitl->baro_disable) { |
|
|
|
|
if (_sitl->baro_disable) { |
|
|
|
|
// barometer is disabled
|
|
|
|
|
return; |
|
|
|
|
} |
|
|
|
|
|
|
|
|
|
sim_alt += sitl->baro_drift * now / 1000.0f; |
|
|
|
|
sim_alt += sitl->baro_noise * rand_float(); |
|
|
|
|
sim_alt += _sitl->baro_drift * now / 1000.0f; |
|
|
|
|
sim_alt += _sitl->baro_noise * rand_float(); |
|
|
|
|
|
|
|
|
|
// add baro glitch
|
|
|
|
|
sim_alt += sitl->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; |
|
|
|
|
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
|
|
|
|
|
_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
|
|
|
|
|
const uint32_t delayed_time = now - sitl->baro_delay; // get time corresponding to delay
|
|
|
|
|
const 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++) { |
|
|
|
|
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)); |
|
|
|
|
(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; |
|
|
|
@ -91,7 +91,7 @@ void AP_Baro_SITL::_timer()
@@ -91,7 +91,7 @@ void AP_Baro_SITL::_timer()
|
|
|
|
|
} |
|
|
|
|
} |
|
|
|
|
if (best_time_delta < 200) { // only output stored state if < 200 msec retrieval error
|
|
|
|
|
sim_alt = buffer[best_index].data; |
|
|
|
|
sim_alt = _buffer[best_index].data; |
|
|
|
|
} |
|
|
|
|
|
|
|
|
|
float sigma, delta, theta; |
|
|
|
|