From 221788281ae76685f1c95f007ee79b8b4cef8515 Mon Sep 17 00:00:00 2001 From: Felix Ruess Date: Mon, 2 Mar 2015 22:39:40 +0100 Subject: [PATCH] [ins] start converting float_invariant --- .../estimation/ins_float_invariant.xml | 28 +- sw/airborne/firmwares/fixedwing/main_ap.c | 14 - .../subsystems/ins/ins_float_invariant.c | 286 ++++++++++-------- .../subsystems/ins/ins_float_invariant.h | 13 +- 4 files changed, 174 insertions(+), 167 deletions(-) diff --git a/conf/settings/estimation/ins_float_invariant.xml b/conf/settings/estimation/ins_float_invariant.xml index 30ac06cd03..bb7da6c8c0 100644 --- a/conf/settings/estimation/ins_float_invariant.xml +++ b/conf/settings/estimation/ins_float_invariant.xml @@ -3,20 +3,20 @@ - - - - - - - - - - - - - - + + + + + + + + + + + + + + diff --git a/sw/airborne/firmwares/fixedwing/main_ap.c b/sw/airborne/firmwares/fixedwing/main_ap.c index 238e3e8542..c0a174491f 100644 --- a/sw/airborne/firmwares/fixedwing/main_ap.c +++ b/sw/airborne/firmwares/fixedwing/main_ap.c @@ -791,20 +791,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 AHRS_TRIGGERED_ATTITUDE_LOOP new_ins_attitude = 1; #endif diff --git a/sw/airborne/subsystems/ins/ins_float_invariant.c b/sw/airborne/subsystems/ins/ins_float_invariant.c index d14126bfd1..47c475f6de 100644 --- a/sw/airborne/subsystems/ins/ins_float_invariant.c +++ b/sw/airborne/subsystems/ins/ins_float_invariant.c @@ -158,14 +158,14 @@ bool_t log_started = FALSE; #endif -struct InsFloatInv ins_impl; +struct InsFloatInv ins_float_inv; /* earth gravity model */ static const struct FloatVect3 A = { 0.f, 0.f, 9.81f }; /* earth magnetic model */ //static const struct FloatVect3 B = { (float)(INS_H_X), (float)(INS_H_Y), (float)(INS_H_Z) }; -#define B ins_impl.mag_h +#define B ins_float_inv.mag_h /* barometer */ bool_t ins_baro_initialized; @@ -195,6 +195,12 @@ static void aligner_cb(uint8_t __attribute__((unused)) sender_id, struct Int32Rates *lp_gyro, struct Int32Vect3 *lp_accel, struct Int32Vect3 *lp_mag); +#ifndef INS_IMU_ID +#define INS_IMU_ID ABI_BROADCAST +#endif +static abi_event gyro_ev; +static void gyro_cb(uint8_t sender_id __attribute__((unused)), + uint32_t stamp, struct Int32Rates *gyro); /* gps */ bool_t ins_gps_fix_once; @@ -217,24 +223,24 @@ void float_quat_vmul_right(struct FloatQuat *mright, const struct FloatQuat *q, static inline void init_invariant_state(void) { // init state - float_quat_identity(&ins_impl.state.quat); - FLOAT_RATES_ZERO(ins_impl.state.bias); - FLOAT_VECT3_ZERO(ins_impl.state.pos); - FLOAT_VECT3_ZERO(ins_impl.state.speed); - ins_impl.state.as = 1.0f; - ins_impl.state.hb = 0.0f; + float_quat_identity(&ins_float_inv.state.quat); + FLOAT_RATES_ZERO(ins_float_inv.state.bias); + FLOAT_VECT3_ZERO(ins_float_inv.state.pos); + FLOAT_VECT3_ZERO(ins_float_inv.state.speed); + ins_float_inv.state.as = 1.0f; + ins_float_inv.state.hb = 0.0f; // init measures - FLOAT_VECT3_ZERO(ins_impl.meas.pos_gps); - FLOAT_VECT3_ZERO(ins_impl.meas.speed_gps); - ins_impl.meas.baro_alt = 0.0f; + FLOAT_VECT3_ZERO(ins_float_inv.meas.pos_gps); + FLOAT_VECT3_ZERO(ins_float_inv.meas.speed_gps); + ins_float_inv.meas.baro_alt = 0.0f; // init baro ins_baro_initialized = FALSE; ins_gps_fix_once = FALSE; } -void ins_init() +void ins_float_inv_init(void) { // init position @@ -268,31 +274,22 @@ void ins_init() init_invariant_state(); // init gains - ins_impl.gains.lv = INS_INV_LV; - ins_impl.gains.lb = INS_INV_LB; - ins_impl.gains.mv = INS_INV_MV; - ins_impl.gains.mvz = INS_INV_MVZ; - ins_impl.gains.mh = INS_INV_MH; - ins_impl.gains.nx = INS_INV_NX; - ins_impl.gains.nxz = INS_INV_NXZ; - ins_impl.gains.nh = INS_INV_NH; - ins_impl.gains.ov = INS_INV_OV; - ins_impl.gains.ob = INS_INV_OB; - ins_impl.gains.rv = INS_INV_RV; - ins_impl.gains.rh = INS_INV_RH; - ins_impl.gains.sh = INS_INV_SH; + ins_float_inv.gains.lv = INS_INV_LV; + ins_float_inv.gains.lb = INS_INV_LB; + ins_float_inv.gains.mv = INS_INV_MV; + ins_float_inv.gains.mvz = INS_INV_MVZ; + ins_float_inv.gains.mh = INS_INV_MH; + ins_float_inv.gains.nx = INS_INV_NX; + ins_float_inv.gains.nxz = INS_INV_NXZ; + ins_float_inv.gains.nh = INS_INV_NH; + ins_float_inv.gains.ov = INS_INV_OV; + ins_float_inv.gains.ob = INS_INV_OB; + ins_float_inv.gains.rv = INS_INV_RV; + ins_float_inv.gains.rh = INS_INV_RH; + ins_float_inv.gains.sh = INS_INV_SH; - ins_impl.is_aligned = FALSE; - ins_impl.reset = FALSE; - - // Bind to ABI messages - AbiBindMsgBARO_ABS(INS_BARO_ID, &baro_ev, baro_cb); - AbiBindMsgIMU_MAG_INT32(INS_MAG_ID, &mag_ev, mag_cb); - AbiBindMsgIMU_LOWPASSED(ABI_BROADCAST, &aligner_ev, aligner_cb); - -#if !INS_UPDATE_FW_ESTIMATOR && PERIODIC_TELEMETRY - register_periodic_telemetry(DefaultPeriodic, "INS_REF", send_ins_ref); -#endif + ins_float_inv.is_aligned = FALSE; + ins_float_inv.reset = FALSE; } @@ -347,15 +344,15 @@ void ins_float_invariant_align(struct Int32Rates *lp_gyro, struct Int32Vect3 *lp_mag) { /* Compute an initial orientation from accel and mag directly as quaternion */ - ahrs_float_get_quat_from_accel_mag(&ins_impl.state.quat, lp_accel, lp_mag); + ahrs_float_get_quat_from_accel_mag(&ins_float_inv.state.quat, lp_accel, lp_mag); /* use average gyro as initial value for bias */ struct FloatRates bias0; RATES_COPY(bias0, *lp_gyro); - RATES_FLOAT_OF_BFP(ins_impl.state.bias, bias0); + RATES_FLOAT_OF_BFP(ins_float_inv.state.bias, bias0); // ins and ahrs are now running - ins_impl.is_aligned = TRUE; + ins_float_inv.is_aligned = TRUE; } void ins_float_invariant_propagate(struct Int32Rates* gyro, struct Int32Vect3* accel, float dt) @@ -364,9 +361,9 @@ void ins_float_invariant_propagate(struct Int32Rates* gyro, struct Int32Vect3* a // realign all the filter if needed // a complete init cycle is required - if (ins_impl.reset) { - ins_impl.reset = FALSE; - ins_impl.is_aligned = FALSE; + if (ins_float_inv.reset) { + ins_float_inv.reset = FALSE; + ins_float_inv.is_aligned = FALSE; init_invariant_state(); } @@ -374,37 +371,37 @@ void ins_float_invariant_propagate(struct Int32Rates* gyro, struct Int32Vect3* a struct Int32Rates gyro_meas_body; struct Int32RMat *body_to_imu_rmat = orientationGetRMat_i(&imu.body_to_imu); int32_rmat_transp_ratemult(&gyro_meas_body, body_to_imu_rmat, gyro); - RATES_FLOAT_OF_BFP(ins_impl.cmd.rates, gyro_meas_body); + RATES_FLOAT_OF_BFP(ins_float_inv.cmd.rates, gyro_meas_body); struct Int32Vect3 accel_meas_body; int32_rmat_transp_vmult(&accel_meas_body, body_to_imu_rmat, accel); - ACCELS_FLOAT_OF_BFP(ins_impl.cmd.accel, accel_meas_body); + ACCELS_FLOAT_OF_BFP(ins_float_inv.cmd.accel, accel_meas_body); // update correction gains - error_output(&ins_impl); + error_output(&ins_float_inv); // propagate model struct inv_state new_state; runge_kutta_4_float((float *)&new_state, - (float *)&ins_impl.state, INV_STATE_DIM, - (float *)&ins_impl.cmd, INV_COMMAND_DIM, + (float *)&ins_float_inv.state, INV_STATE_DIM, + (float *)&ins_float_inv.cmd, INV_COMMAND_DIM, invariant_model, dt); - ins_impl.state = new_state; + ins_float_inv.state = new_state; // normalize quaternion - FLOAT_QUAT_NORMALIZE(ins_impl.state.quat); + FLOAT_QUAT_NORMALIZE(ins_float_inv.state.quat); // set global state - stateSetNedToBodyQuat_f(&ins_impl.state.quat); - RATES_DIFF(body_rates, ins_impl.cmd.rates, ins_impl.state.bias); + stateSetNedToBodyQuat_f(&ins_float_inv.state.quat); + RATES_DIFF(body_rates, ins_float_inv.cmd.rates, ins_float_inv.state.bias); stateSetBodyRates_f(&body_rates); - stateSetPositionNed_f(&ins_impl.state.pos); - stateSetSpeedNed_f(&ins_impl.state.speed); + stateSetPositionNed_f(&ins_float_inv.state.pos); + stateSetSpeedNed_f(&ins_float_inv.state.speed); // untilt accel and remove gravity struct FloatQuat q_b2n; - float_quat_invert(&q_b2n, &ins_impl.state.quat); + float_quat_invert(&q_b2n, &ins_float_inv.state.quat); struct FloatVect3 accel_n; - float_quat_vmult(&accel_n, &q_b2n, &ins_impl.cmd.accel); - VECT3_SMUL(accel_n, accel_n, 1. / (ins_impl.state.as)); + 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_ADD(accel_n, A); stateSetAccelNed_f((struct NedCoor_f *)&accel_n); @@ -412,28 +409,28 @@ void ins_float_invariant_propagate(struct Int32Rates* gyro, struct Int32Vect3* a #if SEND_INVARIANT_FILTER struct FloatEulers eulers; - FLOAT_EULERS_OF_QUAT(eulers, ins_impl.state.quat); + FLOAT_EULERS_OF_QUAT(eulers, ins_float_inv.state.quat); RunOnceEvery(3, - pprz_msg_send_INV_FILTER( - &(DefaultChannel).trans_tx, &(DefaultDevice).device, AC_ID, - &ins_impl.state.quat.qi, - &eulers.phi, - &eulers.theta, - &eulers.psi, - &ins_impl.state.speed.x, - &ins_impl.state.speed.y, - &ins_impl.state.speed.z, - &ins_impl.state.pos.x, - &ins_impl.state.pos.y, - &ins_impl.state.pos.z, - &ins_impl.state.bias.p, - &ins_impl.state.bias.q, - &ins_impl.state.bias.r, - &ins_impl.state.as, - &ins_impl.state.hb, - &ins_impl.meas.baro_alt, - &ins_impl.meas.pos_gps.z) - ); + pprz_msg_send_INV_FILTER(&(DefaultChannel).trans_tx, &(DefaultDevice).device, + AC_ID, + &ins_float_inv.state.quat.qi, + &eulers.phi, + &eulers.theta, + &eulers.psi, + &ins_float_inv.state.speed.x, + &ins_float_inv.state.speed.y, + &ins_float_inv.state.speed.z, + &ins_float_inv.state.pos.x, + &ins_float_inv.state.pos.y, + &ins_float_inv.state.pos.z, + &ins_float_inv.state.bias.p, + &ins_float_inv.state.bias.q, + &ins_float_inv.state.bias.r, + &ins_float_inv.state.as, + &ins_float_inv.state.hb, + &ins_float_inv.meas.baro_alt, + &ins_float_inv.meas.pos_gps.z) + ); #endif #if LOG_INVARIANT_FILTER @@ -446,66 +443,66 @@ void ins_float_invariant_propagate(struct Int32Rates* gyro, struct Int32Vect3* a } else { sdLogWriteLog(&pprzLogFile, "%.3f %.3f %.3f %.3f %.3f %.3f %.3f %.3f %.3f %.3f %.3f %.3f %.3f %.3f %.3f %.3f %.3f %.3f %.3f %.3f %.3f %.3f %.3f %.3f %.3f %.3f %.3f %.3f %.3f %.3f %.3f\n", - ins_impl.cmd.rates.p, - ins_impl.cmd.rates.q, - ins_impl.cmd.rates.r, - ins_impl.cmd.accel.x, - ins_impl.cmd.accel.y, - ins_impl.cmd.accel.z, - ins_impl.meas.pos_gps.x, - ins_impl.meas.pos_gps.y, - ins_impl.meas.pos_gps.z, - ins_impl.meas.speed_gps.x, - ins_impl.meas.speed_gps.y, - ins_impl.meas.speed_gps.z, - ins_impl.meas.mag.x, - ins_impl.meas.mag.y, - ins_impl.meas.mag.z, - ins_impl.meas.baro_alt, - ins_impl.state.quat.qi, - ins_impl.state.quat.qx, - ins_impl.state.quat.qy, - ins_impl.state.quat.qz, - ins_impl.state.bias.p, - ins_impl.state.bias.q, - ins_impl.state.bias.r, - ins_impl.state.speed.x, - ins_impl.state.speed.y, - ins_impl.state.speed.z, - ins_impl.state.pos.x, - ins_impl.state.pos.y, - ins_impl.state.pos.z, - ins_impl.state.hb, - ins_impl.state.as); + ins_float_inv.cmd.rates.p, + ins_float_inv.cmd.rates.q, + ins_float_inv.cmd.rates.r, + ins_float_inv.cmd.accel.x, + ins_float_inv.cmd.accel.y, + ins_float_inv.cmd.accel.z, + ins_float_inv.meas.pos_gps.x, + ins_float_inv.meas.pos_gps.y, + ins_float_inv.meas.pos_gps.z, + ins_float_inv.meas.speed_gps.x, + ins_float_inv.meas.speed_gps.y, + ins_float_inv.meas.speed_gps.z, + ins_float_inv.meas.mag.x, + ins_float_inv.meas.mag.y, + ins_float_inv.meas.mag.z, + ins_float_inv.meas.baro_alt, + ins_float_inv.state.quat.qi, + ins_float_inv.state.quat.qx, + ins_float_inv.state.quat.qy, + ins_float_inv.state.quat.qz, + ins_float_inv.state.bias.p, + ins_float_inv.state.bias.q, + ins_float_inv.state.bias.r, + ins_float_inv.state.speed.x, + ins_float_inv.state.speed.y, + ins_float_inv.state.speed.z, + ins_float_inv.state.pos.x, + ins_float_inv.state.pos.y, + ins_float_inv.state.pos.z, + ins_float_inv.state.hb, + ins_float_inv.state.as); } } #endif } -void ins_update_gps(void) +void ins_float_inv_update_gps(void) { - if (gps.fix == GPS_FIX_3D && ins_impl.is_aligned) { + if (gps.fix == GPS_FIX_3D && ins_float_inv.is_aligned) { ins_gps_fix_once = TRUE; #if INS_UPDATE_FW_ESTIMATOR if (state.utm_initialized_f) { // position (local ned) - ins_impl.meas.pos_gps.x = (gps.utm_pos.north / 100.0f) - state.utm_origin_f.north; - ins_impl.meas.pos_gps.y = (gps.utm_pos.east / 100.0f) - state.utm_origin_f.east; - ins_impl.meas.pos_gps.z = state.utm_origin_f.alt - (gps.hmsl / 1000.0f); + ins_float_inv.meas.pos_gps.x = (gps.utm_pos.north / 100.0f) - state.utm_origin_f.north; + ins_float_inv.meas.pos_gps.y = (gps.utm_pos.east / 100.0f) - state.utm_origin_f.east; + ins_float_inv.meas.pos_gps.z = state.utm_origin_f.alt - (gps.hmsl / 1000.0f); // speed - ins_impl.meas.speed_gps.x = gps.ned_vel.x / 100.0f; - ins_impl.meas.speed_gps.y = gps.ned_vel.y / 100.0f; - ins_impl.meas.speed_gps.z = gps.ned_vel.z / 100.0f; + ins_float_inv.meas.speed_gps.x = gps.ned_vel.x / 100.0f; + ins_float_inv.meas.speed_gps.y = gps.ned_vel.y / 100.0f; + ins_float_inv.meas.speed_gps.z = gps.ned_vel.z / 100.0f; } #else if (state.ned_initialized_f) { struct EcefCoor_f ecef_pos, ecef_vel; ECEF_FLOAT_OF_BFP(ecef_pos, gps.ecef_pos); - ned_of_ecef_point_f(&ins_impl.meas.pos_gps, &state.ned_origin_f, &ecef_pos); + ned_of_ecef_point_f(&ins_float_inv.meas.pos_gps, &state.ned_origin_f, &ecef_pos); ECEF_FLOAT_OF_BFP(ecef_vel, gps.ecef_vel); - ned_of_ecef_vect_f(&ins_impl.meas.speed_gps, &state.ned_origin_f, &ecef_vel); + ned_of_ecef_vect_f(&ins_float_inv.meas.speed_gps, &state.ned_origin_f, &ecef_vel); } #endif } @@ -542,7 +539,7 @@ static void baro_cb(uint8_t __attribute__((unused)) sender_id, float pressure) } i++; } else { /* normal update with baro measurement */ - ins_impl.meas.baro_alt = -pprz_isa_height_of_pressure(pressure, ins_qfe); // Z down + ins_float_inv.meas.baro_alt = -pprz_isa_height_of_pressure(pressure, ins_qfe); // Z down } } @@ -558,7 +555,7 @@ void ins_float_invariant_update_mag(struct Int32Vect3* mag) mag_frozen_count--; if (mag_frozen_count == 0) { // if mag is dead, better set measurements to zero - FLOAT_VECT3_ZERO(ins_impl.meas.mag); + FLOAT_VECT3_ZERO(ins_float_inv.meas.mag); mag_frozen_count = MAG_FROZEN_COUNT; } } else { @@ -567,7 +564,7 @@ void ins_float_invariant_update_mag(struct Int32Vect3* mag) struct Int32Vect3 mag_meas_body; // new values in body frame int32_rmat_transp_vmult(&mag_meas_body, body_to_imu_rmat, mag); - MAGS_FLOAT_OF_BFP(ins_impl.meas.mag, mag_meas_body); + MAGS_FLOAT_OF_BFP(ins_float_inv.meas.mag, mag_meas_body); // reset counter mag_frozen_count = MAG_FROZEN_COUNT; } @@ -606,7 +603,7 @@ static inline void invariant_model(float *o, const float *x, const int n, const /* qd = 0.5 * q * rates_unbiased = -0.5 * rates_unbiased * q */ float_quat_derivative(&s_dot.quat, &rates_unbiased, &(s->quat)); - float_quat_vmul_right(&tmp_quat, &(s->quat), &ins_impl.corr.LE); + 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); @@ -619,20 +616,20 @@ static inline void invariant_model(float *o, const float *x, const int n, const float_quat_vmult((struct FloatVect3 *)&s_dot.speed, &q_b2n, &(c->accel)); VECT3_SMUL(s_dot.speed, s_dot.speed, 1. / (s->as)); VECT3_ADD(s_dot.speed, A); - VECT3_ADD(s_dot.speed, ins_impl.corr.ME); + VECT3_ADD(s_dot.speed, ins_float_inv.corr.ME); /* dot_X = V + NE */ - VECT3_SUM(s_dot.pos, s->speed, ins_impl.corr.NE); + VECT3_SUM(s_dot.pos, s->speed, ins_float_inv.corr.NE); /* bias_dot = q-1 * (OE) * q */ - float_quat_vmult(&tmp_vect, &(s->quat), &ins_impl.corr.OE); + float_quat_vmult(&tmp_vect, &(s->quat), &ins_float_inv.corr.OE); RATES_ASSIGN(s_dot.bias, tmp_vect.x, tmp_vect.y, tmp_vect.z); /* as_dot = as * RE */ - s_dot.as = (s->as) * (ins_impl.corr.RE); + s_dot.as = (s->as) * (ins_float_inv.corr.RE); /* hb_dot = SE */ - s_dot.hb = ins_impl.corr.SE; + s_dot.hb = ins_float_inv.corr.SE; // set output memcpy(o, &s_dot, n * sizeof(float)); @@ -669,7 +666,7 @@ 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_impl.is_aligned + if ((gps.fix == GPS_FIX_3D && ins_float_inv.is_aligned #if INS_UPDATE_FW_ESTIMATOR && state.utm_initialized_f #else @@ -747,17 +744,25 @@ void float_quat_vmul_right(struct FloatQuat *mright, const struct FloatQuat *q, QUAT_ASSIGN(*mright, qi, v2.x, v2.y, v2.z); } -/** temporary functions for INS interface compatibility */ -void ins_propagate(float dt) +static void gyro_cb(uint8_t sender_id __attribute__((unused)), + uint32_t stamp, struct Int32Rates *gyro) { - ins_float_invariant_propagate(&imu.gyro, &imu.accel, dt); + PRINT_CONFIG_MSG("Calculating dt for INS float_invariant 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_float_invariant_propagate(gyro, &imu.accel, dt); + } + last_stamp = stamp; } static void mag_cb(uint8_t sender_id __attribute__((unused)), uint32_t stamp __attribute__((unused)), struct Int32Vect3 *mag) { - if (ins_impl.is_aligned) { + if (ins_float_inv.is_aligned) { ins_float_invariant_update_mag(mag); } } @@ -767,7 +772,22 @@ static void aligner_cb(uint8_t __attribute__((unused)) sender_id, struct Int32Rates *lp_gyro, struct Int32Vect3 *lp_accel, struct Int32Vect3 *lp_mag) { - if (!ins_impl.is_aligned) { + if (!ins_float_inv.is_aligned) { ins_float_invariant_align(lp_gyro, lp_accel, lp_mag); } } + +void ins_float_inv_register(void) +{ + ins_register_impl(ins_float_inv_init, ins_float_inv_update_gps); + + // Bind to ABI messages + AbiBindMsgBARO_ABS(INS_BARO_ID, &baro_ev, baro_cb); + AbiBindMsgIMU_MAG_INT32(INS_MAG_ID, &mag_ev, mag_cb); + AbiBindMsgIMU_GYRO_INT32(INS_IMU_ID, &gyro_ev, gyro_cb); + AbiBindMsgIMU_LOWPASSED(ABI_BROADCAST, &aligner_ev, aligner_cb); + +#if !INS_UPDATE_FW_ESTIMATOR && PERIODIC_TELEMETRY + register_periodic_telemetry(DefaultPeriodic, "INS_REF", send_ins_ref); +#endif +} diff --git a/sw/airborne/subsystems/ins/ins_float_invariant.h b/sw/airborne/subsystems/ins/ins_float_invariant.h index d83dc4f29f..cc8b8fed92 100644 --- a/sw/airborne/subsystems/ins/ins_float_invariant.h +++ b/sw/airborne/subsystems/ins/ins_float_invariant.h @@ -115,15 +115,16 @@ struct InsFloatInv { bool_t is_aligned; }; -extern struct InsFloatInv ins_impl; +extern struct InsFloatInv ins_float_inv; -#define DefaultAhrsImpl ins_impl +#define DefaultAhrsImpl ins_float_inv +#define DefaultInsImpl ins_float_inv -/** dummy for now, will be removed when not using ahrs interface anymore */ -static inline void ins_impl_register(void) {} +extern void ins_float_inv_register(void); +extern void ins_float_inv_update_gps(void); -/** Currently still called from ins_propagate (declared in INS interface). */ -extern void ins_float_invariant_propagate(struct Int32Rates* gyro, struct Int32Vect3* accel, float dt); +extern void ins_float_invariant_propagate(struct Int32Rates* gyro, + struct Int32Vect3* accel, float dt); /** called on IMU_LOWPASSED ABI message */ extern void ins_float_invariant_align(struct Int32Rates *lp_gyro,