Mirror/ardupilot
5
0
Fork 0
mirror of https://github.com/ArduPilot/ardupilot synced 2025-02-23 00:04:02 -04:00
Code Issues Packages Projects Releases Wiki Activity

AP_Baro_SITL: use temp_board_offset and timeout

Browse Source
This commit is contained in:
Josh Henderson 2021-08-16 03:00:20 -04:00 committed by Andrew Tridgell
parent e8dcdd60c5
commit 3c9547caff
2 changed files with 13 additions and 7 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

2
libraries/AP_Baro/AP_Baro_HIL.cpp
View File

@ -66,7 +66,7 @@ void AP_Baro::SimpleUnderWaterAtmosphere(
// \f$T(D)\f$ Temperature underwater at given temperature // \f$T(D)\f$ Temperature underwater at given temperature
// \f$S\f$ Surface temperature at the surface // \f$S\f$ Surface temperature at the surface
// \f$T(D)\approx\frac{S}{1.8 \cdot 10^{-4} \cdot S \cdot T + 1}\f$ // \f$T(D)\approx\frac{S}{1.8 \cdot 10^{-4} \cdot S \cdot T + 1}\f$
const float seaTempSurface = 15.0f; // Celsius const float seaTempSurface = SSL_AIR_TEMPERATURE - C_TO_KELVIN; // Celsius
const float S = seaTempSurface * 0.338f; const float S = seaTempSurface * 0.338f;
theta = 1.0f / ((1.8e-4f) * S * (alt * 1e3f) + 1.0f); theta = 1.0f / ((1.8e-4f) * S * (alt * 1e3f) + 1.0f);
} }

18
libraries/AP_Baro/AP_Baro_SITL.cpp
View File

@ -25,16 +25,22 @@ AP_Baro_SITL::AP_Baro_SITL(AP_Baro &baro) :
} }
} }
// adjust for board temperature // adjust for board temperature warmup on start-up
void AP_Baro_SITL::temperature_adjustment(float &p, float &T) void AP_Baro_SITL::temperature_adjustment(float &p, float &T)
{ {
const float tsec = AP_HAL::millis() * 0.001f; const float tsec = AP_HAL::millis() * 0.001f;
const float T0 = _sitl->temp_start; const float T_sensor = T + _sitl->temp_board_offset;
const float T1 = _sitl->temp_flight;
const float tconst = _sitl->temp_tconst; const float tconst = _sitl->temp_tconst;
if (tsec < 23 * tconst) { // time which past the equation below equals T_sensor within approx. 1E-9
const float T0 = _sitl->temp_start;
T = T_sensor - (T_sensor - T0) * expf(-tsec / tconst);
}
else {
T = T_sensor;
}
const float baro_factor = _sitl->temp_baro_factor; const float baro_factor = _sitl->temp_baro_factor;
const float Tzero = 30.0f; // start baro adjustment at 30C const float Tzero = 30.0f; // start baro adjustment at 30C
T = T1 - (T1 - T0) * expf(-tsec / tconst);
if (is_positive(baro_factor)) { if (is_positive(baro_factor)) {
// this produces a pressure change with temperature that // this produces a pressure change with temperature that
// closely matches what has been observed with a ICM-20789 // closely matches what has been observed with a ICM-20789
@ -112,14 +118,14 @@ void AP_Baro_SITL::_timer()
AP_Baro::SimpleAtmosphere(sim_alt * 0.001f, sigma, delta, theta); AP_Baro::SimpleAtmosphere(sim_alt * 0.001f, sigma, delta, theta);
float p = SSL_AIR_PRESSURE * delta; float p = SSL_AIR_PRESSURE * delta;
float T = 303.16f * theta - C_TO_KELVIN; // Assume 30 degrees at sea level - converted to degrees Kelvin float T = SSL_AIR_TEMPERATURE * theta - C_TO_KELVIN;
temperature_adjustment(p, T); temperature_adjustment(p, T);
#else #else
float rho, delta, theta; float rho, delta, theta;
AP_Baro::SimpleUnderWaterAtmosphere(-sim_alt * 0.001f, rho, delta, theta); AP_Baro::SimpleUnderWaterAtmosphere(-sim_alt * 0.001f, rho, delta, theta);
float p = SSL_AIR_PRESSURE * delta; float p = SSL_AIR_PRESSURE * delta;
float T = 303.16f * theta - C_TO_KELVIN; // Assume 30 degrees at sea level - converted to degrees Kelvin float T = SSL_AIR_TEMPERATURE * theta - C_TO_KELVIN;
#endif #endif
// add in correction for wind effects // add in correction for wind effects