ekf2: add kconfig option to enable/disable baro compensation

This commit is contained in:
Daniel Agar
2023-03-20 10:12:17 -04:00
parent fe0e3acf09
commit 8b2205810b
6 changed files with 16 additions and 0 deletions
+2
View File
@@ -438,6 +438,7 @@ struct parameters {
int32_t valid_timeout_max{5'000'000}; ///< amount of time spent inertial dead reckoning before the estimator reports the state estimates as invalid (uSec)
#if defined(CONFIG_EKF2_BARO_COMPENSATION)
// static barometer pressure position error coefficient along body axes
float static_pressure_coef_xp{0.0f}; // (-)
float static_pressure_coef_xn{0.0f}; // (-)
@@ -447,6 +448,7 @@ struct parameters {
// upper limit on airspeed used for correction (m/s**2)
float max_correction_airspeed {20.0f};
#endif // CONFIG_EKF2_BARO_COMPENSATION
#if defined(CONFIG_EKF2_DRAG_FUSION)
// multi-rotor drag specific force fusion
+2
View File
@@ -231,6 +231,7 @@ void Ekf::constrainStates()
float Ekf::compensateBaroForDynamicPressure(const float baro_alt_uncompensated) const
{
#if defined(CONFIG_EKF2_BARO_COMPENSATION)
if (_control_status.flags.wind && local_position_is_valid()) {
// calculate static pressure error = Pmeas - Ptruth
// model position error sensitivity as a body fixed ellipse with a different scale in the positive and
@@ -258,6 +259,7 @@ float Ekf::compensateBaroForDynamicPressure(const float baro_alt_uncompensated)
// correct baro measurement using pressure error estimate and assuming sea level gravity
return baro_alt_uncompensated + pstatic_err / (_air_density * CONSTANTS_ONE_G);
}
#endif // CONFIG_EKF2_BARO_COMPENSATION
// otherwise return the uncorrected baro measurement
return baro_alt_uncompensated;