[ahrs] float_dcm wrapper

conf/firmwares/subsystems/fixedwing/ahrs_float_dcm.makefile

@@ -5,7 +5,7 @@
 USE_MAGNETOMETER ?= 0
 AHRS_ALIGNER_LED ?= none
 
-AHRS_CFLAGS += -DAHRS_TYPE_H=\"subsystems/ahrs/ahrs_float_dcm.h\"
+AHRS_CFLAGS += -DAHRS_TYPE_H=\"subsystems/ahrs/ahrs_float_dcm_wrapper.h\"
 AHRS_CFLAGS += -DUSE_AHRS_ALIGNER
 AHRS_CFLAGS += -DUSE_AHRS
 
@@ -16,6 +16,7 @@ endif
 AHRS_SRCS   += $(SRC_SUBSYSTEMS)/ahrs.c
 AHRS_SRCS   += $(SRC_SUBSYSTEMS)/ahrs/ahrs_aligner.c
 AHRS_SRCS   += $(SRC_SUBSYSTEMS)/ahrs/ahrs_float_dcm.c
+AHRS_SRCS   += $(SRC_SUBSYSTEMS)/ahrs/ahrs_float_dcm_wrapper.c
 
 
 ifneq ($(AHRS_ALIGNER_LED),none)

sw/airborne/firmwares/fixedwing/main_ap.c

