[fixedwing] remove reduced propagation rate stuff

Also calculate dt if USE_AUTO_AHRS_FREQ or !defined(AHRS_PROPAGATE_FREQUENCY)
paparazzi · Sep 8, 2014 · a77dc5a · a77dc5a
1 parent f9478be
commit a77dc5a
Showing 1 changed file with 23 additions and 55 deletions.
diff --git a/sw/airborne/firmwares/fixedwing/main_ap.c b/sw/airborne/firmwares/fixedwing/main_ap.c
@@ -124,19 +124,12 @@ PRINT_CONFIG_VAR(MODULES_FREQUENCY)
 
 #if USE_AHRS && USE_IMU
 
-#ifndef AHRS_PROPAGATE_FREQUENCY
-#define AHRS_PROPAGATE_FREQUENCY PERIODIC_FREQUENCY
-#endif
-PRINT_CONFIG_VAR(AHRS_PROPAGATE_FREQUENCY)
-#ifndef AHRS_CORRECT_FREQUENCY
-#define AHRS_CORRECT_FREQUENCY PERIODIC_FREQUENCY
-#endif
-PRINT_CONFIG_VAR(AHRS_CORRECT_FREQUENCY)
-
+#ifdef AHRS_PROPAGATE_FREQUENCY
 #if (AHRS_PROPAGATE_FREQUENCY > PERIODIC_FREQUENCY)
 #warning "PERIODIC_FREQUENCY should be least equal or greater than AHRS_PROPAGATE_FREQUENCY"
 INFO_VALUE("it is recommended to configure in your airframe PERIODIC_FREQUENCY to at least ",AHRS_PROPAGATE_FREQUENCY)
 #endif
+#endif
 
 static inline void on_gyro_event( void );
 static inline void on_accel_event( void );
@@ -727,67 +720,43 @@ static inline void on_gps_solution( void ) {
 #if USE_AHRS
 #if USE_IMU
 static inline void on_accel_event( void ) {
+  ImuScaleAccel(imu);
+  if (ahrs.status != AHRS_UNINIT) {
+    ahrs_update_accel();
+  }
 }
 
 static inline void on_gyro_event( void ) {
+#if USE_AUTO_AHRS_FREQ || !defined(AHRS_PROPAGATE_FREQUENCY)
+PRINT_CONFIG_MSG("Calculating dt for AHRS/INS propagation.")
+  // timestamp when last callback was received
+  static float last_ts = 0.f;
+  // current timestamp
+  float now_ts = get_sys_time_float();
+  // dt between this and last callback
+  float dt = now_ts - last_ts;
+  last_ts = now_ts;
+#else
+PRINT_CONFIG_MSG("Using fixed AHRS_PROPAGATE_FREQUENCY for AHRS/INS propagation.")
+PRINT_CONFIG_VAR(AHRS_PROPAGATE_FREQUENCY)
+  const float dt = (1./AHRS_PROPAGATE_FREQUENCY);
+#endif
 
   ahrs_timeout_counter = 0;
 
+  ImuScaleGyro(imu);
+
 #if USE_AHRS_ALIGNER
   // 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;
   }
 #endif
 
-#if PERIODIC_FREQUENCY == 60
-  ImuScaleGyro(imu);
-  ImuScaleAccel(imu);
-
-  ahrs_propagate((1./PERIODIC_FREQUENCY));
-  ahrs_update_accel();
-
-#else //PERIODIC_FREQUENCY
-  static uint8_t _reduced_propagation_rate = 0;
-  static uint8_t _reduced_correction_rate = 0;
-  static struct Int32Vect3 acc_avg;
-  static struct Int32Rates gyr_avg;
-
-  RATES_ADD(gyr_avg, imu.gyro_unscaled);
-  VECT3_ADD(acc_avg, imu.accel_unscaled);
-
-  _reduced_propagation_rate++;
-  if (_reduced_propagation_rate < (PERIODIC_FREQUENCY / AHRS_PROPAGATE_FREQUENCY))
-  {
-    return;
-  }
-  else
-  {
-    _reduced_propagation_rate = 0;
-
-    RATES_SDIV(imu.gyro_unscaled, gyr_avg, (PERIODIC_FREQUENCY / AHRS_PROPAGATE_FREQUENCY) );
-    INT_RATES_ZERO(gyr_avg);
-
-    ImuScaleGyro(imu);
-
-    ahrs_propagate((1./AHRS_PROPAGATE_FREQUENCY));
-
-    _reduced_correction_rate++;
-    if (_reduced_correction_rate >= (AHRS_PROPAGATE_FREQUENCY / AHRS_CORRECT_FREQUENCY))
-    {
-      _reduced_correction_rate = 0;
-      VECT3_SDIV(imu.accel_unscaled, acc_avg, (PERIODIC_FREQUENCY / AHRS_CORRECT_FREQUENCY) );
-      INT_VECT3_ZERO(acc_avg);
-      ImuScaleAccel(imu);
-      ahrs_update_accel();
-    }
-  }
-#endif //PERIODIC_FREQUENCY
+  ahrs_propagate(dt);
 
 #if defined SITL && USE_NPS
   if (nps_bypass_ahrs) sim_overwrite_ahrs();
@@ -812,4 +781,3 @@ static inline void on_mag_event(void)
 #endif // USE_IMU
 
 #endif // USE_AHRS
-