[ahrs] dual ahrs working with icq and mlkf

conf/firmwares/subsystems/shared/ahrs_float_mlkf.makefile

@@ -5,30 +5,43 @@
 USE_MAGNETOMETER ?= 1
 AHRS_ALIGNER_LED ?= none
 
-AHRS_CFLAGS  = -DUSE_AHRS
-AHRS_CFLAGS += -DUSE_AHRS_ALIGNER
+AHRS_MLKF_CFLAGS  = -DUSE_AHRS
+AHRS_MLKF_CFLAGS += -DUSE_AHRS_ALIGNER
 
 ifeq (,$(findstring $(USE_MAGNETOMETER),0 FALSE))
-  AHRS_CFLAGS += -DUSE_MAGNETOMETER
+  AHRS_MLKF_CFLAGS += -DUSE_MAGNETOMETER
 else
 $(error ahrs_float_mlkf needs a magnetometer)
 endif
 
 ifneq ($(AHRS_ALIGNER_LED),none)
-  AHRS_CFLAGS += -DAHRS_ALIGNER_LED=$(AHRS_ALIGNER_LED)
+  AHRS_MLKF_CFLAGS += -DAHRS_ALIGNER_LED=$(AHRS_ALIGNER_LED)
 endif
 
-AHRS_CFLAGS += -DAHRS_TYPE_H=\"subsystems/ahrs/ahrs_float_mlkf_wrapper.h\"
-AHRS_SRCS   += subsystems/ahrs.c
-AHRS_SRCS   += subsystems/ahrs/ahrs_float_mlkf.c
-AHRS_SRCS   += subsystems/ahrs/ahrs_float_mlkf_wrapper.c
-AHRS_SRCS   += subsystems/ahrs/ahrs_aligner.c
+ifdef SECONDARY_AHRS
+ifneq (,$(findstring $(SECONDARY_AHRS), mlkf))
+AHRS_MLKF_SEC = ahrs_mlkf
+endif
+endif
 
-ap.CFLAGS += $(AHRS_CFLAGS)
-ap.srcs += $(AHRS_SRCS)
+ifdef AHRS_MLKF_SEC
+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
+AHRS_MLKF_CFLAGS += -DAHRS_TYPE_H=\"subsystems/ahrs/ahrs_float_mlkf_wrapper.h\"
+AHRS_MLKF_CFLAGS += -DPRIMARY_AHRS=ahrs_mlkf
+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
+AHRS_MLKF_SRCS   += subsystems/ahrs/ahrs_aligner.c
 
-nps.CFLAGS += $(AHRS_CFLAGS)
-nps.srcs += $(AHRS_SRCS)
+ap.CFLAGS += $(AHRS_MLKF_CFLAGS)
+ap.srcs += $(AHRS_MLKF_SRCS)
 
-test_ahrs.CFLAGS += $(AHRS_CFLAGS)
-test_ahrs.srcs += $(AHRS_SRCS)
+nps.CFLAGS += $(AHRS_MLKF_CFLAGS)
+nps.srcs += $(AHRS_MLKF_SRCS)
+
+test_ahrs.CFLAGS += $(AHRS_MLKF_CFLAGS)
+test_ahrs.srcs += $(AHRS_MLKF_SRCS)

conf/firmwares/subsystems/shared/ahrs_int_cmpl_quat.makefile

@@ -19,7 +19,20 @@ ifneq ($(AHRS_ALIGNER_LED),none)
   AHRS_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
+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
+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

conf/settings/estimation/ahrs_secondary.xml

@@ -0,0 +1,11 @@
+<!DOCTYPE settings SYSTEM "../settings.dtd">
+
+<settings>
+  <dl_settings>
+
+    <dl_settings NAME="AHRS">
+       <dl_setting var="ahrs_output_idx" min="0" step="1" max="1" module="subsystems/ahrs" shortname="ahrs_idx" handler="switch"/>
+    </dl_settings>
+
+  </dl_settings>
+</settings>

sw/airborne/subsystems/ahrs.c

@@ -27,35 +27,71 @@
 
 #include "subsystems/ahrs.h"
 
-#ifndef DefaultAhrsImpl
-#warning "DefaultAhrsImpl not set!"
+#ifndef PRIMARY_AHRS
+#error "PRIMARY_AHRS not set!"
 #else
