[ins_invariant] reset state position and speed properly

It prevents the filter to jump to the new position, which can create a
wrong estimation of the accelerometer scale bias.
Aligner is restarted if needed.

Also fix float constants and other small details.

conf/modules/ins_float_invariant.xml

@@ -47,9 +47,6 @@
     <define name="INS_TYPE_H" value="subsystems/ins/ins_float_invariant_wrapper.h" type="string"/>
     <test firmware="rotorcraft"/>
   </makefile>
-  <makefile target="ap|nps" firmware="fixedwing">
-    <define name="INS_FINV_USE_UTM"/>
-  </makefile>
   <makefile target="sim">
     <define name="AHRS_TYPE_H" value="subsystems/ahrs/ahrs_sim.h" type="string"/>
     <define name="USE_AHRS"/>

sw/airborne/subsystems/ahrs/ahrs_aligner.c

@@ -80,14 +80,7 @@ static void send_aligner(struct transport_tx *trans, struct link_device *dev)
 
 void ahrs_aligner_init(void)
 {
-
+  ahrs_aligner_restart();
-  ahrs_aligner.status = AHRS_ALIGNER_RUNNING;
-  INT_RATES_ZERO(gyro_sum);
-  INT_VECT3_ZERO(accel_sum);
-  INT_VECT3_ZERO(mag_sum);
-  samples_idx = 0;
-  ahrs_aligner.noise = 0;
-  ahrs_aligner.low_noise_cnt = 0;
 
   // for now: only bind to gyro message and still read from global imu struct
   AbiBindMsgIMU_GYRO_INT32(AHRS_ALIGNER_IMU_ID, &gyro_ev, gyro_cb);
@@ -97,6 +90,17 @@ void ahrs_aligner_init(void)
 #endif
 }
 
+void ahrs_aligner_restart(void)
+{
+  ahrs_aligner.status = AHRS_ALIGNER_RUNNING;
+  INT_RATES_ZERO(gyro_sum);
+  INT_VECT3_ZERO(accel_sum);
+  INT_VECT3_ZERO(mag_sum);
+  samples_idx = 0;
+  ahrs_aligner.noise = 0;
+  ahrs_aligner.low_noise_cnt = 0;
+}
+
 #ifndef LOW_NOISE_THRESHOLD
 #define LOW_NOISE_THRESHOLD 90000
 #endif

sw/airborne/subsystems/ahrs/ahrs_aligner.h

