[ahrs] global ahrs dispatcher functions

sw/airborne/firmwares/rotorcraft/main.c

@@ -109,6 +109,10 @@ INFO_VALUE("it is recommended to configure in your airframe PERIODIC_FREQUENCY t
 #endif
 #endif
 
+#define __DefaultAhrsRegister(_x) _x ## _register()
+#define _DefaultAhrsRegister(_x) __DefaultAhrsRegister(_x)
+#define DefaultAhrsRegister() _DefaultAhrsRegister(DefaultAhrsImpl)
+
 static inline void on_gyro_event( void );
 static inline void on_accel_event( void );
 static inline void on_gps_event( void );
@@ -161,6 +165,8 @@ STATIC_INLINE void main_init( void ) {
   ahrs_init();
   ins_init();
 
+  DefaultAhrsRegister();
+
 #if USE_GPS
   gps_init();
 #endif
@@ -324,8 +330,8 @@ PRINT_CONFIG_VAR(AHRS_CORRECT_FREQUENCY)
 
   imu_scale_accel(&imu);
 
-  if (ahrs.status != AHRS_UNINIT) {
-    ahrs.update_accel(&imu.accel, dt);
+  if (ahrs.status == AHRS_RUNNING) {
+    ahrs_update_accel(&imu.accel, dt);
   }
 }
 
@@ -347,16 +353,16 @@ PRINT_CONFIG_VAR(AHRS_PROPAGATE_FREQUENCY)
 
   imu_scale_gyro(&imu);
 
-  if (ahrs.status == AHRS_UNINIT) {
+  if (ahrs.status == AHRS_REGISTERED) {
     ahrs_aligner_run();
     if (ahrs_aligner.status == AHRS_ALIGNER_LOCKED) {
-      if (ahrs.align(&ahrs_aligner.lp_gyro, &ahrs_aligner.lp_accel, &ahrs_aligner.lp_mag)) {
+      if (ahrs_align(&ahrs_aligner.lp_gyro, &ahrs_aligner.lp_accel, &ahrs_aligner.lp_mag)) {
         ahrs.status = AHRS_RUNNING;
       }
     }
   }
   else {
-    ahrs.propagate(&imu.gyro_prev, dt);
+    ahrs_propagate(&imu.gyro_prev, dt);
 #ifdef SITL
     if (nps_bypass_ahrs) sim_overwrite_ahrs();
 #endif
@@ -368,7 +374,7 @@ PRINT_CONFIG_VAR(AHRS_PROPAGATE_FREQUENCY)
 }
 
 static inline void on_gps_event(void) {
-  ahrs.update_gps();
+  ahrs_update_gps();
   ins_update_gps();
 #ifdef USE_VEHICLE_INTERFACE
   if (gps.fix == GPS_FIX_3D)
@@ -396,7 +402,7 @@ PRINT_CONFIG_VAR(AHRS_MAG_CORRECT_FREQUENCY)
 #endif
 
   if (ahrs.status == AHRS_RUNNING) {
-    ahrs.update_mag(&imu.mag, dt);
+    ahrs_update_mag(&imu.mag, dt);
   }
 #endif // USE_MAGNETOMETER
 

sw/airborne/subsystems/ahrs.c

@@ -58,18 +58,39 @@ void ahrs_init(void)
   ahrs.update_gps = NULL;
 }
 
-#if 0
-#define WEAK __attribute__((weak))
-// weak functions, used if not explicitly provided by implementation
+bool_t ahrs_align(struct Int32Rates* lp_gyro, struct Int32Vect3* lp_accel,
+                struct Int32Vect3* lp_mag)
+{
+  if (ahrs.align != NULL) {
+    return ahrs.align(lp_gyro, lp_accel, lp_mag);
+  }
+  return FALSE;
+}
 
-void WEAK ahrs_propagate(struct Int32Rates* gyro __attribute__((unused)),
-                         float dt __attribute__((unused))) {}
+void ahrs_propagate(struct Int32Rates* gyro, float dt)
+{
+  if (ahrs.propagate != NULL && ahrs.status == AHRS_RUNNING) {
+    ahrs.propagate(gyro, dt);
+  }
+}
 
-void WEAK ahrs_update_accel(struct Int32Vect3* accel __attribute__((unused)),
-                            float dt __attribute__((unused))) {}
+void ahrs_update_accel(struct Int32Vect3* accel, float dt)
+{
+  if (ahrs.update_accel != NULL && ahrs.status == AHRS_RUNNING) {
+    ahrs.update_accel(accel, dt);
+  }
+}
 
-void WEAK ahrs_update_mag(struct Int32Vect3* mag __attribute__((unused)),
-                          float dt __attribute__((unused))) {}
+void ahrs_update_mag(struct Int32Vect3* mag, float dt)
+{
+  if (ahrs.update_mag != NULL && ahrs.status == AHRS_RUNNING) {
+    ahrs.update_mag(mag, dt);
+  }
+}
 
-void WEAK ahrs_update_gps(void) {}
-#endif
+void ahrs_update_gps(void)
+{
+  if (ahrs.update_gps != NULL && ahrs.status == AHRS_RUNNING) {
+    ahrs.update_gps();
+  }
+}

sw/airborne/subsystems/ahrs.h

@@ -75,45 +75,38 @@ extern void ahrs_register_impl(AhrsInit init, AhrsAlign align, AhrsPropagate pro
  */
 extern void ahrs_init(void);
 
-#if 0
-/** AHRS initialization. Called at startup.
- *  Needs to be implemented by each AHRS algorithm.
- */
-extern void ahrs_init(struct OrientationReps* body_to_imu);
-
 /** Aligns the AHRS. Called after ahrs_aligner has run to set initial attitude and biases.
- *  Needs to be implemented by each AHRS algorithm.
+ * Calls implementation if registered.
  * @return TRUE if ahrs is aligned
  */
 extern bool_t ahrs_align(struct Int32Rates* lp_gyro, struct Int32Vect3* lp_accel,
                          struct Int32Vect3* lp_mag);
 
 /** Propagation. Usually integrates the gyro rates to angles.
- *  Reads the global #imu data struct.
- *  Does nothing if not implemented by specific AHRS algorithm.
- *  @param dt time difference since last propagation in seconds
+ * Calls implementation if registered.
+ * @param gyro pointer to gyro measurement
+ * @param dt time difference since last propagation in seconds
  */
 extern void ahrs_propagate(struct Int32Rates* gyro, float dt);
 
 /** Update AHRS state with accerleration measurements.
- *  Reads the global #imu data struct.
- *  Does nothing if not implemented by specific AHRS algorithm.
- *  @param dt time difference since last update in seconds
+ * Calls implementation if registered.
+ * @param accel pointer to accelerometer measurement
+ * @param dt time difference since last update in seconds
  */
 extern void ahrs_update_accel(struct Int32Vect3* accel, float dt);
 
 /** Update AHRS state with magnetometer measurements.
- *  Reads the global #imu data struct.
- *  Does nothing if not implemented by specific AHRS algorithm.
- *  @param dt time difference since last update in seconds
+ * Calls implementation if registered.
+ * @param mag pointer to magnetometer measurement
+ * @param dt time difference since last update in seconds
  */
 extern void ahrs_update_mag(struct Int32Vect3* mag, float dt);
 
 /** Update AHRS state with GPS measurements.
+ *  Calls implementation if registered.
  *  Reads the global #gps data struct.
- *  Does nothing if not implemented by specific AHRS algorithm.
  */
 extern void ahrs_update_gps(void);
-#endif
 
 #endif /* AHRS_H */