diff --git a/conf/abi.xml b/conf/abi.xml
index be92c384d4d..f219e58f3ac 100644
--- a/conf/abi.xml
+++ b/conf/abi.xml
@@ -44,10 +44,6 @@
-
-
-
-
diff --git a/sw/airborne/firmwares/demo/demo_ahrs_actuators.c b/sw/airborne/firmwares/demo/demo_ahrs_actuators.c
index aadc2eebfee..6acafbb0435 100644
--- a/sw/airborne/firmwares/demo/demo_ahrs_actuators.c
+++ b/sw/airborne/firmwares/demo/demo_ahrs_actuators.c
@@ -107,9 +107,6 @@ static inline void main_init(void)
register_periodic_telemetry(DefaultPeriodic, PPRZ_MSG_ID_ALIVE, send_alive);
register_periodic_telemetry(DefaultPeriodic, PPRZ_MSG_ID_COMMANDS, send_commands);
register_periodic_telemetry(DefaultPeriodic, PPRZ_MSG_ID_ACTUATORS, send_actuators);
-
- // send body_to_imu from here for now
- AbiSendMsgBODY_TO_IMU_QUAT(1, orientationGetQuat_f(&imu.body_to_imu));
}
static inline void main_periodic_task(void)
diff --git a/sw/airborne/firmwares/fixedwing/guidance/energy_ctrl.c b/sw/airborne/firmwares/fixedwing/guidance/energy_ctrl.c
index f0f960dd405..62b99eed2a7 100644
--- a/sw/airborne/firmwares/fixedwing/guidance/energy_ctrl.c
+++ b/sw/airborne/firmwares/fixedwing/guidance/energy_ctrl.c
@@ -134,12 +134,9 @@ float v_ctl_pitch_setpoint;
uint8_t v_ctl_speed_mode; //To be compatible with universal flightplan, not used for etecs
-static struct FloatQuat imu_to_body_quat;
static struct Int32Vect3 accel_imu_meas;
static abi_event accel_ev;
-static abi_event body_to_imu_ev;
-
///////////// DEFAULT SETTINGS ////////////////
#ifndef V_CTL_ALTITUDE_MAX_CLIMB
@@ -208,12 +205,6 @@ static void accel_cb(uint8_t sender_id __attribute__((unused)),
accel_imu_meas = *accel;
}
-static void body_to_imu_cb(uint8_t sender_id __attribute__((unused)),
- struct FloatQuat *q_b2i_f)
-{
- float_quat_invert(&imu_to_body_quat, q_b2i_f);
-}
-
void v_ctl_init(void)
{
/* mode */
@@ -281,10 +272,7 @@ void v_ctl_init(void)
v_ctl_throttle_setpoint = 0;
- float_quat_identity(&imu_to_body_quat);
-
AbiBindMsgIMU_ACCEL(V_CTL_ENERGY_IMU_ID, &accel_ev, accel_cb);
- AbiBindMsgBODY_TO_IMU_QUAT(V_CTL_ENERGY_IMU_ID, &body_to_imu_ev, body_to_imu_cb);
}
static const float dt_attidude = 1.0 / ((float)CONTROL_FREQUENCY);
@@ -370,11 +358,8 @@ void v_ctl_climb_loop(void)
// Actual Acceleration from IMU: attempt to reconstruct the actual kinematic acceleration
#ifndef SITL
/* convert last imu accel measurement to float */
- struct FloatVect3 accel_imu_f;
- ACCELS_FLOAT_OF_BFP(accel_imu_f, accel_imu_meas);
- /* rotate from imu to body frame */
struct FloatVect3 accel_meas_body;
- float_quat_vmult(&accel_meas_body, &imu_to_body_quat, &accel_imu_f);
+ ACCELS_FLOAT_OF_BFP(accel_meas_body, accel_imu_meas);
float vdot = accel_meas_body.x / 9.81f - sinf(stateGetNedToBodyEulers_f()->theta);
#else
float vdot = 0;
diff --git a/sw/airborne/main_ap.c b/sw/airborne/main_ap.c
index d254261c554..e6dc415a2e0 100644
--- a/sw/airborne/main_ap.c
+++ b/sw/airborne/main_ap.c
@@ -108,11 +108,6 @@ void main_ap_init(void)
modules_radio_control_tid = sys_time_register_timer((1. / 60.), NULL); // FIXME
modules_datalink_tid = sys_time_register_timer(DATALINK_PERIOD, NULL);
-#if USE_IMU
- // send body_to_imu from here for now
- AbiSendMsgBODY_TO_IMU_QUAT(1, orientationGetQuat_f(&imu.body_to_imu));
-#endif
-
// Do a failsafe check first
autopilot_failsafe_checks();
diff --git a/sw/airborne/math/pprz_algebra_int.c b/sw/airborne/math/pprz_algebra_int.c
index 77838e93a30..c254ceba6f6 100644
--- a/sw/airborne/math/pprz_algebra_int.c
+++ b/sw/airborne/math/pprz_algebra_int.c
@@ -72,7 +72,7 @@ uint32_t int32_gcd(uint32_t a, uint32_t b)
/** Composition (multiplication) of two rotation matrices.
* _m_a2c = _m_a2b comp _m_b2c , aka _m_a2c = _m_b2c * _m_a2b
*/
-void int32_rmat_comp(struct Int32RMat *m_a2c, struct Int32RMat *m_a2b, struct Int32RMat *m_b2c)
+void int32_rmat_comp(struct Int32RMat *m_a2c, const struct Int32RMat *m_a2b, const struct Int32RMat *m_b2c)
{
m_a2c->m[0] = (m_b2c->m[0] * m_a2b->m[0] + m_b2c->m[1] * m_a2b->m[3] + m_b2c->m[2] * m_a2b->m[6]) >> INT32_TRIG_FRAC;
m_a2c->m[1] = (m_b2c->m[0] * m_a2b->m[1] + m_b2c->m[1] * m_a2b->m[4] + m_b2c->m[2] * m_a2b->m[7]) >> INT32_TRIG_FRAC;
@@ -88,7 +88,7 @@ void int32_rmat_comp(struct Int32RMat *m_a2c, struct Int32RMat *m_a2b, struct In
/** Composition (multiplication) of two rotation matrices.
* _m_a2b = _m_a2c comp_inv _m_b2c , aka _m_a2b = inv(_m_b2c) * _m_a2c
*/
-void int32_rmat_comp_inv(struct Int32RMat *m_a2b, struct Int32RMat *m_a2c, struct Int32RMat *m_b2c)
+void int32_rmat_comp_inv(struct Int32RMat *m_a2b, const struct Int32RMat *m_a2c, const struct Int32RMat *m_b2c)
{
m_a2b->m[0] = (m_b2c->m[0] * m_a2c->m[0] + m_b2c->m[3] * m_a2c->m[3] + m_b2c->m[6] * m_a2c->m[6]) >> INT32_TRIG_FRAC;
m_a2b->m[1] = (m_b2c->m[0] * m_a2c->m[1] + m_b2c->m[3] * m_a2c->m[4] + m_b2c->m[6] * m_a2c->m[7]) >> INT32_TRIG_FRAC;
diff --git a/sw/airborne/math/pprz_algebra_int.h b/sw/airborne/math/pprz_algebra_int.h
index 25a04747f1f..fdd066ec60b 100644
--- a/sw/airborne/math/pprz_algebra_int.h
+++ b/sw/airborne/math/pprz_algebra_int.h
@@ -352,14 +352,14 @@ static inline void int32_rmat_identity(struct Int32RMat *rm)
/** Composition (multiplication) of two rotation matrices.
* m_a2c = m_a2b comp m_b2c , aka m_a2c = m_b2c * m_a2b
*/
-extern void int32_rmat_comp(struct Int32RMat *m_a2c, struct Int32RMat *m_a2b,
- struct Int32RMat *m_b2c);
+extern void int32_rmat_comp(struct Int32RMat *m_a2c, const struct Int32RMat *m_a2b,
+ const struct Int32RMat *m_b2c);
/** Composition (multiplication) of two rotation matrices.
* m_a2b = m_a2c comp_inv m_b2c , aka m_a2b = inv(_m_b2c) * m_a2c
*/
-extern void int32_rmat_comp_inv(struct Int32RMat *m_a2b, struct Int32RMat *m_a2c,
- struct Int32RMat *m_b2c);
+extern void int32_rmat_comp_inv(struct Int32RMat *m_a2b, const struct Int32RMat *m_a2c,
+ const struct Int32RMat *m_b2c);
/** rotate 3D vector by rotation matrix.
* vb = m_a2b * va
diff --git a/sw/airborne/modules/ahrs/ahrs_aligner.c b/sw/airborne/modules/ahrs/ahrs_aligner.c
index aeec8664029..75b10a68b87 100644
--- a/sw/airborne/modules/ahrs/ahrs_aligner.c
+++ b/sw/airborne/modules/ahrs/ahrs_aligner.c
@@ -119,12 +119,13 @@ void ahrs_aligner_run(void)
struct imu_mag_t *mag = imu_get_mag(AHRS_ALIGNER_IMU_ID, false);
// Could not find all sensors
- if(gyro == NULL || accel == NULL || mag == NULL)
+ if(gyro == NULL || accel == NULL)
return;
RATES_ADD(gyro_sum, gyro->scaled);
VECT3_ADD(accel_sum, accel->scaled);
- VECT3_ADD(mag_sum, mag->scaled);
+ if(mag != NULL)
+ VECT3_ADD(mag_sum, mag->scaled);
ref_sensor_samples[samples_idx] = accel->scaled.z;
samples_idx++;
@@ -170,7 +171,7 @@ void ahrs_aligner_run(void)
LED_ON(AHRS_ALIGNER_LED);
#endif
uint32_t now_ts = get_sys_time_usec();
- AbiSendMsgIMU_LOWPASSED(ABI_BROADCAST, now_ts, &ahrs_aligner.lp_gyro,
+ AbiSendMsgIMU_LOWPASSED(AHRS_ALIGNER_IMU_ID, now_ts, &ahrs_aligner.lp_gyro,
&ahrs_aligner.lp_accel, &ahrs_aligner.lp_mag);
}
}
diff --git a/sw/airborne/modules/ahrs/ahrs_float_cmpl.c b/sw/airborne/modules/ahrs/ahrs_float_cmpl.c
index 1e53484a374..fc9d7d765f4 100644
--- a/sw/airborne/modules/ahrs/ahrs_float_cmpl.c
+++ b/sw/airborne/modules/ahrs/ahrs_float_cmpl.c
@@ -87,13 +87,10 @@ void ahrs_fc_init(void)
ahrs_fc.ltp_vel_norm_valid = false;
ahrs_fc.heading_aligned = false;
- /* init ltp_to_imu quaternion as zero/identity rotation */
- float_quat_identity(&ahrs_fc.ltp_to_imu_quat);
-
- orientationSetIdentity(&ahrs_fc.body_to_imu);
- orientationSetIdentity(&ahrs_fc.ltp_to_body);
-
- FLOAT_RATES_ZERO(ahrs_fc.imu_rate);
+ /* init ltp_to_body quaternion as zero/identity rotation */
+ float_quat_identity(&ahrs_fc.ltp_to_body_quat);
+ float_rmat_identity(&ahrs_fc.ltp_to_body_rmat);
+ FLOAT_RATES_ZERO(ahrs_fc.body_rate);
/* set default filter cut-off frequency and damping */
ahrs_fc.accel_omega = AHRS_ACCEL_OMEGA;
@@ -121,16 +118,16 @@ bool ahrs_fc_align(struct FloatRates *lp_gyro, struct FloatVect3 *lp_accel,
#if USE_MAGNETOMETER
/* Compute an initial orientation from accel and mag directly as quaternion */
- ahrs_float_get_quat_from_accel_mag(&ahrs_fc.ltp_to_imu_quat, lp_accel, lp_mag);
+ ahrs_float_get_quat_from_accel_mag(&ahrs_fc.ltp_to_body_quat, lp_accel, lp_mag);
ahrs_fc.heading_aligned = true;
#else
/* Compute an initial orientation from accel and just set heading to zero */
- ahrs_float_get_quat_from_accel(&ahrs_fc.ltp_to_imu_quat, lp_accel);
+ ahrs_float_get_quat_from_accel(&ahrs_fc.ltp_to_body_quat, lp_accel);
ahrs_fc.heading_aligned = false;
#endif
/* Convert initial orientation from quat to rotation matrix representations. */
- float_rmat_of_quat(&ahrs_fc.ltp_to_imu_rmat, &ahrs_fc.ltp_to_imu_quat);
+ float_rmat_of_quat(&ahrs_fc.ltp_to_body_rmat, &ahrs_fc.ltp_to_body_quat);
/* used averaged gyro as initial value for bias */
ahrs_fc.gyro_bias = *lp_gyro;
@@ -151,9 +148,9 @@ void ahrs_fc_propagate(struct FloatRates *gyro, float dt)
#ifdef AHRS_PROPAGATE_LOW_PASS_RATES
const float alpha = 0.1;
- FLOAT_RATES_LIN_CMB(ahrs_fc.imu_rate, ahrs_fc.imu_rate, (1. - alpha), rates, alpha);
+ FLOAT_RATES_LIN_CMB(ahrs_fc.body_rate, ahrs_fc.body_rate, (1. - alpha), rates, alpha);
#else
- RATES_COPY(ahrs_fc.imu_rate, rates);
+ RATES_COPY(ahrs_fc.body_rate, rates);
#endif
/* add correction */
@@ -163,14 +160,14 @@ void ahrs_fc_propagate(struct FloatRates *gyro, float dt)
FLOAT_RATES_ZERO(ahrs_fc.rate_correction);
#if AHRS_PROPAGATE_RMAT
- float_rmat_integrate_fi(&ahrs_fc.ltp_to_imu_rmat, &omega, dt);
- float_rmat_reorthogonalize(&ahrs_fc.ltp_to_imu_rmat);
- float_quat_of_rmat(&ahrs_fc.ltp_to_imu_quat, &ahrs_fc.ltp_to_imu_rmat);
+ float_rmat_integrate_fi(&ahrs_fc.ltp_to_body_rmat, &omega, dt);
+ float_rmat_reorthogonalize(&ahrs_fc.ltp_to_body_rmat);
+ float_quat_of_rmat(&ahrs_fc.ltp_to_body_quat, &ahrs_fc.ltp_to_body_rmat);
#endif
#if AHRS_PROPAGATE_QUAT
- float_quat_integrate(&ahrs_fc.ltp_to_imu_quat, &omega, dt);
- float_quat_normalize(&ahrs_fc.ltp_to_imu_quat);
- float_rmat_of_quat(&ahrs_fc.ltp_to_imu_rmat, &ahrs_fc.ltp_to_imu_quat);
+ float_quat_integrate(&ahrs_fc.ltp_to_body_quat, &omega, dt);
+ float_quat_normalize(&ahrs_fc.ltp_to_body_quat);
+ float_rmat_of_quat(&ahrs_fc.ltp_to_body_rmat, &ahrs_fc.ltp_to_body_quat);
#endif
// increase accel and mag propagation counters
@@ -185,10 +182,10 @@ void ahrs_fc_update_accel(struct FloatVect3 *accel, float dt)
return;
}
- /* last column of roation matrix = ltp z-axis in imu-frame */
- struct FloatVect3 c2 = { RMAT_ELMT(ahrs_fc.ltp_to_imu_rmat, 0, 2),
- RMAT_ELMT(ahrs_fc.ltp_to_imu_rmat, 1, 2),
- RMAT_ELMT(ahrs_fc.ltp_to_imu_rmat, 2, 2)
+ /* last column of roation matrix = ltp z-axis in body-frame */
+ struct FloatVect3 c2 = { RMAT_ELMT(ahrs_fc.ltp_to_body_rmat, 0, 2),
+ RMAT_ELMT(ahrs_fc.ltp_to_body_rmat, 1, 2),
+ RMAT_ELMT(ahrs_fc.ltp_to_body_rmat, 2, 2)
};
struct FloatVect3 imu_accel = *accel;
@@ -213,18 +210,11 @@ void ahrs_fc_update_accel(struct FloatVect3 *accel, float dt)
/* assume tangential velocity along body x-axis */
{ahrs_fc.ltp_vel_norm, 0.0, 0.0};
#endif
- struct FloatRMat *body_to_imu_rmat = orientationGetRMat_f(&ahrs_fc.body_to_imu);
- struct FloatRates body_rate;
- float_rmat_transp_ratemult(&body_rate, body_to_imu_rmat, &ahrs_fc.imu_rate);
struct FloatVect3 acc_c_body;
- VECT3_RATES_CROSS_VECT3(acc_c_body, body_rate, vel_tangential_body);
-
- /* convert centrifugal acceleration from body to imu frame */
- struct FloatVect3 acc_c_imu;
- float_rmat_vmult(&acc_c_imu, body_to_imu_rmat, &acc_c_body);
+ VECT3_RATES_CROSS_VECT3(acc_c_body, ahrs_fc.body_rate, vel_tangential_body);
- /* and subtract it from imu measurement to get a corrected measurement of the gravity vector */
- VECT3_DIFF(pseudo_gravity_measurement, imu_accel, acc_c_imu);
+ /* centrifugal acceleration subtract it from imu measurement to get a corrected measurement of the gravity vector */
+ VECT3_DIFF(pseudo_gravity_measurement, imu_accel, acc_c_body);
} else {
VECT3_COPY(pseudo_gravity_measurement, imu_accel);
@@ -297,7 +287,7 @@ void ahrs_fc_update_mag_full(struct FloatVect3 *mag, float dt)
{
struct FloatVect3 expected_imu;
- float_rmat_vmult(&expected_imu, &ahrs_fc.ltp_to_imu_rmat, &ahrs_fc.mag_h);
+ float_rmat_vmult(&expected_imu, &ahrs_fc.ltp_to_body_rmat, &ahrs_fc.mag_h);
struct FloatVect3 measured_imu = *mag;
struct FloatVect3 residual_imu;
@@ -333,7 +323,7 @@ void ahrs_fc_update_mag_2d(struct FloatVect3 *mag, float dt)
struct FloatVect3 measured_imu = *mag;
struct FloatVect3 measured_ltp;
- float_rmat_transp_vmult(&measured_ltp, &ahrs_fc.ltp_to_imu_rmat, &measured_imu);
+ float_rmat_transp_vmult(&measured_ltp, &ahrs_fc.ltp_to_body_rmat, &measured_imu);
struct FloatVect2 measured_ltp_2d = {measured_ltp.x, measured_ltp.y};
@@ -349,7 +339,7 @@ void ahrs_fc_update_mag_2d(struct FloatVect3 *mag, float dt)
// printf("res : %f\n", residual_ltp.z);
struct FloatVect3 residual_imu;
- float_rmat_vmult(&residual_imu, &ahrs_fc.ltp_to_imu_rmat, &residual_ltp);
+ float_rmat_vmult(&residual_imu, &ahrs_fc.ltp_to_body_rmat, &residual_ltp);
/* Complementary filter proportional gain.
@@ -372,21 +362,21 @@ void ahrs_fc_update_mag_2d_dumb(struct FloatVect3 *mag)
{
/* project mag on local tangeant plane */
- struct FloatEulers ltp_to_imu_euler;
- float_eulers_of_rmat(<p_to_imu_euler, &ahrs_fc.ltp_to_imu_rmat);
+ struct FloatEulers ltp_to_body_euler;
+ float_eulers_of_rmat(<p_to_body_euler, &ahrs_fc.ltp_to_body_rmat);
- const float cphi = cosf(ltp_to_imu_euler.phi);
- const float sphi = sinf(ltp_to_imu_euler.phi);
- const float ctheta = cosf(ltp_to_imu_euler.theta);
- const float stheta = sinf(ltp_to_imu_euler.theta);
+ const float cphi = cosf(ltp_to_body_euler.phi);
+ const float sphi = sinf(ltp_to_body_euler.phi);
+ const float ctheta = cosf(ltp_to_body_euler.theta);
+ const float stheta = sinf(ltp_to_body_euler.theta);
const float mn = ctheta * mag->x + sphi * stheta * mag->y + cphi * stheta * mag->z;
const float me = 0. * mag->x + cphi * mag->y - sphi * mag->z;
- const float res_norm = -RMAT_ELMT(ahrs_fc.ltp_to_imu_rmat, 0, 0) * me +
- RMAT_ELMT(ahrs_fc.ltp_to_imu_rmat, 1, 0) * mn;
- const struct FloatVect3 r2 = {RMAT_ELMT(ahrs_fc.ltp_to_imu_rmat, 2, 0),
- RMAT_ELMT(ahrs_fc.ltp_to_imu_rmat, 2, 1),
- RMAT_ELMT(ahrs_fc.ltp_to_imu_rmat, 2, 2)
+ const float res_norm = -RMAT_ELMT(ahrs_fc.ltp_to_body_rmat, 0, 0) * me +
+ RMAT_ELMT(ahrs_fc.ltp_to_body_rmat, 1, 0) * mn;
+ const struct FloatVect3 r2 = {RMAT_ELMT(ahrs_fc.ltp_to_body_rmat, 2, 0),
+ RMAT_ELMT(ahrs_fc.ltp_to_body_rmat, 2, 1),
+ RMAT_ELMT(ahrs_fc.ltp_to_body_rmat, 2, 2)
};
const float mag_rate_update_gain = 2.5;
RATES_ADD_SCALED_VECT(ahrs_fc.rate_correction, r2, (mag_rate_update_gain * res_norm));
@@ -431,14 +421,11 @@ void ahrs_fc_update_heading(float heading)
FLOAT_ANGLE_NORMALIZE(heading);
- ahrs_fc_recompute_ltp_to_body();
- struct FloatRMat *ltp_to_body_rmat = orientationGetRMat_f(&ahrs_fc.ltp_to_body);
-
// row 0 of ltp_to_body_rmat = body x-axis in ltp frame
// we only consider x and y
struct FloatVect2 expected_ltp = {
- RMAT_ELMT(*ltp_to_body_rmat, 0, 0),
- RMAT_ELMT(*ltp_to_body_rmat, 0, 1)
+ RMAT_ELMT(ahrs_fc.ltp_to_body_rmat, 0, 0),
+ RMAT_ELMT(ahrs_fc.ltp_to_body_rmat, 0, 1)
};
// expected_heading cross measured_heading
@@ -449,7 +436,7 @@ void ahrs_fc_update_heading(float heading)
};
struct FloatVect3 residual_imu;
- float_rmat_vmult(&residual_imu, &ahrs_fc.ltp_to_imu_rmat, &residual_ltp);
+ float_rmat_vmult(&residual_imu, &ahrs_fc.ltp_to_body_rmat, &residual_ltp);
const float heading_rate_update_gain = 2.5;
RATES_ADD_SCALED_VECT(ahrs_fc.rate_correction, residual_imu, heading_rate_update_gain);
@@ -481,11 +468,11 @@ void ahrs_fc_realign_heading(float heading)
q_h_new.qz = sinf(heading / 2.0);
q_h_new.qi = cosf(heading / 2.0);
- ahrs_fc_recompute_ltp_to_body();
- struct FloatQuat *ltp_to_body_quat = orientationGetQuat_f(&ahrs_fc.ltp_to_body);
+ struct FloatQuat ltp_to_body_quat;
+ QUAT_COPY(ltp_to_body_quat, ahrs_fc.ltp_to_body_quat);
/* quaternion representing current heading only */
- struct FloatQuat q_h = *ltp_to_body_quat;
+ struct FloatQuat q_h = ltp_to_body_quat;
q_h.qx = 0.;
q_h.qy = 0.;
float_quat_normalize(&q_h);
@@ -495,37 +482,10 @@ void ahrs_fc_realign_heading(float heading)
float_quat_inv_comp_norm_shortest(&q_c, &q_h, &q_h_new);
/* correct current heading in body frame */
- struct FloatQuat q;
- float_quat_comp_norm_shortest(&q, &q_c, ltp_to_body_quat);
+ float_quat_comp_norm_shortest(&ahrs_fc.ltp_to_body_quat, &q_c, <p_to_body_quat);
/* compute ltp to imu rotation quaternion and matrix */
- struct FloatQuat *body_to_imu_quat = orientationGetQuat_f(&ahrs_fc.body_to_imu);
- float_quat_comp(&ahrs_fc.ltp_to_imu_quat, &q, body_to_imu_quat);
- float_rmat_of_quat(&ahrs_fc.ltp_to_imu_rmat, &ahrs_fc.ltp_to_imu_quat);
+ float_rmat_of_quat(&ahrs_fc.ltp_to_body_rmat, &ahrs_fc.ltp_to_body_quat);
ahrs_fc.heading_aligned = true;
}
-
-void ahrs_fc_set_body_to_imu(struct OrientationReps *body_to_imu)
-{
- ahrs_fc_set_body_to_imu_quat(orientationGetQuat_f(body_to_imu));
-}
-
-void ahrs_fc_set_body_to_imu_quat(struct FloatQuat *q_b2i)
-{
- orientationSetQuat_f(&ahrs_fc.body_to_imu, q_b2i);
-
- if (!ahrs_fc.is_aligned) {
- /* Set ltp_to_imu so that body is zero */
- ahrs_fc.ltp_to_imu_quat = *orientationGetQuat_f(&ahrs_fc.body_to_imu);
- ahrs_fc.ltp_to_imu_rmat = *orientationGetRMat_f(&ahrs_fc.body_to_imu);
- }
-}
-
-void ahrs_fc_recompute_ltp_to_body(void)
-{
- struct FloatQuat *body_to_imu_quat = orientationGetQuat_f(&ahrs_fc.body_to_imu);
- struct FloatQuat ltp_to_body_quat;
- float_quat_comp_inv(<p_to_body_quat, &ahrs_fc.ltp_to_imu_quat, body_to_imu_quat);
- orientationSetQuat_f(&ahrs_fc.ltp_to_body, <p_to_body_quat);
-}
diff --git a/sw/airborne/modules/ahrs/ahrs_float_cmpl.h b/sw/airborne/modules/ahrs/ahrs_float_cmpl.h
index 0699c2bceb4..e82eaa9991b 100644
--- a/sw/airborne/modules/ahrs/ahrs_float_cmpl.h
+++ b/sw/airborne/modules/ahrs/ahrs_float_cmpl.h
@@ -43,9 +43,9 @@ enum AhrsFCStatus {
struct AhrsFloatCmpl {
struct FloatRates gyro_bias;
struct FloatRates rate_correction;
- struct FloatRates imu_rate;
- struct FloatQuat ltp_to_imu_quat;
- struct FloatRMat ltp_to_imu_rmat;
+ struct FloatRates body_rate;
+ struct FloatQuat ltp_to_body_quat;
+ struct FloatRMat ltp_to_body_rmat;
bool correct_gravity; ///< enable gravity correction during coordinated turns
float ltp_vel_norm; ///< velocity norm for gravity correction during coordinated turns
@@ -69,9 +69,6 @@ struct AhrsFloatCmpl {
uint16_t accel_cnt; ///< number of propagations since last accel update
uint16_t mag_cnt; ///< number of propagations since last mag update
- struct OrientationReps body_to_imu;
- struct OrientationReps ltp_to_body;
-
enum AhrsFCStatus status;
bool is_aligned;
};
@@ -79,9 +76,6 @@ struct AhrsFloatCmpl {
extern struct AhrsFloatCmpl ahrs_fc;
extern void ahrs_fc_init(void);
-extern void ahrs_fc_set_body_to_imu(struct OrientationReps *body_to_imu);
-extern void ahrs_fc_set_body_to_imu_quat(struct FloatQuat *q_b2i);
-extern void ahrs_fc_recompute_ltp_to_body(void);
extern bool ahrs_fc_align(struct FloatRates *lp_gyro, struct FloatVect3 *lp_accel,
struct FloatVect3 *lp_mag);
extern void ahrs_fc_propagate(struct FloatRates *gyro, float dt);
diff --git a/sw/airborne/modules/ahrs/ahrs_float_cmpl_wrapper.c b/sw/airborne/modules/ahrs/ahrs_float_cmpl_wrapper.c
index 7fc5782cf3b..b2407ab70ab 100644
--- a/sw/airborne/modules/ahrs/ahrs_float_cmpl_wrapper.c
+++ b/sw/airborne/modules/ahrs/ahrs_float_cmpl_wrapper.c
@@ -48,12 +48,12 @@ static void compute_body_orientation_and_rates(void);
static void send_euler(struct transport_tx *trans, struct link_device *dev)
{
- struct FloatEulers ltp_to_imu_euler;
- float_eulers_of_quat(<p_to_imu_euler, &ahrs_fc.ltp_to_imu_quat);
+ struct FloatEulers ltp_to_body_euler;
+ float_eulers_of_quat(<p_to_body_euler, &ahrs_fc.ltp_to_body_quat);
pprz_msg_send_AHRS_EULER(trans, dev, AC_ID,
- <p_to_imu_euler.phi,
- <p_to_imu_euler.theta,
- <p_to_imu_euler.psi,
+ <p_to_body_euler.phi,
+ <p_to_body_euler.theta,
+ <p_to_body_euler.psi,
&ahrs_fc_id);
}
@@ -68,22 +68,18 @@ static void send_bias(struct transport_tx *trans, struct link_device *dev)
static void send_euler_int(struct transport_tx *trans, struct link_device *dev)
{
/* compute eulers in int (IMU frame) */
- struct FloatEulers ltp_to_imu_euler;
- float_eulers_of_quat(<p_to_imu_euler, &ahrs_fc.ltp_to_imu_quat);
- struct Int32Eulers eulers_imu;
- EULERS_BFP_OF_REAL(eulers_imu, ltp_to_imu_euler);
-
- /* get Eulers in int (body frame) */
- ahrs_fc_recompute_ltp_to_body();
- struct Int32Eulers *eulers_body = orientationGetEulers_i(&ahrs_fc.ltp_to_body);
+ struct FloatEulers ltp_to_body_euler;
+ float_eulers_of_quat(<p_to_body_euler, &ahrs_fc.ltp_to_body_quat);
+ struct Int32Eulers eulers_body;
+ EULERS_BFP_OF_REAL(eulers_body, ltp_to_body_euler);
pprz_msg_send_AHRS_EULER_INT(trans, dev, AC_ID,
- &eulers_imu.phi,
- &eulers_imu.theta,
- &eulers_imu.psi,
- &eulers_body->phi,
- &eulers_body->theta,
- &eulers_body->psi,
+ &eulers_body.phi,
+ &eulers_body.theta,
+ &eulers_body.psi,
+ &eulers_body.phi,
+ &eulers_body.theta,
+ &eulers_body.psi,
&ahrs_fc_id);
}
@@ -131,7 +127,6 @@ static abi_event gyro_ev;
static abi_event accel_ev;
static abi_event mag_ev;
static abi_event aligner_ev;
-static abi_event body_to_imu_ev;
static abi_event geo_mag_ev;
static abi_event gps_ev;
@@ -235,12 +230,6 @@ static void aligner_cb(uint8_t __attribute__((unused)) sender_id,
}
}
-static void body_to_imu_cb(uint8_t sender_id __attribute__((unused)),
- struct FloatQuat *q_b2i_f)
-{
- ahrs_fc_set_body_to_imu_quat(q_b2i_f);
-}
-
static void geo_mag_cb(uint8_t sender_id __attribute__((unused)), struct FloatVect3 *h)
{
ahrs_fc.mag_h = *h;
@@ -266,17 +255,9 @@ static bool ahrs_fc_enable_output(bool enable)
static void compute_body_orientation_and_rates(void)
{
if (ahrs_fc_output_enabled) {
- /* recompute LTP to BODY quaternion */
- ahrs_fc_recompute_ltp_to_body();
- struct FloatQuat *ltp_to_body_quat = orientationGetQuat_f(&ahrs_fc.ltp_to_body);
/* Set state */
- stateSetNedToBodyQuat_f(ltp_to_body_quat);
-
- /* compute body rates */
- struct FloatRates body_rate;
- struct FloatRMat *body_to_imu_rmat = orientationGetRMat_f(&ahrs_fc.body_to_imu);
- float_rmat_transp_ratemult(&body_rate, body_to_imu_rmat, &ahrs_fc.imu_rate);
- stateSetBodyRates_f(&body_rate);
+ stateSetNedToBodyQuat_f(&ahrs_fc.ltp_to_body_quat);
+ stateSetBodyRates_f(&ahrs_fc.body_rate);
}
}
@@ -292,8 +273,7 @@ void ahrs_fc_register(void)
AbiBindMsgIMU_GYRO(AHRS_FC_IMU_ID, &gyro_ev, gyro_cb);
AbiBindMsgIMU_ACCEL(AHRS_FC_IMU_ID, &accel_ev, accel_cb);
AbiBindMsgIMU_MAG(AHRS_FC_MAG_ID, &mag_ev, mag_cb);
- AbiBindMsgIMU_LOWPASSED(ABI_BROADCAST, &aligner_ev, aligner_cb);
- AbiBindMsgBODY_TO_IMU_QUAT(ABI_BROADCAST, &body_to_imu_ev, body_to_imu_cb);
+ AbiBindMsgIMU_LOWPASSED(AHRS_FC_IMU_ID, &aligner_ev, aligner_cb);
AbiBindMsgGEO_MAG(ABI_BROADCAST, &geo_mag_ev, geo_mag_cb);
AbiBindMsgGPS(AHRS_FC_GPS_ID, &gps_ev, gps_cb);
diff --git a/sw/airborne/modules/ahrs/ahrs_float_dcm.c b/sw/airborne/modules/ahrs/ahrs_float_dcm.c
index 3c7d457fda2..237ee50425a 100644
--- a/sw/airborne/modules/ahrs/ahrs_float_dcm.c
+++ b/sw/airborne/modules/ahrs/ahrs_float_dcm.c
@@ -101,13 +101,14 @@ void ahrs_dcm_init(void)
ahrs_dcm.status = AHRS_DCM_UNINIT;
ahrs_dcm.is_aligned = false;
- /* init ltp_to_imu euler with zero */
- FLOAT_EULERS_ZERO(ahrs_dcm.ltp_to_imu_euler);
-
- FLOAT_RATES_ZERO(ahrs_dcm.imu_rate);
+ /* init ltp_to_body euler with zero */
+ FLOAT_EULERS_ZERO(ahrs_dcm.ltp_to_body_euler);
+ FLOAT_RATES_ZERO(ahrs_dcm.body_rate);
/* set inital filter dcm */
- set_dcm_matrix_from_rmat(orientationGetRMat_f(&ahrs_dcm.body_to_imu));
+ struct FloatRMat init_rmat;
+ float_rmat_identity(&init_rmat);
+ set_dcm_matrix_from_rmat(&init_rmat);
ahrs_dcm.gps_speed = 0;
ahrs_dcm.gps_acceleration = 0;
@@ -126,14 +127,14 @@ bool ahrs_dcm_align(struct FloatRates *lp_gyro, struct FloatVect3 *lp_accel,
#else
ahrs_float_get_quat_from_accel(&quat, lp_accel);
#endif
- float_eulers_of_quat(&ahrs_dcm.ltp_to_imu_euler, &quat);
+ float_eulers_of_quat(&ahrs_dcm.ltp_to_body_euler, &quat);
/* Convert initial orientation from quaternion to rotation matrix representations. */
- struct FloatRMat ltp_to_imu_rmat;
- float_rmat_of_quat(<p_to_imu_rmat, &quat);
+ struct FloatRMat ltp_to_body_rmat;
+ float_rmat_of_quat(<p_to_body_rmat, &quat);
/* set filter dcm */
- set_dcm_matrix_from_rmat(<p_to_imu_rmat);
+ set_dcm_matrix_from_rmat(<p_to_body_rmat);
/* use averaged gyro as initial value for bias */
ahrs_dcm.gyro_bias = *lp_gyro;
@@ -148,21 +149,21 @@ bool ahrs_dcm_align(struct FloatRates *lp_gyro, struct FloatVect3 *lp_accel,
void ahrs_dcm_propagate(struct FloatRates *gyro, float dt)
{
/* unbias rate measurement */
- RATES_DIFF(ahrs_dcm.imu_rate, *gyro, ahrs_dcm.gyro_bias);
+ RATES_DIFF(ahrs_dcm.body_rate, *gyro, ahrs_dcm.gyro_bias);
/* Uncouple Motions */
#ifdef IMU_GYRO_P_Q
float dp = 0, dq = 0, dr = 0;
- dp += ahrs_dcm.imu_rate.q * IMU_GYRO_P_Q;
- dp += ahrs_dcm.imu_rate.r * IMU_GYRO_P_R;
- dq += ahrs_dcm.imu_rate.p * IMU_GYRO_Q_P;
- dq += ahrs_dcm.imu_rate.r * IMU_GYRO_Q_R;
- dr += ahrs_dcm.imu_rate.p * IMU_GYRO_R_P;
- dr += ahrs_dcm.imu_rate.q * IMU_GYRO_R_Q;
-
- ahrs_dcm.imu_rate.p += dp;
- ahrs_dcm.imu_rate.q += dq;
- ahrs_dcm.imu_rate.r += dr;
+ dp += ahrs_dcm.body_rate.q * IMU_GYRO_P_Q;
+ dp += ahrs_dcm.body_rate.r * IMU_GYRO_P_R;
+ dq += ahrs_dcm.body_rate.p * IMU_GYRO_Q_P;
+ dq += ahrs_dcm.body_rate.r * IMU_GYRO_Q_R;
+ dr += ahrs_dcm.body_rate.p * IMU_GYRO_R_P;
+ dr += ahrs_dcm.body_rate.q * IMU_GYRO_R_Q;
+
+ ahrs_dcm.body_rate.p += dp;
+ ahrs_dcm.body_rate.q += dq;
+ ahrs_dcm.body_rate.r += dr;
#endif
Matrix_update(dt);
@@ -234,10 +235,10 @@ MESSAGE("MAGNETOMETER FEEDBACK NOT TESTED YET")
float cos_pitch;
float sin_pitch;
- cos_roll = cosf(ahrs_dcm.ltp_to_imu_euler.phi);
- sin_roll = sinf(ahrs_dcm.ltp_to_imu_euler.phi);
- cos_pitch = cosf(ahrs_dcm.ltp_to_imu_euler.theta);
- sin_pitch = sinf(ahrs_dcm.ltp_to_imu_euler.theta);
+ cos_roll = cosf(ahrs_dcm.ltp_to_body_euler.phi);
+ sin_roll = sinf(ahrs_dcm.ltp_to_body_euler.phi);
+ cos_pitch = cosf(ahrs_dcm.ltp_to_body_euler.theta);
+ sin_pitch = sinf(ahrs_dcm.ltp_to_body_euler.theta);
// Pitch&Roll Compensation:
@@ -358,7 +359,9 @@ void Normalize(void)
// Reset on trouble
if (problem) { // Our solution is blowing up and we will force back to initial condition. Hope we are not upside down!
- set_dcm_matrix_from_rmat(orientationGetRMat_f(&ahrs_dcm.body_to_imu));
+ struct FloatRMat init_rmat;
+ float_rmat_identity(&init_rmat);
+ set_dcm_matrix_from_rmat(&init_rmat);
problem = false;
}
}
@@ -485,7 +488,7 @@ void Drift_correction()
void Matrix_update(float dt)
{
- Vector_Add(&Omega[0], &ahrs_dcm.imu_rate.p, &Omega_I[0]); //adding proportional term
+ Vector_Add(&Omega[0], &ahrs_dcm.body_rate.p, &Omega_I[0]); //adding proportional term
Vector_Add(&Omega_Vector[0], &Omega[0], &Omega_P[0]); //adding Integrator term
#if OUTPUTMODE==1 // With corrected data (drift correction)
@@ -500,13 +503,13 @@ void Matrix_update(float dt)
Update_Matrix[2][2] = 0;
#else // Uncorrected data (no drift correction)
Update_Matrix[0][0] = 0;
- Update_Matrix[0][1] = -dt * ahrs_dcm.imu_rate.r; //-z
- Update_Matrix[0][2] = dt * ahrs_dcm.imu_rate.q; //y
- Update_Matrix[1][0] = dt * ahrs_dcm.imu_rate.r; //z
+ Update_Matrix[0][1] = -dt * ahrs_dcm.body_rate.r; //-z
+ Update_Matrix[0][2] = dt * ahrs_dcm.body_rate.q; //y
+ Update_Matrix[1][0] = dt * ahrs_dcm.body_rate.r; //z
Update_Matrix[1][1] = 0;
- Update_Matrix[1][2] = -dt * ahrs_dcm.imu_rate.p;
- Update_Matrix[2][0] = -dt * ahrs_dcm.imu_rate.q;
- Update_Matrix[2][1] = dt * ahrs_dcm.imu_rate.p;
+ Update_Matrix[1][2] = -dt * ahrs_dcm.body_rate.p;
+ Update_Matrix[2][0] = -dt * ahrs_dcm.body_rate.q;
+ Update_Matrix[2][1] = dt * ahrs_dcm.body_rate.p;
Update_Matrix[2][2] = 0;
#endif
@@ -522,28 +525,13 @@ void Matrix_update(float dt)
static void compute_ahrs_representations(void)
{
#if (OUTPUTMODE==2) // Only accelerometer info (debugging purposes)
- ahrs_dcm.ltp_to_imu_euler.phi = atan2(accel_float.y, accel_float.z); // atan2(acc_y,acc_z)
- ahrs_dcm.ltp_to_imu_euler.theta = -asin((accel_float.x) / GRAVITY); // asin(acc_x)
- ahrs_dcm.ltp_to_imu_euler.psi = 0;
+ ahrs_dcm.ltp_to_body_euler.phi = atan2(accel_float.y, accel_float.z); // atan2(acc_y,acc_z)
+ ahrs_dcm.ltp_to_body_euler.theta = -asin((accel_float.x) / GRAVITY); // asin(acc_x)
+ ahrs_dcm.ltp_to_body_euler.psi = 0;
#else
- ahrs_dcm.ltp_to_imu_euler.phi = atan2(DCM_Matrix[2][1], DCM_Matrix[2][2]);
- ahrs_dcm.ltp_to_imu_euler.theta = -asin(DCM_Matrix[2][0]);
- ahrs_dcm.ltp_to_imu_euler.psi = atan2(DCM_Matrix[1][0], DCM_Matrix[0][0]);
- ahrs_dcm.ltp_to_imu_euler.psi += M_PI; // Rotating the angle 180deg to fit for PPRZ
+ ahrs_dcm.ltp_to_body_euler.phi = atan2(DCM_Matrix[2][1], DCM_Matrix[2][2]);
+ ahrs_dcm.ltp_to_body_euler.theta = -asin(DCM_Matrix[2][0]);
+ ahrs_dcm.ltp_to_body_euler.psi = atan2(DCM_Matrix[1][0], DCM_Matrix[0][0]);
+ ahrs_dcm.ltp_to_body_euler.psi += M_PI; // Rotating the angle 180deg to fit for PPRZ
#endif
}
-
-void ahrs_dcm_set_body_to_imu(struct OrientationReps *body_to_imu)
-{
- ahrs_dcm_set_body_to_imu_quat(orientationGetQuat_f(body_to_imu));
-}
-
-void ahrs_dcm_set_body_to_imu_quat(struct FloatQuat *q_b2i)
-{
- orientationSetQuat_f(&ahrs_dcm.body_to_imu, q_b2i);
-
- if (!ahrs_dcm.is_aligned) {
- /* Set ltp_to_imu so that body is zero */
- ahrs_dcm.ltp_to_imu_euler = *orientationGetEulers_f(&ahrs_dcm.body_to_imu);
- }
-}
diff --git a/sw/airborne/modules/ahrs/ahrs_float_dcm.h b/sw/airborne/modules/ahrs/ahrs_float_dcm.h
index e082f6ed748..db2f26f0211 100644
--- a/sw/airborne/modules/ahrs/ahrs_float_dcm.h
+++ b/sw/airborne/modules/ahrs/ahrs_float_dcm.h
@@ -38,8 +38,8 @@ struct AhrsFloatDCM {
struct FloatRates gyro_bias;
struct FloatRates rate_correction;
- struct FloatEulers ltp_to_imu_euler;
- struct FloatRates imu_rate;
+ struct FloatEulers ltp_to_body_euler;
+ struct FloatRates body_rate;
float gps_speed;
float gps_acceleration;
@@ -47,8 +47,6 @@ struct AhrsFloatDCM {
bool gps_course_valid;
uint8_t gps_age;
- struct OrientationReps body_to_imu;
-
enum AhrsDCMStatus status;
bool is_aligned;
};
@@ -84,8 +82,6 @@ extern float imu_health;
#endif
extern void ahrs_dcm_init(void);
-extern void ahrs_dcm_set_body_to_imu(struct OrientationReps *body_to_imu);
-extern void ahrs_dcm_set_body_to_imu_quat(struct FloatQuat *q_b2i);
extern bool ahrs_dcm_align(struct FloatRates *lp_gyro, struct FloatVect3 *lp_accel,
struct FloatVect3 *lp_mag);
extern void ahrs_dcm_propagate(struct FloatRates *gyro, float dt);
diff --git a/sw/airborne/modules/ahrs/ahrs_float_dcm_wrapper.c b/sw/airborne/modules/ahrs/ahrs_float_dcm_wrapper.c
index c1c3a36db5c..f54df181ede 100644
--- a/sw/airborne/modules/ahrs/ahrs_float_dcm_wrapper.c
+++ b/sw/airborne/modules/ahrs/ahrs_float_dcm_wrapper.c
@@ -82,7 +82,6 @@ static abi_event gyro_ev;
static abi_event accel_ev;
static abi_event mag_ev;
static abi_event aligner_ev;
-static abi_event body_to_imu_ev;
static abi_event gps_ev;
@@ -156,12 +155,6 @@ static void aligner_cb(uint8_t __attribute__((unused)) sender_id,
}
}
-static void body_to_imu_cb(uint8_t sender_id __attribute__((unused)),
- struct FloatQuat *q_b2i_f)
-{
- ahrs_dcm_set_body_to_imu_quat(q_b2i_f);
-}
-
static void gps_cb(uint8_t sender_id __attribute__((unused)),
uint32_t stamp __attribute__((unused)),
struct GpsState *gps_s)
@@ -181,16 +174,12 @@ static bool ahrs_dcm_enable_output(bool enable)
static void set_body_orientation_and_rates(void)
{
if (ahrs_dcm_output_enabled) {
- struct FloatRMat *body_to_imu_rmat = orientationGetRMat_f(&ahrs_dcm.body_to_imu);
-
- struct FloatRates body_rate;
- float_rmat_transp_ratemult(&body_rate, body_to_imu_rmat, &ahrs_dcm.imu_rate);
- stateSetBodyRates_f(&body_rate);
-
- struct FloatRMat ltp_to_imu_rmat, ltp_to_body_rmat;
- float_rmat_of_eulers(<p_to_imu_rmat, &ahrs_dcm.ltp_to_imu_euler);
- float_rmat_comp_inv(<p_to_body_rmat, <p_to_imu_rmat, body_to_imu_rmat);
+ /* Set the state */
+ stateSetBodyRates_f(&ahrs_dcm.body_rate);
+ /* Convert eulers to RMaat and set state */
+ struct FloatRMat ltp_to_body_rmat;
+ float_rmat_of_eulers(<p_to_body_rmat, &ahrs_dcm.ltp_to_body_euler);
stateSetNedToBodyRMat_f(<p_to_body_rmat);
}
}
@@ -207,8 +196,7 @@ void ahrs_dcm_register(void)
AbiBindMsgIMU_GYRO(AHRS_DCM_IMU_ID, &gyro_ev, gyro_cb);
AbiBindMsgIMU_ACCEL(AHRS_DCM_IMU_ID, &accel_ev, accel_cb);
AbiBindMsgIMU_MAG(AHRS_DCM_MAG_ID, &mag_ev, mag_cb);
- AbiBindMsgIMU_LOWPASSED(ABI_BROADCAST, &aligner_ev, aligner_cb);
- AbiBindMsgBODY_TO_IMU_QUAT(ABI_BROADCAST, &body_to_imu_ev, body_to_imu_cb);
+ AbiBindMsgIMU_LOWPASSED(AHRS_DCM_IMU_ID, &aligner_ev, aligner_cb);
AbiBindMsgGPS(AHRS_DCM_GPS_ID, &gps_ev, gps_cb);
#if PERIODIC_TELEMETRY
diff --git a/sw/airborne/modules/ahrs/ahrs_float_invariant.c b/sw/airborne/modules/ahrs/ahrs_float_invariant.c
index b2408e0e3ab..af88db5accb 100644
--- a/sw/airborne/modules/ahrs/ahrs_float_invariant.c
+++ b/sw/airborne/modules/ahrs/ahrs_float_invariant.c
@@ -178,10 +178,7 @@ void ahrs_float_invariant_propagate(struct FloatRates* gyro, float dt)
}
// fill command vector
- struct FloatRates gyro_meas_body;
- struct FloatRMat *body_to_imu_rmat = orientationGetRMat_f(&ahrs_float_inv.body_to_imu);
- float_rmat_transp_ratemult(&gyro_meas_body, body_to_imu_rmat, gyro);
- ahrs_float_inv.cmd.rates = gyro_meas_body;
+ RATES_COPY(ahrs_float_inv.cmd.rates, *gyro);
// update correction gains
error_output(&ahrs_float_inv);
@@ -249,10 +246,8 @@ void ahrs_float_invariant_update_mag(struct FloatVect3* mag)
mag_frozen_count = MAG_FROZEN_COUNT;
}
} else {
- // values are moving
- struct FloatRMat *body_to_imu_rmat = orientationGetRMat_f(&ahrs_float_inv.body_to_imu);
// new values in body frame
- float_rmat_transp_vmult(&ahrs_float_inv.meas.mag, body_to_imu_rmat, mag);
+ VECT3_COPY(ahrs_float_inv.meas.mag, *mag);
// reset counter
mag_frozen_count = MAG_FROZEN_COUNT;
}
@@ -380,14 +375,3 @@ void float_quat_vmul_right(struct FloatQuat *mright, const struct FloatQuat *q,
VECT3_ADD(v2, v1);
QUAT_ASSIGN(*mright, qi, v2.x, v2.y, v2.z);
}
-
-void ahrs_float_inv_set_body_to_imu_quat(struct FloatQuat *q_b2i)
-{
- orientationSetQuat_f(&ahrs_float_inv.body_to_imu, q_b2i);
-
- if (!ahrs_float_inv.is_aligned) {
- /* Set ltp_to_imu so that body is zero */
- ahrs_float_inv.state.quat = *q_b2i;
- }
-}
-
diff --git a/sw/airborne/modules/ahrs/ahrs_float_invariant.h b/sw/airborne/modules/ahrs/ahrs_float_invariant.h
index 2301574bbe6..50a93823077 100644
--- a/sw/airborne/modules/ahrs/ahrs_float_invariant.h
+++ b/sw/airborne/modules/ahrs/ahrs_float_invariant.h
@@ -96,10 +96,7 @@ struct AhrsFloatInv {
struct inv_correction_gains corr; ///< correction gains
struct inv_gains gains; ///< tuning gains
- bool reset; ///< flag to request reset/reinit the filter
-
- /** body_to_imu rotation */
- struct OrientationReps body_to_imu;
+ bool reset; ///< flag to request reset/reinit the filte
struct FloatVect3 mag_h;
bool is_aligned;
@@ -108,7 +105,6 @@ struct AhrsFloatInv {
extern struct AhrsFloatInv ahrs_float_inv;
extern void ahrs_float_invariant_init(void);
-extern void ahrs_float_inv_set_body_to_imu_quat(struct FloatQuat *q_b2i);
extern void ahrs_float_invariant_align(struct FloatRates *lp_gyro,
struct FloatVect3 *lp_accel,
struct FloatVect3 *lp_mag);
diff --git a/sw/airborne/modules/ahrs/ahrs_float_invariant_wrapper.c b/sw/airborne/modules/ahrs/ahrs_float_invariant_wrapper.c
index c7814282668..f1a9c0252fd 100644
--- a/sw/airborne/modules/ahrs/ahrs_float_invariant_wrapper.c
+++ b/sw/airborne/modules/ahrs/ahrs_float_invariant_wrapper.c
@@ -35,7 +35,7 @@
#ifndef AHRS_FINV_OUTPUT_ENABLED
#define AHRS_FINV_OUTPUT_ENABLED TRUE
#endif
-PRINT_CONFIG_VAR(AHRS_INV_OUTPUT_ENABLED)
+PRINT_CONFIG_VAR(AHRS_FINV_OUTPUT_ENABLED)
/** if TRUE with push the estimation results to the state interface */
static bool ahrs_finv_output_enabled;
@@ -50,25 +50,16 @@ static void compute_body_orientation_and_rates(void);
static void send_att(struct transport_tx *trans, struct link_device *dev)
{
- /* compute eulers in int (IMU frame) */
- struct FloatEulers ltp_to_imu_euler;
- float_eulers_of_quat(<p_to_imu_euler, &ahrs_float_inv.state.quat);
- struct Int32Eulers eulers_imu;
- EULERS_BFP_OF_REAL(eulers_imu, ltp_to_imu_euler);
-
/* compute Eulers in int (body frame) */
- struct FloatQuat ltp_to_body_quat;
- struct FloatQuat *body_to_imu_quat = orientationGetQuat_f(&ahrs_float_inv.body_to_imu);
- float_quat_comp_inv(<p_to_body_quat, &ahrs_float_inv.state.quat, body_to_imu_quat);
struct FloatEulers ltp_to_body_euler;
- float_eulers_of_quat(<p_to_body_euler, <p_to_body_quat);
+ float_eulers_of_quat(<p_to_body_euler, &ahrs_float_inv.state.quat);
struct Int32Eulers eulers_body;
EULERS_BFP_OF_REAL(eulers_body, ltp_to_body_euler);
pprz_msg_send_AHRS_EULER_INT(trans, dev, AC_ID,
- &eulers_imu.phi,
- &eulers_imu.theta,
- &eulers_imu.psi,
+ &eulers_body.phi,
+ &eulers_body.theta,
+ &eulers_body.psi,
&eulers_body.phi,
&eulers_body.theta,
&eulers_body.psi,
@@ -115,7 +106,6 @@ 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;
static abi_event geo_mag_ev;
/**
@@ -193,12 +183,6 @@ static void aligner_cb(uint8_t __attribute__((unused)) sender_id,
}
}
-static void body_to_imu_cb(uint8_t sender_id __attribute__((unused)),
- struct FloatQuat *q_b2i_f)
-{
- ahrs_float_inv_set_body_to_imu_quat(q_b2i_f);
-}
-
static void geo_mag_cb(uint8_t sender_id __attribute__((unused)), struct FloatVect3 *h)
{
ahrs_float_inv.mag_h = *h;
@@ -216,20 +200,13 @@ static bool ahrs_float_invariant_enable_output(bool enable)
static void compute_body_orientation_and_rates(void)
{
if (ahrs_finv_output_enabled) {
- /* Compute LTP to BODY quaternion */
- struct FloatQuat ltp_to_body_quat;
- struct FloatQuat *body_to_imu_quat = orientationGetQuat_f(&ahrs_float_inv.body_to_imu);
- float_quat_comp_inv(<p_to_body_quat, &ahrs_float_inv.state.quat, body_to_imu_quat);
/* Set state */
- stateSetNedToBodyQuat_f(<p_to_body_quat);
+ stateSetNedToBodyQuat_f(&ahrs_float_inv.state.quat);
/* compute body rates */
struct FloatRates body_rate;
RATES_DIFF(body_rate, ahrs_float_inv.cmd.rates, ahrs_float_inv.state.bias);
- struct FloatRMat *body_to_imu_rmat = orientationGetRMat_f(&ahrs_float_inv.body_to_imu);
- float_rmat_transp_ratemult(&body_rate, body_to_imu_rmat, &body_rate);
stateSetBodyRates_f(&body_rate);
-
}
}
@@ -247,7 +224,6 @@ void ahrs_float_invariant_register(void)
AbiBindMsgIMU_GYRO(AHRS_FINV_IMU_ID, &gyro_ev, gyro_cb);
AbiBindMsgIMU_ACCEL(AHRS_FINV_IMU_ID, &accel_ev, accel_cb);
AbiBindMsgIMU_LOWPASSED(AHRS_FINV_IMU_ID, &aligner_ev, aligner_cb);
- AbiBindMsgBODY_TO_IMU_QUAT(AHRS_FINV_IMU_ID, &body_to_imu_ev, body_to_imu_cb);
AbiBindMsgGEO_MAG(ABI_BROADCAST, &geo_mag_ev, geo_mag_cb);
#if PERIODIC_TELEMETRY
diff --git a/sw/airborne/modules/ahrs/ahrs_float_mlkf.c b/sw/airborne/modules/ahrs/ahrs_float_mlkf.c
index badb6e16def..af412ed7c2c 100644
--- a/sw/airborne/modules/ahrs/ahrs_float_mlkf.c
+++ b/sw/airborne/modules/ahrs/ahrs_float_mlkf.c
@@ -64,9 +64,9 @@ void ahrs_mlkf_init(void)
ahrs_mlkf.is_aligned = false;
- /* init ltp_to_imu quaternion as zero/identity rotation */
- float_quat_identity(&ahrs_mlkf.ltp_to_imu_quat);
- FLOAT_RATES_ZERO(ahrs_mlkf.imu_rate);
+ /* init ltp_to_body quaternion as zero/identity rotation */
+ float_quat_identity(&ahrs_mlkf.ltp_to_body_quat);
+ FLOAT_RATES_ZERO(ahrs_mlkf.body_rate);
VECT3_ASSIGN(ahrs_mlkf.mag_h, AHRS_H_X, AHRS_H_Y, AHRS_H_Z);
@@ -88,28 +88,12 @@ void ahrs_mlkf_init(void)
VECT3_ASSIGN(ahrs_mlkf.mag_noise, AHRS_MAG_NOISE_X, AHRS_MAG_NOISE_Y, AHRS_MAG_NOISE_Z);
}
-void ahrs_mlkf_set_body_to_imu(struct OrientationReps *body_to_imu)
-{
- ahrs_mlkf_set_body_to_imu_quat(orientationGetQuat_f(body_to_imu));
-}
-
-void ahrs_mlkf_set_body_to_imu_quat(struct FloatQuat *q_b2i)
-{
- orientationSetQuat_f(&ahrs_mlkf.body_to_imu, q_b2i);
-
- if (!ahrs_mlkf.is_aligned) {
- /* Set ltp_to_imu so that body is zero */
- ahrs_mlkf.ltp_to_imu_quat = *orientationGetQuat_f(&ahrs_mlkf.body_to_imu);
- }
-}
-
-
bool ahrs_mlkf_align(struct FloatRates *lp_gyro, struct FloatVect3 *lp_accel,
struct FloatVect3 *lp_mag)
{
/* Compute an initial orientation from accel and mag directly as quaternion */
- ahrs_float_get_quat_from_accel_mag(&ahrs_mlkf.ltp_to_imu_quat, lp_accel, lp_mag);
+ ahrs_float_get_quat_from_accel_mag(&ahrs_mlkf.ltp_to_body_quat, lp_accel, lp_mag);
/* used averaged gyro as initial value for bias */
ahrs_mlkf.gyro_bias = *lp_gyro;
@@ -170,14 +154,14 @@ static inline void propagate_ref(struct FloatRates *gyro, float dt)
#ifdef AHRS_PROPAGATE_LOW_PASS_RATES
/* lowpass angular rates */
const float alpha = 0.1;
- FLOAT_RATES_LIN_CMB(ahrs_mlkf.imu_rate, ahrs_mlkf.imu_rate,
+ FLOAT_RATES_LIN_CMB(ahrs_mlkf.body_rate, ahrs_mlkf.body_rate,
(1. - alpha), rates, alpha);
#else
- RATES_COPY(ahrs_mlkf.imu_rate, rates);
+ RATES_COPY(ahrs_mlkf.body_rate, rates);
#endif
/* propagate reference quaternion */
- float_quat_integrate(&ahrs_mlkf.ltp_to_imu_quat, &ahrs_mlkf.imu_rate, dt);
+ float_quat_integrate(&ahrs_mlkf.ltp_to_body_quat, &ahrs_mlkf.body_rate, dt);
}
@@ -189,9 +173,9 @@ static inline void propagate_state(float dt)
{
/* predict covariance */
- const float dp = ahrs_mlkf.imu_rate.p * dt;
- const float dq = ahrs_mlkf.imu_rate.q * dt;
- const float dr = ahrs_mlkf.imu_rate.r * dt;
+ const float dp = ahrs_mlkf.body_rate.p * dt;
+ const float dq = ahrs_mlkf.body_rate.q * dt;
+ const float dr = ahrs_mlkf.body_rate.r * dt;
float F[6][6] = {{ 1., dr, -dq, -dt, 0., 0. },
{ -dr, 1., dp, 0., -dt, 0. },
@@ -225,7 +209,7 @@ static inline void update_state(const struct FloatVect3 *i_expected, struct Floa
/* converted expected measurement from inertial to body frame */
struct FloatVect3 b_expected;
- float_quat_vmult(&b_expected, &ahrs_mlkf.ltp_to_imu_quat, i_expected);
+ float_quat_vmult(&b_expected, &ahrs_mlkf.ltp_to_body_quat, i_expected);
// S = HPH' + JRJ
float H[3][6] = {{ 0., -b_expected.z, b_expected.y, 0., 0., 0.},
@@ -294,12 +278,12 @@ static inline void update_state_heading(const struct FloatVect3 *i_expected,
/* converted expected measurement from inertial to body frame */
struct FloatVect3 b_expected;
- float_quat_vmult(&b_expected, &ahrs_mlkf.ltp_to_imu_quat, i_expected);
+ float_quat_vmult(&b_expected, &ahrs_mlkf.ltp_to_body_quat, i_expected);
/* set roll/pitch errors to zero to only correct heading */
struct FloatVect3 i_h_2d = {i_expected->y, -i_expected->x, 0.f};
struct FloatVect3 b_yaw;
- float_quat_vmult(&b_yaw, &ahrs_mlkf.ltp_to_imu_quat, &i_h_2d);
+ float_quat_vmult(&b_yaw, &ahrs_mlkf.ltp_to_body_quat, &i_h_2d);
// S = HPH' + JRJ
float H[3][6] = {{ 0., 0., b_yaw.x, 0., 0., 0.},
{ 0., 0., b_yaw.y, 0., 0., 0.},
@@ -359,9 +343,9 @@ static inline void reset_state(void)
ahrs_mlkf.gibbs_cor.qi = 2.;
struct FloatQuat q_tmp;
- float_quat_comp(&q_tmp, &ahrs_mlkf.ltp_to_imu_quat, &ahrs_mlkf.gibbs_cor);
+ float_quat_comp(&q_tmp, &ahrs_mlkf.ltp_to_body_quat, &ahrs_mlkf.gibbs_cor);
float_quat_normalize(&q_tmp);
- ahrs_mlkf.ltp_to_imu_quat = q_tmp;
+ ahrs_mlkf.ltp_to_body_quat = q_tmp;
float_quat_identity(&ahrs_mlkf.gibbs_cor);
}
diff --git a/sw/airborne/modules/ahrs/ahrs_float_mlkf.h b/sw/airborne/modules/ahrs/ahrs_float_mlkf.h
index 74d062c9eb2..12c2ebfce8c 100644
--- a/sw/airborne/modules/ahrs/ahrs_float_mlkf.h
+++ b/sw/airborne/modules/ahrs/ahrs_float_mlkf.h
@@ -41,9 +41,8 @@ enum AhrsMlkfStatus {
};
struct AhrsMlkf {
- struct FloatQuat ltp_to_imu_quat; ///< Rotation from LocalTangentPlane to IMU frame as unit quaternion
struct FloatQuat ltp_to_body_quat; ///< Rotation from LocalTangentPlane to body frame as unit quaternion
- struct FloatRates imu_rate; ///< Rotational velocity in IMU frame
+ struct FloatRates body_rate; ///< Rotational velocity in body frame
struct FloatRates gyro_bias;
struct FloatVect3 mag_h;
@@ -54,9 +53,6 @@ struct AhrsMlkf {
float P[6][6];
float lp_accel;
- /** body_to_imu rotation */
- struct OrientationReps body_to_imu;
-
enum AhrsMlkfStatus status;
bool is_aligned;
};
@@ -64,8 +60,6 @@ struct AhrsMlkf {
extern struct AhrsMlkf ahrs_mlkf;
extern void ahrs_mlkf_init(void);
-extern void ahrs_mlkf_set_body_to_imu(struct OrientationReps *body_to_imu);
-extern void ahrs_mlkf_set_body_to_imu_quat(struct FloatQuat *q_b2i);
extern bool ahrs_mlkf_align(struct FloatRates *lp_gyro, struct FloatVect3 *lp_accel,
struct FloatVect3 *lp_mag);
extern void ahrs_mlkf_propagate(struct FloatRates *gyro, float dt);
diff --git a/sw/airborne/modules/ahrs/ahrs_float_mlkf_wrapper.c b/sw/airborne/modules/ahrs/ahrs_float_mlkf_wrapper.c
index 07c57b0abaf..7d08b418149 100644
--- a/sw/airborne/modules/ahrs/ahrs_float_mlkf_wrapper.c
+++ b/sw/airborne/modules/ahrs/ahrs_float_mlkf_wrapper.c
@@ -48,12 +48,12 @@ static void set_body_state_from_quat(void);
static void send_euler(struct transport_tx *trans, struct link_device *dev)
{
- struct FloatEulers ltp_to_imu_euler;
- float_eulers_of_quat(<p_to_imu_euler, &ahrs_mlkf.ltp_to_imu_quat);
+ struct FloatEulers ltp_to_body_euler;
+ float_eulers_of_quat(<p_to_body_euler, &ahrs_mlkf.ltp_to_body_quat);
pprz_msg_send_AHRS_EULER(trans, dev, AC_ID,
- <p_to_imu_euler.phi,
- <p_to_imu_euler.theta,
- <p_to_imu_euler.psi,
+ <p_to_body_euler.phi,
+ <p_to_body_euler.theta,
+ <p_to_body_euler.psi,
&ahrs_mlkf_id);
}
@@ -102,7 +102,6 @@ static abi_event gyro_ev;
static abi_event accel_ev;
static abi_event mag_ev;
static abi_event aligner_ev;
-static abi_event body_to_imu_ev;
static abi_event geo_mag_ev;
@@ -179,12 +178,6 @@ static void aligner_cb(uint8_t __attribute__((unused)) sender_id,
}
}
-static void body_to_imu_cb(uint8_t sender_id __attribute__((unused)),
- struct FloatQuat *q_b2i_f)
-{
- ahrs_mlkf_set_body_to_imu_quat(q_b2i_f);
-}
-
static void geo_mag_cb(uint8_t sender_id __attribute__((unused)), struct FloatVect3 *h)
{
ahrs_mlkf.mag_h = *h;
@@ -202,20 +195,9 @@ static bool ahrs_mlkf_enable_output(bool enable)
static void set_body_state_from_quat(void)
{
if (ahrs_mlkf_output_enabled) {
- struct FloatQuat *body_to_imu_quat = orientationGetQuat_f(&ahrs_mlkf.body_to_imu);
- struct FloatRMat *body_to_imu_rmat = orientationGetRMat_f(&ahrs_mlkf.body_to_imu);
-
- /* Compute LTP to BODY quaternion */
- struct FloatQuat ltp_to_body_quat;
- float_quat_comp_inv(<p_to_body_quat, &ahrs_mlkf.ltp_to_imu_quat, body_to_imu_quat);
/* Set in state interface */
- stateSetNedToBodyQuat_f(<p_to_body_quat);
-
- /* compute body rates */
- struct FloatRates body_rate;
- float_rmat_transp_ratemult(&body_rate, body_to_imu_rmat, &ahrs_mlkf.imu_rate);
- /* Set state */
- stateSetBodyRates_f(&body_rate);
+ stateSetNedToBodyQuat_f(&ahrs_mlkf.ltp_to_body_quat);
+ stateSetBodyRates_f(&ahrs_mlkf.body_rate);
}
}
@@ -231,8 +213,7 @@ void ahrs_mlkf_register(void)
AbiBindMsgIMU_GYRO(AHRS_MLKF_IMU_ID, &gyro_ev, gyro_cb);
AbiBindMsgIMU_ACCEL(AHRS_MLKF_IMU_ID, &accel_ev, accel_cb);
AbiBindMsgIMU_MAG(AHRS_MLKF_MAG_ID, &mag_ev, mag_cb);
- AbiBindMsgIMU_LOWPASSED(ABI_BROADCAST, &aligner_ev, aligner_cb);
- AbiBindMsgBODY_TO_IMU_QUAT(ABI_BROADCAST, &body_to_imu_ev, body_to_imu_cb);
+ AbiBindMsgIMU_LOWPASSED(AHRS_MLKF_IMU_ID, &aligner_ev, aligner_cb);
AbiBindMsgGEO_MAG(ABI_BROADCAST, &geo_mag_ev, geo_mag_cb);
#if PERIODIC_TELEMETRY
diff --git a/sw/airborne/modules/ahrs/ahrs_int_cmpl_quat.c b/sw/airborne/modules/ahrs/ahrs_int_cmpl_quat.c
index d3ee04ad998..bb8cc73c068 100644
--- a/sw/airborne/modules/ahrs/ahrs_int_cmpl_quat.c
+++ b/sw/airborne/modules/ahrs/ahrs_int_cmpl_quat.c
@@ -121,10 +121,9 @@ void ahrs_icq_init(void)
ahrs_icq.ltp_vel_norm_valid = false;
ahrs_icq.heading_aligned = false;
- /* init ltp_to_imu quaternion as zero/identity rotation */
- int32_quat_identity(&ahrs_icq.ltp_to_imu_quat);
-
- INT_RATES_ZERO(ahrs_icq.imu_rate);
+ /* init ltp_to_body quaternion as zero/identity rotation */
+ int32_quat_identity(&ahrs_icq.ltp_to_body_quat);
+ INT_RATES_ZERO(ahrs_icq.body_rate);
INT_RATES_ZERO(ahrs_icq.gyro_bias);
INT_RATES_ZERO(ahrs_icq.rate_correction);
@@ -161,12 +160,12 @@ bool ahrs_icq_align(struct Int32Rates *lp_gyro, struct Int32Vect3 *lp_accel,
#if USE_MAGNETOMETER
/* Compute an initial orientation from accel and mag directly as quaternion */
- ahrs_int_get_quat_from_accel_mag(&ahrs_icq.ltp_to_imu_quat,
+ ahrs_int_get_quat_from_accel_mag(&ahrs_icq.ltp_to_body_quat,
lp_accel, lp_mag);
ahrs_icq.heading_aligned = true;
#else
/* Compute an initial orientation from accel and just set heading to zero */
- ahrs_int_get_quat_from_accel(&ahrs_icq.ltp_to_imu_quat, lp_accel);
+ ahrs_int_get_quat_from_accel(&ahrs_icq.ltp_to_body_quat, lp_accel);
ahrs_icq.heading_aligned = false;
// supress unused arg warning
lp_mag = lp_mag;
@@ -194,11 +193,11 @@ void ahrs_icq_propagate(struct Int32Rates *gyro, float dt)
/* low pass rate */
#ifdef AHRS_PROPAGATE_LOW_PASS_RATES
- RATES_SMUL(ahrs_icq.imu_rate, ahrs_icq.imu_rate, AHRS_PROPAGATE_LOW_PASS_RATES_MUL);
- RATES_ADD(ahrs_icq.imu_rate, omega);
- RATES_SDIV(ahrs_icq.imu_rate, ahrs_icq.imu_rate, AHRS_PROPAGATE_LOW_PASS_RATES_DIV);
+ RATES_SMUL(ahrs_icq.body_rate, ahrs_icq.body_rate, AHRS_PROPAGATE_LOW_PASS_RATES_MUL);
+ RATES_ADD(ahrs_icq.body_rate, omega);
+ RATES_SDIV(ahrs_icq.body_rate, ahrs_icq.body_rate, AHRS_PROPAGATE_LOW_PASS_RATES_DIV);
#else
- RATES_COPY(ahrs_icq.imu_rate, omega);
+ RATES_COPY(ahrs_icq.body_rate, omega);
#endif
/* add correction */
@@ -207,9 +206,9 @@ void ahrs_icq_propagate(struct Int32Rates *gyro, float dt)
INT_RATES_ZERO(ahrs_icq.rate_correction);
/* integrate quaternion */
- int32_quat_integrate_fi(&ahrs_icq.ltp_to_imu_quat, &ahrs_icq.high_rez_quat,
+ int32_quat_integrate_fi(&ahrs_icq.ltp_to_body_quat, &ahrs_icq.high_rez_quat,
&omega, freq);
- int32_quat_normalize(&ahrs_icq.ltp_to_imu_quat);
+ int32_quat_normalize(&ahrs_icq.ltp_to_body_quat);
// increase accel and mag propagation counters
ahrs_icq.accel_cnt++;
@@ -246,11 +245,11 @@ void ahrs_icq_update_accel(struct Int32Vect3 *accel, float dt)
}
// c2 = ltp z-axis in imu-frame
- struct Int32RMat ltp_to_imu_rmat;
- int32_rmat_of_quat(<p_to_imu_rmat, &ahrs_icq.ltp_to_imu_quat);
- struct Int32Vect3 c2 = { RMAT_ELMT(ltp_to_imu_rmat, 0, 2),
- RMAT_ELMT(ltp_to_imu_rmat, 1, 2),
- RMAT_ELMT(ltp_to_imu_rmat, 2, 2)
+ struct Int32RMat ltp_to_body_rmat;
+ int32_rmat_of_quat(<p_to_body_rmat, &ahrs_icq.ltp_to_body_quat);
+ struct Int32Vect3 c2 = { RMAT_ELMT(ltp_to_body_rmat, 0, 2),
+ RMAT_ELMT(ltp_to_body_rmat, 1, 2),
+ RMAT_ELMT(ltp_to_body_rmat, 2, 2)
};
struct Int32Vect3 residual;
@@ -279,20 +278,13 @@ void ahrs_icq_update_accel(struct Int32Vect3 *accel, float dt)
/* assume tangential velocity along body x-axis */
{ahrs_icq.ltp_vel_norm >> COMPUTATION_FRAC, 0, 0};
#endif
- struct Int32RMat *body_to_imu_rmat = orientationGetRMat_i(&ahrs_icq.body_to_imu);
- struct Int32Rates body_rate;
- int32_rmat_transp_ratemult(&body_rate, body_to_imu_rmat, &ahrs_icq.imu_rate);
struct Int32Vect3 acc_c_body;
- VECT3_RATES_CROSS_VECT3(acc_c_body, body_rate, vel_tangential_body);
+ VECT3_RATES_CROSS_VECT3(acc_c_body, ahrs_icq.body_rate, vel_tangential_body);
INT32_VECT3_RSHIFT(acc_c_body, acc_c_body, ACC_FROM_CROSS_FRAC);
- /* convert centrifucal acceleration from body to imu frame */
- struct Int32Vect3 acc_c_imu;
- int32_rmat_vmult(&acc_c_imu, body_to_imu_rmat, &acc_c_body);
-
- /* and subtract it from imu measurement to get a corrected measurement
+ /* centrifucal acceleration subtract it from imu measurement to get a corrected measurement
* of the gravity vector */
- VECT3_DIFF(pseudo_gravity_measurement, *accel, acc_c_imu);
+ VECT3_DIFF(pseudo_gravity_measurement, *accel, acc_c_body);
} else {
VECT3_COPY(pseudo_gravity_measurement, *accel);
}
@@ -410,11 +402,11 @@ void ahrs_icq_set_mag_gains(void)
static inline void ahrs_icq_update_mag_full(struct Int32Vect3 *mag, float dt)
{
- struct Int32RMat ltp_to_imu_rmat;
- int32_rmat_of_quat(<p_to_imu_rmat, &ahrs_icq.ltp_to_imu_quat);
+ struct Int32RMat ltp_to_body_rmat;
+ int32_rmat_of_quat(<p_to_body_rmat, &ahrs_icq.ltp_to_body_quat);
struct Int32Vect3 expected_imu;
- int32_rmat_vmult(&expected_imu, <p_to_imu_rmat, &ahrs_icq.mag_h);
+ int32_rmat_vmult(&expected_imu, <p_to_body_rmat, &ahrs_icq.mag_h);
struct Int32Vect3 residual;
VECT3_CROSS_PRODUCT(residual, *mag, expected_imu);
@@ -467,11 +459,11 @@ static inline void ahrs_icq_update_mag_2d(struct Int32Vect3 *mag, float dt)
/* normalize expected ltp in 2D (x,y) */
int32_vect2_normalize(&expected_ltp, INT32_MAG_FRAC);
- struct Int32RMat ltp_to_imu_rmat;
- int32_rmat_of_quat(<p_to_imu_rmat, &ahrs_icq.ltp_to_imu_quat);
+ struct Int32RMat ltp_to_body_rmat;
+ int32_rmat_of_quat(<p_to_body_rmat, &ahrs_icq.ltp_to_body_quat);
struct Int32Vect3 measured_ltp;
- int32_rmat_transp_vmult(&measured_ltp, <p_to_imu_rmat, mag);
+ int32_rmat_transp_vmult(&measured_ltp, <p_to_body_rmat, mag);
/* normalize measured ltp in 2D (x,y) */
struct Int32Vect2 measured_ltp_2d = {measured_ltp.x, measured_ltp.y};
@@ -485,15 +477,15 @@ static inline void ahrs_icq_update_mag_2d(struct Int32Vect3 *mag, float dt)
};
- struct Int32Vect3 residual_imu;
- int32_rmat_vmult(&residual_imu, <p_to_imu_rmat, &residual_ltp);
+ struct Int32Vect3 residual_body;
+ int32_rmat_vmult(&residual_body, <p_to_body_rmat, &residual_ltp);
/* Complementary filter proportionnal gain.
* Kp = 2 * mag_zeta * mag_omega
* final Kp with frequency correction = Kp * ahrs_icq.mag_cnt
* with ahrs_icq.mag_cnt beeing the number of propagations since last update
*
- * residual_imu FRAC = residual_ltp FRAC = 17
+ * residual_body FRAC = residual_ltp FRAC = 17
* rate_correction FRAC: RATE_FRAC = 12
* FRAC conversion: 2^12 / 2^17 = 1/32
*
@@ -502,15 +494,15 @@ static inline void ahrs_icq_update_mag_2d(struct Int32Vect3 *mag, float dt)
*/
int32_t inv_rate_gain = (int32_t)(32.0 / (ahrs_icq.mag_kp * ahrs_icq.mag_cnt));
- ahrs_icq.rate_correction.p += (residual_imu.x / inv_rate_gain);
- ahrs_icq.rate_correction.q += (residual_imu.y / inv_rate_gain);
- ahrs_icq.rate_correction.r += (residual_imu.z / inv_rate_gain);
+ ahrs_icq.rate_correction.p += (residual_body.x / inv_rate_gain);
+ ahrs_icq.rate_correction.q += (residual_body.y / inv_rate_gain);
+ ahrs_icq.rate_correction.r += (residual_body.z / inv_rate_gain);
/* Complementary filter integral gain
* Correct the gyro bias.
* Ki = omega^2 * dt
*
- * residual_imu FRAC = residual_ltp FRAC = 17
+ * residual_body FRAC = residual_ltp FRAC = 17
* high_rez_bias FRAC: RATE_FRAC+28 = 40
* FRAC conversion: 2^40 / 2^17 = 2^23
*
@@ -518,9 +510,9 @@ static inline void ahrs_icq_update_mag_2d(struct Int32Vect3 *mag, float dt)
*/
int32_t bias_gain = (int32_t)(ahrs_icq.mag_ki * dt * (1 << 23));
- ahrs_icq.high_rez_bias.p -= (residual_imu.x * bias_gain);
- ahrs_icq.high_rez_bias.q -= (residual_imu.y * bias_gain);
- ahrs_icq.high_rez_bias.r -= (residual_imu.z * bias_gain);
+ ahrs_icq.high_rez_bias.p -= (residual_body.x * bias_gain);
+ ahrs_icq.high_rez_bias.q -= (residual_body.y * bias_gain);
+ ahrs_icq.high_rez_bias.r -= (residual_body.z * bias_gain);
INT_RATES_RSHIFT(ahrs_icq.gyro_bias, ahrs_icq.high_rez_bias, 28);
@@ -569,11 +561,8 @@ void ahrs_icq_update_heading(int32_t heading)
// row 0 of ltp_to_body_rmat = body x-axis in ltp frame
// we only consider x and y
- struct Int32Quat *body_to_imu_quat = orientationGetQuat_i(&ahrs_icq.body_to_imu);
- struct Int32Quat ltp_to_body_quat;
- int32_quat_comp_inv(<p_to_body_quat, &ahrs_icq.ltp_to_imu_quat, body_to_imu_quat);
struct Int32RMat ltp_to_body_rmat;
- int32_rmat_of_quat(<p_to_body_rmat, <p_to_body_quat);
+ int32_rmat_of_quat(<p_to_body_rmat, &ahrs_icq.ltp_to_body_quat);
struct Int32Vect2 expected_ltp = {
RMAT_ELMT(ltp_to_body_rmat, 0, 0),
RMAT_ELMT(ltp_to_body_rmat, 0, 1)
@@ -590,18 +579,16 @@ void ahrs_icq_update_heading(int32_t heading)
(expected_ltp.x * heading_y - expected_ltp.y * heading_x) / (1 << INT32_ANGLE_FRAC)
};
- struct Int32Vect3 residual_imu;
- struct Int32RMat ltp_to_imu_rmat;
- int32_rmat_of_quat(<p_to_imu_rmat, &ahrs_icq.ltp_to_imu_quat);
- int32_rmat_vmult(&residual_imu, <p_to_imu_rmat, &residual_ltp);
+ struct Int32Vect3 residual_body;
+ int32_rmat_vmult(&residual_body, <p_to_body_rmat, &residual_ltp);
// residual FRAC = TRIG_FRAC + TRIG_FRAC = 14 + 14 = 28
// rate_correction FRAC = RATE_FRAC = 12
// 2^12 / 2^28 * 4.0 = 1/2^14
// (1<qi),
&(quat->qx),
&(quat->qy),
@@ -64,13 +64,13 @@ static void send_quat(struct transport_tx *trans, struct link_device *dev)
static void send_euler(struct transport_tx *trans, struct link_device *dev)
{
- struct Int32Eulers ltp_to_imu_euler;
- int32_eulers_of_quat(<p_to_imu_euler, &ahrs_icq.ltp_to_imu_quat);
+ struct Int32Eulers ltp_to_body_euler;
+ int32_eulers_of_quat(<p_to_body_euler, &ahrs_icq.ltp_to_body_quat);
struct Int32Eulers *eulers = stateGetNedToBodyEulers_i();
pprz_msg_send_AHRS_EULER_INT(trans, dev, AC_ID,
- <p_to_imu_euler.phi,
- <p_to_imu_euler.theta,
- <p_to_imu_euler.psi,
+ <p_to_body_euler.phi,
+ <p_to_body_euler.theta,
+ <p_to_body_euler.psi,
&(eulers->phi),
&(eulers->theta),
&(eulers->psi),
@@ -132,7 +132,6 @@ static abi_event gyro_ev;
static abi_event accel_ev;
static abi_event mag_ev;
static abi_event aligner_ev;
-static abi_event body_to_imu_ev;
static abi_event geo_mag_ev;
static abi_event gps_ev;
@@ -223,12 +222,6 @@ static void aligner_cb(uint8_t __attribute__((unused)) sender_id,
}
}
-static void body_to_imu_cb(uint8_t sender_id __attribute__((unused)),
- struct FloatQuat *q_b2i_f)
-{
- ahrs_icq_set_body_to_imu_quat(q_b2i_f);
-}
-
static void geo_mag_cb(uint8_t sender_id __attribute__((unused)), struct FloatVect3 *h)
{
VECT3_ASSIGN(ahrs_icq.mag_h, MAG_BFP_OF_REAL(h->x), MAG_BFP_OF_REAL(h->y),
@@ -252,19 +245,9 @@ static bool ahrs_icq_enable_output(bool enable)
static void set_body_state_from_quat(void)
{
if (ahrs_icq_output_enabled) {
- /* Compute LTP to BODY quaternion */
- struct Int32Quat ltp_to_body_quat;
- struct Int32Quat *body_to_imu_quat = orientationGetQuat_i(&ahrs_icq.body_to_imu);
- int32_quat_comp_inv(<p_to_body_quat, &ahrs_icq.ltp_to_imu_quat, body_to_imu_quat);
- /* Set state */
- stateSetNedToBodyQuat_i(<p_to_body_quat);
-
- /* compute body rates */
- struct Int32Rates body_rate;
- struct Int32RMat *body_to_imu_rmat = orientationGetRMat_i(&ahrs_icq.body_to_imu);
- int32_rmat_transp_ratemult(&body_rate, body_to_imu_rmat, &ahrs_icq.imu_rate);
/* Set state */
- stateSetBodyRates_i(&body_rate);
+ stateSetNedToBodyQuat_i(&ahrs_icq.ltp_to_body_quat);
+ stateSetBodyRates_i(&ahrs_icq.body_rate);
}
}
@@ -280,8 +263,7 @@ void ahrs_icq_register(void)
AbiBindMsgIMU_GYRO(AHRS_ICQ_IMU_ID, &gyro_ev, gyro_cb);
AbiBindMsgIMU_ACCEL(AHRS_ICQ_IMU_ID, &accel_ev, accel_cb);
AbiBindMsgIMU_MAG(AHRS_ICQ_MAG_ID, &mag_ev, mag_cb);
- AbiBindMsgIMU_LOWPASSED(ABI_BROADCAST, &aligner_ev, aligner_cb);
- AbiBindMsgBODY_TO_IMU_QUAT(ABI_BROADCAST, &body_to_imu_ev, body_to_imu_cb);
+ AbiBindMsgIMU_LOWPASSED(AHRS_ICQ_IMU_ID, &aligner_ev, aligner_cb);
AbiBindMsgGEO_MAG(ABI_BROADCAST, &geo_mag_ev, geo_mag_cb);
AbiBindMsgGPS(AHRS_ICQ_GPS_ID, &gps_ev, gps_cb);
diff --git a/sw/airborne/modules/ahrs/ahrs_madgwick.c b/sw/airborne/modules/ahrs/ahrs_madgwick.c
index a3175cdc8b8..e18ba1f3918 100644
--- a/sw/airborne/modules/ahrs/ahrs_madgwick.c
+++ b/sw/airborne/modules/ahrs/ahrs_madgwick.c
@@ -78,11 +78,9 @@ void ahrs_madgwick_propagate(struct FloatRates* gyro, float dt)
init_state();
}
- // unbias and rotate gyro
+ // unbias gyro
struct FloatRates gyro_unbiased;
- RATES_DIFF(gyro_unbiased, *gyro, ahrs_madgwick.bias);
- struct FloatRMat *body_to_imu_rmat = orientationGetRMat_f(&ahrs_madgwick.body_to_imu);
- float_rmat_transp_ratemult(&ahrs_madgwick.rates, body_to_imu_rmat, &gyro_unbiased);
+ RATES_DIFF(ahrs_madgwick.rates, *gyro, ahrs_madgwick.bias);
// Rate of change of quaternion from gyroscope
float_quat_derivative(&qdot, &ahrs_madgwick.rates, &ahrs_madgwick.quat);
@@ -143,14 +141,3 @@ void ahrs_madgwick_update_accel(struct FloatVect3* accel)
{
ahrs_madgwick.accel = *accel;
}
-
-void ahrs_madgwick_set_body_to_imu_quat(struct FloatQuat *q_b2i)
-{
- orientationSetQuat_f(&ahrs_madgwick.body_to_imu, q_b2i);
-
- if (!ahrs_madgwick.is_aligned) {
- /* Set ltp_to_imu so that body is zero */
- ahrs_madgwick.quat = *q_b2i;
- }
-}
-
diff --git a/sw/airborne/modules/ahrs/ahrs_madgwick.h b/sw/airborne/modules/ahrs/ahrs_madgwick.h
index 5c279208999..c9a3b53a171 100644
--- a/sw/airborne/modules/ahrs/ahrs_madgwick.h
+++ b/sw/airborne/modules/ahrs/ahrs_madgwick.h
@@ -40,7 +40,6 @@ struct AhrsMadgwick {
struct FloatRates rates; ///< Measured gyro rates
struct FloatRates bias; ///< Gyro bias (from alignment)
struct FloatVect3 accel; ///< Measured accelerometers
- struct OrientationReps body_to_imu; ///< body_to_imu rotation
bool reset; ///< flag to request reset/reinit the filter
bool is_aligned; ///< aligned flag
};
@@ -48,7 +47,6 @@ struct AhrsMadgwick {
extern struct AhrsMadgwick ahrs_madgwick;
extern void ahrs_madgwick_init(void);
-extern void ahrs_madgwick_set_body_to_imu_quat(struct FloatQuat *q_b2i);
extern void ahrs_madgwick_align(struct FloatRates *lp_gyro, struct FloatVect3 *lp_accel);
extern void ahrs_madgwick_propagate(struct FloatRates* gyro, float dt);
extern void ahrs_madgwick_update_accel(struct FloatVect3* accel);
diff --git a/sw/airborne/modules/ahrs/ahrs_madgwick_wrapper.c b/sw/airborne/modules/ahrs/ahrs_madgwick_wrapper.c
index 831edb5879a..48f083cb937 100644
--- a/sw/airborne/modules/ahrs/ahrs_madgwick_wrapper.c
+++ b/sw/airborne/modules/ahrs/ahrs_madgwick_wrapper.c
@@ -56,11 +56,8 @@ static void send_att(struct transport_tx *trans, struct link_device *dev)
EULERS_BFP_OF_REAL(eulers_imu, ltp_to_imu_euler);
/* compute Eulers in int (body frame) */
- struct FloatQuat ltp_to_body_quat;
- struct FloatQuat *body_to_imu_quat = orientationGetQuat_f(&ahrs_madgwick.body_to_imu);
- float_quat_comp_inv(<p_to_body_quat, &ahrs_madgwick.quat, body_to_imu_quat);
struct FloatEulers ltp_to_body_euler;
- float_eulers_of_quat(<p_to_body_euler, <p_to_body_quat);
+ float_eulers_of_quat(<p_to_body_euler, &ahrs_madgwick.quat);
struct Int32Eulers eulers_body;
EULERS_BFP_OF_REAL(eulers_body, ltp_to_body_euler);
@@ -105,7 +102,6 @@ PRINT_CONFIG_VAR(AHRS_MADGWICK_MAG_ID)
static abi_event gyro_ev;
static abi_event accel_ev;
static abi_event aligner_ev;
-static abi_event body_to_imu_ev;
/**
* Call ahrs_madgwick_propagate on new gyro measurements.
@@ -169,12 +165,6 @@ static void aligner_cb(uint8_t __attribute__((unused)) sender_id,
}
}
-static void body_to_imu_cb(uint8_t sender_id __attribute__((unused)),
- struct FloatQuat *q_b2i_f)
-{
- ahrs_madgwick_set_body_to_imu_quat(q_b2i_f);
-}
-
static bool ahrs_madgwick_enable_output(bool enable)
{
ahrs_madgwick_output_enabled = enable;
@@ -187,12 +177,8 @@ static bool ahrs_madgwick_enable_output(bool enable)
static void compute_body_orientation_and_rates(void)
{
if (ahrs_madgwick_output_enabled) {
- /* Compute LTP to BODY quaternion */
- struct FloatQuat ltp_to_body_quat;
- struct FloatQuat *body_to_imu_quat = orientationGetQuat_f(&ahrs_madgwick.body_to_imu);
- float_quat_comp_inv(<p_to_body_quat, &ahrs_madgwick.quat, body_to_imu_quat);
/* Set state */
- stateSetNedToBodyQuat_f(<p_to_body_quat);
+ stateSetNedToBodyQuat_f(&ahrs_madgwick.quat);
/* compute body rates */
stateSetBodyRates_f(&ahrs_madgwick.rates);
@@ -212,7 +198,6 @@ void ahrs_madgwick_register(void)
AbiBindMsgIMU_GYRO(AHRS_MADGWICK_IMU_ID, &gyro_ev, gyro_cb);
AbiBindMsgIMU_ACCEL(AHRS_MADGWICK_IMU_ID, &accel_ev, accel_cb);
AbiBindMsgIMU_LOWPASSED(AHRS_MADGWICK_IMU_ID, &aligner_ev, aligner_cb);
- AbiBindMsgBODY_TO_IMU_QUAT(AHRS_MADGWICK_IMU_ID, &body_to_imu_ev, body_to_imu_cb);
#if PERIODIC_TELEMETRY
register_periodic_telemetry(DefaultPeriodic, PPRZ_MSG_ID_AHRS_EULER_INT, send_att);
diff --git a/sw/airborne/modules/imu/imu.c b/sw/airborne/modules/imu/imu.c
index b96c36732b7..55b99e1c723 100644
--- a/sw/airborne/modules/imu/imu.c
+++ b/sw/airborne/modules/imu/imu.c
@@ -33,22 +33,51 @@
#include "modules/core/abi.h"
#include "modules/energy/electrical.h"
+/** By default disable IMU integration calculations */
#ifndef IMU_INTEGRATION
-#define IMU_INTEGRATION true
+#define IMU_INTEGRATION false
#endif
+/** Default gyro calibration is empty */
#ifndef IMU_GYRO_CALIB
#define IMU_GYRO_CALIB {}
#endif
+/** Default accel calibration is empty */
#ifndef IMU_ACCEL_CALIB
#define IMU_ACCEL_CALIB {}
#endif
+/** Default mag calibration is empty */
#ifndef IMU_MAG_CALIB
#define IMU_MAG_CALIB {}
#endif
+/** Default body to imu is 0 (radians) */
+#if !defined(IMU_BODY_TO_IMU_PHI) && !defined(IMU_BODY_TO_IMU_THETA) && !defined(IMU_BODY_TO_IMU_PSI)
+#define IMU_BODY_TO_IMU_PHI 0
+#define IMU_BODY_TO_IMU_THETA 0
+#define IMU_BODY_TO_IMU_PSI 0
+#endif
+PRINT_CONFIG_VAR(IMU_BODY_TO_IMU_PHI)
+PRINT_CONFIG_VAR(IMU_BODY_TO_IMU_THETA)
+PRINT_CONFIG_VAR(IMU_BODY_TO_IMU_PSI)
+
+/* Detect depricated gyro configurations */
+#if defined(IMU_GYRO_P_NEUTRAL) || defined(IMU_GYRO_Q_NEUTRAL) || defined(IMU_GYRO_R_NEUTRAL) || defined(IMU_GYRO_P_SENS) || defined(IMU_GYRO_Q_SENS) || defined(IMU_GYRO_R_SENS)
+#error "Using older gyro calibration `IMU_GYRO_*` defines please replace with `IMU_GYRO_CALIB`!"
+#endif
+
+/* Detect depricated accel configurations */
+#if defined(IMU_ACCEL_X_NEUTRAL) || defined(IMU_ACCEL_Y_NEUTRAL) || defined(IMU_ACCEL_Z_NEUTRAL) || defined(IMU_ACCEL_X_SENS) || defined(IMU_ACCEL_Y_SENS) || defined(IMU_ACCEL_Z_SENS)
+#error "Using older accel calibration `IMU_ACCEL_*` defines please replace with `IMU_ACCEL_CALIB`!"
+#endif
+
+/* Detect depricated mag configurations */
+#if defined(IMU_MAG_X_NEUTRAL) || defined(IMU_MAG_Y_NEUTRAL) || defined(IMU_MAG_Z_NEUTRAL) || defined(IMU_MAG_X_SENS) || defined(IMU_MAG_Y_SENS) || defined(IMU_MAG_Z_SENS)
+#error "Using older mag calibration `IMU_MAG_*` defines please replace with `IMU_MAG_CALIB`!"
+#endif
+
#if PERIODIC_TELEMETRY
#include "modules/datalink/telemetry.h"
@@ -169,6 +198,7 @@ static abi_event imu_gyro_raw_ev, imu_accel_raw_ev, imu_mag_raw_ev;
static void imu_gyro_raw_cb(uint8_t sender_id, uint32_t stamp, struct Int32Rates *data, uint8_t samples);
static void imu_accel_raw_cb(uint8_t sender_id, uint32_t stamp, struct Int32Vect3 *data, uint8_t samples);
static void imu_mag_raw_cb(uint8_t sender_id, uint32_t stamp, struct Int32Vect3 *data);
+static void imu_set_body_to_imu_eulers(struct FloatEulers *body_to_imu_eulers);
void imu_init(void)
{
@@ -177,9 +207,10 @@ void imu_init(void)
return;
// Set the Body to IMU rotation
- struct FloatEulers body_to_imu_eulers =
- {IMU_BODY_TO_IMU_PHI, IMU_BODY_TO_IMU_THETA, IMU_BODY_TO_IMU_PSI};
+ struct FloatEulers body_to_imu_eulers = {IMU_BODY_TO_IMU_PHI, IMU_BODY_TO_IMU_THETA, IMU_BODY_TO_IMU_PSI};
orientationSetEulers_f(&imu.body_to_imu, &body_to_imu_eulers);
+ struct Int32RMat *body_to_imu_rmat = orientationGetRMat_i(&imu.body_to_imu);
+
// Set the calibrated sensors
const struct imu_gyro_t gyro_calib[] = IMU_GYRO_CALIB;
@@ -204,7 +235,7 @@ void imu_init(void)
imu.gyros[i].calibrated = true;
}
imu.gyros[i].last_stamp = 0;
- int32_rmat_identity(&imu.gyros[i].imu_to_sensor);
+ RMAT_COPY(imu.gyros[i].body_to_sensor, *body_to_imu_rmat);
// Accel initialization calibrated/non-calibrated
if(i >= accel_calib_len) {
@@ -219,7 +250,7 @@ void imu_init(void)
imu.accels[i].calibrated = true;
}
imu.accels[i].last_stamp = 0;
- int32_rmat_identity(&imu.accels[i].imu_to_sensor);
+ RMAT_COPY(imu.accels[i].body_to_sensor, *body_to_imu_rmat);
// Mag initialization calibrated/non-calibrated
if(i >= mag_calib_len) {
@@ -234,7 +265,7 @@ void imu_init(void)
imu.mags[i] = mag_calib[i];
imu.mags[i].calibrated = true;
}
- int32_rmat_identity(&imu.mags[i].imu_to_sensor);
+ RMAT_COPY(imu.mags[i].body_to_sensor, *body_to_imu_rmat);
}
// Bind to raw measurements
@@ -277,8 +308,12 @@ void imu_set_defaults_gyro(uint8_t abi_id, const struct Int32RMat *imu_to_sensor
return;
// Copy the defaults
- if(imu_to_sensor != NULL)
- RMAT_COPY(gyro->imu_to_sensor, *imu_to_sensor);
+ if(imu_to_sensor != NULL) {
+ struct Int32RMat body_to_sensor;
+ struct Int32RMat *body_to_imu = orientationGetRMat_i(&imu.body_to_imu);
+ int32_rmat_comp(&body_to_sensor, body_to_imu, imu_to_sensor);
+ RMAT_COPY(gyro->body_to_sensor, body_to_sensor);
+ }
if(neutral != NULL && !gyro->calibrated)
RATES_COPY(gyro->neutral, *neutral);
if(scale != NULL && !gyro->calibrated) {
@@ -306,8 +341,12 @@ void imu_set_defaults_accel(uint8_t abi_id, const struct Int32RMat *imu_to_senso
return;
// Copy the defaults
- if(imu_to_sensor != NULL)
- RMAT_COPY(accel->imu_to_sensor, *imu_to_sensor);
+ if(imu_to_sensor != NULL) {
+ struct Int32RMat body_to_sensor;
+ struct Int32RMat *body_to_imu = orientationGetRMat_i(&imu.body_to_imu);
+ int32_rmat_comp(&body_to_sensor, body_to_imu, imu_to_sensor);
+ RMAT_COPY(accel->body_to_sensor, body_to_sensor);
+ }
if(neutral != NULL && !accel->calibrated)
VECT3_COPY(accel->neutral, *neutral);
if(scale != NULL && !accel->calibrated) {
@@ -335,8 +374,12 @@ void imu_set_defaults_mag(uint8_t abi_id, const struct Int32RMat *imu_to_sensor,
return;
// Copy the defaults
- if(imu_to_sensor != NULL)
- RMAT_COPY(mag->imu_to_sensor, *imu_to_sensor);
+ if(imu_to_sensor != NULL) {
+ struct Int32RMat body_to_sensor;
+ struct Int32RMat *body_to_imu = orientationGetRMat_i(&imu.body_to_imu);
+ int32_rmat_comp(&body_to_sensor, body_to_imu, imu_to_sensor);
+ RMAT_COPY(mag->body_to_sensor, body_to_sensor);
+ }
if(neutral != NULL && !mag->calibrated)
VECT3_COPY(mag->neutral, *neutral);
if(scale != NULL && !mag->calibrated) {
@@ -362,7 +405,7 @@ static void imu_gyro_raw_cb(uint8_t sender_id, uint32_t stamp, struct Int32Rates
scaled.r = (gyro->unscaled.r - gyro->neutral.r) * gyro->scale[0].r / gyro->scale[1].r;
// Rotate the sensor
- int32_rmat_ratemult(&scaled_rot, &gyro->imu_to_sensor, &scaled);
+ int32_rmat_transp_ratemult(&scaled_rot, &gyro->body_to_sensor, &scaled);
#if IMU_INTEGRATION
// Only integrate if we have gotten a previous measurement and didn't overflow the timer
@@ -413,7 +456,7 @@ static void imu_accel_raw_cb(uint8_t sender_id, uint32_t stamp, struct Int32Vect
scaled.z = (accel->unscaled.z - accel->neutral.z) * accel->scale[0].z / accel->scale[1].z;
// Rotate the sensor
- int32_rmat_transp_vmult(&scaled_rot, &accel->imu_to_sensor, &scaled);
+ int32_rmat_transp_vmult(&scaled_rot, &accel->body_to_sensor, &scaled);
#if IMU_INTEGRATION
// Only integrate if we have gotten a previous measurement and didn't overflow the timer
@@ -470,7 +513,7 @@ static void imu_mag_raw_cb(uint8_t sender_id, uint32_t stamp, struct Int32Vect3
scaled.z = (mag->unscaled.z - mag_correction.z - mag->neutral.z) * mag->scale[0].z / mag->scale[1].z;
// Rotate the sensor
- int32_rmat_transp_vmult(&mag->scaled, &mag->imu_to_sensor, &scaled);
+ int32_rmat_transp_vmult(&mag->scaled, &mag->body_to_sensor, &scaled);
AbiSendMsgIMU_MAG(sender_id, stamp, &mag->scaled);
}
@@ -543,13 +586,46 @@ struct imu_mag_t *imu_get_mag(uint8_t sender_id, bool create) {
return mag;
}
+/**
+ * @brief Set the body to IMU rotation in eulers
+ * This will update all the sensor values
+ * @param body_to_imu_eulers 321 Euler angles in radians
+ */
+static void imu_set_body_to_imu_eulers(struct FloatEulers *body_to_imu_eulers)
+{
+ struct Int32RMat new_body_to_imu, diff_body_to_imu;
+ struct Int32Eulers body_to_imu_eulers_i;
+ // Convert to RMat
+ struct Int32RMat *old_body_to_imu = orientationGetRMat_i(&imu.body_to_imu);
+ EULERS_BFP_OF_REAL(body_to_imu_eulers_i, *body_to_imu_eulers);
+ int32_rmat_of_eulers(&new_body_to_imu, &body_to_imu_eulers_i);
+
+ // Calculate the difference between old and new
+ int32_rmat_comp_inv(&diff_body_to_imu, &new_body_to_imu, old_body_to_imu);
+
+ // Apply the difference to all sensors
+ struct Int32RMat old_rmat;
+ for(uint8_t i = 0; i < IMU_MAX_SENSORS; i++) {
+ old_rmat = imu.gyros[i].body_to_sensor;
+ int32_rmat_comp(&imu.gyros[i].body_to_sensor, &diff_body_to_imu, &old_rmat);
+
+ old_rmat = imu.accels[i].body_to_sensor;
+ int32_rmat_comp(&imu.accels[i].body_to_sensor, &diff_body_to_imu, &old_rmat);
+
+ old_rmat = imu.mags[i].body_to_sensor;
+ int32_rmat_comp(&imu.mags[i].body_to_sensor, &diff_body_to_imu, &old_rmat);
+ }
+
+ // Set the current body to imu
+ orientationSetEulers_f(&imu.body_to_imu, body_to_imu_eulers);
+}
+
void imu_SetBodyToImuPhi(float phi)
{
struct FloatEulers body_to_imu_eulers;
body_to_imu_eulers = *orientationGetEulers_f(&imu.body_to_imu);
body_to_imu_eulers.phi = phi;
- orientationSetEulers_f(&imu.body_to_imu, &body_to_imu_eulers);
- AbiSendMsgBODY_TO_IMU_QUAT(1, orientationGetQuat_f(&imu.body_to_imu));
+ imu_set_body_to_imu_eulers(&body_to_imu_eulers);
}
void imu_SetBodyToImuTheta(float theta)
@@ -557,8 +633,7 @@ void imu_SetBodyToImuTheta(float theta)
struct FloatEulers body_to_imu_eulers;
body_to_imu_eulers = *orientationGetEulers_f(&imu.body_to_imu);
body_to_imu_eulers.theta = theta;
- orientationSetEulers_f(&imu.body_to_imu, &body_to_imu_eulers);
- AbiSendMsgBODY_TO_IMU_QUAT(1, orientationGetQuat_f(&imu.body_to_imu));
+ imu_set_body_to_imu_eulers(&body_to_imu_eulers);
}
void imu_SetBodyToImuPsi(float psi)
@@ -566,8 +641,7 @@ void imu_SetBodyToImuPsi(float psi)
struct FloatEulers body_to_imu_eulers;
body_to_imu_eulers = *orientationGetEulers_f(&imu.body_to_imu);
body_to_imu_eulers.psi = psi;
- orientationSetEulers_f(&imu.body_to_imu, &body_to_imu_eulers);
- AbiSendMsgBODY_TO_IMU_QUAT(1, orientationGetQuat_f(&imu.body_to_imu));
+ imu_set_body_to_imu_eulers(&body_to_imu_eulers);
}
void imu_SetBodyToImuCurrent(float set)
@@ -582,17 +656,14 @@ void imu_SetBodyToImuCurrent(float set)
// adjust imu_to_body roll and pitch by current NedToBody roll and pitch
body_to_imu_eulers.phi += stateGetNedToBodyEulers_f()->phi;
body_to_imu_eulers.theta += stateGetNedToBodyEulers_f()->theta;
- orientationSetEulers_f(&imu.body_to_imu, &body_to_imu_eulers);
- AbiSendMsgBODY_TO_IMU_QUAT(1, orientationGetQuat_f(&imu.body_to_imu));
+ imu_set_body_to_imu_eulers(&body_to_imu_eulers);
} else {
// indicate that we couldn't set to current roll/pitch
imu.b2i_set_current = false;
}
} else {
// reset to BODY_TO_IMU as defined in airframe file
- struct FloatEulers body_to_imu_eulers =
- {IMU_BODY_TO_IMU_PHI, IMU_BODY_TO_IMU_THETA, IMU_BODY_TO_IMU_PSI};
- orientationSetEulers_f(&imu.body_to_imu, &body_to_imu_eulers);
- AbiSendMsgBODY_TO_IMU_QUAT(1, orientationGetQuat_f(&imu.body_to_imu));
+ struct FloatEulers body_to_imu_eulers = {IMU_BODY_TO_IMU_PHI, IMU_BODY_TO_IMU_THETA, IMU_BODY_TO_IMU_PSI};
+ imu_set_body_to_imu_eulers(&body_to_imu_eulers);
}
}
diff --git a/sw/airborne/modules/imu/imu.h b/sw/airborne/modules/imu/imu.h
index 0337b99dbaf..4e1fbcddd4e 100644
--- a/sw/airborne/modules/imu/imu.h
+++ b/sw/airborne/modules/imu/imu.h
@@ -44,7 +44,7 @@ struct imu_gyro_t {
struct Int32Rates unscaled;
struct Int32Rates neutral;
struct Int32Rates scale[2];
- struct Int32RMat imu_to_sensor; ///< Rotation from imu to sensor frame
+ struct Int32RMat body_to_sensor; ///< Rotation from body to sensor frame
};
struct imu_accel_t {
@@ -55,7 +55,7 @@ struct imu_accel_t {
struct Int32Vect3 unscaled;
struct Int32Vect3 neutral;
struct Int32Vect3 scale[2];
- struct Int32RMat imu_to_sensor; ///< Rotation from imu to sensor frame
+ struct Int32RMat body_to_sensor; ///< Rotation from body to sensor frame
};
struct imu_mag_t {
@@ -66,7 +66,7 @@ struct imu_mag_t {
struct Int32Vect3 neutral;
struct Int32Vect3 scale[2];
struct FloatVect3 current_scale;
- struct Int32RMat imu_to_sensor; ///< Rotation from imu to sensor frame
+ struct Int32RMat body_to_sensor; ///< Rotation from body to sensor frame
};
@@ -89,6 +89,7 @@ extern struct Imu imu;
/** External functions */
extern void imu_init(void);
+
extern void imu_set_defaults_gyro(uint8_t abi_id, const struct Int32RMat *imu_to_sensor, const struct Int32Rates *neutral, const struct Int32Rates *scale);
extern void imu_set_defaults_accel(uint8_t abi_id, const struct Int32RMat *imu_to_sensor, const struct Int32Vect3 *neutral, const struct Int32Vect3 *scale);
extern void imu_set_defaults_mag(uint8_t abi_id, const struct Int32RMat *imu_to_sensor, const struct Int32Vect3 *neutral, const struct Int32Vect3 *scale);
@@ -103,11 +104,4 @@ extern void imu_SetBodyToImuPsi(float psi);
extern void imu_SetBodyToImuCurrent(float set);
extern void imu_ResetBodyToImu(float reset);
-#if !defined IMU_BODY_TO_IMU_PHI && !defined IMU_BODY_TO_IMU_THETA && !defined IMU_BODY_TO_IMU_PSI
-#define IMU_BODY_TO_IMU_PHI 0
-#define IMU_BODY_TO_IMU_THETA 0
-#define IMU_BODY_TO_IMU_PSI 0
-#endif
-
-
#endif /* IMU_H */
diff --git a/sw/airborne/modules/ins/ins_alt_float.c b/sw/airborne/modules/ins/ins_alt_float.c
index 9a8fcd5b247..8683945b144 100644
--- a/sw/airborne/modules/ins/ins_alt_float.c
+++ b/sw/airborne/modules/ins/ins_alt_float.c
@@ -88,10 +88,6 @@ static void gps_cb(uint8_t sender_id, uint32_t stamp, struct GpsState *gps_s);
static abi_event accel_ev;
static void accel_cb(uint8_t sender_id, uint32_t stamp, struct Int32Vect3 *accel);
-static abi_event body_to_imu_ev;
-static void body_to_imu_cb(uint8_t sender_id, struct FloatQuat *q_b2i_f);
-static struct OrientationReps body_to_imu;
-
static void alt_kalman_reset(void);
static void alt_kalman_init(void);
static void alt_kalman(float z_meas, float dt);
@@ -110,10 +106,6 @@ void ins_alt_float_init(void)
ins_altf.origin_initialized = false;
#endif
- // set initial body to imu to 0
- struct Int32Eulers b2i0 = { 0, 0, 0 };
- orientationSetEulers_i(&body_to_imu, &b2i0);
-
alt_kalman_init();
#if USE_BAROMETER
@@ -132,7 +124,6 @@ void ins_alt_float_init(void)
#endif
AbiBindMsgGPS(INS_ALT_GPS_ID, &gps_ev, gps_cb);
AbiBindMsgIMU_ACCEL(INS_ALT_IMU_ID, &accel_ev, accel_cb);
- AbiBindMsgBODY_TO_IMU_QUAT(INS_ALT_IMU_ID, &body_to_imu_ev, body_to_imu_cb);
}
/** Reset the geographic reference to the current GPS fix */
@@ -373,17 +364,10 @@ static void accel_cb(uint8_t sender_id __attribute__((unused)),
struct Int32Vect3 *accel)
{
// untilt accel and remove gravity
- struct Int32Vect3 accel_body, accel_ned;
- struct Int32RMat *body_to_imu_rmat = orientationGetRMat_i(&body_to_imu);
- int32_rmat_transp_vmult(&accel_body, body_to_imu_rmat, accel);
+ struct Int32Vect3 accel_ned;
+ stateSetAccelBody_i(accel);
struct Int32RMat *ned_to_body_rmat = stateGetNedToBodyRMat_i();
- int32_rmat_transp_vmult(&accel_ned, ned_to_body_rmat, &accel_body);
+ int32_rmat_transp_vmult(&accel_ned, ned_to_body_rmat, accel);
accel_ned.z += ACCEL_BFP_OF_REAL(9.81);
stateSetAccelNed_i((struct NedCoor_i *)&accel_ned);
}
-
-static void body_to_imu_cb(uint8_t sender_id __attribute__((unused)),
- struct FloatQuat *q_b2i_f)
-{
- orientationSetQuat_f(&body_to_imu, q_b2i_f);
-}
diff --git a/sw/airborne/modules/ins/ins_ekf2.cpp b/sw/airborne/modules/ins/ins_ekf2.cpp
index b7279b15038..7cba61bb562 100644
--- a/sw/airborne/modules/ins/ins_ekf2.cpp
+++ b/sw/airborne/modules/ins/ins_ekf2.cpp
@@ -284,7 +284,6 @@ static abi_event gyro_ev;
static abi_event accel_ev;
static abi_event mag_ev;
static abi_event gps_ev;
-static abi_event body_to_imu_ev;
static abi_event optical_flow_ev;
/* All ABI callbacks */
@@ -295,7 +294,6 @@ static void gyro_cb(uint8_t sender_id, uint32_t stamp, struct Int32Rates *gyro);
static void accel_cb(uint8_t sender_id, uint32_t stamp, struct Int32Vect3 *accel);
static void mag_cb(uint8_t sender_id, uint32_t stamp, struct Int32Vect3 *mag);
static void gps_cb(uint8_t sender_id, uint32_t stamp, struct GpsState *gps_s);
-static void body_to_imu_cb(uint8_t sender_id, struct FloatQuat *q_b2i_f);
static void optical_flow_cb(uint8_t sender_id, uint32_t stamp, int32_t flow_x, int32_t flow_y, int32_t flow_der_x,
int32_t flow_der_y, float quality, float size_divergence);
@@ -569,7 +567,6 @@ void ins_ekf2_init(void)
AbiBindMsgIMU_ACCEL(INS_EKF2_ACCEL_ID, &accel_ev, accel_cb);
AbiBindMsgIMU_MAG(INS_EKF2_MAG_ID, &mag_ev, mag_cb);
AbiBindMsgGPS(INS_EKF2_GPS_ID, &gps_ev, gps_cb);
- AbiBindMsgBODY_TO_IMU_QUAT(ABI_BROADCAST, &body_to_imu_ev, body_to_imu_cb);
AbiBindMsgOPTICAL_FLOW(INS_EKF2_OF_ID, &optical_flow_ev, optical_flow_cb);
}
@@ -802,14 +799,8 @@ static void agl_cb(uint8_t __attribute__((unused)) sender_id, uint32_t stamp, fl
static void gyro_cb(uint8_t __attribute__((unused)) sender_id,
uint32_t stamp, struct Int32Rates *gyro)
{
- FloatRates imu_rate;
- struct FloatRMat *body_to_imu_rmat = orientationGetRMat_f(&ekf2.body_to_imu);
-
// Convert Gyro information to float
- RATES_FLOAT_OF_BFP(imu_rate, *gyro);
-
- // Rotate with respect to Body To IMU
- float_rmat_transp_ratemult(&ekf2.gyro, body_to_imu_rmat, &imu_rate);
+ RATES_FLOAT_OF_BFP(ekf2.gyro, *gyro);
// Calculate the Gyro interval
if (ekf2.gyro_stamp > 0) {
@@ -828,14 +819,8 @@ static void gyro_cb(uint8_t __attribute__((unused)) sender_id,
static void accel_cb(uint8_t sender_id __attribute__((unused)),
uint32_t stamp, struct Int32Vect3 *accel)
{
- struct FloatVect3 accel_imu;
- struct FloatRMat *body_to_imu_rmat = orientationGetRMat_f(&ekf2.body_to_imu);
-
// Convert Accelerometer information to float
- ACCELS_FLOAT_OF_BFP(accel_imu, *accel);
-
- // Rotate with respect to Body To IMU
- float_rmat_transp_vmult(&ekf2.accel, body_to_imu_rmat, &accel_imu);
+ ACCELS_FLOAT_OF_BFP(ekf2.accel, *accel);
// Calculate the Accelerometer interval
if (ekf2.accel_stamp > 0) {
@@ -855,8 +840,7 @@ static void mag_cb(uint8_t __attribute__((unused)) sender_id,
uint32_t stamp,
struct Int32Vect3 *mag)
{
- struct FloatRMat *body_to_imu_rmat = orientationGetRMat_f(&ekf2.body_to_imu);
- struct FloatVect3 mag_gauss, mag_body;
+ struct FloatVect3 mag_gauss;
magSample sample;
sample.time_us = stamp;
@@ -866,13 +850,10 @@ static void mag_cb(uint8_t __attribute__((unused)) sender_id,
mag_gauss.y *= 0.4f;
mag_gauss.z *= 0.4f;
- // Rotate with respect to Body To IMU
- float_rmat_transp_vmult(&mag_body, body_to_imu_rmat, &mag_gauss);
-
// Publish information to the EKF
- sample.mag(0) = mag_body.x;
- sample.mag(1) = mag_body.y;
- sample.mag(2) = mag_body.z;
+ sample.mag(0) = mag_gauss.x;
+ sample.mag(1) = mag_gauss.y;
+ sample.mag(2) = mag_gauss.z;
ekf.setMagData(sample);
ekf2.got_imu_data = true;
@@ -910,13 +891,6 @@ static void gps_cb(uint8_t sender_id __attribute__((unused)),
ekf.setGpsData(gps_msg);
}
-/* Save the Body to IMU information */
-static void body_to_imu_cb(uint8_t sender_id __attribute__((unused)),
- struct FloatQuat *q_b2i_f)
-{
- orientationSetQuat_f(&ekf2.body_to_imu, q_b2i_f);
-}
-
/* Update INS based on Optical Flow information */
static void optical_flow_cb(uint8_t sender_id __attribute__((unused)),
uint32_t stamp,
diff --git a/sw/airborne/modules/ins/ins_ekf2.h b/sw/airborne/modules/ins/ins_ekf2.h
index 6894c91041a..9398cfb05f0 100644
--- a/sw/airborne/modules/ins/ins_ekf2.h
+++ b/sw/airborne/modules/ins/ins_ekf2.h
@@ -53,8 +53,7 @@ struct ekf2_t {
float qnh; ///< QNH value in hPa
uint8_t quat_reset_counter; ///< Amount of quaternion resets from the EKF2
uint64_t ltp_stamp; ///< Last LTP change timestamp from the EKF2
- struct LtpDef_i ltp_def; ///< Latest LTP definition from the quat_reset_counterEKF2
- struct OrientationReps body_to_imu; ///< Body to IMU rotation
+ struct LtpDef_i ltp_def; ///< Latest LTP definition from the quat_reset_counter EKF2
bool got_imu_data; ///< If we received valid IMU data (any sensor)
int32_t mag_fusion_type;
diff --git a/sw/airborne/modules/ins/ins_float_invariant.c b/sw/airborne/modules/ins/ins_float_invariant.c
index fb443bcef8d..44ea15a56f2 100644
--- a/sw/airborne/modules/ins/ins_float_invariant.c
+++ b/sw/airborne/modules/ins/ins_float_invariant.c
@@ -353,9 +353,8 @@ void ins_float_invariant_propagate(struct FloatRates* gyro, struct FloatVect3* a
}
// fill command vector in body frame
- struct FloatRMat *body_to_imu_rmat = orientationGetRMat_f(&ins_float_inv.body_to_imu);
- float_rmat_transp_ratemult(&ins_float_inv.cmd.rates, body_to_imu_rmat, gyro);
- float_rmat_transp_vmult(&ins_float_inv.cmd.accel, body_to_imu_rmat, accel);
+ RATES_COPY(ins_float_inv.cmd.rates, *gyro);
+ VECT3_COPY(ins_float_inv.cmd.accel, *accel);
struct Int32Vect3 body_accel_i;
ACCELS_BFP_OF_REAL(body_accel_i, ins_float_inv.cmd.accel);
@@ -575,10 +574,8 @@ void ins_float_invariant_update_mag(struct FloatVect3* mag)
mag_frozen_count = MAG_FROZEN_COUNT;
}
} else {
- // values are moving
- struct FloatRMat *body_to_imu_rmat = orientationGetRMat_f(&ins_float_inv.body_to_imu);
// new values in body frame
- float_rmat_transp_vmult(&ins_float_inv.meas.mag, body_to_imu_rmat, mag);
+ VECT3_COPY(ins_float_inv.meas.mag, *mag);
// reset counter
mag_frozen_count = MAG_FROZEN_COUNT;
}
@@ -747,13 +744,3 @@ void float_quat_vmul_right(struct FloatQuat *mright, const struct FloatQuat *q,
VECT3_ADD(v2, v1);
QUAT_ASSIGN(*mright, qi, v2.x, v2.y, v2.z);
}
-
-void ins_float_inv_set_body_to_imu_quat(struct FloatQuat *q_b2i)
-{
- orientationSetQuat_f(&ins_float_inv.body_to_imu, q_b2i);
-
- if (!ins_float_inv.is_aligned) {
- /* Set ltp_to_imu so that body is zero */
- ins_float_inv.state.quat = *q_b2i;
- }
-}
diff --git a/sw/airborne/modules/ins/ins_float_invariant.h b/sw/airborne/modules/ins/ins_float_invariant.h
index a6516ff07c7..f95770ff34a 100644
--- a/sw/airborne/modules/ins/ins_float_invariant.h
+++ b/sw/airborne/modules/ins/ins_float_invariant.h
@@ -112,9 +112,6 @@ struct InsFloatInv {
bool reset; ///< flag to request reset/reinit the filter
- /** body_to_imu rotation */
- struct OrientationReps body_to_imu;
-
struct FloatVect3 mag_h;
bool is_aligned;
};
@@ -122,7 +119,6 @@ struct InsFloatInv {
extern struct InsFloatInv ins_float_inv;
extern void ins_float_invariant_init(void);
-extern void ins_float_inv_set_body_to_imu_quat(struct FloatQuat *q_b2i);
extern void ins_float_invariant_align(struct FloatRates *lp_gyro,
struct FloatVect3 *lp_accel,
struct FloatVect3 *lp_mag);
diff --git a/sw/airborne/modules/ins/ins_float_invariant_wrapper.c b/sw/airborne/modules/ins/ins_float_invariant_wrapper.c
index 38cb94939ae..069ee88b88c 100644
--- a/sw/airborne/modules/ins/ins_float_invariant_wrapper.c
+++ b/sw/airborne/modules/ins/ins_float_invariant_wrapper.c
@@ -113,7 +113,6 @@ static abi_event baro_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;
@@ -186,12 +185,6 @@ static void aligner_cb(uint8_t __attribute__((unused)) sender_id,
}
}
-static void body_to_imu_cb(uint8_t sender_id __attribute__((unused)),
- struct FloatQuat *q_b2i_f)
-{
- 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)),
@@ -232,7 +225,6 @@ void ins_float_invariant_wrapper_init(void)
AbiBindMsgIMU_GYRO(INS_FINV_IMU_ID, &gyro_ev, gyro_cb);
AbiBindMsgIMU_ACCEL(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(INS_FINV_MAG_ID, &mag_ev, mag_cb);
AbiBindMsgGEO_MAG(ABI_BROADCAST, &geo_mag_ev, geo_mag_cb);
diff --git a/sw/airborne/modules/ins/ins_gps_passthrough.c b/sw/airborne/modules/ins/ins_gps_passthrough.c
index 9e9ceb21ac3..bc8bccfa0b4 100644
--- a/sw/airborne/modules/ins/ins_gps_passthrough.c
+++ b/sw/airborne/modules/ins/ins_gps_passthrough.c
@@ -66,10 +66,6 @@ struct InsGpsPassthrough ins_gp;
static abi_event accel_ev;
static void accel_cb(uint8_t sender_id, uint32_t stamp, struct Int32Vect3 *accel);
-static abi_event body_to_imu_ev;
-static void body_to_imu_cb(uint8_t sender_id, struct FloatQuat *q_b2i_f);
-static struct OrientationReps body_to_imu;
-
/** ABI binding for gps data.
* Used for GPS ABI messages.
@@ -171,7 +167,6 @@ void ins_gps_passthrough_init(void)
AbiBindMsgGPS(INS_PT_GPS_ID, &gps_ev, gps_cb);
AbiBindMsgIMU_ACCEL(INS_PT_IMU_ID, &accel_ev, accel_cb);
- AbiBindMsgBODY_TO_IMU_QUAT(INS_PT_IMU_ID, &body_to_imu_ev, body_to_imu_cb);
}
void ins_reset_local_origin(void)
@@ -200,20 +195,11 @@ static void accel_cb(uint8_t sender_id __attribute__((unused)),
struct Int32Vect3 *accel)
{
// untilt accel and remove gravity
- struct Int32Vect3 accel_body, accel_ned;
- struct Int32RMat *body_to_imu_rmat = orientationGetRMat_i(&body_to_imu);
- int32_rmat_transp_vmult(&accel_body, body_to_imu_rmat, accel);
- stateSetAccelBody_i(&accel_body);
+ struct Int32Vect3 accel_ned;
+ stateSetAccelBody_i(accel);
struct Int32RMat *ned_to_body_rmat = stateGetNedToBodyRMat_i();
- int32_rmat_transp_vmult(&accel_ned, ned_to_body_rmat, &accel_body);
+ int32_rmat_transp_vmult(&accel_ned, ned_to_body_rmat, accel);
accel_ned.z += ACCEL_BFP_OF_REAL(9.81);
stateSetAccelNed_i((struct NedCoor_i *)&accel_ned);
VECT3_COPY(ins_gp.ltp_accel, accel_ned);
}
-
-static void body_to_imu_cb(uint8_t sender_id __attribute__((unused)),
- struct FloatQuat *q_b2i_f)
-{
- orientationSetQuat_f(&body_to_imu, q_b2i_f);
-}
-
diff --git a/sw/airborne/modules/ins/ins_int.c b/sw/airborne/modules/ins/ins_int.c
index bfe4489a321..8016ebd35b7 100644
--- a/sw/airborne/modules/ins/ins_int.c
+++ b/sw/airborne/modules/ins/ins_int.c
@@ -120,12 +120,14 @@ static void baro_cb(uint8_t sender_id, uint32_t stamp, float pressure);
#ifndef INS_INT_IMU_ID
#define INS_INT_IMU_ID ABI_BROADCAST
#endif
+PRINT_CONFIG_VAR(INS_INT_IMU_ID)
static abi_event accel_ev;
static void accel_cb(uint8_t sender_id, uint32_t stamp, struct Int32Vect3 *accel);
#ifndef INS_INT_GPS_ID
#define INS_INT_GPS_ID GPS_MULTI_ID
#endif
+PRINT_CONFIG_VAR(INS_INT_GPS_ID)
static abi_event gps_ev;
static void gps_cb(uint8_t sender_id, uint32_t stamp, struct GpsState *gps_s);
@@ -135,6 +137,7 @@ static void gps_cb(uint8_t sender_id, uint32_t stamp, struct GpsState *gps_s);
#ifndef INS_INT_VEL_ID
#define INS_INT_VEL_ID ABI_BROADCAST
#endif
+PRINT_CONFIG_VAR(INS_INT_VEL_ID)
static abi_event vel_est_ev;
static void vel_est_cb(uint8_t sender_id,
uint32_t stamp,
@@ -143,6 +146,7 @@ static void vel_est_cb(uint8_t sender_id,
#ifndef INS_INT_POS_ID
#define INS_INT_POS_ID ABI_BROADCAST
#endif
+PRINT_CONFIG_VAR(INS_INT_POS_ID)
static abi_event pos_est_ev;
static void pos_est_cb(uint8_t sender_id,
uint32_t stamp,
@@ -155,6 +159,7 @@ static void pos_est_cb(uint8_t sender_id,
#ifndef INS_INT_AGL_ID
#define INS_INT_AGL_ID ABI_BROADCAST
#endif
+PRINT_CONFIG_VAR(INS_INT_AGL_ID)
static abi_event agl_ev; ///< The agl ABI event
static void agl_cb(uint8_t sender_id, uint32_t stamp, float distance);
@@ -283,12 +288,8 @@ void ins_reset_altitude_ref(void)
void ins_int_propagate(struct Int32Vect3 *accel, float dt)
{
/* untilt accels */
- struct Int32Vect3 accel_meas_body;
- struct Int32RMat *body_to_imu_rmat = orientationGetRMat_i(&imu.body_to_imu);
- int32_rmat_transp_vmult(&accel_meas_body, body_to_imu_rmat, accel);
- stateSetAccelBody_i(&accel_meas_body);
struct Int32Vect3 accel_meas_ltp;
- int32_rmat_transp_vmult(&accel_meas_ltp, stateGetNedToBodyRMat_i(), &accel_meas_body);
+ int32_rmat_transp_vmult(&accel_meas_ltp, stateGetNedToBodyRMat_i(), accel);
float z_accel_meas_float = ACCEL_FLOAT_OF_BFP(accel_meas_ltp.z);
diff --git a/sw/airborne/modules/ins/ins_mekf_wind_wrapper.c b/sw/airborne/modules/ins/ins_mekf_wind_wrapper.c
index 3735504e88e..117166204fe 100644
--- a/sw/airborne/modules/ins/ins_mekf_wind_wrapper.c
+++ b/sw/airborne/modules/ins/ins_mekf_wind_wrapper.c
@@ -206,7 +206,6 @@ 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;
static abi_event geo_mag_ev;
static abi_event gps_ev;
@@ -303,10 +302,7 @@ static void gyro_cb(uint8_t sender_id __attribute__((unused)),
{
if (ins_mekf_wind.is_aligned) {
struct FloatRates gyro_f, gyro_body;
- RATES_FLOAT_OF_BFP(gyro_f, *gyro);
- struct FloatRMat *body_to_imu_rmat = orientationGetRMat_f(&ins_mekf_wind.body_to_imu);
- // new values in body frame
- float_rmat_transp_ratemult(&gyro_body, body_to_imu_rmat, &gyro_f);
+ RATES_FLOAT_OF_BFP(gyro_body, *gyro);
#if USE_AUTO_INS_FREQ || !defined(INS_PROPAGATE_FREQUENCY)
PRINT_CONFIG_MSG("Calculating dt for INS MEKF_WIND propagation.")
@@ -383,11 +379,7 @@ static void accel_cb(uint8_t sender_id __attribute__((unused)),
uint32_t stamp __attribute__((unused)),
struct Int32Vect3 *accel)
{
- struct FloatVect3 accel_f;
- ACCELS_FLOAT_OF_BFP(accel_f, *accel);
- struct FloatRMat *body_to_imu_rmat = orientationGetRMat_f(&ins_mekf_wind.body_to_imu);
- // new values in body frame
- float_rmat_transp_vmult(&ins_mekf_wind_accel, body_to_imu_rmat, &accel_f);
+ ACCELS_FLOAT_OF_BFP(ins_mekf_wind_accel, *accel);
}
static void mag_cb(uint8_t sender_id __attribute__((unused)),
@@ -395,11 +387,9 @@ static void mag_cb(uint8_t sender_id __attribute__((unused)),
struct Int32Vect3 *mag)
{
if (ins_mekf_wind.is_aligned) {
- struct FloatVect3 mag_f, mag_body;
- MAGS_FLOAT_OF_BFP(mag_f, *mag);
- struct FloatRMat *body_to_imu_rmat = orientationGetRMat_f(&ins_mekf_wind.body_to_imu);
- // new values in body frame
- float_rmat_transp_vmult(&mag_body, body_to_imu_rmat, &mag_f);
+ struct FloatVect3 mag_body;
+ MAGS_FLOAT_OF_BFP(mag_body, *mag);
+
// only correct attitude if GPS is not initialized
ins_mekf_wind_update_mag(&mag_body, !ins_mekf_wind.gps_initialized);
@@ -420,19 +410,14 @@ static void aligner_cb(uint8_t __attribute__((unused)) sender_id,
struct Int32Vect3 *lp_mag)
{
if (!ins_mekf_wind.is_aligned) {
- struct FloatRMat *body_to_imu_rmat = orientationGetRMat_f(&ins_mekf_wind.body_to_imu);
-
- struct FloatRates gyro_f, gyro_body;
- RATES_FLOAT_OF_BFP(gyro_f, *lp_gyro);
- float_rmat_transp_ratemult(&gyro_body, body_to_imu_rmat, &gyro_f);
+ struct FloatRates gyro_body;
+ RATES_FLOAT_OF_BFP(gyro_body, *lp_gyro);
- struct FloatVect3 accel_f, accel_body;
- ACCELS_FLOAT_OF_BFP(accel_f, *lp_accel);
- float_rmat_transp_vmult(&accel_body, body_to_imu_rmat, &accel_f);
+ struct FloatVect3 accel_body;
+ ACCELS_FLOAT_OF_BFP(accel_body, *lp_accel);
- struct FloatVect3 mag_f, mag_body;
- MAGS_FLOAT_OF_BFP(mag_f, *lp_mag);
- float_rmat_transp_vmult(&mag_body, body_to_imu_rmat, &mag_f);
+ struct FloatVect3 mag_body;
+ MAGS_FLOAT_OF_BFP(mag_body, *lp_mag);
struct FloatQuat quat;
ahrs_float_get_quat_from_accel_mag(&quat, &accel_body, &mag_body);
@@ -445,16 +430,6 @@ static void aligner_cb(uint8_t __attribute__((unused)) sender_id,
}
}
-static void body_to_imu_cb(uint8_t sender_id __attribute__((unused)),
- struct FloatQuat *q_b2i)
-{
- orientationSetQuat_f(&ins_mekf_wind.body_to_imu, q_b2i);
- if (!ins_mekf_wind.is_aligned) {
- // set ltp_to_imu so that body is zero
- ins_mekf_wind_set_quat(q_b2i);
- }
-}
-
static void geo_mag_cb(uint8_t sender_id __attribute__((unused)), struct FloatVect3 *h)
{
ins_mekf_wind_set_mag_h(h);
@@ -593,7 +568,6 @@ void ins_mekf_wind_wrapper_init(void)
AbiBindMsgIMU_GYRO(INS_MEKF_WIND_IMU_ID, &gyro_ev, gyro_cb);
AbiBindMsgIMU_ACCEL(INS_MEKF_WIND_IMU_ID, &accel_ev, accel_cb);
AbiBindMsgIMU_LOWPASSED(INS_MEKF_WIND_IMU_ID, &aligner_ev, aligner_cb);
- AbiBindMsgBODY_TO_IMU_QUAT(INS_MEKF_WIND_IMU_ID, &body_to_imu_ev, body_to_imu_cb);
AbiBindMsgGEO_MAG(ABI_BROADCAST, &geo_mag_ev, geo_mag_cb);
AbiBindMsgGPS(INS_MEKF_WIND_GPS_ID, &gps_ev, gps_cb);
diff --git a/sw/airborne/modules/ins/ins_mekf_wind_wrapper.h b/sw/airborne/modules/ins/ins_mekf_wind_wrapper.h
index 7c4b9db9981..f8c1394215b 100644
--- a/sw/airborne/modules/ins/ins_mekf_wind_wrapper.h
+++ b/sw/airborne/modules/ins/ins_mekf_wind_wrapper.h
@@ -33,7 +33,6 @@
/** filter structure
*/
struct InsMekfWind {
- struct OrientationReps body_to_imu;
bool is_aligned;
bool baro_initialized;
bool gps_initialized;
diff --git a/sw/airborne/modules/ins/ins_skeleton.c b/sw/airborne/modules/ins/ins_skeleton.c
index 7664da75428..2279da4010f 100644
--- a/sw/airborne/modules/ins/ins_skeleton.c
+++ b/sw/airborne/modules/ins/ins_skeleton.c
@@ -49,7 +49,7 @@ PRINT_CONFIG_MSG("USE_INS_NAV_INIT defaulting to TRUE")
#endif
PRINT_CONFIG_VAR(INS_MODULE_BARO_ID)
-/** IMU (accel, body_to_imu) */
+/** IMU (accel) */
#ifndef INS_MODULE_IMU_ID
#define INS_MODULE_IMU_ID ABI_BROADCAST
#endif
@@ -66,7 +66,6 @@ PRINT_CONFIG_VAR(INS_MODULE_GPS_ID)
static abi_event baro_ev;
static abi_event accel_ev;
static abi_event gps_ev;
-static abi_event body_to_imu_ev;
struct InsModuleInt ins_module;
@@ -137,11 +136,6 @@ static void gps_cb(uint8_t sender_id __attribute__((unused)),
last_stamp = stamp;
}
-static void body_to_imu_cb(uint8_t sender_id __attribute__((unused)),
- struct FloatQuat *q_b2i_f)
-{
- orientationSetQuat_f(&ins_module.body_to_imu, q_b2i_f);
-}
/*********************************************************************
* weak functions that are used if not implemented in a module
********************************************************************/
@@ -185,12 +179,9 @@ void WEAK ins_module_update_gps(struct GpsState *gps_s, float dt __attribute__((
void WEAK ins_module_propagate(struct Int32Vect3 *accel, float dt __attribute__((unused)))
{
/* untilt accels */
- struct Int32Vect3 accel_meas_body;
- struct Int32RMat *body_to_imu_rmat = orientationGetRMat_i(&ins_module.body_to_imu);
- int32_rmat_transp_vmult(&accel_meas_body, body_to_imu_rmat, accel);
- stateSetAccelBody_i(&accel_meas_body);
+ stateSetAccelBody_i(accel);
struct Int32Vect3 accel_meas_ltp;
- int32_rmat_transp_vmult(&accel_meas_ltp, stateGetNedToBodyRMat_i(), &accel_meas_body);
+ int32_rmat_transp_vmult(&accel_meas_ltp, stateGetNedToBodyRMat_i(), accel);
VECT3_COPY(ins_module.ltp_accel, accel_meas_ltp);
}
@@ -239,6 +230,5 @@ void ins_module_wrapper_init(void)
AbiBindMsgBARO_ABS(INS_MODULE_BARO_ID, &baro_ev, baro_cb);
AbiBindMsgIMU_ACCEL(INS_MODULE_IMU_ID, &accel_ev, accel_cb);
AbiBindMsgGPS(INS_MODULE_GPS_ID, &gps_ev, gps_cb);
- AbiBindMsgBODY_TO_IMU_QUAT(INS_MODULE_IMU_ID, &body_to_imu_ev, body_to_imu_cb);
}
diff --git a/sw/airborne/modules/ins/ins_skeleton.h b/sw/airborne/modules/ins/ins_skeleton.h
index 8214251284d..3a4c39291b3 100644
--- a/sw/airborne/modules/ins/ins_skeleton.h
+++ b/sw/airborne/modules/ins/ins_skeleton.h
@@ -48,9 +48,6 @@ struct InsModuleInt {
/** internal copy of last GPS message */
struct GpsState gps;
-
- /** body_to_imu rotation */
- struct OrientationReps body_to_imu;
};
/** global INS state */