[ahrs] int_cmpl_quat: precompute some gains for efficiency

conf/settings/estimation/ahrs_int_cmpl_quat.xml

@@ -5,10 +5,10 @@
 
     <dl_settings NAME="AHRS">
        <dl_setting var="ahrs_impl.use_gravity_heuristic" min="0" step="1" max="1" module="subsystems/ahrs/ahrs_int_cmpl_quat" shortname="gravity_heuristic" values="OFF|ON" param="AHRS_GRAVITY_UPDATE_NORM_HEURISTIC"/>
-       <dl_setting var="ahrs_impl.accel_omega" min="0.02" step="0.02" max="0.2" module="subsystems/ahrs/ahrs_int_cmpl_quat" shortname="acc_omega" param="AHRS_ACCEL_OMEGA" unit="rad/s"/>
+       <dl_setting var="ahrs_impl.accel_omega" min="0.02" step="0.02" max="0.2" module="subsystems/ahrs/ahrs_int_cmpl_quat" shortname="acc_omega" param="AHRS_ACCEL_OMEGA" unit="rad/s" handler="SetAccelOmega"/>
-       <dl_setting var="ahrs_impl.accel_zeta" min="0.7" step="0.05" max="1.5" module="subsystems/ahrs/ahrs_int_cmpl_quat" shortname="acc_zeta" param="AHRS_ACCEL_ZETA"/>
+       <dl_setting var="ahrs_impl.accel_zeta" min="0.7" step="0.05" max="1.5" module="subsystems/ahrs/ahrs_int_cmpl_quat" shortname="acc_zeta" param="AHRS_ACCEL_ZETA" handler="SetAccelZeta"/>
-<dl_setting var="ahrs_impl.mag_omega" min="0.02" step="0.01" max="0.1" module="subsystems/ahrs/ahrs_int_cmpl_quat" shortname="mag_omega" param="AHRS_MAG_OMEGA" unit="rad/s"/>
+       <dl_setting var="ahrs_impl.mag_omega" min="0.02" step="0.01" max="0.1" module="subsystems/ahrs/ahrs_int_cmpl_quat" shortname="mag_omega" param="AHRS_MAG_OMEGA" unit="rad/s" handler="SetMagOmega"/>
-       <dl_setting var="ahrs_impl.mag_zeta" min="0.7" step="0.05" max="1.5" module="subsystems/ahrs/ahrs_int_cmpl_quat" shortname="mag_zeta" param="AHRS_MAG_ZETA"/>
+       <dl_setting var="ahrs_impl.mag_zeta" min="0.7" step="0.05" max="1.5" module="subsystems/ahrs/ahrs_int_cmpl_quat" shortname="mag_zeta" param="AHRS_MAG_ZETA" handler="SetMagZeta"/>
     </dl_settings>
 
   </dl_settings>

sw/airborne/subsystems/ahrs/ahrs_int_cmpl_quat.c

