[ins] start converting ins_int

sw/airborne/firmwares/rotorcraft/main.c

@@ -109,6 +109,10 @@ INFO_VALUE("it is recommended to configure in your airframe PERIODIC_FREQUENCY t
 #define _DefaultAhrsRegister(_x) __DefaultAhrsRegister(_x)
 #define DefaultAhrsRegister() _DefaultAhrsRegister(DefaultAhrsImpl)
 
+#define __DefaultInsRegister(_x) _x ## _register()
+#define _DefaultInsRegister(_x) __DefaultInsRegister(_x)
+#define DefaultInsRegister() _DefaultInsRegister(DefaultInsImpl)
+
 static inline void on_gyro_event(void);
 static inline void on_accel_event(void);
 static inline void on_gps_event(void);
@@ -164,6 +168,7 @@ STATIC_INLINE void main_init(void)
   ins_init();
 
   DefaultAhrsRegister();
+  DefaultInsRegister();
 
 #if USE_GPS
   gps_init();
@@ -363,20 +368,6 @@ static inline void on_gyro_event( void ) {
   if (nps_bypass_ahrs) sim_overwrite_ahrs();
 #endif
 
-#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
-  ins_propagate(dt);
-
 #ifdef USE_VEHICLE_INTERFACE
   vi_notify_imu_available();
 #endif

sw/airborne/subsystems/ins.c

@@ -35,10 +35,29 @@
 
 struct Ins ins;
 
+void ins_register_impl(InsInit init, InsUpdateGps update_gps)
+{
+  ins.init = init;
+  ins.update_gps = update_gps;
+
+  ins.init();
+}
+
+void ins_init(void)
+{
+  ins.init = NULL;
+  ins.update_gps = NULL;
+}
+
+void ins_update_gps(void)
+{
+  if (ins.update_gps != NULL) {
+    ins.update_gps();
+  }
+}
 
 // weak functions, used if not explicitly provided by implementation
 
-
 void WEAK ins_reset_local_origin(void)
 {
 #if USE_GPS
@@ -82,6 +101,3 @@ void WEAK ins_reset_utm_zone(struct UtmCoor_f *utm)
 void WEAK ins_reset_utm_zone(struct UtmCoor_f *utm __attribute__((unused))) {}
 #endif
 
-
-void WEAK ins_update_gps(void) {}
-

sw/airborne/subsystems/ins.h

@@ -37,15 +37,22 @@
 #include INS_TYPE_H
 #endif
 
+typedef void (*InsInit)(void);
+typedef void (*InsUpdateGps)(void);
+
 /** Inertial Navigation System state */
 struct Ins {
+  InsInit init;
+  InsUpdateGps update_gps;
 };
 
 /** global INS state */
 extern struct Ins ins;
 
+extern void ins_register_impl(InsInit init, InsUpdateGps update_gps);
+
 /** INS initialization. Called at startup.
- *  Needs to be implemented by each INS algorithm.
+ *  Initializes the global ins struct.
  */
 extern void ins_init(void);
 
@@ -69,16 +76,10 @@ extern void ins_reset_altitude_ref(void);
  */
 extern void ins_reset_utm_zone(struct UtmCoor_f *utm);
 
-/** Propagation. Usually integrates the gyro rates to angles.
- *  Reads the global #imu data struct.
- *  Does nothing if not implemented by specific INS algorithm.
- *  @param dt time difference since last propagation in seconds
- */
-extern void ins_propagate(float dt);
 
 /** Update INS state with GPS measurements.
+ *  Calls implementation if registered.
  *  Reads the global #gps data struct.
- *  Does nothing if not implemented by specific INS algorithm.
  */
 extern void ins_update_gps(void);
 

sw/airborne/subsystems/ins/ins_int.c

@@ -116,7 +116,15 @@ PRINT_CONFIG_VAR(INS_BARO_ID)
 abi_event baro_ev;
 static void baro_cb(uint8_t sender_id, float pressure);
 
-struct InsInt ins_impl;
+/** ABI binding for IMU data.
+ * Used accel ABI messages.
+ */
+#ifndef INS_INT_IMU_ID
+#define INS_INT_IMU_ID ABI_BROADCAST
+#endif
+static abi_event accel_ev;
+
+struct InsInt ins_int;
 
 #if PERIODIC_TELEMETRY
 #include "subsystems/datalink/telemetry.h"
@@ -124,24 +132,24 @@ struct InsInt ins_impl;
 static void send_ins(struct transport_tx *trans, struct link_device *dev)
 {
   pprz_msg_send_INS(trans, dev, AC_ID,
-                    &ins_impl.ltp_pos.x, &ins_impl.ltp_pos.y, &ins_impl.ltp_pos.z,
+                    &ins_int.ltp_pos.x, &ins_int.ltp_pos.y, &ins_int.ltp_pos.z,
-                    &ins_impl.ltp_speed.x, &ins_impl.ltp_speed.y, &ins_impl.ltp_speed.z,
+                    &ins_int.ltp_speed.x, &ins_int.ltp_speed.y, &ins_int.ltp_speed.z,
-                    &ins_impl.ltp_accel.x, &ins_impl.ltp_accel.y, &ins_impl.ltp_accel.z);
+                    &ins_int.ltp_accel.x, &ins_int.ltp_accel.y, &ins_int.ltp_accel.z);
 }
 
 static void send_ins_z(struct transport_tx *trans, struct link_device *dev)
 {
   pprz_msg_send_INS_Z(trans, dev, AC_ID,
-                      &ins_impl.baro_z, &ins_impl.ltp_pos.z, &ins_impl.ltp_speed.z, &ins_impl.ltp_accel.z);
+                      &ins_int.baro_z, &ins_int.ltp_pos.z, &ins_int.ltp_speed.z, &ins_int.ltp_accel.z);
 }
 
 static void send_ins_ref(struct transport_tx *trans, struct link_device *dev)
 {
-  if (ins_impl.ltp_initialized) {
+  if (ins_int.ltp_initialized) {
     pprz_msg_send_INS_REF(trans, dev, AC_ID,
-                          &ins_impl.ltp_def.ecef.x, &ins_impl.ltp_def.ecef.y, &ins_impl.ltp_def.ecef.z,
+                          &ins_int.ltp_def.ecef.x, &ins_int.ltp_def.ecef.y, &ins_int.ltp_def.ecef.z,
-                          &ins_impl.ltp_def.lla.lat, &ins_impl.ltp_def.lla.lon, &ins_impl.ltp_def.lla.alt,
+                          &ins_int.ltp_def.lla.lat, &ins_int.ltp_def.lla.lon, &ins_int.ltp_def.lla.alt,
-                          &ins_impl.ltp_def.hmsl, &ins_impl.qfe);
+                          &ins_int.ltp_def.hmsl, &ins_int.qfe);
   }
 }
 #endif
@@ -154,28 +162,28 @@ static void ins_update_from_hff(void);
 #endif
 
 
-void ins_init(void)
+void ins_int_init(void)
 {
 
 #if USE_INS_NAV_INIT
   ins_init_origin_from_flightplan();
-  ins_impl.ltp_initialized = TRUE;
+  ins_int.ltp_initialized = TRUE;
 #else
-  ins_impl.ltp_initialized  = FALSE;
+  ins_int.ltp_initialized  = FALSE;
 #endif
 
   // Bind to BARO_ABS message
   AbiBindMsgBARO_ABS(INS_BARO_ID, &baro_ev, baro_cb);
-  ins_impl.baro_initialized = FALSE;
+  ins_int.baro_initialized = FALSE;
 
 #if USE_SONAR
-  ins_impl.update_on_agl = INS_SONAR_UPDATE_ON_AGL;
+  ins_int.update_on_agl = INS_SONAR_UPDATE_ON_AGL;
   // Bind to AGL message
   AbiBindMsgAGL(INS_SONAR_ID, &sonar_ev, sonar_cb);
 #endif
 
-  ins_impl.vf_reset = FALSE;
+  ins_int.vf_reset = FALSE;
-  ins_impl.hf_realign = FALSE;
+  ins_int.hf_realign = FALSE;
 
   /* init vertical and horizontal filters */
   vff_init_zero();
@@ -183,9 +191,9 @@ void ins_init(void)
   b2_hff_init(0., 0., 0., 0.);
 #endif
 
-  INT32_VECT3_ZERO(ins_impl.ltp_pos);
+  INT32_VECT3_ZERO(ins_int.ltp_pos);
-  INT32_VECT3_ZERO(ins_impl.ltp_speed);
+  INT32_VECT3_ZERO(ins_int.ltp_speed);
-  INT32_VECT3_ZERO(ins_impl.ltp_accel);
+  INT32_VECT3_ZERO(ins_int.ltp_accel);
 
 #if PERIODIC_TELEMETRY
   register_periodic_telemetry(DefaultPeriodic, "INS", send_ins);
@@ -198,23 +206,23 @@ void ins_reset_local_origin(void)
 {
 #if USE_GPS
   if (gps.fix == GPS_FIX_3D) {
-    ltp_def_from_ecef_i(&ins_impl.ltp_def, &gps.ecef_pos);
+    ltp_def_from_ecef_i(&ins_int.ltp_def, &gps.ecef_pos);
-    ins_impl.ltp_def.lla.alt = gps.lla_pos.alt;
+    ins_int.ltp_def.lla.alt = gps.lla_pos.alt;
-    ins_impl.ltp_def.hmsl = gps.hmsl;
+    ins_int.ltp_def.hmsl = gps.hmsl;
-    ins_impl.ltp_initialized = TRUE;
+    ins_int.ltp_initialized = TRUE;
-    stateSetLocalOrigin_i(&ins_impl.ltp_def);
+    stateSetLocalOrigin_i(&ins_int.ltp_def);
   }
   else {
-    ins_impl.ltp_initialized = FALSE;
+    ins_int.ltp_initialized = FALSE;
   }
 #else
-  ins_impl.ltp_initialized = FALSE;
+  ins_int.ltp_initialized = FALSE;
 #endif
 
 #if USE_HFF
-  ins_impl.hf_realign = TRUE;
+  ins_int.hf_realign = TRUE;
 #endif
-  ins_impl.vf_reset = TRUE;
+  ins_int.vf_reset = TRUE;
 }
 
 void ins_reset_altitude_ref(void)
@@ -225,40 +233,40 @@ void ins_reset_altitude_ref(void)
     .lon = state.ned_origin_i.lla.lon,
     .alt = gps.lla_pos.alt
   };
-  ltp_def_from_lla_i(&ins_impl.ltp_def, &lla);
+  ltp_def_from_lla_i(&ins_int.ltp_def, &lla);
-  ins_impl.ltp_def.hmsl = gps.hmsl;
+  ins_int.ltp_def.hmsl = gps.hmsl;
-  stateSetLocalOrigin_i(&ins_impl.ltp_def);
+  stateSetLocalOrigin_i(&ins_int.ltp_def);
 #endif
-  ins_impl.vf_reset = TRUE;
+  ins_int.vf_reset = TRUE;
 }
 