@@ -149,7 +149,7 @@ volatile uint8_t ahrs_timeout_counter = 0;
 static void send_filter_status(struct transport_tx *trans, struct link_device *dev)
 {
   uint8_t mde = 3;
-  if (ahrs.status == AHRS_UNINIT) { mde = 2; }
+  if (!DefaultAhrsImpl.is_aligned) { mde = 2; }
   if (ahrs_timeout_counter > 10) { mde = 5; }
   uint16_t val = 0;
   pprz_msg_send_STATE_FILTER_STATUS(trans, dev, AC_ID, &mde, &val);
@@ -770,19 +770,6 @@ static inline void on_gyro_event(void)
   // current timestamp
   uint32_t now_ts = get_sys_time_usec();
 
-#if USE_AUTO_AHRS_FREQ || !defined(AHRS_PROPAGATE_FREQUENCY)
-  PRINT_CONFIG_MSG("Calculating dt for INS propagation.")
-    // timestamp in usec when last callback was received
-    static uint32_t last_ts = 0;
-  // dt between this and last callback in seconds
-  float dt = (float)(now_ts - last_ts) / 1e6;
-  last_ts = now_ts;
-#else
-  PRINT_CONFIG_MSG("Using fixed AHRS_PROPAGATE_FREQUENCY for INS propagation.")
-    PRINT_CONFIG_VAR(AHRS_PROPAGATE_FREQUENCY)
-    const float dt = 1. / (AHRS_PROPAGATE_FREQUENCY);
-#endif
-
   ahrs_timeout_counter = 0;
 
   imu_scale_gyro(&imu);
@@ -790,12 +777,8 @@ static inline void on_gyro_event(void)
   AbiSendMsgIMU_GYRO_INT32(1, &now_ts, &imu.gyro_prev);
 
 #if USE_AHRS_ALIGNER
-  // Run aligner on raw data as it also makes averages.
-  if (ahrs.status != AHRS_RUNNING) {
+  if (ahrs_aligner.status != AHRS_ALIGNER_LOCKED) {
     ahrs_aligner_run();
-    if (ahrs_aligner.status == AHRS_ALIGNER_LOCKED) {
-      ahrs.status = AHRS_RUNNING;
-    }
     return;
   }
 #endif

sw/airborne/subsystems/ahrs/ahrs_float_dcm.c

@@ -27,7 +27,6 @@
 
 #include "std.h"
 
-#include "subsystems/ahrs.h"
 #include "subsystems/ahrs/ahrs_float_dcm.h"
 #include "subsystems/ahrs/ahrs_float_utils.h"
 #include "firmwares/fixedwing/autopilot.h"	// launch detection
@@ -36,7 +35,6 @@
 #include "math/pprz_algebra_float.h"
 
 #include "state.h"
-#include "subsystems/abi.h"
 
 #if USE_GPS
 #include "subsystems/gps.h"
@@ -93,70 +91,6 @@ float imu_health = 0.;
 #endif
 
 
-/** ABI binding for IMU data.
- * Used for gyro, accel and mag ABI messages.
- */
-#ifndef AHRS_DCM_IMU_ID
-#define AHRS_DCM_IMU_ID ABI_BROADCAST
-#endif
-static abi_event gyro_ev;
-static abi_event accel_ev;
-static abi_event mag_ev;
-
-static abi_event aligner_ev;
-
-static void gyro_cb(uint8_t __attribute__((unused)) sender_id, const uint32_t* stamp,
-                    const struct Int32Rates* gyro)
-{
-#if USE_AUTO_AHRS_FREQ || !defined(AHRS_PROPAGATE_FREQUENCY)
-PRINT_CONFIG_MSG("Calculating dt for AHRS dcm propagation.")
-  /* timestamp in usec when last callback was received */
-  static uint32_t last_stamp = 0;
-  if (last_stamp > 0 && ahrs_dcm.status == AHRS_DCM_RUNNING) {
-    float dt = (float)(*stamp - last_stamp) * 1e-6;
-    ahrs_dcm_propagate((struct Int32Rates*)gyro, dt);
-  }
-  last_stamp = *stamp;
-#else
-PRINT_CONFIG_MSG("Using fixed AHRS_PROPAGATE_FREQUENCY for AHRS dcm propagation.")
-PRINT_CONFIG_VAR(AHRS_PROPAGATE_FREQUENCY)
-  if (ahrs_dcm.status == AHRS_DCM_RUNNING) {
-    const float dt = 1. / (AHRS_PROPAGATE_FREQUENCY);
-    ahrs_dcm_propagate((struct Int32Rates*)gyro, dt);
-  }
-#endif
-}
-
-static void accel_cb(uint8_t sender_id __attribute__((unused)),
-                     const uint32_t* stamp __attribute__((unused)),
-                     const struct Int32Vect3* accel)
-{
-  if (ahrs_dcm.status == AHRS_DCM_RUNNING) {
-    ahrs_dcm_update_accel((struct Int32Vect3*)accel);
-  }
-}
-
-static void mag_cb(uint8_t sender_id __attribute__((unused)),
-                   const uint32_t* stamp __attribute__((unused)),
-                   const struct Int32Vect3* mag)
-{
-  if (ahrs_dcm.status == AHRS_DCM_RUNNING) {
-    ahrs_dcm_update_mag((struct Int32Vect3*)mag);
-  }
-}
-
-static void aligner_cb(uint8_t __attribute__((unused)) sender_id,
-                       const uint32_t* stamp __attribute__((unused)),
-                       const struct Int32Rates* lp_gyro, const struct Int32Vect3* lp_accel,
-                       const struct Int32Vect3* lp_mag)
-{
-  if (ahrs_dcm.status != AHRS_DCM_RUNNING) {
-    ahrs_dcm_align((struct Int32Rates*)lp_gyro, (struct Int32Vect3*)lp_accel,
-                   (struct Int32Vect3*)lp_mag);
-  }
-}
-
-
 static inline void set_dcm_matrix_from_rmat(struct FloatRMat *rmat)
 {
   for (int i=0; i<3; i++) {
@@ -166,40 +100,24 @@ static inline void set_dcm_matrix_from_rmat(struct FloatRMat *rmat)
   }
 }
 
-void ahrs_dcm_register(void)
+void ahrs_dcm_init(void)
 {
-  ahrs_register_impl(ahrs_dcm_init, ahrs_dcm_update_gps);
-}
-
-void ahrs_dcm_init(struct OrientationReps* body_to_imu)
-{
-  /* save body_to_imu pointer */
-  ahrs_dcm.body_to_imu = body_to_imu;
-
   ahrs_dcm.status = AHRS_DCM_UNINIT;
+  ahrs_dcm.is_aligned = FALSE;
 
-  /* Set ltp_to_imu so that body is zero */
-  memcpy(&ahrs_dcm.ltp_to_imu_euler, orientationGetEulers_f(ahrs_dcm.body_to_imu),
-         sizeof(struct FloatEulers));
+  /* init ltp_to_imu euler with zero */
+  FLOAT_EULERS_ZERO(ahrs_dcm.ltp_to_imu_euler);
 
   FLOAT_RATES_ZERO(ahrs_dcm.imu_rate);
 
   /* set inital filter dcm */
-  set_dcm_matrix_from_rmat(orientationGetRMat_f(ahrs_dcm.body_to_imu));
+  set_dcm_matrix_from_rmat(orientationGetRMat_f(&ahrs_dcm.body_to_imu));
 
   ahrs_dcm.gps_speed = 0;
   ahrs_dcm.gps_acceleration = 0;
   ahrs_dcm.gps_course = 0;
   ahrs_dcm.gps_course_valid = FALSE;
   ahrs_dcm.gps_age = 100;
-
-  /*
-   * Subscribe to scaled IMU measurements and attach callbacks
-   */
-  AbiBindMsgIMU_GYRO_INT32(AHRS_DCM_IMU_ID, &gyro_ev, gyro_cb);
-  AbiBindMsgIMU_ACCEL_INT32(AHRS_DCM_IMU_ID, &accel_ev, accel_cb);
-  AbiBindMsgIMU_MAG_INT32(AHRS_DCM_IMU_ID, &mag_ev, mag_cb);
-  AbiBindMsgIMU_LOWPASSED(ABI_BROADCAST, &aligner_ev, aligner_cb);
 }
 
 bool_t ahrs_dcm_align(struct Int32Rates* lp_gyro, struct Int32Vect3* lp_accel,
@@ -452,7 +370,7 @@ void Normalize(void)
 
   // Reset on trouble
   if (problem) {                // Our solution is blowing up and we will force back to initial condition.  Hope we are not upside down!
-    set_dcm_matrix_from_rmat(orientationGetRMat_f(ahrs_dcm.body_to_imu));
+    set_dcm_matrix_from_rmat(orientationGetRMat_f(&ahrs_dcm.body_to_imu));
     problem = FALSE;
   }
 }
@@ -599,7 +517,7 @@ void Matrix_update(float dt)
  */
 static inline void set_body_orientation_and_rates(void) {
 
-  struct FloatRMat *body_to_imu_rmat = orientationGetRMat_f(ahrs_dcm.body_to_imu);
+  struct FloatRMat *body_to_imu_rmat = orientationGetRMat_f(&ahrs_dcm.body_to_imu);
 
   struct FloatRates body_rate;
   float_rmat_transp_ratemult(&body_rate, body_to_imu_rmat, &ahrs_dcm.imu_rate);
@@ -645,3 +563,18 @@ static inline void compute_ahrs_representations(void) {
   */
 }
 
+void ahrs_dcm_set_body_to_imu(struct OrientationReps* body_to_imu)
+{
+  ahrs_dcm_set_body_to_imu_quat(orientationGetQuat_f(body_to_imu));
+}
+
+void ahrs_dcm_set_body_to_imu_quat(struct FloatQuat* q_b2i)
+{
+  orientationSetQuat_f(&ahrs_dcm.body_to_imu, q_b2i);
+
+  if (!ahrs_dcm.is_aligned) {
+    /* Set ltp_to_imu so that body is zero */
+    memcpy(&ahrs_dcm.ltp_to_imu_euler, orientationGetEulers_f(&ahrs_dcm.body_to_imu),
+           sizeof(struct FloatEulers));
+  }
+}

sw/airborne/subsystems/ahrs/ahrs_float_dcm.h

@@ -26,6 +26,7 @@
 
 #include <inttypes.h>
 #include "math/pprz_algebra_float.h"
+#include "math/pprz_orientation_conversion.h"
 
 enum AhrsDCMStatus {
   AHRS_DCM_UNINIT,
@@ -45,14 +46,13 @@ struct AhrsFloatDCM {
   bool_t gps_course_valid;
   uint8_t gps_age;
 
-  struct OrientationReps* body_to_imu;
+  struct OrientationReps body_to_imu;
 
   enum AhrsDCMStatus status;
+  bool_t is_aligned;
 };
 extern struct AhrsFloatDCM ahrs_dcm;
 
-#define DefaultAhrsImpl ahrs_dcm
-
 // DCM Parameters
 
 //#define Kp_ROLLPITCH 0.2
@@ -79,8 +79,9 @@ extern int renorm_blowup_count;
 extern float imu_health;
 #endif
 
-extern void ahrs_dcm_register(void);
-extern void ahrs_dcm_init(struct OrientationReps* body_to_imu);
+extern void ahrs_dcm_init(void);
+extern void ahrs_dcm_set_body_to_imu(struct OrientationReps* body_to_imu);
+extern void ahrs_dcm_set_body_to_imu_quat(struct FloatQuat* q_b2i);
 extern bool_t ahrs_dcm_align(struct Int32Rates* lp_gyro, struct Int32Vect3* lp_accel,
                              struct Int32Vect3* lp_mag);
 extern void ahrs_dcm_propagate(struct Int32Rates* gyro, float dt);

sw/airborne/subsystems/ahrs/ahrs_float_dcm_wrapper.c

@@ -0,0 +1,114 @@
+/*
+ * Copyright (C) 2015 Felix Ruess <felix.ruess@gmail.com>
+ *
+ * This file is part of paparazzi.
+ *
+ * paparazzi is free software; you can redistribute it and/or modify
+ * it under the terms of the GNU General Public License as published by
+ * the Free Software Foundation; either version 2, or (at your option)
+ * any later version.
+ *
+ * paparazzi is distributed in the hope that it will be useful,
+ * but WITHOUT ANY WARRANTY; without even the implied warranty of
+ * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
+ * GNU General Public License for more details.
+ *
+ * You should have received a copy of the GNU General Public License
+ * along with Paparazzi; see the file COPYING.  If not, see
+ * <http://www.gnu.org/licenses/>.
+ */
+
+/**
+ * @file subsystems/ahrs/ahrs_float_dcm_wrapper.c
+ *
+ * Paparazzi specific wrapper to run floating point complementary filter.
+ */
+
+#include "subsystems/ahrs/ahrs_float_dcm_wrapper.h"
+#include "subsystems/ahrs.h"
+#include "subsystems/abi.h"
+#include "state.h"
+
+/** ABI binding for IMU data.
+ * Used for gyro, accel and mag ABI messages.
+ */
+#ifndef AHRS_DCM_IMU_ID
+#define AHRS_DCM_IMU_ID ABI_BROADCAST
+#endif
+static abi_event gyro_ev;
+static abi_event accel_ev;
+static abi_event mag_ev;
+static abi_event aligner_ev;
+static abi_event body_to_imu_ev;
+
+
+static void gyro_cb(uint8_t __attribute__((unused)) sender_id, const uint32_t* stamp,
+                    const struct Int32Rates* gyro)
+{
+#if USE_AUTO_AHRS_FREQ || !defined(AHRS_PROPAGATE_FREQUENCY)
+PRINT_CONFIG_MSG("Calculating dt for AHRS dcm propagation.")
+  /* timestamp in usec when last callback was received */
+  static uint32_t last_stamp = 0;
+  if (last_stamp > 0 && ahrs_dcm.is_aligned) {
+    float dt = (float)(*stamp - last_stamp) * 1e-6;
+    ahrs_dcm_propagate((struct Int32Rates*)gyro, dt);
+  }
+  last_stamp = *stamp;
+#else
+PRINT_CONFIG_MSG("Using fixed AHRS_PROPAGATE_FREQUENCY for AHRS dcm propagation.")
+PRINT_CONFIG_VAR(AHRS_PROPAGATE_FREQUENCY)
+  if (ahrs_dcm.is_aligned) {
+    const float dt = 1. / (AHRS_PROPAGATE_FREQUENCY);
+    ahrs_dcm_propagate((struct Int32Rates*)gyro, dt);
+  }
+#endif
+}
+
+static void accel_cb(uint8_t sender_id __attribute__((unused)),
+                     const uint32_t* stamp __attribute__((unused)),
+                     const struct Int32Vect3* accel)
+{
+  if (ahrs_dcm.is_aligned) {
+    ahrs_dcm_update_accel((struct Int32Vect3*)accel);
+  }
+}
+
+static void mag_cb(uint8_t sender_id __attribute__((unused)),
+                   const uint32_t* stamp __attribute__((unused)),
+                   const struct Int32Vect3* mag)
+{
+  if (ahrs_dcm.is_aligned) {
+    ahrs_dcm_update_mag((struct Int32Vect3*)mag);
+  }
+}
+
+static void aligner_cb(uint8_t __attribute__((unused)) sender_id,
+                       const uint32_t* stamp __attribute__((unused)),
+                       const struct Int32Rates* lp_gyro, const struct Int32Vect3* lp_accel,
+                       const struct Int32Vect3* lp_mag)
+{
+  if (!ahrs_dcm.is_aligned) {
+    ahrs_dcm_align((struct Int32Rates*)lp_gyro, (struct Int32Vect3*)lp_accel,
+                  (struct Int32Vect3*)lp_mag);
+  }
+}
+
+static void body_to_imu_cb(uint8_t sender_id __attribute__((unused)),
+                           const struct FloatQuat* q_b2i_f)
+{
+  ahrs_dcm_set_body_to_imu_quat((struct FloatQuat*)q_b2i_f);
+}
+
+void ahrs_dcm_register(void)
+{
+  ahrs_register_impl(ahrs_dcm_init, ahrs_dcm_update_gps);
+
+  /*
+   * Subscribe to scaled IMU measurements and attach callbacks
+   */
+  AbiBindMsgIMU_GYRO_INT32(AHRS_DCM_IMU_ID, &gyro_ev, gyro_cb);
+  AbiBindMsgIMU_ACCEL_INT32(AHRS_DCM_IMU_ID, &accel_ev, accel_cb);
+  AbiBindMsgIMU_MAG_INT32(AHRS_DCM_IMU_ID, &mag_ev, mag_cb);
+  AbiBindMsgIMU_LOWPASSED(ABI_BROADCAST, &aligner_ev, aligner_cb);
+  AbiBindMsgBODY_TO_IMU_QUAT(ABI_BROADCAST, &body_to_imu_ev, body_to_imu_cb);
+}

sw/airborne/subsystems/ahrs/ahrs_float_dcm_wrapper.h

@@ -0,0 +1,36 @@
+/*
+ * Copyright (C) 2015 Felix Ruess <felix.ruess@gmail.com>
+ *
+ * This file is part of paparazzi.
+ *
+ * paparazzi is free software; you can redistribute it and/or modify
+ * it under the terms of the GNU General Public License as published by
+ * the Free Software Foundation; either version 2, or (at your option)
+ * any later version.
+ *
+ * paparazzi is distributed in the hope that it will be useful,
+ * but WITHOUT ANY WARRANTY; without even the implied warranty of
+ * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
+ * GNU General Public License for more details.
+ *
+ * You should have received a copy of the GNU General Public License
+ * along with Paparazzi; see the file COPYING.  If not, see
+ * <http://www.gnu.org/licenses/>.
+ */
+
+/**
+ * @file subsystems/ahrs/ahrs_float_dcm_wrapper.h
+ *
+ * Paparazzi specific wrapper to run floating point DCM filter.
+ */
+
+#ifndef AHRS_FLOAT_DCM_WRAPPER_H
+#define AHRS_FLOAT_DCM_WRAPPER_H
+
+#include "subsystems/ahrs/ahrs_float_dcm.h"
+
+#define DefaultAhrsImpl ahrs_dcm
+
+extern void ahrs_dcm_register(void);
+
+#endif /* AHRS_FLOAT_DCM_WRAPPER_H */