@@ -48,6 +48,7 @@ struct AhrsAligner {
 extern struct AhrsAligner ahrs_aligner;
 
 extern void ahrs_aligner_init(void);
+extern void ahrs_aligner_restart(void);
 extern void ahrs_aligner_run(void);
 
 #endif /* AHRS_ALIGNER_H */

sw/airborne/subsystems/ins/ins_float_invariant.c

@@ -35,7 +35,7 @@
 
 #include "generated/airframe.h"
 #include "generated/flight_plan.h"
-#if INS_FINV_USE_UTM
+#if FIXEDWING_FIRMWARE
 #include "firmwares/fixedwing/nav.h"
 #endif
 
@@ -82,55 +82,55 @@ bool log_started = false;
 // Default values for the tuning gains
 // Tuning parameter of speed error on attitude (e-2)
 #ifndef INS_INV_LV
-#define INS_INV_LV 2.
+#define INS_INV_LV 2.f
 #endif
 // Tuning parameter of mag error on attitude (e-2)
 #ifndef INS_INV_LB
-#define INS_INV_LB 6.
+#define INS_INV_LB 6.f
 #endif
 // Tuning parameter of horizontal speed error on speed
 #ifndef INS_INV_MV
-#define INS_INV_MV 8.
+#define INS_INV_MV 8.f
 #endif
 // Tuning parameter of vertical speed error on speed
 #ifndef INS_INV_MVZ
-#define INS_INV_MVZ 15.
+#define INS_INV_MVZ 15.f
 #endif
 // Tuning parameter of baro error on vertical speed
 #ifndef INS_INV_MH
-#define INS_INV_MH 0.2
+#define INS_INV_MH 0.2f
 #endif
 // Tuning parameter of horizontal position error on position
 #ifndef INS_INV_NX
-#define INS_INV_NX 0.8
+#define INS_INV_NX 0.8f
 #endif
 // Tuning parameter of vertical position error on position
 #ifndef INS_INV_NXZ
-#define INS_INV_NXZ 0.5
+#define INS_INV_NXZ 0.5f
 #endif
 // Tuning parameter of baro error on vertical position
 #ifndef INS_INV_NH
-#define INS_INV_NH 1.2
+#define INS_INV_NH 1.2f
 #endif
 // Tuning parameter of speed error on gyro biases (e-3)
 #ifndef INS_INV_OV
-#define INS_INV_OV 1.2
+#define INS_INV_OV 1.2f
 #endif
 // Tuning parameter of mag error on gyro biases (e-3)
 #ifndef INS_INV_OB
-#define INS_INV_OB 1.
+#define INS_INV_OB 1.f
 #endif
 // Tuning parameter of speed error on accel biases (e-2)
 #ifndef INS_INV_RV
-#define INS_INV_RV 4.
+#define INS_INV_RV 4.f
 #endif
 // Tuning parameter of baro error on accel biases (vertical projection) (e-8)
 #ifndef INS_INV_RH
-#define INS_INV_RH 8.
+#define INS_INV_RH 8.f
 #endif
 // Tuning parameter of baro error on baro bias
 #ifndef INS_INV_SH
-#define INS_INV_SH 0.01
+#define INS_INV_SH 0.01f
 #endif
 
 
@@ -220,7 +220,7 @@ void ins_float_invariant_init(void)
 {
 
   // init position
-#if INS_FINV_USE_UTM
+#if FIXEDWING_FIRMWARE
   struct UtmCoor_f utm0;
   utm0.north = (float)nav_utm_north0;
   utm0.east = (float)nav_utm_east0;
@@ -281,7 +281,7 @@ void ins_float_invariant_init(void)
 
 void ins_reset_local_origin(void)
 {
-#if INS_FINV_USE_UTM
+#if FIXEDWING_FIRMWARE
   struct UtmCoor_f utm = utm_float_from_gps(&gps, 0);
   // reset state UTM ref
   stateSetLocalUtmOrigin_f(&utm);
@@ -291,11 +291,14 @@ void ins_reset_local_origin(void)
   ltp_def.hmsl = gps.hmsl;
   stateSetLocalOrigin_i(&ltp_def);
 #endif
+  // reset state position and velocity to zero
+  FLOAT_VECT3_ZERO(ins_float_inv.state.pos);
+  FLOAT_VECT3_ZERO(ins_float_inv.state.speed);
 }
 
 void ins_reset_altitude_ref(void)
 {
-#if INS_FINV_USE_UTM
+#if FIXEDWING_FIRMWARE
   struct UtmCoor_f utm = state.utm_origin_f;
   utm.alt = gps.hmsl / 1000.0f;
   stateSetLocalUtmOrigin_f(&utm);
@@ -310,6 +313,9 @@ void ins_reset_altitude_ref(void)
   ltp_def.hmsl = gps.hmsl;
   stateSetLocalOrigin_i(&ltp_def);
 #endif
+  // reset vertical position and velocity to zero
+  ins_float_inv.state.pos.z = 0.f;
+  ins_float_inv.state.speed.z = 0.f;
 }
 
 void ins_float_invariant_align(struct FloatRates *lp_gyro,
@@ -380,7 +386,7 @@ void ins_float_invariant_propagate(struct FloatRates* gyro, struct FloatVect3* a
   float_quat_invert(&q_b2n, &ins_float_inv.state.quat);
   struct FloatVect3 accel_n;
   float_quat_vmult(&accel_n, &q_b2n, &ins_float_inv.cmd.accel);
-  VECT3_SMUL(accel_n, accel_n, 1. / (ins_float_inv.state.as));
+  VECT3_SMUL(accel_n, accel_n, 1.f / (ins_float_inv.state.as));
   VECT3_ADD(accel_n, A);
   stateSetAccelNed_f((struct NedCoor_f *)&accel_n);
 
@@ -440,9 +446,7 @@ void ins_float_invariant_update_gps(struct GpsState *gps_s)
 {
 
   if (gps_s->fix >= GPS_FIX_3D && ins_float_inv.is_aligned) {
-    ins_gps_fix_once = true;
+#if FIXEDWING_FIRMWARE
-
-#if INS_FINV_USE_UTM
     if (state.utm_initialized_f) {
       struct UtmCoor_f utm = utm_float_from_gps(gps_s, nav_utm_zone0);
       // position (local ned)
@@ -465,6 +469,12 @@ void ins_float_invariant_update_gps(struct GpsState *gps_s)
       ned_of_ecef_vect_f(&ins_float_inv.meas.speed_gps, &state.ned_origin_f, &ecef_vel);
     }
 #endif
+    // when getting first GPS pos (or after reset), set state to current pos and speed
+    if (!ins_gps_fix_once) {
+      ins_float_inv.state.pos = ins_float_inv.meas.pos_gps;
+      ins_float_inv.state.speed = ins_float_inv.meas.speed_gps;
+      ins_gps_fix_once = true;
+    }
   }
 
 #if !USE_MAGNETOMETER
@@ -504,7 +514,7 @@ void ins_float_invariant_update_baro(float pressure)
       baro_prev = pressure;
     }
     baro_moy = (baro_moy * (i - 1) + pressure) / i;
-    alpha = (10.*alpha + (baro_moy - baro_prev)) / (11.0f);
+    alpha = (10.f*alpha + (baro_moy - baro_prev)) / (11.0f);
     baro_prev = baro_moy;
     // test stop condition
     if (fabs(alpha) < 0.1f) {
@@ -574,7 +584,7 @@ static inline void invariant_model(float *o, const float *x, const int n, const
   float_quat_vmul_right(&tmp_quat, &(s->quat), &ins_float_inv.corr.LE);
   QUAT_ADD(s_dot.quat, tmp_quat);
 
-  float norm2_r = 1. - FLOAT_QUAT_NORM2(s->quat);
+  float norm2_r = 1.f - FLOAT_QUAT_NORM2(s->quat);
   QUAT_SMUL(tmp_quat, s->quat, norm2_r);
   QUAT_ADD(s_dot.quat, tmp_quat);
 
@@ -582,7 +592,7 @@ static inline void invariant_model(float *o, const float *x, const int n, const
   struct FloatQuat q_b2n;
   float_quat_invert(&q_b2n, &(s->quat));
   float_quat_vmult((struct FloatVect3 *)&s_dot.speed, &q_b2n, &(c->accel));
-  VECT3_SMUL(s_dot.speed, s_dot.speed, 1. / (s->as));
+  VECT3_SMUL(s_dot.speed, s_dot.speed, 1.f / (s->as));
   VECT3_ADD(s_dot.speed, A);
   VECT3_ADD(s_dot.speed, ins_float_inv.corr.ME);
 
@@ -624,7 +634,7 @@ static inline void error_output(struct InsFloatInv *_ins)
   float_quat_vmult(&YBt, &q_b2n, &(_ins->meas.mag));
 
   float_quat_vmult(&I, &q_b2n, &(_ins->cmd.accel));
-  VECT3_SMUL(I, I, 1. / (_ins->state.as));
+  VECT3_SMUL(I, I, 1.f / (_ins->state.as));
 
   /*--------- E = ( ŷ - y ) ----------*/
   /* Eb = ( B - YBt ) */
@@ -632,8 +642,8 @@ static inline void error_output(struct InsFloatInv *_ins)
 
   // pos and speed error only if GPS data are valid
   // or while waiting first GPS data to prevent diverging
-  if ((gps.fix >= GPS_FIX_3D && ins_float_inv.is_aligned
+  if ((gps.fix >= GPS_FIX_3D && _ins->is_aligned
-#if INS_FINV_USE_UTM
+#if FIXEDWING_FIRMWARE
        && state.utm_initialized_f
 #else
        && state.ned_initialized_f
@@ -653,34 +663,34 @@ static inline void error_output(struct InsFloatInv *_ins)
   /*--------------Gains--------------*/
 
   /**** LvEv + LbEb = -lvIa x Ev +  lb < B x Eb, Ia > Ia *****/
-  VECT3_SMUL(Itemp, I, -_ins->gains.lv / 100.);
+  VECT3_SMUL(Itemp, I, -_ins->gains.lv / 100.f);
   VECT3_CROSS_PRODUCT(Evtemp, Itemp, Ev);
 
   VECT3_CROSS_PRODUCT(Ebtemp, B, Eb);
   temp = VECT3_DOT_PRODUCT(Ebtemp, I);
-  temp = (_ins->gains.lb / 100.) * temp;
+  temp = (_ins->gains.lb / 100.f) * temp;
 
   VECT3_SMUL(Ebtemp, I, temp);
   VECT3_ADD(Evtemp, Ebtemp);
   VECT3_COPY(_ins->corr.LE, Evtemp);
 
   /***** MvEv + MhEh = -mv * Ev + (-mh * <Eh,e3>)********/
-  _ins->corr.ME.x = (-_ins->gains.mv) * Ev.x + 0.;
+  _ins->corr.ME.x = (-_ins->gains.mv) * Ev.x + 0.f;
-  _ins->corr.ME.y = (-_ins->gains.mv) * Ev.y + 0.;
+  _ins->corr.ME.y = (-_ins->gains.mv) * Ev.y + 0.f;
   _ins->corr.ME.z = ((-_ins->gains.mvz) * Ev.z) + ((-_ins->gains.mh) * Eh);
 
   /****** NxEx + NhEh = -nx * Ex + (-nh * <Eh, e3>) ********/
-  _ins->corr.NE.x = (-_ins->gains.nx) * Ex.x + 0.;
+  _ins->corr.NE.x = (-_ins->gains.nx) * Ex.x + 0.f;
-  _ins->corr.NE.y = (-_ins->gains.nx) * Ex.y + 0.;
+  _ins->corr.NE.y = (-_ins->gains.nx) * Ex.y + 0.f;
   _ins->corr.NE.z = ((-_ins->gains.nxz) * Ex.z) + ((-_ins->gains.nh) * Eh);
 
   /****** OvEv + ObEb = ovIa x Ev - ob < B x Eb, Ia > Ia ********/
-  VECT3_SMUL(Itemp, I, _ins->gains.ov / 1000.);
+  VECT3_SMUL(Itemp, I, _ins->gains.ov / 1000.f);
   VECT3_CROSS_PRODUCT(Evtemp, Itemp, Ev);
 
   VECT3_CROSS_PRODUCT(Ebtemp, B, Eb);
   temp = VECT3_DOT_PRODUCT(Ebtemp, I);
-  temp = (-_ins->gains.ob / 1000.) * temp;
+  temp = (-_ins->gains.ob / 1000.f) * temp;
 
   VECT3_SMUL(Ebtemp, I, temp);
   VECT3_ADD(Evtemp, Ebtemp);
@@ -688,7 +698,7 @@ static inline void error_output(struct InsFloatInv *_ins)
 
   /* a scalar */
   /****** RvEv + RhEh = rv < Ia, Ev > + (-rhEh) **************/
-  _ins->corr.RE = ((_ins->gains.rv / 100.) * VECT3_DOT_PRODUCT(Ev, I)) + ((-_ins->gains.rh / 10000.) * Eh);
+  _ins->corr.RE = ((_ins->gains.rv / 100.f) * VECT3_DOT_PRODUCT(Ev, I)) + ((-_ins->gains.rh / 10000.f) * Eh);
 
   /****** ShEh ******/
   _ins->corr.SE = (_ins->gains.sh) * Eh;

sw/airborne/subsystems/ins/ins_float_invariant_wrapper.c

@@ -136,6 +136,12 @@ static void gyro_cb(uint8_t sender_id __attribute__((unused)),
   struct FloatRates gyro_f;
   RATES_FLOAT_OF_BFP(gyro_f, *gyro);
 
+#if USE_AHRS_ALIGNER
+  if (ins_float_inv.reset) {
+    ahrs_aligner_restart();
+  }
+#endif
+
 #if USE_AUTO_INS_FREQ || !defined(INS_PROPAGATE_FREQUENCY)
   PRINT_CONFIG_MSG("Calculating dt for INS float_invariant propagation.")
   /* timestamp in usec when last callback was received */