mirror of
https://gitee.com/mirrors_PX4/PX4-Autopilot.git
synced 2026-10-23 06:33:33 +08:00
ekf2: add kconfig option to enable/disable baro compensation
This commit is contained in:
@@ -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
|
||||
|
||||
@@ -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;
|
||||
|
||||
Reference in New Issue
Block a user