|
|
|
@ -23,14 +23,14 @@ AP_Baro_SITL::AP_Baro_SITL(AP_Baro &baro) :
@@ -23,14 +23,14 @@ AP_Baro_SITL::AP_Baro_SITL(AP_Baro &baro) :
|
|
|
|
|
// adjust for board temperature
|
|
|
|
|
void AP_Baro_SITL::temperature_adjustment(float &p, float &T) |
|
|
|
|
{ |
|
|
|
|
float tsec = AP_HAL::millis() * 0.001; |
|
|
|
|
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 Tzero = 30; // start baro adjustment at 30C
|
|
|
|
|
T = T1 - (T1 - T0)*expf(-tsec / tconst); |
|
|
|
|
if (baro_factor > 0) { |
|
|
|
|
const float Tzero = 30.0f; // start baro adjustment at 30C
|
|
|
|
|
T = T1 - (T1 - T0) * expf(-tsec / tconst); |
|
|
|
|
if (is_positive(baro_factor)) { |
|
|
|
|
// 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.
|
|
|
|
@ -42,7 +42,7 @@ void AP_Baro_SITL::_timer()
@@ -42,7 +42,7 @@ void AP_Baro_SITL::_timer()
|
|
|
|
|
{ |
|
|
|
|
|
|
|
|
|
// 100Hz
|
|
|
|
|
uint32_t now = AP_HAL::millis(); |
|
|
|
|
const uint32_t now = AP_HAL::millis(); |
|
|
|
|
if ((now - _last_sample_time) < 10) { |
|
|
|
|
return; |
|
|
|
|
} |
|
|
|
@ -55,7 +55,7 @@ void AP_Baro_SITL::_timer()
@@ -55,7 +55,7 @@ void AP_Baro_SITL::_timer()
|
|
|
|
|
return; |
|
|
|
|
} |
|
|
|
|
|
|
|
|
|
sim_alt += sitl->baro_drift * now / 1000; |
|
|
|
|
sim_alt += sitl->baro_drift * now / 1000.0f; |
|
|
|
|
sim_alt += sitl->baro_noise * rand_float(); |
|
|
|
|
|
|
|
|
|
// add baro glitch
|
|
|
|
@ -77,7 +77,7 @@ void AP_Baro_SITL::_timer()
@@ -77,7 +77,7 @@ void AP_Baro_SITL::_timer()
|
|
|
|
|
} |
|
|
|
|
|
|
|
|
|
// return delayed measurement
|
|
|
|
|
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++) { |
|
|
|
@ -95,9 +95,9 @@ void AP_Baro_SITL::_timer()
@@ -95,9 +95,9 @@ void AP_Baro_SITL::_timer()
|
|
|
|
|
} |
|
|
|
|
|
|
|
|
|
float sigma, delta, theta; |
|
|
|
|
const float p0 = 101325; |
|
|
|
|
const float p0 = 101325.0f; |
|
|
|
|
|
|
|
|
|
AP_Baro::SimpleAtmosphere(sim_alt*0.001f, sigma, delta, theta); |
|
|
|
|
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
|
|
|
|
|
|
|
|
|
|