YawEst: simplify AHRS accelerometer gain computation

Replace the multiple if-else statements by a generic equation.
- For a multicopter, the attenuation factor is 2 and symmetrical
- For a fixedwing, the attenuation factor is 1 if the acceleration is
positive and that centripetal correction is available and 2 otherwise.

Note that the function "sq" needs to be const in order to be used in a
const function.

EKF/EKFGSF_yaw.cpp

@@ -46,8 +46,11 @@ void EKFGSF_yaw::update(const imuSample& imu_sample,
 		return;
 	}
 
+	// calculate common values used by the AHRS complementary filter models
+	_ahrs_accel_norm = _ahrs_accel.norm();
+
 	// AHRS prediction cycle for each model - this always runs
-	ahrsCalcAccelGain();
+	_ahrs_accel_fusion_gain = ahrsCalcAccelGain();
 	for (uint8_t model_index = 0; model_index < N_MODELS_EKFGSF; model_index ++) {
 		predictEKF(model_index);
 	}
@@ -567,29 +570,24 @@ Dcmf EKFGSF_yaw::taitBryan312ToRotMat(const Vector3f &rot312)
 	return R;
 }
 
-void EKFGSF_yaw::ahrsCalcAccelGain()
+float EKFGSF_yaw::ahrsCalcAccelGain() const
 {
-	// calculate common values used by the AHRS complementary filter models
-	_ahrs_accel_norm = _ahrs_accel.norm();
-
 	// Calculate the acceleration fusion gain using a continuous function that is unity at 1g and zero
-	// at the min and mas g value. Allow for more acceleration when flying as a fixed wing vehicle using centripetal
+	// at the min and max g value. Allow for more acceleration when flying as a fixed wing vehicle using centripetal
 	// acceleration correction as higher and more sustained g will be experienced.
 	// Use a quadratic instead of linear function to prevent vibration around 1g reducing the tilt correction effectiveness.
-	const float accel_g = _ahrs_accel_norm / CONSTANTS_ONE_G;
+	// see https://www.desmos.com/calculator/dbqbxvnwfg
-	if (accel_g > 1.0f) {
+
-		if (_true_airspeed > FLT_EPSILON && accel_g < 2.0f) {
+	float attenuation = 2.f;
-			_ahrs_accel_fusion_gain = _tilt_gain * sq(2.0f - accel_g);
+	const bool centripetal_accel_compensation_enabled = (_true_airspeed > FLT_EPSILON);
-		} else if (accel_g < 1.5f) {
+
-			_ahrs_accel_fusion_gain = _tilt_gain * sq(3.0f - 2.0f * accel_g);
+	if (centripetal_accel_compensation_enabled
-		} else {
+	    && _ahrs_accel_norm > CONSTANTS_ONE_G) {
-			_ahrs_accel_fusion_gain = 0.0f;
+		attenuation = 1.f;
-		}
-	} else if (accel_g > 0.5f) {
-		_ahrs_accel_fusion_gain = _tilt_gain * sq(2.0f * accel_g - 1.0f);
-	} else {
-		_ahrs_accel_fusion_gain = 0.0f;
 	}
+
+	const float delta_accel_g = (_ahrs_accel_norm - CONSTANTS_ONE_G) / CONSTANTS_ONE_G;
+	return _tilt_gain * sq(1.f - math::min(attenuation * fabsf(delta_accel_g), 1.f));
 }
 
 Matrix3f EKFGSF_yaw::ahrsPredictRotMat(const Matrix3f &R, const Vector3f &g)

EKF/EKFGSF_yaw.h

@@ -81,7 +81,7 @@ private:
 	float _ahrs_accel_norm;			// length of _ahrs_accel specific force vector used by AHRS calculation (m/s/s)
 
 	// calculate the gain from gravity vector misalingment to tilt correction to be used by all AHRS filters
-	void ahrsCalcAccelGain();
+	float ahrsCalcAccelGain() const;
 
 	// update specified AHRS rotation matrix using IMU and optionally true airspeed data
 	void ahrsPredict(const uint8_t model_index);
@@ -121,7 +121,7 @@ private:
 	// return false if update failed
 	bool updateEKF(const uint8_t model_index);
 
-	inline float sq(float x) { return x * x; };
+	inline float sq(float x) const { return x * x; };
 
 	// converts Tait-Bryan 312 sequence of rotations from frame 1 to frame 2
 	// to the corresponding rotation matrix that rotates from frame 2 to frame 1