[ahrs] check if all AHRS are aligned

paparazzi · Sep 2, 2015 · c00ed72 · c00ed72
1 parent e827cf2
commit c00ed72
Show file tree

Hide file tree

Showing 4 changed files with 35 additions and 4 deletions.
diff --git a/sw/airborne/subsystems/ahrs.c b/sw/airborne/subsystems/ahrs.c
@@ -49,17 +49,19 @@ PRINT_CONFIG_VAR(SECONDARY_AHRS)
 /** references a registered AHRS implementation */
 struct AhrsImpl {
   AhrsEnableOutput enable;
+  AhrsIsAligned aligned;
 };
 
 struct AhrsImpl ahrs_impls[AHRS_NB_IMPL];
 uint8_t ahrs_output_idx;
 
-void ahrs_register_impl(AhrsEnableOutput enable)
+void ahrs_register_impl(AhrsEnableOutput enable, AhrsIsAligned aligned)
 {
   int i;
   for (i=0; i < AHRS_NB_IMPL; i++) {
     if (ahrs_impls[i].enable == NULL) {
       ahrs_impls[i].enable = enable;
+      ahrs_impls[i].enable = aligned;
       break;
     }
   }
@@ -70,6 +72,7 @@ void ahrs_init(void)
   int i;
   for (i=0; i < AHRS_NB_IMPL; i++) {
     ahrs_impls[i].enable = NULL;
+    ahrs_impls[i].aligned = NULL;
   }
 
   RegisterAhrs(PRIMARY_AHRS);
@@ -83,6 +86,7 @@ void ahrs_init(void)
 
 int ahrs_switch(uint8_t idx)
 {
+	// TODO: prevent switching from an aligned AHRS to a non-aligned
   if (idx >= AHRS_NB_IMPL) { return -1; }
   if (ahrs_impls[idx].enable == NULL) { return -1; }
   /* first disable other AHRS output */
@@ -97,3 +101,15 @@ int ahrs_switch(uint8_t idx)
   ahrs_output_idx = idx;
   return ahrs_output_idx;
 }
+
+bool_t ahrs_all_aligned(void)
+{
+	int i;
+	for (i=0; i < AHRS_NB_IMPL; i++) {
+    if (ahrs_impls[i].aligned != NULL) {
+      if (!ahrs_impls[i].aligned())
+      	return FALSE;
+    }
+  }
+  return TRUE;
+}
diff --git a/sw/airborne/subsystems/ahrs.h b/sw/airborne/subsystems/ahrs.h
@@ -40,6 +40,7 @@
 #endif
 
 typedef bool_t (*AhrsEnableOutput)(bool_t);
+typedef bool_t (*AhrsIsAligned)(void);
 
 /* for settings when using secondary AHRS */
 extern uint8_t ahrs_output_idx;
@@ -49,7 +50,7 @@ extern uint8_t ahrs_output_idx;
  * 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);
+extern void ahrs_register_impl(AhrsEnableOutput enable, AhrsIsAligned aligned);
 
 /** AHRS initialization. Called at startup.
  * Registers/initializes the default AHRS.
@@ -62,4 +63,8 @@ extern void ahrs_init(void);
  */
 extern int ahrs_switch(uint8_t idx);
 
+/** AHRS initialization check. Returns if all registered AHRS implementations are running.
+ */
+extern bool_t ahrs_all_aligned(void);
+
 #endif /* AHRS_H */
diff --git a/sw/airborne/subsystems/ahrs/ahrs_float_mlkf_wrapper.c b/sw/airborne/subsystems/ahrs/ahrs_float_mlkf_wrapper.c
@@ -167,6 +167,11 @@ static bool_t ahrs_mlkf_enable_output(bool_t enable)
   return ahrs_mlkf_output_enabled;
 }
 
+static bool_t ahrs_mlkf_is_aligned(void)
+{
+  return ahrs_mlkf.is_aligned;
+}
+
 /**
  * Compute body orientation and rates from imu orientation and rates
  */
@@ -194,7 +199,7 @@ void ahrs_mlkf_register(void)
 {
   ahrs_mlkf_output_enabled = AHRS_MLKF_OUTPUT_ENABLED;
   ahrs_mlkf_init();
-  ahrs_register_impl(ahrs_mlkf_enable_output);
+  ahrs_register_impl(ahrs_mlkf_enable_output,ahrs_mlkf_is_aligned);
 
   /*
    * Subscribe to scaled IMU measurements and attach callbacks

diff --git a/sw/airborne/subsystems/ahrs/ahrs_int_cmpl_quat_wrapper.c b/sw/airborne/subsystems/ahrs/ahrs_int_cmpl_quat_wrapper.c
@@ -242,6 +242,11 @@ static bool_t ahrs_icq_enable_output(bool_t enable)
   return ahrs_icq_output_enabled;
 }
 
+static bool_t ahrs_icq_is_aligned(void)
+{
+  return ahrs_icq.is_aligned;
+}
+
 /** Rotate angles and rates from imu to body frame and set state */
 static void set_body_state_from_quat(void)
 {
@@ -266,7 +271,7 @@ void ahrs_icq_register(void)
 {
   ahrs_icq_output_enabled = AHRS_ICQ_OUTPUT_ENABLED;
   ahrs_icq_init();
-  ahrs_register_impl(ahrs_icq_enable_output);
+  ahrs_register_impl(ahrs_icq_enable_output, ahrs_icq_is_aligned);
 
   /*
    * Subscribe to scaled IMU measurements and attach callbacks