Mirrors_Framework/paparazzi
1
0
Fork 0
mirror of https://github.com/paparazzi/paparazzi.git synced 2026-05-09 22:49:53 +08:00
Code Issues Actions 26 Packages Projects Releases Wiki Activity

[ins] start converting float_invariant

Browse Source
This commit is contained in:
Felix Ruess
2015-03-02 22:39:40 +01:00
parent 9a1888b214
commit 221788281a
4 changed files with 174 additions and 167 deletions
Download Patch File Download Diff File Expand all files Collapse all files
conf/settings/estimation/ins_float_invariant.xml
+14 -14
View File
@@ -3,20 +3,20 @@
<settings>
<dl_settings>
<dl_settings NAME="invariant">
<dl_setting MAX="1" MIN="1" STEP="1" VAR="ins_impl.reset" shortname="reset"/>
<dl_setting MAX="10" MIN="0." STEP="0.001" VAR="ins_impl.gains.lv" shortname="lv" module="subsystems/ins/ins_float_invariant"/>
<dl_setting MAX="10" MIN="0." STEP="0.001" VAR="ins_impl.gains.lb" shortname="lb"/>
<dl_setting MAX="10" MIN="0." STEP="0.001" VAR="ins_impl.gains.mv" shortname="mv"/>
<dl_setting MAX="10" MIN="0." STEP="0.01" VAR="ins_impl.gains.mh" shortname="mh"/>
<dl_setting MAX="20" MIN="0." STEP="0.01" VAR="ins_impl.gains.nx" shortname="nx"/>
<dl_setting MAX="10" MIN="0." STEP="0.01" VAR="ins_impl.gains.nxz" shortname="nxz"/>
<dl_setting MAX="30" MIN="0." STEP="0.01" VAR="ins_impl.gains.mvz" shortname="mvz"/>
<dl_setting MAX="5" MIN="0." STEP="0.01" VAR="ins_impl.gains.nh" shortname="nh"/>
<dl_setting MAX="5" MIN="0." STEP="0.01" VAR="ins_impl.gains.ov" shortname="ov"/>
<dl_setting MAX="3" MIN="0." STEP="0.01" VAR="ins_impl.gains.ob" shortname="ob"/>
<dl_setting MAX="10" MIN="0." STEP="0.01" VAR="ins_impl.gains.rv" shortname="rv"/>
<dl_setting MAX="10" MIN="0." STEP="0.01" VAR="ins_impl.gains.rh" shortname="rh"/>
<dl_setting MAX="1" MIN="0." STEP="0.001" VAR="ins_impl.gains.sh" shortname="sh"/>
<dl_setting MAX="1" MIN="1" STEP="1" VAR="ins_float_inv.reset" shortname="reset"/>
<dl_setting MAX="10" MIN="0." STEP="0.001" VAR="ins_float_inv.gains.lv" shortname="lv" module="subsystems/ins/ins_float_invariant"/>
<dl_setting MAX="10" MIN="0." STEP="0.001" VAR="ins_float_inv.gains.lb" shortname="lb"/>
<dl_setting MAX="10" MIN="0." STEP="0.001" VAR="ins_float_inv.gains.mv" shortname="mv"/>
<dl_setting MAX="10" MIN="0." STEP="0.01" VAR="ins_float_inv.gains.mh" shortname="mh"/>
<dl_setting MAX="20" MIN="0." STEP="0.01" VAR="ins_float_inv.gains.nx" shortname="nx"/>
<dl_setting MAX="10" MIN="0." STEP="0.01" VAR="ins_float_inv.gains.nxz" shortname="nxz"/>
<dl_setting MAX="30" MIN="0." STEP="0.01" VAR="ins_float_inv.gains.mvz" shortname="mvz"/>
<dl_setting MAX="5" MIN="0." STEP="0.01" VAR="ins_float_inv.gains.nh" shortname="nh"/>
<dl_setting MAX="5" MIN="0." STEP="0.01" VAR="ins_float_inv.gains.ov" shortname="ov"/>
<dl_setting MAX="3" MIN="0." STEP="0.01" VAR="ins_float_inv.gains.ob" shortname="ob"/>
<dl_setting MAX="10" MIN="0." STEP="0.01" VAR="ins_float_inv.gains.rv" shortname="rv"/>
<dl_setting MAX="10" MIN="0." STEP="0.01" VAR="ins_float_inv.gains.rh" shortname="rh"/>
<dl_setting MAX="1" MIN="0." STEP="0.001" VAR="ins_float_inv.gains.sh" shortname="sh"/>
</dl_settings>
</dl_settings>
</settings>
sw/airborne/firmwares/fixedwing/main_ap.c
-14
View File
@@ -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
sw/airborne/subsystems/ins/ins_float_invariant.c
+153 -133
View File
@@ -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
}
sw/airborne/subsystems/ins/ins_float_invariant.h
+7 -6
View File
@@ -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,