ahrs.is_algined

sw/airborne/firmwares/rotorcraft/autopilot.c

@@ -92,10 +92,7 @@ bool_t   autopilot_detect_ground_once;
 #include "subsystems/ahrs.h"
 static inline int ahrs_is_aligned(void)
 {
-  /* FIXME: proper ahrs status management
-   * maybe use one global AhrsStatus enum again that all implementations need to use
-   */
-  return (DefaultAhrsImpl.status == AHRS_MLKF_RUNNING);
+  return DefaultAhrsImpl.is_aligned;
 }
 #else
 PRINT_CONFIG_MSG("Using AUTOPILOT_DISABLE_AHRS_KILL")

sw/airborne/subsystems/ahrs/ahrs_float_mlkf.c

@@ -59,7 +59,7 @@ struct AhrsMlkf ahrs_mlkf;
 
 void ahrs_mlkf_init(void) {
 
-  ahrs_mlkf.status = AHRS_MLKF_UNINIT;
+  ahrs_mlkf.is_aligned = FALSE;
 
   /* init ltp_to_imu quaternion as zero/identity rotation */
   float_quat_identity(&ahrs_mlkf.ltp_to_imu_quat);
@@ -93,7 +93,7 @@ void ahrs_mlkf_set_body_to_imu_quat(struct FloatQuat* q_b2i)
 {
   orientationSetQuat_f(&ahrs_mlkf.body_to_imu, q_b2i);
 
-  if (ahrs_mlkf.status == AHRS_MLKF_UNINIT) {
+  if (!ahrs_mlkf.is_aligned) {
     /* Set ltp_to_imu so that body is zero */
     memcpy(&ahrs_mlkf.ltp_to_imu_quat, orientationGetQuat_f(&ahrs_mlkf.body_to_imu),
            sizeof(struct FloatQuat));
@@ -116,7 +116,7 @@ bool_t ahrs_mlkf_align(struct Int32Rates* lp_gyro, struct Int32Vect3* lp_accel,
   RATES_COPY(bias0, *lp_gyro);
   RATES_FLOAT_OF_BFP(ahrs_mlkf.gyro_bias, bias0);
 
-  ahrs_mlkf.status = AHRS_MLKF_RUNNING;
+  ahrs_mlkf.is_aligned = TRUE;
 
   return TRUE;
 }

sw/airborne/subsystems/ahrs/ahrs_float_mlkf.h

@@ -58,6 +58,7 @@ struct AhrsMlkf {
   struct OrientationReps body_to_imu;
 
   enum AhrsMlkfStatus status;
+  bool_t is_aligned;
 };
 
 extern struct AhrsMlkf ahrs_mlkf;

sw/airborne/subsystems/ahrs/ahrs_float_mlkf_wrapper.c

@@ -60,7 +60,7 @@ PRINT_CONFIG_MSG("Calculating dt for AHRS_MLKF propagation.")
   /* timestamp in usec when last callback was received */
   static uint32_t last_stamp = 0;
 
-  if (last_stamp > 0 && ahrs_mlkf.status == AHRS_MLKF_RUNNING) {
+  if (last_stamp > 0 && ahrs_mlkf.is_aligned) {
     float dt = (float)(*stamp - last_stamp) * 1e-6;
     ahrs_mlkf_propagate((struct Int32Rates*)gyro, dt);
   }
@@ -79,7 +79,7 @@ static void accel_cb(uint8_t sender_id __attribute__((unused)),
                      const uint32_t* stamp __attribute__((unused)),
                      const struct Int32Vect3* accel)
 {
-  if (ahrs_mlkf.status == AHRS_MLKF_RUNNING) {
+  if (ahrs_mlkf.is_aligned) {
     ahrs_mlkf_update_accel((struct Int32Vect3*)accel);
   }
 }
@@ -88,7 +88,7 @@ static void mag_cb(uint8_t sender_id __attribute__((unused)),
                    const uint32_t* stamp __attribute__((unused)),
                    const struct Int32Vect3* mag)
 {
-  if (ahrs_mlkf.status == AHRS_MLKF_RUNNING) {
+  if (ahrs_mlkf.is_aligned) {
     ahrs_mlkf_update_mag((struct Int32Vect3*)mag);
   }
 }
@@ -98,7 +98,7 @@ static void aligner_cb(uint8_t __attribute__((unused)) sender_id,
                        const struct Int32Rates* lp_gyro, const struct Int32Vect3* lp_accel,
                        const struct Int32Vect3* lp_mag)
 {
-  if (ahrs_mlkf.status != AHRS_MLKF_RUNNING) {
+  if (!ahrs_mlkf.is_aligned) {
     ahrs_mlkf_align((struct Int32Rates*)lp_gyro, (struct Int32Vect3*)lp_accel,
                     (struct Int32Vect3*)lp_mag);
   }