Separate Definition of Periodic/IMU freq, Propagation Freq and Correction Freq

conf/airframes/TU_Delft/MicrojetCDW.xml

@@ -184,9 +184,11 @@
 
   <firmware name="fixedwing">
     <target name="ap" 			board="tiny_1.1">
-      <param name="PERIODIC_FREQUENCY" value="960" />
+      <configure name="PERIODIC_FREQUENCY" value="960" /> <!-- IMU FREQ -->
-      <param name="AHRS_ALIGNER_LED" value="1" />
+      <configure name="AHRS_PROPAGATE_FREQUENCY" value="120" />
-      <param name="CPU_LED" value="2" />
+      <configure name="AHRS_CORRECT_FREQUENCY" value="60" />
+      <configure name="AHRS_ALIGNER_LED" value="1" />
+      <configure name="CPU_LED" value="2" />
     </target>
     <target name="sim" 			board="pc" />
 

conf/autopilot/subsystems/fixedwing/attitude_dcm.makefile

@@ -7,14 +7,27 @@ ifeq ($(ARCH), lpc21)
 
 ap.CFLAGS += -DUSE_AHRS
 
-ap.srcs += $(SRC_SUBSYSTEMS)/ahrs.c
+ap.srcs   += $(SRC_SUBSYSTEMS)/ahrs.c
-ap.srcs += $(SRC_SUBSYSTEMS)/ahrs/ahrs_aligner.c
+ap.srcs   += $(SRC_SUBSYSTEMS)/ahrs/ahrs_aligner.c
-ap.srcs += $(SRC_SUBSYSTEMS)/ahrs/ahrs_float_dcm.c
+ap.srcs   += $(SRC_SUBSYSTEMS)/ahrs/ahrs_float_dcm.c
 ap.CFLAGS += -DAHRS_ALIGNER_LED=$(AHRS_ALIGNER_LED)
 
-  ifdef CPU_LED
+ifdef CPU_LED
-    ap.CFLAGS += -DAHRS_CPU_LED=$(CPU_LED)
+  ap.CFLAGS += -DAHRS_CPU_LED=$(CPU_LED)
-  endif
+endif
+
+ifdef AHRS_PROPAGATE_FREQUENCY
+else
+  AHRS_PROPAGATE_FREQUENCY = 60
+endif
+
+ifdef AHRS_CORRECT_FREQUENCY
+else
+  AHRS_CORRECT_FREQUENCY = 60
+endif
+
+ap.CFLAGS += -DAHRS_PROPAGATE_FREQUENCY=$(AHRS_PROPAGATE_FREQUENCY)
+ap.CFLAGS += -DAHRS_CORRECT_FREQUENCY=$(AHRS_CORRECT_FREQUENCY)
 
 endif
 

conf/settings/tuning_ins.xml

@@ -32,6 +32,7 @@
     <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="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/arch/lpc21/subsystems/imu/imu_b2_arch.c

@@ -23,6 +23,7 @@
 
 #include "subsystems/imu.h"
 
+int imu_overrun_error = 0;
 volatile uint8_t imu_ssp_status;
 static void SSP_ISR(void) __attribute__((naked));
 #if 0

sw/airborne/arch/lpc21/subsystems/imu/imu_b2_arch.h

@@ -45,6 +45,8 @@
 #define IMU_SSP_STA_BUSY_MAX1168   1
 #define IMU_SSP_STA_BUSY_MS2100    2
 extern volatile uint8_t imu_ssp_status;
+extern int imu_overrun_error;
+
 
 
 

sw/airborne/firmwares/fixedwing/main_ap.c

@@ -381,15 +381,7 @@ void periodic_task_ap( void ) {
 
 #ifdef USE_IMU
   // Run at PERIODIC_FREQUENCY (60Hz if not defined)
-
-#ifdef AHRS_CPU_LED
-    LED_ON(AHRS_CPU_LED);
-#endif
-
   imu_periodic();
-#ifdef AHRS_CPU_LED
-    LED_OFF(AHRS_CPU_LED);
-#endif
 
 #endif // USE_IMU
 
@@ -669,42 +661,67 @@ void event_task_ap( void ) {
 
 #ifdef USE_AHRS
 static inline void on_gyro_accel_event( void ) {
-  static uint8_t _reduced_rate = 0;
+  static uint8_t _reduced_propagation_rate = 0;
+  static uint8_t _reduced_correction_rate = 0;
+  static struct Int32Vect3 acc_avg, gyr_avg;
 
 #ifdef AHRS_CPU_LED
     LED_ON(AHRS_CPU_LED);
 #endif
 
-  ImuScaleGyro(imu);
-  ImuScaleAccel(imu);
 
-  
+
-  if (ahrs.status == AHRS_UNINIT) {
+  gyr_avg.x += imu.gyro_unscaled.p;
-    ahrs_aligner_run();
+  gyr_avg.y += imu.gyro_unscaled.q;
-    if (ahrs_aligner.status == AHRS_ALIGNER_LOCKED)
+  gyr_avg.z += imu.gyro_unscaled.r;
-      ahrs_align();
+  INT32_VECT3_ADD(acc_avg, imu.accel_unscaled);
+
+  _reduced_propagation_rate++;
+  if (_reduced_propagation_rate < (PERIODIC_FREQUENCY / AHRS_PROPAGATE_FREQUENCY))
+  {
   }
-  else {
+  else
+  {
+    _reduced_propagation_rate = 0;
+
+    INT32_VECT3_SDIV(gyr_avg, gyr_avg, (PERIODIC_FREQUENCY / AHRS_PROPAGATE_FREQUENCY) );
+    imu.gyro_unscaled.p = gyr_avg.x;
+    imu.gyro_unscaled.q = gyr_avg.y;
+    imu.gyro_unscaled.r = gyr_avg.z; 
+    INT_VECT3_ZERO(gyr_avg);
+
+    ImuScaleGyro(imu);
+
+    
+    if (ahrs.status == AHRS_UNINIT) {
+      ImuScaleAccel(imu);
+      ahrs_aligner_run();
+      if (ahrs_aligner.status == AHRS_ALIGNER_LOCKED)
+        ahrs_align();
+    }
+    else {
 
-    //if (_reduced_rate % 4)
-    {
       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();
+      }
+
     }
-
-
-    _reduced_rate++;
-    if (_reduced_rate >= (PERIODIC_FREQUENCY / 60))
-    {
-      _reduced_rate = 0;
-      ahrs_update_accel();
-      ahrs_update_fw_estimator();
-    }
-
   }
   
 #ifdef AHRS_CPU_LED
     LED_OFF(AHRS_CPU_LED);
 #endif
+
 }
 
 static inline void on_mag_event(void) {

sw/airborne/subsystems/ahrs/ahrs_float_dcm.c

@@ -50,10 +50,7 @@ float ins_pitch_neutral = INS_PITCH_NEUTRAL_DEFAULT;
 // Positive yaw : clockwise
 
 // DCM Working variables
-#ifndef PERIODIC_FREQUENCY
+float G_Dt=1. / ((float) AHRS_PROPAGATE_FREQUENCY );
-#define PERIODIC_FREQUENCY 60
-#endif
-float G_Dt=1. / ((float) PERIODIC_FREQUENCY );
 
 struct FloatVect3 accel_float = {0,0,0};