[ahrs] replace DefaultAhrsImpl with PRIMARY_AHRS

paparazzi · Mar 10, 2015 · 54146b9 · 54146b9
1 parent d25486c
commit 54146b9
Show file tree

Hide file tree

Showing 21 changed files with 81 additions and 57 deletions.
diff --git a/conf/firmwares/subsystems/shared/ahrs_float_mlkf.makefile b/conf/firmwares/subsystems/shared/ahrs_float_mlkf.makefile
@@ -20,18 +20,20 @@ endif
 
 ifdef SECONDARY_AHRS
 ifneq (,$(findstring $(SECONDARY_AHRS), mlkf))
-AHRS_MLKF_SEC = ahrs_mlkf
-endif
-endif
-
-ifdef AHRS_MLKF_SEC
+# this is the secondary AHRS
 AHRS_MLKF_CFLAGS += -DAHRS_SECONDARY_TYPE_H=\"subsystems/ahrs/ahrs_float_mlkf_wrapper.h\"
 AHRS_MLKF_CFLAGS += -DSECONDARY_AHRS=ahrs_mlkf
 AHRS_MLKF_CFLAGS += -DAHRS_MLKF_OUTPUT_ENABLED=FALSE
 else
+# this is the primary AHRS
 AHRS_MLKF_CFLAGS += -DAHRS_TYPE_H=\"subsystems/ahrs/ahrs_float_mlkf_wrapper.h\"
 AHRS_MLKF_CFLAGS += -DPRIMARY_AHRS=ahrs_mlkf
 endif
+else
+# plain old single AHRS usage
+AHRS_MLKF_CFLAGS += -DAHRS_TYPE_H=\"subsystems/ahrs/ahrs_float_mlkf_wrapper.h\"
+endif
+
 AHRS_MLKF_SRCS   += subsystems/ahrs.c
 AHRS_MLKF_SRCS   += subsystems/ahrs/ahrs_float_mlkf.c
 AHRS_MLKF_SRCS   += subsystems/ahrs/ahrs_float_mlkf_wrapper.c

diff --git a/conf/firmwares/subsystems/shared/ahrs_int_cmpl_quat.makefile b/conf/firmwares/subsystems/shared/ahrs_int_cmpl_quat.makefile
@@ -8,41 +8,43 @@
 USE_MAGNETOMETER ?= 1
 AHRS_ALIGNER_LED ?= none
 
-AHRS_CFLAGS  = -DUSE_AHRS
-AHRS_CFLAGS += -DUSE_AHRS_ALIGNER
+AHRS_ICQ_CFLAGS  = -DUSE_AHRS
+AHRS_ICQ_CFLAGS += -DUSE_AHRS_ALIGNER
 
 ifeq (,$(findstring $(USE_MAGNETOMETER),0 FALSE))
-  AHRS_CFLAGS += -DUSE_MAGNETOMETER
+  AHRS_ICQ_CFLAGS += -DUSE_MAGNETOMETER
 endif
 
 ifneq ($(AHRS_ALIGNER_LED),none)
-  AHRS_CFLAGS += -DAHRS_ALIGNER_LED=$(AHRS_ALIGNER_LED)
+  AHRS_ICQ_CFLAGS += -DAHRS_ALIGNER_LED=$(AHRS_ALIGNER_LED)
 endif
 
 ifdef SECONDARY_AHRS
 ifneq (,$(findstring $(SECONDARY_AHRS),ahrs_icq int_cmpl_quat))
-AHRS_ICQ_SEC = ahrs_icq
-endif
+# this is the secondary AHRS
+AHRS_ICQ_CFLAGS += -DAHRS_SECONDARY_TYPE_H=\"subsystems/ahrs/ahrs_int_cmpl_quat_wrapper.h\"
+AHRS_ICQ_CFLAGS += -DSECONDARY_AHRS=ahrs_icq
+AHRS_ICQ_CFLAGS += -DAHRS_ICQ_OUTPUT_ENABLED=FALSE
+else
+# this is the primary AHRS
+AHRS_ICQ_CFLAGS += -DAHRS_TYPE_H=\"subsystems/ahrs/ahrs_int_cmpl_quat_wrapper.h\"
+AHRS_ICQ_CFLAGS += -DPRIMARY_AHRS=ahrs_icq
 endif
