dark0707
/
px4-firmware
forked from Archive/PX4-Autopilot
1
0
Fork 0
Code Pull Requests Activity

yaw_est: force set gyro bias when at rest 

The gyro bias estimate from EFK2 is really good when at rest and should
be used by the yaw estimator to prevent heading drifts due to poor
heading observability.
This commit is contained in:
bresch 2024-03-05 16:23:38 +01:00 committed by Daniel Agar
parent 67e68783cf
commit 6f9a378247
2 changed files with 3 additions and 3 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

4
src/modules/ekf2/EKF/EKFGSF_yaw.h
View File

@ -67,11 +67,11 @@ public:
void setTrueAirspeed(float true_airspeed) { _true_airspeed = true_airspeed; }
void setGyroBias(const Vector3f &imu_gyro_bias)
void setGyroBias(const Vector3f &imu_gyro_bias, const bool force = false)
{
// Initialise to gyro bias estimate from main filter because there could be a large
// uncorrected rate gyro bias error about the gravity vector
if (!_ahrs_ekf_gsf_tilt_aligned || !_ekf_gsf_vel_fuse_started) {
if (!_ahrs_ekf_gsf_tilt_aligned || !_ekf_gsf_vel_fuse_started || force) {
// init gyro bias for each model
for (uint8_t model_index = 0; model_index < N_MODELS_EKFGSF; model_index++) {
_ahrs_ekf_gsf[model_index].gyro_bias = imu_gyro_bias;

2
src/modules/ekf2/EKF/gps_control.cpp
View File

@ -47,7 +47,7 @@ void Ekf::controlGpsFusion(const imuSample &imu_delayed)
}
if (!gyro_bias_inhibited()) {
_yawEstimator.setGyroBias(getGyroBias());
_yawEstimator.setGyroBias(getGyroBias(), _control_status.flags.vehicle_at_rest);
}
// run EKF-GSF yaw estimator once per imu_delayed update