ahrs: init ahrs_aligner in ahrs_init instead of main

sw/airborne/firmwares/demo/demo_ahrs_actuators.c

@@ -48,7 +48,6 @@
 #include "state.h"
 #include "subsystems/imu.h"
 #include "subsystems/ahrs.h"
-#include "subsystems/ahrs/ahrs_aligner.h"
 
 #include "subsystems/commands.h"
 #include "subsystems/actuators.h"
@@ -94,9 +93,7 @@ static inline void main_init(void)
   actuators_init();
 
   modules_init();
-#if USE_AHRS_ALIGNER
-  ahrs_aligner_init();
-#endif
+
   ahrs_init();
 
   settings_init();

sw/airborne/firmwares/fixedwing/main_ap.c

@@ -50,9 +50,6 @@
 #if USE_AHRS
 #include "subsystems/ahrs.h"
 #endif
-#if USE_AHRS_ALIGNER
-#include "subsystems/ahrs/ahrs_aligner.h"
-#endif
 #if USE_BARO_BOARD
 #include "subsystems/sensors/baro.h"
 PRINT_CONFIG_MSG_VALUE("USE_BARO_BOARD is TRUE, reading onboard baro: ", BARO_BOARD)
@@ -165,10 +162,6 @@ void init_ap(void)
 
   /************* Sensors initialization ***************/
 
-#if USE_AHRS_ALIGNER
-  ahrs_aligner_init();
-#endif
-
 #if USE_AHRS
   ahrs_init();
 #endif

sw/airborne/firmwares/rotorcraft/main_ap.c

@@ -62,9 +62,6 @@ PRINT_CONFIG_MSG_VALUE("USE_BARO_BOARD is TRUE, reading onboard baro: ", BARO_BO
 #include "subsystems/radio_control.h"
 
 #include "subsystems/ahrs.h"
-#if USE_AHRS_ALIGNER
-#include "subsystems/ahrs/ahrs_aligner.h"
-#endif
 
 #include "state.h"
 
@@ -142,10 +139,6 @@ void main_init(void)
   baro_init();
 #endif
 
-#if USE_AHRS_ALIGNER
-  ahrs_aligner_init();
-#endif
-
 #if USE_AHRS
   ahrs_init();
 #endif

sw/airborne/subsystems/ahrs.c

@@ -27,6 +27,11 @@
 
 #include "subsystems/ahrs.h"
 
+#if USE_AHRS_ALIGNER
+#include "subsystems/ahrs/ahrs_aligner.h"
+#endif
+
+
 #ifndef PRIMARY_AHRS
 #error "PRIMARY_AHRS not set!"
 #else
@@ -79,6 +84,10 @@ void ahrs_init(void)
 
   // enable primary AHRS by default
   ahrs_switch(0);
+
+#if USE_AHRS_ALIGNER
+  ahrs_aligner_init();
+#endif
 }
 
 int ahrs_switch(uint8_t idx)

sw/airborne/subsystems/ahrs/ahrs_float_invariant.c

@@ -29,7 +29,6 @@
 #include "subsystems/ahrs/ahrs_float_invariant.h"
 
 #include "subsystems/ahrs/ahrs_int_utils.h"
-#include "subsystems/ahrs/ahrs_aligner.h"
 
 #include "generated/airframe.h"
 

sw/airborne/subsystems/ins/ins_float_invariant.c

@@ -29,7 +29,6 @@
 #include "subsystems/ins/ins_float_invariant.h"
 
 #include "subsystems/ahrs/ahrs_int_utils.h"
-#include "subsystems/ahrs/ahrs_aligner.h"
 
 #include "subsystems/ins.h"
 #include "subsystems/gps.h"

sw/airborne/test/ahrs/ahrs_on_synth.c

@@ -4,7 +4,6 @@
 
 #include "subsystems/imu.h"
 #include "subsystems/ahrs.h"
-#include "subsystems/ahrs/ahrs_aligner.h"
 #include "../simulator/nps/nps_random.h"
 
 #include "../pprz_algebra_print.h"
@@ -111,7 +110,6 @@ void aos_init(int traj_nb)
 
   imu_init();
   ahrs_init();
-  ahrs_aligner_init();
 
 #ifdef PERFECT_SENSORS
   RATES_ASSIGN(aos.gyro_bias,  RadOfDeg(0.), RadOfDeg(0.), RadOfDeg(0.));

sw/airborne/test/subsystems/test_ahrs.c

@@ -50,7 +50,6 @@
 
 #include "subsystems/imu.h"
 #include "subsystems/ahrs.h"
-#include "subsystems/ahrs/ahrs_aligner.h"
 
 static inline void main_init(void);
 static inline void main_periodic_task(void);
@@ -75,9 +74,6 @@ static inline void main_init(void)
   sys_time_register_timer((1. / PERIODIC_FREQUENCY), NULL);
 
   modules_init();
-#if USE_AHRS_ALIGNER
-  ahrs_aligner_init();
-#endif
   ahrs_init();
   pprz_dl_init();
   downlink_init();