fly316/PX4-Autopilot
7
0
Fork 0
mirror of https://gitee.com/mirrors_PX4/PX4-Autopilot.git synced 2026-10-16 22:03:33 +08:00
Code Issues Packages Projects Releases Wiki Activity

ekf2: move baro dynamic pressure compensation to aid_sources/barometer

Browse Source
This commit is contained in:
Daniel Agar
2024-05-02 17:40:58 -04:00
parent 4f0eb72fc9
commit 6b3b66619b
3 changed files with 41 additions and 40 deletions
Download Patch File Download Diff File Expand all files Collapse all files
src/modules/ekf2/EKF/aid_sources/barometer/baro_height_control.cpp
+36
View File
@@ -196,3 +196,39 @@ void Ekf::stopBaroHgtFusion()
_control_status.flags.baro_hgt = false;
}
}
#if defined(CONFIG_EKF2_BARO_COMPENSATION)
float Ekf::compensateBaroForDynamicPressure(const float baro_alt_uncompensated) const
{
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 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 airspeed_earth = velocity_earth - wind_velocity_earth;
const Vector3f airspeed_body = _state.quat_nominal.rotateVectorInverse(airspeed_earth);
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 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);
}
// otherwise return the uncorrected baro measurement
return baro_alt_uncompensated;
}
#endif // CONFIG_EKF2_BARO_COMPENSATION
src/modules/ekf2/EKF/ekf.h
+5 -4
View File
@@ -947,10 +947,6 @@ private:
// and a scalar innovation value
void fuse(const VectorState &K, float innovation);
#if defined(CONFIG_EKF2_BARO_COMPENSATION)
float compensateBaroForDynamicPressure(float baro_alt_uncompensated) const;
#endif // CONFIG_EKF2_BARO_COMPENSATION
// calculate the earth rotation vector from a given latitude
Vector3f calcEarthRateNED(float lat_rad) const;
@@ -1079,6 +1075,11 @@ private:
void stopBaroHgtFusion();
void updateGroundEffect();
# if defined(CONFIG_EKF2_BARO_COMPENSATION)
float compensateBaroForDynamicPressure(float baro_alt_uncompensated) const;
# endif // CONFIG_EKF2_BARO_COMPENSATION
#endif // CONFIG_EKF2_BAROMETER
#if defined(CONFIG_EKF2_GRAVITY_FUSION)
src/modules/ekf2/EKF/ekf_helper.cpp
-36
View File
@@ -56,42 +56,6 @@ bool Ekf::isHeightResetRequired() const
return (continuous_bad_accel_hgt || hgt_fusion_timeout);
}
#if defined(CONFIG_EKF2_BARO_COMPENSATION)
float Ekf::compensateBaroForDynamicPressure(const float baro_alt_uncompensated) const
{
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 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 airspeed_earth = velocity_earth - wind_velocity_earth;
const Vector3f airspeed_body = _state.quat_nominal.rotateVectorInverse(airspeed_earth);
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 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);
}
// otherwise return the uncorrected baro measurement
return baro_alt_uncompensated;
}
#endif // CONFIG_EKF2_BARO_COMPENSATION
// calculate the earth rotation vector
Vector3f Ekf::calcEarthRateNED(float lat_rad) const
{