-
-ifdef AHRS_ICQ_SEC
-AHRS_CFLAGS += -DAHRS_SECONDARY_TYPE_H=\"subsystems/ahrs/ahrs_int_cmpl_quat_wrapper.h\"
-AHRS_CFLAGS += -DSECONDARY_AHRS=ahrs_icq
-AHRS_CFLAGS += -DAHRS_ICQ_OUTPUT_ENABLED=FALSE
 else
-AHRS_CFLAGS += -DAHRS_TYPE_H=\"subsystems/ahrs/ahrs_int_cmpl_quat_wrapper.h\"
-AHRS_CFLAGS += -DPRIMARY_AHRS=ahrs_icq
+# plain old single AHRS usage
+AHRS_ICQ_CFLAGS += -DAHRS_TYPE_H=\"subsystems/ahrs/ahrs_int_cmpl_quat_wrapper.h\"
 endif
-AHRS_SRCS   += subsystems/ahrs.c
-AHRS_SRCS   += subsystems/ahrs/ahrs_int_cmpl_quat.c
-AHRS_SRCS   += subsystems/ahrs/ahrs_int_cmpl_quat_wrapper.c
-AHRS_SRCS   += subsystems/ahrs/ahrs_aligner.c
 
-ap.CFLAGS += $(AHRS_CFLAGS)
-ap.srcs += $(AHRS_SRCS)
+AHRS_ICQ_SRCS   += subsystems/ahrs.c
+AHRS_ICQ_SRCS   += subsystems/ahrs/ahrs_int_cmpl_quat.c
+AHRS_ICQ_SRCS   += subsystems/ahrs/ahrs_int_cmpl_quat_wrapper.c
+AHRS_ICQ_SRCS   += subsystems/ahrs/ahrs_aligner.c
+
+ap.CFLAGS += $(AHRS_ICQ_CFLAGS)
+ap.srcs += $(AHRS_ICQ_SRCS)
 
-nps.CFLAGS += $(AHRS_CFLAGS)
-nps.srcs += $(AHRS_SRCS)
+nps.CFLAGS += $(AHRS_ICQ_CFLAGS)
+nps.srcs += $(AHRS_ICQ_SRCS)
 
-test_ahrs.CFLAGS += $(AHRS_CFLAGS)
-test_ahrs.srcs += $(AHRS_SRCS)
+test_ahrs.CFLAGS += $(AHRS_ICQ_CFLAGS)
+test_ahrs.srcs += $(AHRS_ICQ_SRCS)
diff --git a/sw/airborne/firmwares/fixedwing/main_ap.c b/sw/airborne/firmwares/fixedwing/main_ap.c
@@ -184,11 +184,7 @@ void init_ap(void)
 #endif
 
 #if USE_AHRS
-#if defined SITL && !USE_NPS
-  ahrs_sim_init();
-#else
   ahrs_init();
-#endif
 #endif
 
   ins_init();