-void ins_propagate(float dt)
+void ins_int_propagate(struct Int32Vect3 *accel, float dt)
 {
   /* untilt accels */
   struct Int32Vect3 accel_meas_body;
   struct Int32RMat *body_to_imu_rmat = orientationGetRMat_i(&imu.body_to_imu);
-  int32_rmat_transp_vmult(&accel_meas_body, body_to_imu_rmat, &imu.accel);
+  int32_rmat_transp_vmult(&accel_meas_body, body_to_imu_rmat, accel);
   struct Int32Vect3 accel_meas_ltp;
   int32_rmat_transp_vmult(&accel_meas_ltp, stateGetNedToBodyRMat_i(), &accel_meas_body);
 
   float z_accel_meas_float = ACCEL_FLOAT_OF_BFP(accel_meas_ltp.z);
-  if (ins_impl.baro_initialized) {
+  if (ins_int.baro_initialized) {
     vff_propagate(z_accel_meas_float, dt);
     ins_update_from_vff();
   } else { // feed accel from the sensors
     // subtract -9.81m/s2 (acceleration measured due to gravity,
     // but vehicle not accelerating in ltp)
-    ins_impl.ltp_accel.z = accel_meas_ltp.z + ACCEL_BFP_OF_REAL(9.81);
+    ins_int.ltp_accel.z = accel_meas_ltp.z + ACCEL_BFP_OF_REAL(9.81);
   }
 
 #if USE_HFF
   /* propagate horizontal filter */
   b2_hff_propagate();
-  /* convert and copy result to ins_impl */
+  /* convert and copy result to ins_int */
   ins_update_from_hff();
 #else
-  ins_impl.ltp_accel.x = accel_meas_ltp.x;
+  ins_int.ltp_accel.x = accel_meas_ltp.x;
-  ins_impl.ltp_accel.y = accel_meas_ltp.y;
+  ins_int.ltp_accel.y = accel_meas_ltp.y;
 #endif /* USE_HFF */
 
   ins_ned_to_state();
@@ -266,24 +274,24 @@ void ins_propagate(float dt)
 
 static void baro_cb(uint8_t __attribute__((unused)) sender_id, float pressure)
 {
-  if (!ins_impl.baro_initialized && pressure > 1e-7) {
+  if (!ins_int.baro_initialized && pressure > 1e-7) {
     // wait for a first positive value
-    ins_impl.qfe = pressure;
+    ins_int.qfe = pressure;
-    ins_impl.baro_initialized = TRUE;
+    ins_int.baro_initialized = TRUE;
   }
 
-  if (ins_impl.baro_initialized) {
+  if (ins_int.baro_initialized) {
-    if (ins_impl.vf_reset) {
+    if (ins_int.vf_reset) {
-      ins_impl.vf_reset = FALSE;
+      ins_int.vf_reset = FALSE;
-      ins_impl.qfe = pressure;
+      ins_int.qfe = pressure;
       vff_realign(0.);
       ins_update_from_vff();
     } else {
-      ins_impl.baro_z = -pprz_isa_height_of_pressure(pressure, ins_impl.qfe);
+      ins_int.baro_z = -pprz_isa_height_of_pressure(pressure, ins_int.qfe);
 #if USE_VFF_EXTENDED
-      vff_update_baro(ins_impl.baro_z);
+      vff_update_baro(ins_int.baro_z);
 #else
-      vff_update(ins_impl.baro_z);
+      vff_update(ins_int.baro_z);
 #endif
     }
     ins_ned_to_state();
@@ -291,22 +299,22 @@ static void baro_cb(uint8_t __attribute__((unused)) sender_id, float pressure)
 }
 
 #if USE_GPS
-void ins_update_gps(void)
+void ins_int_update_gps(void)
 {
   if (gps.fix == GPS_FIX_3D) {
-    if (!ins_impl.ltp_initialized) {
+    if (!ins_int.ltp_initialized) {
-      ltp_def_from_ecef_i(&ins_impl.ltp_def, &gps.ecef_pos);
+      ltp_def_from_ecef_i(&ins_int.ltp_def, &gps.ecef_pos);
-      ins_impl.ltp_def.lla.alt = gps.lla_pos.alt;
+      ins_int.ltp_def.lla.alt = gps.lla_pos.alt;
-      ins_impl.ltp_def.hmsl = gps.hmsl;
+      ins_int.ltp_def.hmsl = gps.hmsl;
-      ins_impl.ltp_initialized = TRUE;
+      ins_int.ltp_initialized = TRUE;
-      stateSetLocalOrigin_i(&ins_impl.ltp_def);
+      stateSetLocalOrigin_i(&ins_int.ltp_def);
     }
 
     struct NedCoor_i gps_pos_cm_ned;
-    ned_of_ecef_point_i(&gps_pos_cm_ned, &ins_impl.ltp_def, &gps.ecef_pos);
+    ned_of_ecef_point_i(&gps_pos_cm_ned, &ins_int.ltp_def, &gps.ecef_pos);
     /// @todo maybe use gps.ned_vel directly??
     struct NedCoor_i gps_speed_cm_s_ned;
-    ned_of_ecef_vect_i(&gps_speed_cm_s_ned, &ins_impl.ltp_def, &gps.ecef_vel);
+    ned_of_ecef_vect_i(&gps_speed_cm_s_ned, &ins_int.ltp_def, &gps.ecef_vel);
 
 #if INS_USE_GPS_ALT
     vff_update_z_conf((float)gps_pos_cm_ned.z / 100.0, INS_VFF_R_GPS);
@@ -322,27 +330,29 @@ void ins_update_gps(void)
     VECT2_ASSIGN(gps_speed_m_s_ned, gps_speed_cm_s_ned.x, gps_speed_cm_s_ned.y);
     VECT2_SDIV(gps_speed_m_s_ned, gps_speed_m_s_ned, 100.);
 
-    if (ins_impl.hf_realign) {
+    if (ins_int.hf_realign) {
-      ins_impl.hf_realign = FALSE;
+      ins_int.hf_realign = FALSE;
       const struct FloatVect2 zero = {0.0f, 0.0f};
       b2_hff_realign(gps_pos_m_ned, zero);
     }
     // run horizontal filter
     b2_hff_update_gps(&gps_pos_m_ned, &gps_speed_m_s_ned);
-    // convert and copy result to ins_impl
+    // convert and copy result to ins_int
     ins_update_from_hff();
 
 #else  /* hff not used */
     /* simply copy horizontal pos/speed from gps */
-    INT32_VECT2_SCALE_2(ins_impl.ltp_pos, gps_pos_cm_ned,
+    INT32_VECT2_SCALE_2(ins_int.ltp_pos, gps_pos_cm_ned,
                         INT32_POS_OF_CM_NUM, INT32_POS_OF_CM_DEN);
-    INT32_VECT2_SCALE_2(ins_impl.ltp_speed, gps_speed_cm_s_ned,
+    INT32_VECT2_SCALE_2(ins_int.ltp_speed, gps_speed_cm_s_ned,
                         INT32_SPEED_OF_CM_S_NUM, INT32_SPEED_OF_CM_S_DEN);
 #endif /* USE_HFF */
 
     ins_ned_to_state();
   }
 }
+#else
+void ins_int_update_gps(void) {}
 #endif /* USE_GPS */
 
 
@@ -357,10 +367,10 @@ static void sonar_cb(uint8_t __attribute__((unused)) sender_id, float distance)
       && stabilization_cmd[COMMAND_THRUST] < INS_SONAR_THROTTLE_THRESHOLD
 #endif
 #ifdef INS_SONAR_BARO_THRESHOLD
-      && ins_impl.baro_z > -INS_SONAR_BARO_THRESHOLD /* z down */
+      && ins_int.baro_z > -INS_SONAR_BARO_THRESHOLD /* z down */
 #endif
-      && ins_impl.update_on_agl
+      && ins_int.update_on_agl
-      && ins_impl.baro_initialized) {
+      && ins_int.baro_initialized) {
     vff_update_z_conf(-(distance), VFF_R_SONAR_0 + VFF_R_SONAR_OF_M * fabsf(distance));
     last_offset = vff.offset;
   } else {
@@ -384,18 +394,18 @@ static void ins_init_origin_from_flightplan(void)
   struct EcefCoor_i ecef_nav0;
   ecef_of_lla_i(&ecef_nav0, &llh_nav0);
 
-  ltp_def_from_ecef_i(&ins_impl.ltp_def, &ecef_nav0);
+  ltp_def_from_ecef_i(&ins_int.ltp_def, &ecef_nav0);
-  ins_impl.ltp_def.hmsl = NAV_ALT0;
+  ins_int.ltp_def.hmsl = NAV_ALT0;
-  stateSetLocalOrigin_i(&ins_impl.ltp_def);
+  stateSetLocalOrigin_i(&ins_int.ltp_def);
 
 }
 
 /** copy position and speed to state interface */
 static void ins_ned_to_state(void)
 {
-  stateSetPositionNed_i(&ins_impl.ltp_pos);
+  stateSetPositionNed_i(&ins_int.ltp_pos);
-  stateSetSpeedNed_i(&ins_impl.ltp_speed);
+  stateSetSpeedNed_i(&ins_int.ltp_speed);
-  stateSetAccelNed_i(&ins_impl.ltp_accel);
+  stateSetAccelNed_i(&ins_int.ltp_accel);
 
 #if defined SITL && USE_NPS
   if (nps_bypass_ins) {
@@ -407,20 +417,45 @@ static void ins_ned_to_state(void)
 /** update ins state from vertical filter */
 static void ins_update_from_vff(void)
 {
-  ins_impl.ltp_accel.z = ACCEL_BFP_OF_REAL(vff.zdotdot);
+  ins_int.ltp_accel.z = ACCEL_BFP_OF_REAL(vff.zdotdot);
-  ins_impl.ltp_speed.z = SPEED_BFP_OF_REAL(vff.zdot);
+  ins_int.ltp_speed.z = SPEED_BFP_OF_REAL(vff.zdot);
-  ins_impl.ltp_pos.z   = POS_BFP_OF_REAL(vff.z);
+  ins_int.ltp_pos.z   = POS_BFP_OF_REAL(vff.z);
 }
 
 #if USE_HFF
 /** update ins state from horizontal filter */
 static void ins_update_from_hff(void)
 {
-  ins_impl.ltp_accel.x = ACCEL_BFP_OF_REAL(b2_hff_state.xdotdot);
+  ins_int.ltp_accel.x = ACCEL_BFP_OF_REAL(b2_hff_state.xdotdot);
-  ins_impl.ltp_accel.y = ACCEL_BFP_OF_REAL(b2_hff_state.ydotdot);
+  ins_int.ltp_accel.y = ACCEL_BFP_OF_REAL(b2_hff_state.ydotdot);
-  ins_impl.ltp_speed.x = SPEED_BFP_OF_REAL(b2_hff_state.xdot);
+  ins_int.ltp_speed.x = SPEED_BFP_OF_REAL(b2_hff_state.xdot);
-  ins_impl.ltp_speed.y = SPEED_BFP_OF_REAL(b2_hff_state.ydot);
+  ins_int.ltp_speed.y = SPEED_BFP_OF_REAL(b2_hff_state.ydot);
-  ins_impl.ltp_pos.x   = POS_BFP_OF_REAL(b2_hff_state.x);
+  ins_int.ltp_pos.x   = POS_BFP_OF_REAL(b2_hff_state.x);
-  ins_impl.ltp_pos.y   = POS_BFP_OF_REAL(b2_hff_state.y);
+  ins_int.ltp_pos.y   = POS_BFP_OF_REAL(b2_hff_state.y);
 }
 #endif
+
+
+static void accel_cb(uint8_t sender_id __attribute__((unused)),
+                     uint32_t stamp, struct Int32Vect3 *accel)
+{
+  PRINT_CONFIG_MSG("Calculating dt for INS int propagation.")
+  /* timestamp in usec when last callback was received */
+  static uint32_t last_stamp = 0;
+
+  if (last_stamp > 0) {
+    float dt = (float)(stamp - last_stamp) * 1e-6;
+    ins_int_propagate(accel, dt);
+  }
+  last_stamp = stamp;
+}
+
+void ins_int_register(void)
+{
+  ins_register_impl(ins_int_init, ins_int_update_gps);
+
+  /*
+   * Subscribe to scaled IMU measurements and attach callbacks
+   */
+  AbiBindMsgIMU_ACCEL_INT32(INS_INT_IMU_ID, &accel_ev, accel_cb);
+}

sw/airborne/subsystems/ins/ins_int.h

@@ -65,6 +65,17 @@ struct InsInt {
 };
 
 /** global INS state */
-extern struct InsInt ins_impl;
+extern struct InsInt ins_int;
+
+extern void ins_int_init(void);
+extern void ins_int_propagate(struct Int32Vect3 *accel, float dt);
+extern void ins_int_update_gps(void);
+
+
+#ifndef DefaultInsImpl
+#define DefaultInsImpl ins_int
+#endif
+
+extern void ins_int_register(void);
 
 #endif /* INS_INT_H */