Faster Aligner

conf/settings/tuning_ins.xml

@@ -32,7 +32,9 @@
     <dl_settings NAME="control">
 
       <dl_settings NAME="ins">
-        <dl_setting MAX="100" MIN="0" STEP="1" VAR="imu_overrun_error" shortname="imu_overrun" module="subsystems/imu" />
+        <dl_setting MAX="100" MIN="0" STEP="1" VAR="imu_health" shortname="health" module="subsystems/ahrs" />
+        <dl_setting MAX="100" MIN="0" STEP="1" VAR="renorm_sqrt_count" shortname="err_norm" module="subsystems/ahrs" />
+        <dl_setting MAX="100" MIN="0" STEP="1" VAR="imu_overrun_error" shortname="err_adc" module="subsystems/imu" />
         <dl_setting MAX="0.3" MIN="-0.3" STEP="0.01" VAR="ins_roll_neutral" shortname="roll_neutral" module="subsystems/ahrs" param="INS_ROLL_NEUTRAL_DEFAULT" unit="rad"/>
         <dl_setting MAX="0.5" MIN="-0.3" STEP="0.01" VAR="ins_pitch_neutral" shortname="pitch_neutral" param="INS_PITCH_NEUTRAL_DEFAULT" unit="rad"/>
       </dl_settings>

sw/airborne/firmwares/fixedwing/main_ap.c

@@ -669,6 +669,15 @@ static inline void on_gyro_accel_event( void ) {
     LED_ON(AHRS_CPU_LED);
 #endif
 
+  // Run aligner on raw data as it also makes averages.
+  if (ahrs.status == AHRS_UNINIT) {
+    ImuScaleGyro(imu);
+    ImuScaleAccel(imu);
+    ahrs_aligner_run();
+    if (ahrs_aligner.status == AHRS_ALIGNER_LOCKED)
+      ahrs_align();
+    return;
+  }
 
 
   gyr_avg.x += imu.gyro_unscaled.p;
@@ -692,29 +701,18 @@ static inline void on_gyro_accel_event( void ) {
 
     ImuScaleGyro(imu);
 
-    
+    ahrs_propagate();
-    if (ahrs.status == AHRS_UNINIT) {
+
+    _reduced_correction_rate++;
+    if (_reduced_correction_rate >= (AHRS_PROPAGATE_FREQUENCY / AHRS_CORRECT_FREQUENCY))
+    {
+      _reduced_correction_rate = 0;
+      INT32_VECT3_SDIV(acc_avg, acc_avg, (PERIODIC_FREQUENCY / AHRS_CORRECT_FREQUENCY) );
+      INT32_VECT3_COPY(imu.accel_unscaled, acc_avg);
+      INT_VECT3_ZERO(acc_avg);
       ImuScaleAccel(imu);
-      ahrs_aligner_run();
+      ahrs_update_accel();
-      if (ahrs_aligner.status == AHRS_ALIGNER_LOCKED)
+      ahrs_update_fw_estimator();
-        ahrs_align();
-    }
-    else {
-
-      ahrs_propagate();
-
-      _reduced_correction_rate++;
-      if (_reduced_correction_rate >= (AHRS_PROPAGATE_FREQUENCY / AHRS_CORRECT_FREQUENCY))
-      {
-        _reduced_correction_rate = 0;
-        INT32_VECT3_SDIV(acc_avg, acc_avg, (PERIODIC_FREQUENCY / AHRS_CORRECT_FREQUENCY) );
-        INT32_VECT3_COPY(imu.accel_unscaled, acc_avg);
-        INT_VECT3_ZERO(acc_avg);
-        ImuScaleAccel(imu);
-        ahrs_update_accel();
-        ahrs_update_fw_estimator();
-      }
-
     }
   }
   

sw/airborne/subsystems/ahrs/ahrs_float_dcm.c

@@ -50,7 +50,7 @@ float ins_pitch_neutral = INS_PITCH_NEUTRAL_DEFAULT;
 // Positive yaw : clockwise
 
 // DCM Working variables
-float G_Dt=1. / ((float) AHRS_PROPAGATE_FREQUENCY );
+const float G_Dt = 1. / ((float) AHRS_PROPAGATE_FREQUENCY );
 
 struct FloatVect3 accel_float = {0,0,0};
 
@@ -72,6 +72,13 @@ void Normalize(void);
 void Drift_correction(void);
 void Matrix_update(void);
 
+#if PERFORMANCE_REPORTING == 1
+int renorm_sqrt_count = 0;
+int renorm_blowup_count = 0;
+float imu_health = 0.;
+#endif
+
+
 /**************************************************/
 
 void ahrs_update_fw_estimator( void )
@@ -96,6 +103,8 @@ void ahrs_update_fw_estimator( void )
   estimator_theta = ahrs_float.ltp_to_imu_euler.theta - ins_pitch_neutral;
   estimator_psi   = ahrs_float.ltp_to_imu_euler.psi;
 
+  estimator_p = Omega_Vector[0];
+
 //  estimator_p     = 
 }
 

sw/airborne/subsystems/ahrs/ahrs_float_dcm.h

@@ -59,7 +59,7 @@ void ahrs_update_fw_estimator(void);
 // Mode 2 = direct accelerometer -> euler
 
 
-#define PERFORMANCE_REPORTING 0
+#define PERFORMANCE_REPORTING 1
 #if PERFORMANCE_REPORTING == 1
 extern int renorm_sqrt_count;
 extern int renorm_blowup_count;