diff --git a/sw/airborne/modules/ins/ahrs_chimu.h b/sw/airborne/modules/ins/ahrs_chimu.h
@@ -35,7 +35,9 @@ struct AhrsChimu {
 
 extern struct AhrsChimu ahrs_chimu;
 
-#define DefaultAhrsImpl ahrs_chimu
+#ifndef PRIMARY_AHRS
+#define PRIMARY_AHRS ahrs_chimu
+#endif
 
 extern void ahrs_chimu_register(void);
 extern void ahrs_chimu_init(void);

diff --git a/sw/airborne/modules/ins/ahrs_chimu_spi.c b/sw/airborne/modules/ins/ahrs_chimu_spi.c
@@ -49,7 +49,9 @@ static void gps_cb(uint8_t sender_id __attribute__((unused)),
 }
 void ahrs_chimu_register(void)
 {
-  ahrs_register_impl(ahrs_chimu_init);
+  ahrs_chimu_init();
+  /// @TODO: provide enable function
+  ahrs_register_impl(NULL);
   AbiBindMsgGPS(ABI_BROADCAST, &gps_ev, gps_cb);
 }
 

diff --git a/sw/airborne/modules/ins/ahrs_chimu_uart.c b/sw/airborne/modules/ins/ahrs_chimu_uart.c
@@ -34,7 +34,9 @@ struct AhrsChimu ahrs_chimu;
 
 void ahrs_chimu_register(void)
 {
-  ahrs_register_impl(ahrs_chimu_init);
+  ahrs_chimu_init();
+  /// @TODO: provide enable function
+  ahrs_register_impl(NULL);
 }
 
 void ahrs_chimu_init(void)

diff --git a/sw/airborne/subsystems/ahrs/ahrs_ardrone2.c b/sw/airborne/subsystems/ahrs/ahrs_ardrone2.c
@@ -70,7 +70,9 @@ static void send_ahrs_ad2(struct transport_tx *trans, struct link_device *dev)
 
 void ahrs_ardrone2_register(void)
 {
-  ahrs_register_impl(ahrs_ardrone2_init);
+  ahrs_ardrone2_init();
+  /// @TODO: provide enable function
+  ahrs_register_impl(NULL);
 }
 
 void ahrs_ardrone2_init(void)

diff --git a/sw/airborne/subsystems/ahrs/ahrs_ardrone2.h b/sw/airborne/subsystems/ahrs/ahrs_ardrone2.h
@@ -47,7 +47,9 @@ struct AhrsARDrone {
 };
 extern struct AhrsARDrone ahrs_ardrone2;
 
-#define DefaultAhrsImpl ahrs_ardrone2
+#ifndef PRIMARY_AHRS
+#define PRIMARY_AHRS ahrs_ardrone2
+#endif
 
 extern void ahrs_ardrone2_register(void);
 extern void ahrs_ardrone2_init(void);

diff --git a/sw/airborne/subsystems/ahrs/ahrs_float_cmpl_wrapper.c b/sw/airborne/subsystems/ahrs/ahrs_float_cmpl_wrapper.c
@@ -187,7 +187,9 @@ static void gps_cb(uint8_t sender_id __attribute__((unused)),
 
 void ahrs_fc_register(void)
 {
-  ahrs_register_impl(ahrs_fc_init);
+  ahrs_fc_init();
+  /// @TODO: provide enable function
+  ahrs_register_impl(NULL);
 
   /*
    * Subscribe to scaled IMU measurements and attach callbacks

diff --git a/sw/airborne/subsystems/ahrs/ahrs_float_cmpl_wrapper.h b/sw/airborne/subsystems/ahrs/ahrs_float_cmpl_wrapper.h
@@ -29,7 +29,9 @@
 
 #include "subsystems/ahrs/ahrs_float_cmpl.h"
 
-#define DefaultAhrsImpl ahrs_fc
+#ifndef PRIMARY_AHRS
+#define PRIMARY_AHRS ahrs_fc
+#endif
 
 extern void ahrs_fc_register(void);
 

diff --git a/sw/airborne/subsystems/ahrs/ahrs_float_dcm_wrapper.c b/sw/airborne/subsystems/ahrs/ahrs_float_dcm_wrapper.c
@@ -130,7 +130,9 @@ static void gps_cb(uint8_t sender_id __attribute__((unused)),
 
 void ahrs_dcm_register(void)
 {
-  ahrs_register_impl(ahrs_dcm_init);
+  ahrs_dcm_init();
+  /// @TODO: provide enable function
+  ahrs_register_impl(NULL);
 
   /*
    * Subscribe to scaled IMU measurements and attach callbacks

diff --git a/sw/airborne/subsystems/ahrs/ahrs_float_dcm_wrapper.h b/sw/airborne/subsystems/ahrs/ahrs_float_dcm_wrapper.h
@@ -29,7 +29,9 @@
 
 #include "subsystems/ahrs/ahrs_float_dcm.h"
 
-#define DefaultAhrsImpl ahrs_dcm
+#ifndef PRIMARY_AHRS
+#define PRIMARY_AHRS ahrs_dcm
+#endif
 
 extern void ahrs_dcm_register(void);
 

diff --git a/sw/airborne/subsystems/ahrs/ahrs_float_mlkf_wrapper.h b/sw/airborne/subsystems/ahrs/ahrs_float_mlkf_wrapper.h
@@ -29,7 +29,9 @@
 
 #include "subsystems/ahrs/ahrs_float_mlkf.h"
 
-#define DefaultAhrsImpl ahrs_mlkf
+#ifndef PRIMARY_AHRS
+#define PRIMARY_AHRS ahrs_mlkf
+#endif
 
 extern void ahrs_mlkf_register(void);
 

diff --git a/sw/airborne/subsystems/ahrs/ahrs_gx3.c b/sw/airborne/subsystems/ahrs/ahrs_gx3.c
@@ -339,7 +339,9 @@ void ahrs_gx3_init(void)
 
 void ahrs_gx3_register(void)
 {
-  ahrs_register_impl(ahrs_gx3_init);
+  ahrs_gx3_init();
+  /// @TODO: provide enable function
+  ahrs_register_impl(NULL);
 }
 
 

diff --git a/sw/airborne/subsystems/ahrs/ahrs_gx3.h b/sw/airborne/subsystems/ahrs/ahrs_gx3.h
@@ -83,7 +83,10 @@ struct AhrsGX3 {
 };
 
 extern struct AhrsGX3 ahrs_gx3;
-#define DefaultAhrsImpl ahrs_gx3
+
+#ifndef PRIMARY_AHRS
+#define PRIMARY_AHRS ahrs_gx3
+#endif
 
 extern void ahrs_gx3_init(void);
 extern void ahrs_gx3_align(void);

diff --git a/sw/airborne/subsystems/ahrs/ahrs_int_cmpl_euler_wrapper.c b/sw/airborne/subsystems/ahrs/ahrs_int_cmpl_euler_wrapper.c
@@ -148,7 +148,9 @@ static void body_to_imu_cb(uint8_t sender_id __attribute__((unused)),
 
 void ahrs_ice_register(void)
 {
-  ahrs_register_impl(ahrs_ice_init);
+  ahrs_ice_init();
+  /// @TODO: provide enable function
+  ahrs_register_impl(NULL);
 
   /*
    * Subscribe to scaled IMU measurements and attach callbacks

diff --git a/sw/airborne/subsystems/ahrs/ahrs_int_cmpl_euler_wrapper.h b/sw/airborne/subsystems/ahrs/ahrs_int_cmpl_euler_wrapper.h
@@ -29,7 +29,9 @@
 
 #include "subsystems/ahrs/ahrs_int_cmpl_euler.h"
 
-#define DefaultAhrsImpl ahrs_ice
+#ifndef PRIMARY_AHRS
+#define PRIMARY_AHRS ahrs_ice
+#endif
 
 extern void ahrs_ice_register(void);
 

diff --git a/sw/airborne/subsystems/ahrs/ahrs_sim.c b/sw/airborne/subsystems/ahrs/ahrs_sim.c
@@ -61,6 +61,8 @@ void update_ahrs_from_sim(void)
 }
 
 
-void ahrs_sim_init(void)
+void ahrs_sim_register(void)
 {
+  // dummy, simple ocaml sim only supports one basic fake AHRS anyway
+  ahrs_register_impl(NULL);
 }
diff --git a/sw/airborne/subsystems/ahrs/ahrs_sim.h b/sw/airborne/subsystems/ahrs/ahrs_sim.h
@@ -36,6 +36,8 @@ extern float ins_roll_neutral;
 extern float ins_pitch_neutral;
 
 extern void update_ahrs_from_sim(void);
-extern void ahrs_sim_init(void);
+extern void ahrs_sim_register(void);
+
+#define PRIMARY_AHRS ahrs_sim
 
 #endif /* AHRS_SIM_H */
diff --git a/sw/airborne/subsystems/ins/ins_float_invariant_wrapper.h b/sw/airborne/subsystems/ins/ins_float_invariant_wrapper.h
@@ -33,7 +33,4 @@
 
 extern void ins_float_invariant_register(void);
 
-// dummy ahrs stuff for now (only for DefaultAhrsImpl.is_aligned)
-#define DefaultAhrsImpl ins_float_inv
-
 #endif /* INS_FLOAT_INVARIANT_WRAPPER_H */
diff --git a/sw/airborne/test/subsystems/test_ahrs.c b/sw/airborne/test/subsystems/test_ahrs.c
@@ -53,10 +53,6 @@ static inline void on_gyro_event(void);
 static inline void on_accel_event(void);
 static inline void on_mag_event(void);
 
-#define __DefaultAhrsRegister(_x) _x ## _register()
-#define _DefaultAhrsRegister(_x) __DefaultAhrsRegister(_x)
-#define DefaultAhrsRegister() _DefaultAhrsRegister(DefaultAhrsImpl)
-
 int main(void)
 {
   main_init();
@@ -80,8 +76,6 @@ static inline void main_init(void)
   ahrs_init();
   downlink_init();
 
-  DefaultAhrsRegister();
-
   mcu_int_enable();
 }