-PRINT_CONFIG_VAR(DefaultAhrsImpl)
+PRINT_CONFIG_VAR(PRIMARY_AHRS)
 #endif
 
-#define __DefaultAhrsRegister(_x) _x ## _register()
-#define _DefaultAhrsRegister(_x) __DefaultAhrsRegister(_x)
-#define DefaultAhrsRegister() _DefaultAhrsRegister(DefaultAhrsImpl)
+#ifdef SECONDARY_AHRS
+PRINT_CONFIG_VAR(SECONDARY_AHRS)
+#endif
 
-/** Attitude and Heading Reference System state */
-struct Ahrs {
-  /* function pointers to actual implementation, set by ahrs_register_impl */
-  AhrsInit init;
+#define __RegisterAhrs(_x) _x ## _register()
+#define _RegisterAhrs(_x) __RegisterAhrs(_x)
+#define RegisterAhrs(_x) _RegisterAhrs(_x)
+
+/** maximum number of AHRS implementations that can register */
+#ifndef AHRS_NB_IMPL
+#define AHRS_NB_IMPL 2
+#endif
+
+/** references a registered AHRS implementation */
+struct AhrsImpl {
+  AhrsEnableOutput enable;
 };
 
-struct Ahrs ahrs;
+struct AhrsImpl ahrs_impls[AHRS_NB_IMPL];
+uint8_t ahrs_output_idx;
 
-void ahrs_register_impl(AhrsInit init)
+void ahrs_register_impl(AhrsEnableOutput enable)
 {
-  ahrs.init = init;
-
-  ahrs.init();
+  int i;
+  for (i=0; i < AHRS_NB_IMPL; i++) {
+    if (ahrs_impls[i].enable == NULL) {
+      ahrs_impls[i].enable = enable;
+      break;
+    }
+  }
 }
 
 void ahrs_init(void)
 {
-  ahrs.init = NULL;
-#ifdef DefaultAhrsImpl
-  DefaultAhrsRegister();
+  int i;
+  for (i=0; i < AHRS_NB_IMPL; i++) {
+    ahrs_impls[i].enable = NULL;
+  }
+  ahrs_output_idx = 0;
+
+  RegisterAhrs(PRIMARY_AHRS);
+#ifdef SECONDARY_AHRS
+  RegisterAhrs(SECONDARY_AHRS);
 #endif
 }
+
+int ahrs_switch(uint8_t idx)
+{
+  if (idx >= AHRS_NB_IMPL) { return -1; }
+  if (ahrs_impls[idx].enable == NULL) { return -1; }
+  /* first disable other AHRS output */
+  int i;
+  for (i=0; i < AHRS_NB_IMPL; i++) {
+    if (ahrs_impls[i].enable != NULL) {
+      ahrs_impls[i].enable(FALSE);
+    }
+  }
+  /* enable requested AHRS */
+  ahrs_impls[idx].enable(TRUE);
+  ahrs_output_idx = idx;
+  return ahrs_output_idx;
+}

sw/airborne/subsystems/ahrs.h

@@ -29,18 +29,37 @@
 
 #include "std.h"
 
-/* underlying includes (needed for parameters) */
+/* include actual (primary) implementation header */
 #ifdef AHRS_TYPE_H
 #include AHRS_TYPE_H
 #endif
 
-typedef void (*AhrsInit)(void);
+/* include secondary implementation header */
+#ifdef AHRS_SECONDARY_TYPE_H
+#include AHRS_SECONDARY_TYPE_H
+#endif
 
-extern void ahrs_register_impl(AhrsInit init);
+typedef bool_t (*AhrsEnableOutput)(bool_t);
+
+/* for settings when using secondary AHRS */
+extern uint8_t ahrs_output_idx;
+
+/**
+ * Register an AHRS implementation.
+ * Adds it to an internal list.
+ * @param enable pointer to function to enable/disable the output of registering AHRS
+ */
+extern void ahrs_register_impl(AhrsEnableOutput enable);
 
 /** AHRS initialization. Called at startup.
- * Initialized the global AHRS struct.
+ * Registers/initializes the default AHRS.
  */
 extern void ahrs_init(void);
 
+/**
+ * Switch to the output of another AHRS impl.
+ * @param idx index of the AHRS impl (0 = PRIMARY_AHRS, 1 = SECONDARY_AHRS).
+ */
+extern int ahrs_switch(uint8_t idx);
+
 #endif /* AHRS_H */

sw/airborne/subsystems/ahrs/ahrs_float_mlkf.c

@@ -42,6 +42,11 @@
 
 //#include <stdio.h>
 
+#ifndef AHRS_MLKF_OUTPUT_ENABLED
+#define AHRS_MLKF_OUTPUT_ENABLED TRUE
+#endif
+PRINT_CONFIG_VAR(AHRS_MLKF_OUTPUT_ENABLED)
+
 #ifndef AHRS_MAG_NOISE_X
 #define AHRS_MAG_NOISE_X 0.2
 #define AHRS_MAG_NOISE_Y 0.2
@@ -66,6 +71,7 @@ void ahrs_mlkf_init(void)
 {
 
   ahrs_mlkf.is_aligned = FALSE;
+  ahrs_mlkf.output_enabled = AHRS_MLKF_OUTPUT_ENABLED;
 
   /* init ltp_to_imu quaternion as zero/identity rotation */
   float_quat_identity(&ahrs_mlkf.ltp_to_imu_quat);
@@ -116,7 +122,9 @@ bool_t ahrs_mlkf_align(struct Int32Rates *lp_gyro, struct Int32Vect3 *lp_accel,
   ahrs_float_get_quat_from_accel_mag(&ahrs_mlkf.ltp_to_imu_quat, lp_accel, lp_mag);
 
   /* set initial body orientation */
-  set_body_state_from_quat();
+  if (ahrs_mlkf.output_enabled) {
+    set_body_state_from_quat();
+  }
 
   /* used averaged gyro as initial value for bias */
   struct Int32Rates bias0;
@@ -132,7 +140,9 @@ void ahrs_mlkf_propagate(struct Int32Rates *gyro, float dt)
 {
   propagate_ref(gyro, dt);
   propagate_state(dt);
-  set_body_state_from_quat();
+  if (ahrs_mlkf.output_enabled) {
+    set_body_state_from_quat();
+  }
 }
 
 void ahrs_mlkf_update_accel(struct Int32Vect3 *accel)
@@ -147,6 +157,9 @@ void ahrs_mlkf_update_accel(struct Int32Vect3 *accel)
   struct FloatVect3 g_noise = {1. + dn, 1. + dn, 1. + dn};
   update_state(&earth_g, &imu_g, &g_noise);
   reset_state();
+  if (ahrs_mlkf.output_enabled) {
+    set_body_state_from_quat();
+  }
 }
 
 void ahrs_mlkf_update_mag(struct Int32Vect3 *mag)
@@ -156,6 +169,9 @@ void ahrs_mlkf_update_mag(struct Int32Vect3 *mag)
 #else
   ahrs_mlkf_update_mag_2d(mag);
 #endif
+  if (ahrs_mlkf.output_enabled) {
+    set_body_state_from_quat();
+  }
 }
 
 void ahrs_mlkf_update_mag_2d(struct Int32Vect3 *mag)

sw/airborne/subsystems/ahrs/ahrs_float_mlkf.h

@@ -59,6 +59,7 @@ struct AhrsMlkf {
 
   enum AhrsMlkfStatus status;
   bool_t is_aligned;
+  bool_t output_enabled; ///< if TRUE with push the estimation results to the state interface
 };
 
 extern struct AhrsMlkf ahrs_mlkf;

sw/airborne/subsystems/ahrs/ahrs_float_mlkf_wrapper.c

@@ -135,9 +135,16 @@ static void geo_mag_cb(uint8_t sender_id __attribute__((unused)), struct FloatVe
   memcpy(&ahrs_mlkf.mag_h, h, sizeof(struct FloatVect3));
 }
 
+static bool_t ahrs_mlkf_enable_output(bool_t enable)
+{
+  ahrs_mlkf.output_enabled = enable;
+  return ahrs_mlkf.output_enabled;
+}
+
 void ahrs_mlkf_register(void)
 {
-  ahrs_register_impl(ahrs_mlkf_init);
+  ahrs_mlkf_init();
+  ahrs_register_impl(ahrs_mlkf_enable_output);
 
   /*
    * Subscribe to scaled IMU measurements and attach callbacks

sw/airborne/subsystems/ahrs/ahrs_int_cmpl_quat.c

@@ -60,6 +60,10 @@ PRINT_CONFIG_MSG("LOW PASS FILTER ON GYRO RATES")
 #error "AHRS_USE_GPS_HEADING needs USE_GPS to be TRUE"
 #endif
 
+#ifndef AHRS_ICQ_OUTPUT_ENABLED
+#define AHRS_ICQ_OUTPUT_ENABLED TRUE
+#endif
+PRINT_CONFIG_VAR(AHRS_ICQ_OUTPUT_ENABLED)
 
 /*
  * default gains for correcting attitude and bias from accel/mag
@@ -112,6 +116,7 @@ void ahrs_icq_init(void)
 {
 
   ahrs_icq.status = AHRS_ICQ_UNINIT;
+  ahrs_icq.output_enabled = AHRS_ICQ_OUTPUT_ENABLED;
   ahrs_icq.is_aligned = FALSE;
 
   ahrs_icq.ltp_vel_norm_valid = FALSE;
@@ -168,7 +173,9 @@ bool_t ahrs_icq_align(struct Int32Rates *lp_gyro, struct Int32Vect3 *lp_accel,
   lp_mag = lp_mag;
 #endif
 
-  set_body_state_from_quat();
+  if (ahrs_icq.output_enabled) {
+    set_body_state_from_quat();
+  }
 
   /* Use low passed gyro value as initial bias */
   RATES_COPY(ahrs_icq.gyro_bias, *lp_gyro);
@@ -209,7 +216,9 @@ void ahrs_icq_propagate(struct Int32Rates *gyro, float dt)
                           &omega, freq);
   int32_quat_normalize(&ahrs_icq.ltp_to_imu_quat);
 
-  set_body_state_from_quat();
+  if (ahrs_icq.output_enabled) {
+    set_body_state_from_quat();
+  }
 
   // increase accel and mag propagation counters
   ahrs_icq.accel_cnt++;
@@ -639,7 +648,9 @@ void ahrs_icq_realign_heading(int32_t heading)
   int32_quat_comp(&ahrs_icq.ltp_to_imu_quat, &ltp_to_body_quat, body_to_imu_quat);
 
   /* Set state */
-  stateSetNedToBodyQuat_i(&ltp_to_body_quat);
+  if (ahrs_icq.output_enabled) {
+    stateSetNedToBodyQuat_i(&ltp_to_body_quat);
+  }
 
   ahrs_icq.heading_aligned = TRUE;
 }