@@ -135,8 +135,10 @@ void ahrs_init(void) {
   /* set default filter cut-off frequency and damping */
   ahrs_impl.accel_omega = AHRS_ACCEL_OMEGA;
   ahrs_impl.accel_zeta = AHRS_ACCEL_ZETA;
+  ahrs_set_accel_gains();
   ahrs_impl.mag_omega = AHRS_MAG_OMEGA;
   ahrs_impl.mag_zeta = AHRS_MAG_ZETA;
+  ahrs_set_mag_gains();
 
   /* set default gravity heuristic */
   ahrs_impl.weight = 1.0;
@@ -147,11 +149,7 @@ void ahrs_init(void) {
   ahrs_impl.correct_gravity = FALSE;
 #endif
 
-#if AHRS_GRAVITY_UPDATE_NORM_HEURISTIC
+  ahrs_impl.use_gravity_heuristic = AHRS_GRAVITY_UPDATE_NORM_HEURISTIC;
-  ahrs_impl.use_gravity_heuristic = TRUE;
-#else
-  ahrs_impl.use_gravity_heuristic = FALSE;
-#endif
 
   VECT3_ASSIGN(ahrs_impl.mag_h, MAG_BFP_OF_REAL(AHRS_H_X),
                MAG_BFP_OF_REAL(AHRS_H_Y), MAG_BFP_OF_REAL(AHRS_H_Z));
@@ -175,8 +173,8 @@ void ahrs_align(void) {
   set_body_state_from_quat();
 
   /* Use low passed gyro value as initial bias */
-  RATES_COPY( ahrs_impl.gyro_bias, ahrs_aligner.lp_gyro);
+  RATES_COPY(ahrs_impl.gyro_bias, ahrs_aligner.lp_gyro);
-  RATES_COPY( ahrs_impl.high_rez_bias, ahrs_aligner.lp_gyro);
+  RATES_COPY(ahrs_impl.high_rez_bias, ahrs_aligner.lp_gyro);
   INT_RATES_LSHIFT(ahrs_impl.high_rez_bias, ahrs_impl.high_rez_bias, 28);
 
   ahrs.status = AHRS_RUNNING;
@@ -213,6 +211,28 @@ void ahrs_propagate(void) {
 }
 
 
+void ahrs_set_accel_gains(void) {
+  /* Complementary filter proportionnal gain
+   * Kp = 2 * omega * zeta * AHRS_PROPAGATE_FREQUENCY / AHRS_CORRECT_FREQUENCY
+   *
+   * accel_inv_kp = 1 / (Kp * FRAC_conversion / cross_product_gain)
+   * accel_inv_kp = 4096 * 9.81 / Kp
+   */
+  ahrs_impl.accel_inv_kp = 4096 * 9.81 /
+    (2 * ahrs_impl.accel_omega * ahrs_impl.accel_zeta *
+     AHRS_PROPAGATE_FREQUENCY / AHRS_PROPAGATE_FREQUENCY);
+
+  /* Complementary filter integral gain
+   * Ki = omega^2 / AHRS_CORRECT_FREQUENCY
+   *
+   * accel_inv_ki = 2^5 / (Ki * FRAC_conversion / cross_product_gain)
+   * accel_inv_ki = 2^5 / 2^16 * 9.81 / Ki
+   * accel_inv_ki = 9.81 / 2048 / Ki
+   */
+  ahrs_impl.accel_inv_ki = AHRS_CORRECT_FREQUENCY * 9.81 / 2048 /
+    (ahrs_impl.accel_omega * ahrs_impl.accel_omega);
+}
+
 void ahrs_update_accel(void) {
 
   // c2 = ltp z-axis in imu-frame
@@ -283,23 +303,22 @@ void ahrs_update_accel(void) {
   }
 
   /* Complementary filter proportional gain.
-   * Kp = 2 * zeta * omega * weight * AHRS_PROPAGATE_FREQUENCY / AHRS_CORRECT_FREQUENCY
+   * Kp = 2 * zeta * omega * AHRS_PROPAGATE_FREQUENCY / AHRS_CORRECT_FREQUENCY
    *
    * residual FRAC : ACCEL_FRAC + TRIG_FRAC = 10 + 14 = 24
    * rate_correction FRAC: RATE_FRAC = 12
    * FRAC_conversion: 2^12 / 2^24 = 1 / 4096
    * cross_product_gain : 9.81 m/s2
    *
-   * Kp = 1 / inv_rate_scale / FRAC_conversion * cross_product_gain
+   * accel_inv_kp = 1 / (Kp * FRAC_conversion / cross_product_gain)
-   * inv_rate_scale= 4096 * 9.81 / (2 * zeta * omega * weight *
+   * accel_inv_kp = 4096 * 9.81 / Kp
-   *                 AHRS_PROPAGATE_FREQUENCY / AHRS_CORRECT_FREQUENCY )
+   *
-   * inv_rate_scale= 2048 * 9.81 * AHRS_CORRECT_FREQUENCY /
+   * inv_rate_scale = 1 / (weight * Kp * FRAC_conversion / cross_product_gain)
-   *                 (omega * zeta * weight * AHRS_PROPAGATE_FREQUENCY )
+   * inv_rate_scale = inv_kp / weight
    */
-
+  int32_t inv_rate_scale = (int32_t)(ahrs_impl.accel_inv_kp / ahrs_impl.weight);
-  int32_t inv_rate_scale = 2048 * 9.81 * AHRS_CORRECT_FREQUENCY /
-    (ahrs_impl.weight * ahrs_impl.accel_omega * ahrs_impl.accel_zeta * AHRS_PROPAGATE_FREQUENCY);
   Bound(inv_rate_scale, 8192, 4194304);
+
   ahrs_impl.rate_correction.p -= residual.x / inv_rate_scale;
   ahrs_impl.rate_correction.q -= residual.y / inv_rate_scale;
   ahrs_impl.rate_correction.r -= residual.z / inv_rate_scale;
@@ -307,20 +326,22 @@ void ahrs_update_accel(void) {
 
   /* Complementary filter integral gain
    * Correct the gyro bias.
+   * Ki = omega^2 / AHRS_CORRECT_FREQUENCY
    *
-   * Ki = (omega*weight)^2/AHRS_CORRECT_FREQUENCY
    * residual FRAC = ACCEL_FRAC + TRIG_FRAC = 10 + 14 = 24
    * high_rez_bias = RATE_FRAC+28 = 40
    * FRAC_conversion: 2^40 / 2^24 = 2^16
    * cross_product_gain : 9.81 m/s2
    *
-   * Ki = 1 / FRAC_conversion / inv_bias_gain * cross_product_gain * 2^5
+   * accel_inv_ki = 2^5 / (Ki * FRAC_conversion / cross_product_gain)
-   * inv_bias_gain = 9.81 / 2^16 / (omega * omega * weight * weight / AHRS_CORRECT_FREQUENCY) * 2^5
+   * accel_inv_ki = 2^5 / 2^16 * 9.81 * Ki = 9.81 / 2^11 * Ki
-   * inv_bias_gain = 9.81 * AHRS_CORRECT_FREQUENCY / (omega * omega * weight * weight * 2048)
+   *
+   * inv_bias_gain = 2^5 / (weight^2 * Ki * FRAC_conversion / cross_product_gain)
+   * inv_bias_gain = accel_inv_ki / weight^2
    */
 
-  int32_t inv_bias_gain = 9.81 * AHRS_CORRECT_FREQUENCY /
+  int32_t inv_bias_gain = (int32_t)(ahrs_impl.accel_inv_ki /
-    (ahrs_impl.weight * ahrs_impl.weight * ahrs_impl.accel_omega * ahrs_impl.accel_omega * 2048);
+                                    (ahrs_impl.weight * ahrs_impl.weight));
   Bound(inv_bias_gain, 8, 65536)
   ahrs_impl.high_rez_bias.p += (residual.x / inv_bias_gain) << 5;
   ahrs_impl.high_rez_bias.q += (residual.y / inv_bias_gain) << 5;
@@ -339,6 +360,15 @@ void ahrs_update_mag(void) {
 #endif
 }
 
+void ahrs_set_mag_gains(void) {
+  /* Complementary filter proportionnal gain = 2*omega*zeta */
+  ahrs_impl.mag_kp = 2 * ahrs_impl.mag_zeta * ahrs_impl.mag_omega *
+    AHRS_PROPAGATE_FREQUENCY / AHRS_MAG_CORRECT_FREQUENCY;
+  /* Complementary filter integral gain = omega^2 */
+  ahrs_impl.mag_ki = ahrs_impl.mag_omega * ahrs_impl.mag_omega /
+    AHRS_MAG_CORRECT_FREQUENCY;
+}
+
 
 static inline void ahrs_update_mag_full(void) {
 
@@ -350,7 +380,6 @@ static inline void ahrs_update_mag_full(void) {
 
   struct Int32Vect3 residual;
   INT32_VECT3_CROSS_PRODUCT(residual, imu.mag, expected_imu);
-  //INT32_VECT3_RSHIFT(residual,residual,5);
 
   /* Complementary filter proportionnal gain.
    * Kp = 2 * mag_zeta * mag_omega * AHRS_PROPAGATE_FREQUENCY / AHRS_MAG_CORRECT_FREQUENCY
@@ -359,15 +388,11 @@ static inline void ahrs_update_mag_full(void) {
    * rate_correction FRAC: RATE_FRAC = 12
    * FRAC conversion: 2^12 / 2^22 = 1/1024
    *
-   * Kp = 1/ inv_rate_gain / FRAC_conversion
+   * inv_rate_gain = 1 / Kp / FRAC_conversion
-   * inv_rate_gain = 1 / (2 * mag_zeta * mag_omega * AHRS_PROPAGATE_FREQUENCY) *
+   * inv_rate_gain = 1024 / Kp
-   *                 AHRS_MAG_CORRECT_FREQUENCY / FRAC_conversion
-   * inv_rate_gain = 1024 * AHRS_MAG_CORRECT_FREQUENCY /
-   *                 (2 * mag_zeta * mag_omega * AHRS_PROPAGATE_FREQUENCY)
    */
 
-  const int32_t inv_rate_gain = 512 * AHRS_MAG_CORRECT_FREQUENCY /
+  const int32_t inv_rate_gain = (int32_t)(1024.0 / ahrs_impl.mag_kp);
-    (ahrs_impl.mag_zeta * ahrs_impl.mag_omega * AHRS_PROPAGATE_FREQUENCY);
 
   ahrs_impl.rate_correction.p += residual.x / inv_rate_gain;
   ahrs_impl.rate_correction.q += residual.y / inv_rate_gain;
@@ -376,19 +401,15 @@ static inline void ahrs_update_mag_full(void) {
   /* Complementary filter integral gain
    * Correct the gyro bias.
    * Ki = omega^2 / AHRS_MAG_CORRECT_FREQUENCY
+   *
    * residual FRAC: 2* MAG_FRAC = 22
    * high_rez_bias FRAC: RATE_FRAC+28 = 40
    * FRAC conversion: 2^40 / 2^22 = 2^18
    *
-   * Ki = bias_gain / FRAC_conversion = ahrs_impl.omega * ahrs_impl.omega /
+   * bias_gain = Ki * FRAC_conversion = Ki * 2^18
-   *      AHRS_MAG_CORRECT_FREQUENCY
-   * bias_gain = ahrs_impl.mag_omega * ahrs_impl.mag_omega /
-   *             AHRS_MAG_CORRECT_FREQUENCY * FRAC_conversion
-   * bias_gain = ahrs_impl.mag_omega * ahrs_impl.mag_omega * 2^18 /
-   *             AHRS_MAG_CORRECT_FREQUENCY
    */
-  const int32_t bias_gain = ahrs_impl.mag_omega * ahrs_impl.mag_omega * (1<<18) /
+  const int32_t bias_gain = (int32_t)(ahrs_impl.mag_ki * (1<<18));
-    AHRS_MAG_CORRECT_FREQUENCY;
+
   ahrs_impl.high_rez_bias.p -= residual.x * bias_gain;
   ahrs_impl.high_rez_bias.q -= residual.y * bias_gain;
   ahrs_impl.high_rez_bias.r -= residual.z * bias_gain;
@@ -427,21 +448,16 @@ static inline void ahrs_update_mag_2d(void) {
 
   /* Complementary filter proportionnal gain.
    * Kp = 2 * mag_zeta * mag_omega * AHRS_PROPAGATE_FREQUENCY / AHRS_MAG_CORRECT_FREQUENCY
+   *
    * residual_imu FRAC = residual_ltp FRAC = 17
    * rate_correction FRAC: RATE_FRAC = 12
    * FRAC conversion: 2^12 / 2^17 = 1/32
    *
-   * Kp = 1/ inv_rate_gain / FRAC_conversion
+   * inv_rate_gain = 1 / Kp / FRAC_conversion
-   * inv_rate_gain = 1 / (2 * mag_zeta * mag_omega * AHRS_PROPAGATE_FREQUENCY) *
+   * inv_rate_gain = 32 / Kp
-   *                 AHRS_MAG_CORRECT_FREQUENCY / FRAC_conversion
-   * inv_rate_gain = 32 * AHRS_MAG_CORRECT_FREQUENCY /
-   *                (2 * mag_zeta * mag_omega * AHRS_PROPAGATE_FREQUENCY)
-   * inv_rate_gain = 16 * AHRS_MAG_CORRECT_FREQUENCY /
-   *                 (mag_zeta * mag_omega * AHRS_PROPAGATE_FREQUENCY)
    */
+  int32_t inv_rate_gain = (int32_t)(32.0 / ahrs_impl.mag_kp);
 
-  int32_t inv_rate_gain = 16 * AHRS_MAG_CORRECT_FREQUENCY /
-    (ahrs_impl.mag_zeta * ahrs_impl.mag_omega * AHRS_PROPAGATE_FREQUENCY);
   ahrs_impl.rate_correction.p += (residual_imu.x / inv_rate_gain);
   ahrs_impl.rate_correction.q += (residual_imu.y / inv_rate_gain);
   ahrs_impl.rate_correction.r += (residual_imu.z / inv_rate_gain);
@@ -449,17 +465,15 @@ static inline void ahrs_update_mag_2d(void) {
   /* Complementary filter integral gain
    * Correct the gyro bias.
    * Ki = omega^2 / AHRS_MAG_CORRECT_FREQUENCY
+   *
    * residual_imu FRAC = residual_ltp FRAC = 17
    * high_rez_bias FRAC: RATE_FRAC+28 = 40
    * FRAC conversion: 2^40 / 2^17 = 2^23
    *
-   * Ki = bias_gain / FRAC_conversion = omega * omega / AHRS_MAG_CORRECT_FREQUENCY
+   * bias_gain = Ki * FRAC_conversion = Ki * 2^23
-   * bias_gain = mag_omega * mag_omega / AHRS_MAG_CORRECT_FREQUENCY * FRAC_conversion
-   * bias_gain = mag_omega * mag_omega / AHRS_MAG_CORRECT_FREQUENCY * 2^23
    */
+  int32_t bias_gain = (int32_t)(ahrs_impl.mag_ki * (1 << 23));
 
-  int32_t bias_gain = ahrs_impl.mag_omega * ahrs_impl.mag_omega * (1 << 23) /
-     AHRS_MAG_CORRECT_FREQUENCY;
   ahrs_impl.high_rez_bias.p -= (residual_imu.x * bias_gain);
   ahrs_impl.high_rez_bias.q -= (residual_imu.y * bias_gain);
   ahrs_impl.high_rez_bias.r -= (residual_imu.z * bias_gain);

sw/airborne/subsystems/ahrs/ahrs_int_cmpl_quat.h

@@ -1,5 +1,5 @@
 /*
- * Copyright (C) 2011 The Paparazzi Team
+ * Copyright (C) 2008-2013 The Paparazzi Team
  *
  * This file is part of paparazzi.
  *
@@ -46,16 +46,23 @@ struct AhrsIntCmplQuat {
   struct Int64Rates  high_rez_bias;
   struct Int32Quat   ltp_to_imu_quat;
   struct Int32Vect3  mag_h;
+
+  int32_t ltp_vel_norm;
+  bool_t ltp_vel_norm_valid;
+  bool_t heading_aligned;
+  float weight;
+  float accel_inv_kp;
+  float accel_inv_ki;
+  float mag_kp;
+  float mag_ki;
+
+  /* parameters/options that can be changed */
+  bool_t correct_gravity;
+  bool_t use_gravity_heuristic;
   float accel_omega;  ///< filter cut-off frequency for correcting the attitude from accels (pseudo-gravity measurement)
   float accel_zeta;   ///< filter damping for correcting the gyro-bias from accels (pseudo-gravity measurement)
   float mag_omega;    ///< filter cut-off frequency for correcting the attitude (heading) from magnetometer
   float mag_zeta;     ///< filter damping for correcting the gyro bias from magnetometer
-  float weight;
-  int32_t ltp_vel_norm;
-  bool_t ltp_vel_norm_valid;
-  bool_t correct_gravity;
-  bool_t use_gravity_heuristic;
-  bool_t heading_aligned;
 };
 
 extern struct AhrsIntCmplQuat ahrs_impl;
@@ -74,10 +81,37 @@ void ahrs_update_heading(int32_t heading);
  */
 void ahrs_realign_heading(int32_t heading);
 
+
+/// update pre-computed inv_kp and inv_ki gains from acc_omega and acc_zeta
+extern void ahrs_set_accel_gains(void);
+
+static inline void ahrs_int_cmpl_quat_SetAccelOmega(float omega) {
+  ahrs_impl.accel_omega = omega;
+  ahrs_set_accel_gains();
+}
+
+static inline void ahrs_int_cmpl_quat_SetAccelZeta(float zeta) {
+  ahrs_impl.accel_zeta = zeta;
+  ahrs_set_accel_gains();
+}
+
+/// update pre-computed kp and ki gains from mag_omega and mag_zeta
+extern void ahrs_set_mag_gains(void);
+
+static inline void ahrs_int_cmpl_quat_SetMagOmega(float omega) {
+  ahrs_impl.mag_omega = omega;
+  ahrs_set_mag_gains();
+}
+
+static inline void ahrs_int_cmpl_quat_SetMagZeta(float zeta) {
+  ahrs_impl.mag_zeta = zeta;
+  ahrs_set_mag_gains();
+}
+
+
 #ifdef AHRS_UPDATE_FW_ESTIMATOR
 extern float ins_roll_neutral;
 extern float ins_pitch_neutral;
 #endif
 
-
 #endif /* AHRS_INT_CMPL_QUAT_H */