diff --git a/conf/airframes/ENAC/conf_enac.xml b/conf/airframes/ENAC/conf_enac.xml index fe532310d2..88ce782000 100644 --- a/conf/airframes/ENAC/conf_enac.xml +++ b/conf/airframes/ENAC/conf_enac.xml @@ -17,7 +17,7 @@ radio="radios/cockpitSX.xml" telemetry="telemetry/default_rotorcraft.xml" flight_plan="flight_plans/rotorcraft_basic_safety.xml" - settings="settings/rotorcraft_basic.xml settings/estimation/ins_float_invariant.xml" + settings="settings/rotorcraft_basic.xml" settings_modules="modules/stabilization_int_quat.xml modules/nav_basic_rotorcraft.xml modules/guidance_rotorcraft.xml modules/gps_ubx_ucenter.xml modules/gps.xml modules/imu_common.xml" gui_color="yellow" /> @@ -72,7 +72,7 @@ radio="radios/R617FS_5ch_neg.xml" telemetry="telemetry/default_fixedwing_imu.xml" flight_plan="flight_plans/basic.xml" - settings="settings/fixedwing_basic.xml settings/estimation/ins_float_invariant.xml [settings/nps.xml]" + settings="settings/fixedwing_basic.xml [settings/nps.xml]" settings_modules="[modules/gps_ubx_ucenter.xml] modules/gps.xml modules/nav_basic_fw.xml modules/guidance_full_pid_fw.xml modules/stabilization_adaptive_fw.xml modules/airspeed_adc.xml modules/imu_common.xml modules/ahrs_float_dcm.xml" gui_color="#ffff7d7d0000" /> diff --git a/conf/airframes/ENAC/fixed-wing/jp.xml b/conf/airframes/ENAC/fixed-wing/jp.xml index 22b673e795..2436cfd0b6 100644 --- a/conf/airframes/ENAC/fixed-wing/jp.xml +++ b/conf/airframes/ENAC/fixed-wing/jp.xml @@ -3,7 +3,6 @@ - @@ -22,6 +21,8 @@ + @@ -38,7 +39,7 @@ - + diff --git a/conf/modules/ins_float_invariant.xml b/conf/modules/ins_float_invariant.xml index cbab1f6a7a..9bf6453797 100644 --- a/conf/modules/ins_float_invariant.xml +++ b/conf/modules/ins_float_invariant.xml @@ -9,6 +9,26 @@ + + + + + + + + + + + + + + + + + + + +
diff --git a/conf/settings/estimation/ins_float_invariant.xml b/conf/settings/estimation/ins_float_invariant.xml deleted file mode 100644 index 213756a6ad..0000000000 --- a/conf/settings/estimation/ins_float_invariant.xml +++ /dev/null @@ -1,22 +0,0 @@ - - - - - - - - - - - - - - - - - - - - - - diff --git a/sw/airborne/subsystems/ins/ins_float_invariant.c b/sw/airborne/subsystems/ins/ins_float_invariant.c index 48e41acedb..15e75b819d 100644 --- a/sw/airborne/subsystems/ins/ins_float_invariant.c +++ b/sw/airborne/subsystems/ins/ins_float_invariant.c @@ -149,6 +149,11 @@ bool ins_baro_initialized; /* gps */ bool ins_gps_fix_once; +/* min speed to update heading from GPS when mag are not used */ +#ifndef INS_INV_HEADING_UPDATE_GPS_MIN_SPEED +#define INS_INV_HEADING_UPDATE_GPS_MIN_SPEED 5.f +#endif + /* error computation */ static inline void error_output(struct InsFloatInv *_ins); @@ -237,9 +242,15 @@ void ins_float_invariant_init(void) stateSetLocalOrigin_i(<p_def); #endif +#if USE_MAGNETOMETER B.x = INS_H_X; B.y = INS_H_Y; B.z = INS_H_Z; +#else + B.x = 1.f; // when using GPS as magnetometer, mag field is true north + B.y = 0.f; + B.z = 0.f; +#endif // init state and measurements init_invariant_state(); @@ -303,10 +314,15 @@ void ins_reset_altitude_ref(void) void ins_float_invariant_align(struct FloatRates *lp_gyro, struct FloatVect3 *lp_accel, - struct FloatVect3 *lp_mag) + struct FloatVect3 *lp_mag __attribute__((unused))) { +#if USE_MAGNETOMETER /* Compute an initial orientation from accel and mag directly as quaternion */ ahrs_float_get_quat_from_accel_mag(&ins_float_inv.state.quat, lp_accel, lp_mag); +#else + /* Compute an initial orientation from accel only directly as quaternion */ + ahrs_float_get_quat_from_accel(&ins_float_inv.state.quat, lp_accel); +#endif /* use average gyro as initial value for bias */ ins_float_inv.state.bias = *lp_gyro; @@ -451,12 +467,30 @@ void ins_float_invariant_update_gps(struct GpsState *gps_s) #endif } +#if !USE_MAGNETOMETER + // Use pseudo-mag rebuilt from GPS horizontal velocity + struct FloatVect2 vel = { ins_float_inv.meas.speed_gps.x, ins_float_inv.meas.speed_gps.y }; + float vel_norm = float_vect2_norm(&vel); + if (vel_norm > INS_INV_HEADING_UPDATE_GPS_MIN_SPEED) { + struct FloatVect3 pseudo_mag = { + vel.x / vel_norm, + -vel.y / vel_norm, + 0.f + }; + ins_float_invariant_update_mag(&pseudo_mag); + } + else { + // if speed is tool low, better set measurements to zero + FLOAT_VECT3_ZERO(ins_float_inv.meas.mag); + } +#endif + } void ins_float_invariant_update_baro(float pressure) { - static float ins_qfe = 101325.0f; + static float ins_qfe = PPRZ_ISA_SEA_LEVEL_PRESSURE; static float alpha = 10.0f; static int32_t i = 1; static float baro_moy = 0.0f; @@ -473,7 +507,7 @@ void ins_float_invariant_update_baro(float pressure) alpha = (10.*alpha + (baro_moy - baro_prev)) / (11.0f); baro_prev = baro_moy; // test stop condition - if (fabs(alpha) < 0.005f) { + if (fabs(alpha) < 0.1f) { ins_qfe = baro_moy; ins_baro_initialized = true; } @@ -532,14 +566,6 @@ static inline void invariant_model(float *o, const float *x, const int n, const struct FloatVect3 tmp_vect; struct FloatQuat tmp_quat; - // test accel sensitivity - if (fabs(s->as) < 0.1) { - // too small, return x_dot = 0 to avoid division by 0 - float_vect_zero(o, n); - // TODO set ins state to error - return; - } - /* dot_q = 0.5 * q * (x_rates - x_bias) + LE * q + (1 - ||q||^2) * q */ RATES_DIFF(rates_unbiased, c->rates, s->bias); /* qd = 0.5 * q * rates_unbiased = -0.5 * rates_unbiased * q */ @@ -569,6 +595,10 @@ static inline void invariant_model(float *o, const float *x, const int n, const /* as_dot = as * RE */ s_dot.as = (s->as) * (ins_float_inv.corr.RE); + // keep as in a reasonable range, so 50% around the nominal value + if (s->as < 0.5f || s->as > 1.5f) { + s_dot.as = 0.f; + } /* hb_dot = SE */ s_dot.hb = ins_float_inv.corr.SE; @@ -588,12 +618,6 @@ static inline void error_output(struct InsFloatInv *_ins) float Eh; float temp; - // test accel sensitivity - if (fabs(_ins->state.as) < 0.1) { - // too small, don't do anything to avoid division by 0 - return; - } - /* YBt = q * yB * q-1 */ struct FloatQuat q_b2n; float_quat_invert(&q_b2n, &(_ins->state.quat)); diff --git a/sw/airborne/subsystems/ins/ins_float_invariant_wrapper.c b/sw/airborne/subsystems/ins/ins_float_invariant_wrapper.c index 3719acf18c..79eb2d8c2b 100644 --- a/sw/airborne/subsystems/ins/ins_float_invariant_wrapper.c +++ b/sw/airborne/subsystems/ins/ins_float_invariant_wrapper.c @@ -29,6 +29,9 @@ #include "subsystems/abi.h" #include "mcu_periph/sys_time.h" #include "message_pragmas.h" +#if USE_AHRS_ALIGNER +#include "subsystems/ahrs/ahrs_aligner.h" +#endif #ifndef INS_FINV_FILTER_ID #define INS_FINV_FILTER_ID 2 @@ -89,10 +92,14 @@ PRINT_CONFIG_VAR(INS_FINV_BARO_ID) PRINT_CONFIG_VAR(INS_FINV_IMU_ID) /** magnetometer */ +#if USE_MAGNETOMETER #ifndef INS_FINV_MAG_ID #define INS_FINV_MAG_ID ABI_BROADCAST #endif PRINT_CONFIG_VAR(INS_FINV_MAG_ID) +#else +PRINT_CONFIG_MSG("INS invariant use GPS heading as magnetometer") +#endif /** ABI binding for gps data. * Used for GPS ABI messages. @@ -103,12 +110,14 @@ PRINT_CONFIG_VAR(INS_FINV_MAG_ID) PRINT_CONFIG_VAR(INS_FINV_GPS_ID) static abi_event baro_ev; -static abi_event mag_ev; static abi_event gyro_ev; static abi_event accel_ev; static abi_event aligner_ev; static abi_event body_to_imu_ev; +#if USE_MAGNETOMETER +static abi_event mag_ev; static abi_event geo_mag_ev; +#endif static abi_event gps_ev; static void baro_cb(uint8_t __attribute__((unused)) sender_id, __attribute__((unused)) uint32_t stamp, float pressure) @@ -154,17 +163,6 @@ static void accel_cb(uint8_t sender_id __attribute__((unused)), ACCELS_FLOAT_OF_BFP(ins_finv_accel, *accel); } -static void mag_cb(uint8_t sender_id __attribute__((unused)), - uint32_t stamp __attribute__((unused)), - struct Int32Vect3 *mag) -{ - if (ins_float_inv.is_aligned) { - struct FloatVect3 mag_f; - MAGS_FLOAT_OF_BFP(mag_f, *mag); - ins_float_invariant_update_mag(&mag_f); - } -} - static void aligner_cb(uint8_t __attribute__((unused)) sender_id, uint32_t stamp __attribute__((unused)), struct Int32Rates *lp_gyro, struct Int32Vect3 *lp_accel, @@ -188,10 +186,23 @@ static void body_to_imu_cb(uint8_t sender_id __attribute__((unused)), ins_float_inv_set_body_to_imu_quat(q_b2i_f); } +#if USE_MAGNETOMETER +static void mag_cb(uint8_t sender_id __attribute__((unused)), + uint32_t stamp __attribute__((unused)), + struct Int32Vect3 *mag) +{ + if (ins_float_inv.is_aligned) { + struct FloatVect3 mag_f; + MAGS_FLOAT_OF_BFP(mag_f, *mag); + ins_float_invariant_update_mag(&mag_f); + } +} + static void geo_mag_cb(uint8_t sender_id __attribute__((unused)), struct FloatVect3 *h) { ins_float_inv.mag_h = *h; } +#endif static void gps_cb(uint8_t sender_id __attribute__((unused)), uint32_t stamp __attribute__((unused)), @@ -203,16 +214,23 @@ static void gps_cb(uint8_t sender_id __attribute__((unused)), void ins_float_invariant_wrapper_init(void) { + // aligner +#if USE_AHRS_ALIGNER + ahrs_aligner_init(); +#endif + ins_float_invariant_init(); // Bind to ABI messages AbiBindMsgBARO_ABS(INS_FINV_BARO_ID, &baro_ev, baro_cb); - AbiBindMsgIMU_MAG_INT32(INS_FINV_MAG_ID, &mag_ev, mag_cb); AbiBindMsgIMU_GYRO_INT32(INS_FINV_IMU_ID, &gyro_ev, gyro_cb); AbiBindMsgIMU_ACCEL_INT32(INS_FINV_IMU_ID, &accel_ev, accel_cb); AbiBindMsgIMU_LOWPASSED(INS_FINV_IMU_ID, &aligner_ev, aligner_cb); AbiBindMsgBODY_TO_IMU_QUAT(INS_FINV_IMU_ID, &body_to_imu_ev, body_to_imu_cb); +#if USE_MAGNETOMETER + AbiBindMsgIMU_MAG_INT32(INS_FINV_MAG_ID, &mag_ev, mag_cb); AbiBindMsgGEO_MAG(ABI_BROADCAST, &geo_mag_ev, geo_mag_cb); +#endif AbiBindMsgGPS(INS_FINV_GPS_ID, &gps_ev, gps_cb); #if PERIODIC_TELEMETRY && !INS_FINV_USE_UTM