sw/airborne/subsystems/ahrs/ahrs_int_cmpl_quat.h

@@ -102,6 +102,7 @@ struct AhrsIntCmplQuat {
 
   enum AhrsICQStatus status; ///< status of the AHRS, AHRS_ICQ_UNINIT or AHRS_ICQ_RUNNING
   bool_t is_aligned;
+  bool_t output_enabled; ///< if TRUE with push the estimation results to the state interface
 };
 
 extern struct AhrsIntCmplQuat ahrs_icq;

sw/airborne/subsystems/ahrs/ahrs_int_cmpl_quat_wrapper.c

@@ -209,9 +209,16 @@ static void gps_cb(uint8_t sender_id __attribute__((unused)),
   ahrs_icq_update_gps(gps_s);
 }
 
+static bool_t ahrs_icq_enable_output(bool_t enable)
+{
+  ahrs_icq.output_enabled = enable;
+  return ahrs_icq.output_enabled;
+}
+
 void ahrs_icq_register(void)
 {
-  ahrs_register_impl(ahrs_icq_init);
+  ahrs_icq_init();
+  ahrs_register_impl(ahrs_icq_enable_output);
 
   /*
    * Subscribe to scaled IMU measurements and attach callbacks

sw/airborne/subsystems/ahrs/ahrs_int_cmpl_quat_wrapper.h

@@ -29,7 +29,9 @@
 
 #include "subsystems/ahrs/ahrs_int_cmpl_quat.h"
 
-#define DefaultAhrsImpl ahrs_icq
+#ifndef PRIMARY_AHRS
+#define PRIMARY_AHRS ahrs_icq
+#endif
 
 extern void ahrs_icq_register(void);