Skip to content

Commit

Permalink
[imu] Change IMU to body frame
Browse files Browse the repository at this point in the history
  • Loading branch information
@fvantienen
fvantienen committed Sep 20, 2022
1 parent 0c8efcb commit 5096acf
Show file tree
Hide file tree
Showing 39 changed files with 362 additions and 710 deletions.
4 changes: 0 additions & 4 deletions conf/abi.xml
View file
Expand Up @@ -44,10 +44,6 @@
<field name="mag" type="struct Int32Vect3 *"/>
</message>

<message name="BODY_TO_IMU_QUAT" id="8">
<field name="q_b2i_f" type="struct FloatQuat *"/>
</message>

<message name="GEO_MAG" id="9">
<field name="h" type="struct FloatVect3 *" unit="1.0"/>
</message>
Expand Down
3 changes: 0 additions & 3 deletions sw/airborne/firmwares/demo/demo_ahrs_actuators.c
View file
Expand Up @@ -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)
Expand Down
17 changes: 1 addition & 16 deletions sw/airborne/firmwares/fixedwing/guidance/energy_ctrl.c
View file
Expand Up @@ -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
Expand Down Expand Up @@ -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 */
Expand Down Expand Up @@ -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);
Expand Down Expand Up @@ -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;
Expand Down
5 changes: 0 additions & 5 deletions sw/airborne/main_ap.c
View file
Expand Up @@ -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();

Expand Down
4 changes: 2 additions & 2 deletions sw/airborne/math/pprz_algebra_int.c
View file
Expand Up @@ -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;
Expand All @@ -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;
Expand Down
8 changes: 4 additions & 4 deletions sw/airborne/math/pprz_algebra_int.h
View file
Expand Up @@ -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
Expand Down
7 changes: 4 additions & 3 deletions sw/airborne/modules/ahrs/ahrs_aligner.c
View file
Expand Up @@ -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++;
Expand Down Expand Up @@ -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);
}
}
Expand Down
128 changes: 44 additions & 84 deletions sw/airborne/modules/ahrs/ahrs_float_cmpl.c
View file
Expand Up @@ -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;
Expand Down Expand Up @@ -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;
Expand All @@ -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 */
Expand All @@ -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
Expand All @@ -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;
Expand All @@ -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);
Expand Down Expand Up @@ -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;
Expand Down Expand Up @@ -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};

Expand All @@ -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.
Expand All @@ -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(&ltp_to_imu_euler, &ahrs_fc.ltp_to_imu_rmat);
struct FloatEulers ltp_to_body_euler;
float_eulers_of_rmat(&ltp_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));
Expand Down Expand Up @@ -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
Expand All @@ -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);
Expand Down Expand Up @@ -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);
Expand All @@ -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, &ltp_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(&ltp_to_body_quat, &ahrs_fc.ltp_to_imu_quat, body_to_imu_quat);
orientationSetQuat_f(&ahrs_fc.ltp_to_body, &ltp_to_body_quat);
}

0 comments on commit 5096acf

Please sign in to comment.