diff --git a/conf/modules/ins_float_invariant.xml b/conf/modules/ins_float_invariant.xml
index d8778a200f..d37411d957 100644
--- a/conf/modules/ins_float_invariant.xml
+++ b/conf/modules/ins_float_invariant.xml
@@ -47,9 +47,6 @@
-
-
-
diff --git a/sw/airborne/subsystems/ahrs/ahrs_aligner.c b/sw/airborne/subsystems/ahrs/ahrs_aligner.c
index 1b0919523a..c9d1931979 100644
--- a/sw/airborne/subsystems/ahrs/ahrs_aligner.c
+++ b/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.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;
+ ahrs_aligner_restart();
// 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
diff --git a/sw/airborne/subsystems/ahrs/ahrs_aligner.h b/sw/airborne/subsystems/ahrs/ahrs_aligner.h
index f5629e9fd7..dabd611a12 100644
--- a/sw/airborne/subsystems/ahrs/ahrs_aligner.h
+++ b/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 */
diff --git a/sw/airborne/subsystems/ins/ins_float_invariant.c b/sw/airborne/subsystems/ins/ins_float_invariant.c
index 15e75b819d..7577ef1bcd 100644
--- a/sw/airborne/subsystems/ins/ins_float_invariant.c
+++ b/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(<p_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(<p_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 INS_FINV_USE_UTM
+#if FIXEDWING_FIRMWARE
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 INS_FINV_USE_UTM
+ if ((gps.fix >= GPS_FIX_3D && _ins->is_aligned
+#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 * )********/
- _ins->corr.ME.x = (-_ins->gains.mv) * Ev.x + 0.;
- _ins->corr.ME.y = (-_ins->gains.mv) * Ev.y + 0.;
+ _ins->corr.ME.x = (-_ins->gains.mv) * Ev.x + 0.f;
+ _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 * ) ********/
- _ins->corr.NE.x = (-_ins->gains.nx) * Ex.x + 0.;
- _ins->corr.NE.y = (-_ins->gains.nx) * Ex.y + 0.;
+ _ins->corr.NE.x = (-_ins->gains.nx) * Ex.x + 0.f;
+ _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;
diff --git a/sw/airborne/subsystems/ins/ins_float_invariant_wrapper.c b/sw/airborne/subsystems/ins/ins_float_invariant_wrapper.c
index 79eb2d8c2b..f3dabad578 100644
--- a/sw/airborne/subsystems/ins/ins_float_invariant_wrapper.c
+++ b/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 */