diff --git a/src/modules/ekf2/EKF/baro_height_control.cpp b/src/modules/ekf2/EKF/baro_height_control.cpp index e34cb19744..8b77b8236b 100644 --- a/src/modules/ekf2/EKF/baro_height_control.cpp +++ b/src/modules/ekf2/EKF/baro_height_control.cpp @@ -51,7 +51,7 @@ void Ekf::controlBaroHeightFusion() if (_baro_buffer && _baro_buffer->pop_first_older_than(_imu_sample_delayed.time_us, &baro_sample)) { - const float measurement = baro_sample.hgt; + const float measurement = compensateBaroForDynamicPressure(baro_sample.hgt); const float measurement_var = sq(_params.baro_noise); const float innov_gate = fmaxf(_params.baro_innov_gate, 1.f); @@ -60,10 +60,10 @@ void Ekf::controlBaroHeightFusion() if (measurement_valid) { if (_baro_counter == 0) { - _baro_lpf.reset(baro_sample.hgt); + _baro_lpf.reset(measurement); } else { - _baro_lpf.update(baro_sample.hgt); + _baro_lpf.update(measurement); } if (_baro_counter <= _obs_buffer_length) { diff --git a/src/modules/ekf2/EKF/ekf.h b/src/modules/ekf2/EKF/ekf.h index 93a8bec694..0668f5a2ad 100644 --- a/src/modules/ekf2/EKF/ekf.h +++ b/src/modules/ekf2/EKF/ekf.h @@ -864,7 +864,7 @@ private: // and a scalar innovation value void fuse(const Vector24f &K, float innovation); - float compensateBaroForDynamicPressure(float baro_alt_uncompensated) const override; + float compensateBaroForDynamicPressure(float baro_alt_uncompensated) const; // calculate the earth rotation vector from a given latitude Vector3f calcEarthRateNED(float lat_rad) const; diff --git a/src/modules/ekf2/EKF/ekf_helper.cpp b/src/modules/ekf2/EKF/ekf_helper.cpp index 0b5b0fbde6..8eef82f6a4 100644 --- a/src/modules/ekf2/EKF/ekf_helper.cpp +++ b/src/modules/ekf2/EKF/ekf_helper.cpp @@ -511,31 +511,36 @@ void Ekf::constrainStates() float Ekf::compensateBaroForDynamicPressure(const float baro_alt_uncompensated) const { - // calculate static pressure error = Pmeas - Ptruth - // model position error sensitivity as a body fixed ellipse with a different scale in the positive and - // negative X and Y directions. Used to correct baro data for positional errors - const matrix::Dcmf R_to_body(_output_new.quat_nominal.inversed()); + 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 + // negative X and Y directions. Used to correct baro data for positional errors - // Calculate airspeed in body frame - const Vector3f velocity_earth = _output_new.vel - _vel_imu_rel_body_ned; + // Calculate airspeed in body frame + const Vector3f vel_imu_rel_body_ned = _R_to_earth * (_ang_rate_delayed_raw % _params.imu_pos_body); + const Vector3f velocity_earth = _state.vel - vel_imu_rel_body_ned; - const Vector3f wind_velocity_earth(_state.wind_vel(0), _state.wind_vel(1), 0.0f); + const Vector3f wind_velocity_earth(_state.wind_vel(0), _state.wind_vel(1), 0.0f); - const Vector3f airspeed_earth = velocity_earth - wind_velocity_earth; + const Vector3f airspeed_earth = velocity_earth - wind_velocity_earth; - const Vector3f airspeed_body = R_to_body * airspeed_earth; + const Vector3f airspeed_body = _state.quat_nominal.rotateVectorInverse(airspeed_earth); - const Vector3f K_pstatic_coef(airspeed_body(0) >= 0.0f ? _params.static_pressure_coef_xp : - _params.static_pressure_coef_xn, - airspeed_body(1) >= 0.0f ? _params.static_pressure_coef_yp : _params.static_pressure_coef_yn, - _params.static_pressure_coef_z); + const Vector3f K_pstatic_coef( + airspeed_body(0) >= 0.f ? _params.static_pressure_coef_xp : _params.static_pressure_coef_xn, + airspeed_body(1) >= 0.f ? _params.static_pressure_coef_yp : _params.static_pressure_coef_yn, + _params.static_pressure_coef_z); - const Vector3f airspeed_squared = matrix::min(airspeed_body.emult(airspeed_body), sq(_params.max_correction_airspeed)); + const Vector3f airspeed_squared = matrix::min(airspeed_body.emult(airspeed_body), sq(_params.max_correction_airspeed)); - const float pstatic_err = 0.5f * _air_density * (airspeed_squared.dot(K_pstatic_coef)); + const float pstatic_err = 0.5f * _air_density * (airspeed_squared.dot(K_pstatic_coef)); - // correct baro measurement using pressure error estimate and assuming sea level gravity - return baro_alt_uncompensated + pstatic_err / (_air_density * CONSTANTS_ONE_G); + // correct baro measurement using pressure error estimate and assuming sea level gravity + return baro_alt_uncompensated + pstatic_err / (_air_density * CONSTANTS_ONE_G); + } + + // otherwise return the uncorrected baro measurement + return baro_alt_uncompensated; } // calculate the earth rotation vector diff --git a/src/modules/ekf2/EKF/estimator_interface.cpp b/src/modules/ekf2/EKF/estimator_interface.cpp index fcb890c5c7..42b2380859 100644 --- a/src/modules/ekf2/EKF/estimator_interface.cpp +++ b/src/modules/ekf2/EKF/estimator_interface.cpp @@ -221,7 +221,7 @@ void EstimatorInterface::setBaroData(const baroSample &baro_sample) baroSample baro_sample_new; baro_sample_new.time_us = time_us; - baro_sample_new.hgt = compensateBaroForDynamicPressure(baro_sample.hgt); + baro_sample_new.hgt = baro_sample.hgt; _baro_buffer->push(baro_sample_new); _time_last_baro_buffer_push = _newest_high_rate_imu_sample.time_us; diff --git a/src/modules/ekf2/EKF/estimator_interface.h b/src/modules/ekf2/EKF/estimator_interface.h index 412b865c1a..69cb788907 100644 --- a/src/modules/ekf2/EKF/estimator_interface.h +++ b/src/modules/ekf2/EKF/estimator_interface.h @@ -396,8 +396,6 @@ protected: // this is the previous status of the filter control modes - used to detect mode transitions filter_control_status_u _control_status_prev{}; - virtual float compensateBaroForDynamicPressure(const float baro_alt_uncompensated) const = 0; - // these are used to record single frame events for external monitoring and should NOT be used for // state logic becasue they will be cleared externally after being read. warning_event_status_u _warning_events{};