From 1fe70b2d6a1029005c46ad2d0f4c1dd4485c3da2 Mon Sep 17 00:00:00 2001 From: RomanBapst Date: Wed, 26 Jun 2019 09:37:25 +0200 Subject: [PATCH] ekf2: fixed calculation of static pressure error Signed-off-by: RomanBapst --- src/modules/ekf2/ekf2_main.cpp | 4 ++-- 1 file changed, 2 insertions(+), 2 deletions(-) diff --git a/src/modules/ekf2/ekf2_main.cpp b/src/modules/ekf2/ekf2_main.cpp index 1b6ee6e290..5058acf99e 100644 --- a/src/modules/ekf2/ekf2_main.cpp +++ b/src/modules/ekf2/ekf2_main.cpp @@ -966,8 +966,8 @@ void Ekf2::run() const float y_v2 = fminf(vel_body_wind(1) * vel_body_wind(1), max_airspeed_sq); const float z_v2 = fminf(vel_body_wind(2) * vel_body_wind(2), max_airspeed_sq); - const float pstatic_err = 0.5f * airdata.rho * - (K_pstatic_coef_x * x_v2) + (K_pstatic_coef_y * y_v2) + (_param_ekf2_pcoef_z.get() * z_v2); + const float pstatic_err = 0.5f * airdata.rho * ( + K_pstatic_coef_x * x_v2 + K_pstatic_coef_y * y_v2 + _param_ekf2_pcoef_z.get() * z_v2); // correct baro measurement using pressure error estimate and assuming sea level gravity balt_data_avg += pstatic_err / (airdata.rho * CONSTANTS_ONE_G);