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.

src/modules/ekf2/EKF/EKFGSF_yaw.h

@@ -67,11 +67,11 @@ class EKFGSF_yaw
 
 	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;

src/modules/ekf2/EKF/gps_control.cpp

@@ -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