[ahrs] fixed all implementations

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

Hide file tree

Showing 4 changed files with 24 additions and 4 deletions.
diff --git a/sw/airborne/subsystems/ahrs/ahrs_float_cmpl_wrapper.c b/sw/airborne/subsystems/ahrs/ahrs_float_cmpl_wrapper.c
@@ -215,6 +215,11 @@ static bool_t ahrs_fc_enable_output(bool_t enable)
   return ahrs_fc_output_enabled;
 }
 
+static bool_t ahrs_fc_is_aligned(void)
+{
+  return ahrs_fc.is_aligned;
+}
+
 /**
  * Compute body orientation and rates from imu orientation and rates
  */
@@ -240,7 +245,7 @@ void ahrs_fc_register(void)
 {
   ahrs_fc_output_enabled = AHRS_FC_OUTPUT_ENABLED;
   ahrs_fc_init();
-  ahrs_register_impl(ahrs_fc_enable_output);
+  ahrs_register_impl(ahrs_fc_enable_output, ahrs_fc_is_aligned);
 
   /*
    * Subscribe to scaled IMU measurements and attach callbacks

diff --git a/sw/airborne/subsystems/ahrs/ahrs_float_dcm_wrapper.c b/sw/airborne/subsystems/ahrs/ahrs_float_dcm_wrapper.c
@@ -157,6 +157,11 @@ static bool_t ahrs_dcm_enable_output(bool_t enable)
   return ahrs_dcm_output_enabled;
 }
 
+static bool_t ahrs_dcm_is_aligned(void)
+{
+  return ahrs_dcm.is_aligned;
+}
+
 /**
  * Compute body orientation and rates from imu orientation and rates
  */
@@ -181,7 +186,7 @@ void ahrs_dcm_register(void)
 {
   ahrs_dcm_output_enabled = AHRS_DCM_OUTPUT_ENABLED;
   ahrs_dcm_init();
-  ahrs_register_impl(ahrs_dcm_enable_output);
+  ahrs_register_impl(ahrs_dcm_enable_output,ahrs_dcm_is_aligned);
 
   /*
    * Subscribe to scaled IMU measurements and attach callbacks

diff --git a/sw/airborne/subsystems/ahrs/ahrs_float_invariant_wrapper.c b/sw/airborne/subsystems/ahrs/ahrs_float_invariant_wrapper.c
@@ -189,6 +189,11 @@ static bool_t ahrs_float_invariant_enable_output(bool_t enable)
   return ahrs_finv_output_enabled;
 }
 
+static bool_t ahrs_float_invariant_is_aligned(void)
+{
+  return ahrs_float_inv.is_aligned;
+}
+
 /**
  * Compute body orientation and rates from imu orientation and rates
  */
@@ -217,7 +222,7 @@ void ahrs_float_invariant_register(void)
 {
   ahrs_finv_output_enabled = AHRS_FINV_OUTPUT_ENABLED;
   ahrs_float_invariant_init();
-  ahrs_register_impl(ahrs_float_invariant_enable_output);
+  ahrs_register_impl(ahrs_float_invariant_enable_output,ahrs_float_invariant_is_aligned);
 
   /*
    * Subscribe to scaled IMU measurements and attach callbacks

diff --git a/sw/airborne/subsystems/ahrs/ahrs_int_cmpl_euler_wrapper.c b/sw/airborne/subsystems/ahrs/ahrs_int_cmpl_euler_wrapper.c
@@ -173,6 +173,11 @@ static bool_t ahrs_ice_enable_output(bool_t enable)
   return ahrs_ice_output_enabled;
 }
 
+static bool_t ahrs_ice_is_aligned(void)
+{
+  return ahrs_ice.is_aligned;
+}
+
 /* Rotate angles and rates from imu to body frame and set state */
 static void set_body_state_from_euler(void)
 {
@@ -198,7 +203,7 @@ void ahrs_ice_register(void)
 {
   ahrs_ice_output_enabled = AHRS_ICE_OUTPUT_ENABLED;
   ahrs_ice_init();
-  ahrs_register_impl(ahrs_ice_enable_output);
+  ahrs_register_impl(ahrs_ice_enable_output,ahrs_ice_is_aligned);
 
   /*
    * Subscribe to scaled IMU measurements and attach callbacks