bool_t ahrs_icq_align(struct Int32Rates *lp_gyro, struct Int32Vect3 *lp_accel,
struct Int32Vect3 *lp_mag)
{
#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,
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_icq.heading_aligned = FALSE;
// supress unused arg warning
lp_mag = lp_mag;
#endif
/* Use low passed gyro value as initial bias */
RATES_COPY(ahrs_icq.gyro_bias, *lp_gyro);
RATES_COPY(ahrs_icq.high_rez_bias, *lp_gyro);
INT_RATES_LSHIFT(ahrs_icq.high_rez_bias, ahrs_icq.high_rez_bias, 28);
ahrs_icq.status = AHRS_ICQ_RUNNING;
ahrs_icq.is_aligned = TRUE;
return TRUE;
}
static void store_filter_output(int i) {
#ifdef OUTPUT_IN_BODY_FRAME
QUAT_COPY(output[i].quat_est, ahrs_impl.ltp_to_body_quat);
RATES_COPY(output[i].rate_est, ahrs_impl.body_rate);
#else
QUAT_COPY(output[i].quat_est, ahrs_impl.ltp_to_imu_quat);
RATES_COPY(output[i].rate_est, ahrs_impl.imu_rate);
#endif /* OUTPUT_IN_BODY_FRAME */
RATES_COPY(output[i].bias_est, ahrs_impl.gyro_bias);
// memcpy(output[i].P, ahrs_impl.P, sizeof(ahrs_impl.P));
}
void imu_aspirin2_event(void)
{
mpu60x0_spi_event(&imu_aspirin2.mpu);
if (imu_aspirin2.mpu.data_available) {
/* HMC5883 has xzy order of axes in returned data */
struct Int32Vect3 mag;
mag.x = Int16FromBuf(imu_aspirin2.mpu.data_ext, 0);
mag.z = Int16FromBuf(imu_aspirin2.mpu.data_ext, 2);
mag.y = Int16FromBuf(imu_aspirin2.mpu.data_ext, 4);
#ifdef LISA_M_LONGITUDINAL_X
RATES_ASSIGN(imu.gyro_unscaled,
imu_aspirin2.mpu.data_rates.rates.q,
-imu_aspirin2.mpu.data_rates.rates.p,
imu_aspirin2.mpu.data_rates.rates.r);
VECT3_ASSIGN(imu.accel_unscaled,
imu_aspirin2.mpu.data_accel.vect.y,
-imu_aspirin2.mpu.data_accel.vect.x,
imu_aspirin2.mpu.data_accel.vect.z);
VECT3_ASSIGN(imu.mag_unscaled, -mag.x, -mag.y, mag.z);
#else
#ifdef LISA_S
#ifdef LISA_S_UPSIDE_DOWN
RATES_ASSIGN(imu.gyro_unscaled,
imu_aspirin2.mpu.data_rates.rates.p,
-imu_aspirin2.mpu.data_rates.rates.q,
-imu_aspirin2.mpu.data_rates.rates.r);
VECT3_ASSIGN(imu.accel_unscaled,
imu_aspirin2.mpu.data_accel.vect.x,
-imu_aspirin2.mpu.data_accel.vect.y,
-imu_aspirin2.mpu.data_accel.vect.z);
VECT3_ASSIGN(imu.mag_unscaled, mag.x, -mag.y, -mag.z);
#else
RATES_COPY(imu.gyro_unscaled, imu_aspirin2.mpu.data_rates.rates);
VECT3_COPY(imu.accel_unscaled, imu_aspirin2.mpu.data_accel.vect);
VECT3_COPY(imu.mag_unscaled, mag);
#endif
#else
RATES_COPY(imu.gyro_unscaled, imu_aspirin2.mpu.data_rates.rates);
VECT3_COPY(imu.accel_unscaled, imu_aspirin2.mpu.data_accel.vect);
VECT3_ASSIGN(imu.mag_unscaled, mag.y, -mag.x, mag.z)
#endif
#endif
imu_aspirin2.mpu.data_available = FALSE;
imu_aspirin2.gyro_valid = TRUE;
imu_aspirin2.accel_valid = TRUE;
imu_aspirin2.mag_valid = TRUE;
}
}
void ahrs_icq_propagate(struct Int32Rates *gyro, float dt)
{
int32_t freq = (int32_t)(1. / dt);
/* unbias gyro */
struct Int32Rates omega;
RATES_DIFF(omega, *gyro, ahrs_icq.gyro_bias);
/* low pass rate */
#ifdef AHRS_PROPAGATE_LOW_PASS_RATES
RATES_SMUL(ahrs_icq.imu_rate, ahrs_icq.imu_rate, 2);
RATES_ADD(ahrs_icq.imu_rate, omega);
RATES_SDIV(ahrs_icq.imu_rate, ahrs_icq.imu_rate, 3);
#else
RATES_COPY(ahrs_icq.imu_rate, omega);
#endif
/* add correction */
RATES_ADD(omega, ahrs_icq.rate_correction);
/* and zeros it */
INT_RATES_ZERO(ahrs_icq.rate_correction);
/* integrate quaternion */
int32_quat_integrate_fi(&ahrs_icq.ltp_to_imu_quat, &ahrs_icq.high_rez_quat,
&omega, freq);
int32_quat_normalize(&ahrs_icq.ltp_to_imu_quat);
// increase accel and mag propagation counters
ahrs_icq.accel_cnt++;
ahrs_icq.mag_cnt++;
}
void ahrs_align(void) {
#if USE_MAGNETOMETER
/* Compute an initial orientation from accel and mag directly as quaternion */
ahrs_float_get_quat_from_accel_mag(&ahrs_impl.ltp_to_imu_quat, &ahrs_aligner.lp_accel, &ahrs_aligner.lp_mag);
ahrs_impl.heading_aligned = TRUE;
#else
/* Compute an initial orientation from accel and just set heading to zero */
ahrs_float_get_quat_from_accel(&ahrs_impl.ltp_to_imu_quat, &ahrs_aligner.lp_accel);
ahrs_impl.heading_aligned = FALSE;
#endif
/* Convert initial orientation from quat to rotation matrix representations. */
FLOAT_RMAT_OF_QUAT(ahrs_impl.ltp_to_imu_rmat, ahrs_impl.ltp_to_imu_quat);
/* Compute initial body orientation */
compute_body_orientation_and_rates();
/* used averaged gyro as initial value for bias */
struct Int32Rates bias0;
RATES_COPY(bias0, ahrs_aligner.lp_gyro);
RATES_FLOAT_OF_BFP(ahrs_impl.gyro_bias, bias0);
ahrs.status = AHRS_RUNNING;
}
void ahrs_propagate(void) {
/* unbias gyro */
struct Int32Rates omega;
RATES_DIFF(omega, imu.gyro_prev, ahrs_impl.gyro_bias);
/* low pass rate */
//#ifdef AHRS_PROPAGATE_LOW_PASS_RATES
if (gyro_lowpass_filter > 1) {
RATES_SMUL(ahrs_impl.imu_rate, ahrs_impl.imu_rate, gyro_lowpass_filter-1);
RATES_ADD(ahrs_impl.imu_rate, omega);
RATES_SDIV(ahrs_impl.imu_rate, ahrs_impl.imu_rate, gyro_lowpass_filter);
//#else
} else {
RATES_COPY(ahrs_impl.imu_rate, omega);
//#endif
}
/* add correction */
RATES_ADD(omega, ahrs_impl.rate_correction);
/* and zeros it */
INT_RATES_ZERO(ahrs_impl.rate_correction);
/* integrate quaternion */
INT32_QUAT_INTEGRATE_FI(ahrs_impl.ltp_to_imu_quat, ahrs_impl.high_rez_quat, omega, AHRS_PROPAGATE_FREQUENCY);
INT32_QUAT_NORMALIZE(ahrs_impl.ltp_to_imu_quat);
set_body_state_from_quat();
}
/**
* Handle all the events of the Navstik IMU components.
* When there is data available convert it to the correct axis and save it in the imu structure.
*/
void imu_navstik_event(void)
{
uint32_t now_ts = get_sys_time_usec();
/* MPU-60x0 event taks */
mpu60x0_i2c_event(&imu_navstik.mpu);
if (imu_navstik.mpu.data_available) {
/* default orientation as should be printed on the pcb, z-down, ICs down */
RATES_COPY(imu.gyro_unscaled, imu_navstik.mpu.data_rates.rates);
VECT3_COPY(imu.accel_unscaled, imu_navstik.mpu.data_accel.vect);
imu_navstik.mpu.data_available = false;
imu_scale_gyro(&imu);
imu_scale_accel(&imu);
AbiSendMsgIMU_GYRO_INT32(IMU_BOARD_ID, now_ts, &imu.gyro);
AbiSendMsgIMU_ACCEL_INT32(IMU_BOARD_ID, now_ts, &imu.accel);
}
/* HMC58XX event task */
hmc58xx_event(&imu_navstik.hmc);
if (imu_navstik.hmc.data_available) {
imu.mag_unscaled.x = imu_navstik.hmc.data.vect.y;
imu.mag_unscaled.y = -imu_navstik.hmc.data.vect.x;
imu.mag_unscaled.z = imu_navstik.hmc.data.vect.z;
imu_navstik.hmc.data_available = false;
imu_scale_mag(&imu);
AbiSendMsgIMU_MAG_INT32(IMU_BOARD_ID, now_ts, &imu.mag);
}
}
void imu_aspirin_event(void)
{
adxl345_spi_event(&imu_aspirin.acc_adxl);
if (imu_aspirin.acc_adxl.data_available) {
VECT3_COPY(imu.accel_unscaled, imu_aspirin.acc_adxl.data.vect);
imu_aspirin.acc_adxl.data_available = FALSE;
imu_aspirin.accel_valid = TRUE;
}
/* If the itg3200 I2C transaction has succeeded: convert the data */
itg3200_event(&imu_aspirin.gyro_itg);
if (imu_aspirin.gyro_itg.data_available) {
RATES_COPY(imu.gyro_unscaled, imu_aspirin.gyro_itg.data.rates);
imu_aspirin.gyro_itg.data_available = FALSE;
imu_aspirin.gyro_valid = TRUE;
}
/* HMC58XX event task */
hmc58xx_event(&imu_aspirin.mag_hmc);
if (imu_aspirin.mag_hmc.data_available) {
#ifdef IMU_ASPIRIN_VERSION_1_0
VECT3_COPY(imu.mag_unscaled, imu_aspirin.mag_hmc.data.vect);
#else // aspirin 1.5 with hmc5883
imu.mag_unscaled.x = imu_aspirin.mag_hmc.data.vect.y;
imu.mag_unscaled.y = -imu_aspirin.mag_hmc.data.vect.x;
imu.mag_unscaled.z = imu_aspirin.mag_hmc.data.vect.z;
#endif
imu_aspirin.mag_hmc.data_available = FALSE;
imu_aspirin.mag_valid = TRUE;
}
}
void imu_navgo_event( void )
{
// If the itg3200 I2C transaction has succeeded: convert the data
itg3200_event();
if (itg3200_data_available) {
RATES_COPY(imu.gyro_unscaled, itg3200_data);
itg3200_data_available = FALSE;
gyr_valid = TRUE;
}
// If the adxl345 I2C transaction has succeeded: convert the data
adxl345_event();
if (adxl345_data_available) {
VECT3_ASSIGN(imu.accel_unscaled, adxl345_data.y, -adxl345_data.x, adxl345_data.z);
adxl345_data_available = FALSE;
acc_valid = TRUE;
}
// HMC58XX event task
hmc58xx_event();
if (hmc58xx_data_available) {
VECT3_ASSIGN(imu.mag_unscaled, -hmc58xx_data.x, -hmc58xx_data.y, hmc58xx_data.z);
hmc58xx_data_available = FALSE;
mag_valid = TRUE;
}
}
void ahrs_propagate(void) {
/* unbias gyro */
struct Int32Rates omega;
RATES_DIFF(omega, imu.gyro_prev, ahrs_impl.gyro_bias);
/* low pass rate */
#ifdef AHRS_PROPAGATE_LOW_PASS_RATES
RATES_SMUL(ahrs.imu_rate, ahrs.imu_rate,2);
RATES_ADD(ahrs.imu_rate, omega);
RATES_SDIV(ahrs.imu_rate, ahrs.imu_rate, 3);
#else
RATES_COPY(ahrs.imu_rate, omega);
#endif
/* add correction */
RATES_ADD(omega, ahrs_impl.rate_correction);
/* and zeros it */
INT_RATES_ZERO(ahrs_impl.rate_correction);
/* integrate quaternion */
INT32_QUAT_INTEGRATE_FI(ahrs.ltp_to_imu_quat, ahrs_impl.high_rez_quat, omega, AHRS_PROPAGATE_FREQUENCY);
INT32_QUAT_NORMALIZE(ahrs.ltp_to_imu_quat);
compute_imu_euler_and_rmat_from_quat();
compute_body_orientation();
}
void imu_mpu_hmc_event(void)
{
uint32_t now_ts = get_sys_time_usec();
mpu60x0_spi_event(&imu_mpu_hmc.mpu);
if (imu_mpu_hmc.mpu.data_available) {
RATES_COPY(imu.gyro_unscaled, imu_mpu_hmc.mpu.data_rates.rates);
VECT3_COPY(imu.accel_unscaled, imu_mpu_hmc.mpu.data_accel.vect);
imu_mpu_hmc.mpu.data_available = false;
imu_scale_gyro(&imu);
imu_scale_accel(&imu);
AbiSendMsgIMU_GYRO_INT32(IMU_MPU6000_HMC_ID, now_ts, &imu.gyro);
AbiSendMsgIMU_ACCEL_INT32(IMU_MPU6000_HMC_ID, now_ts, &imu.accel);
}
/* HMC58XX event task */
hmc58xx_event(&imu_mpu_hmc.hmc);
if (imu_mpu_hmc.hmc.data_available) {
/* mag rotated by 90deg around z axis relative to MPU */
imu.mag_unscaled.x = imu_mpu_hmc.hmc.data.vect.y;
imu.mag_unscaled.y = -imu_mpu_hmc.hmc.data.vect.x;
imu.mag_unscaled.z = imu_mpu_hmc.hmc.data.vect.z;
imu_mpu_hmc.hmc.data_available = false;
imu_scale_mag(&imu);
AbiSendMsgIMU_MAG_INT32(IMU_MPU6000_HMC_ID, now_ts, &imu.mag);
}
}
bool_t ahrs_fc_align(struct Int32Rates *lp_gyro, struct Int32Vect3 *lp_accel,
struct Int32Vect3 *lp_mag)
{
#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_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_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);
/* used averaged gyro as initial value for bias */
struct Int32Rates bias0;
RATES_COPY(bias0, *lp_gyro);
RATES_FLOAT_OF_BFP(ahrs_fc.gyro_bias, bias0);
ahrs_fc.status = AHRS_FC_RUNNING;
ahrs_fc.is_aligned = TRUE;
return TRUE;
}
void ahrs_propagate(void) {
/* converts gyro to floating point */
struct FloatRates gyro_float;
RATES_FLOAT_OF_BFP(gyro_float, imu.gyro_prev);
/* unbias measurement */
RATES_SUB(gyro_float, ahrs_impl.gyro_bias);
#ifdef AHRS_PROPAGATE_LOW_PASS_RATES
const float alpha = 0.1;
FLOAT_RATES_LIN_CMB(ahrs_impl.imu_rate, ahrs_impl.imu_rate, (1.-alpha), gyro_float, alpha);
#else
RATES_COPY(ahrs_impl.imu_rate,gyro_float);
#endif
/* add correction */
struct FloatRates omega;
RATES_SUM(omega, gyro_float, ahrs_impl.rate_correction);
/* and zeros it */
FLOAT_RATES_ZERO(ahrs_impl.rate_correction);
const float dt = 1./AHRS_PROPAGATE_FREQUENCY;
#if AHRS_PROPAGATE_RMAT
FLOAT_RMAT_INTEGRATE_FI(ahrs_impl.ltp_to_imu_rmat, omega, dt);
float_rmat_reorthogonalize(&ahrs_impl.ltp_to_imu_rmat);
FLOAT_QUAT_OF_RMAT(ahrs_impl.ltp_to_imu_quat, ahrs_impl.ltp_to_imu_rmat);
#endif
#if AHRS_PROPAGATE_QUAT
FLOAT_QUAT_INTEGRATE(ahrs_impl.ltp_to_imu_quat, omega, dt);
FLOAT_QUAT_NORMALIZE(ahrs_impl.ltp_to_imu_quat);
FLOAT_RMAT_OF_QUAT(ahrs_impl.ltp_to_imu_rmat, ahrs_impl.ltp_to_imu_quat);
#endif
compute_body_orientation_and_rates();
}
void ahrs_align(void) {
/* Compute an initial orientation using euler angles */
ahrs_int_get_euler_from_accel_mag(&ahrs.ltp_to_imu_euler, &ahrs_aligner.lp_accel, &ahrs_aligner.lp_mag);
/* Convert initial orientation in quaternion and rotation matrice representations. */
compute_imu_quat_and_rmat_from_euler();
compute_body_orientation();
/* Use low passed gyro value as initial bias */
RATES_COPY( ahrs_impl.gyro_bias, ahrs_aligner.lp_gyro);
RATES_COPY( ahrs_impl.high_rez_bias, ahrs_aligner.lp_gyro);
INT_RATES_LSHIFT(ahrs_impl.high_rez_bias, ahrs_impl.high_rez_bias, 28);
ahrs.status = AHRS_RUNNING;
}
/*
* Compute body orientation and rates from imu orientation and rates
*/
void compute_body_orientation_and_rates(void) {
/* set ltp_to_body to same as ltp_to_imu, currently no difference simulated */
QUAT_COPY(ahrs_float.ltp_to_body_quat, ahrs_float.ltp_to_imu_quat);
EULERS_COPY(ahrs_float.ltp_to_body_euler, ahrs_float.ltp_to_imu_euler);
RMAT_COPY(ahrs_float.ltp_to_body_rmat, ahrs_float.ltp_to_imu_rmat);
RATES_COPY(ahrs_float.body_rate, ahrs_float.imu_rate);
}
void WEAK imu_scale_gyro(struct Imu* _imu)
{
RATES_COPY(_imu->gyro_prev, _imu->gyro);
_imu->gyro.p = ((_imu->gyro_unscaled.p - _imu->gyro_neutral.p) * IMU_GYRO_P_SIGN *
IMU_GYRO_P_SENS_NUM) / IMU_GYRO_P_SENS_DEN;
_imu->gyro.q = ((_imu->gyro_unscaled.q - _imu->gyro_neutral.q) * IMU_GYRO_Q_SIGN *
IMU_GYRO_Q_SENS_NUM) / IMU_GYRO_Q_SENS_DEN;
_imu->gyro.r = ((_imu->gyro_unscaled.r - _imu->gyro_neutral.r) * IMU_GYRO_R_SIGN *
IMU_GYRO_R_SENS_NUM) / IMU_GYRO_R_SENS_DEN;
}
void sim_overwrite_ahrs(void) {
struct FloatQuat quat_f;
QUAT_COPY(quat_f, fdm.ltp_to_body_quat);
stateSetNedToBodyQuat_f(&quat_f);
struct FloatRates rates_f;
RATES_COPY(rates_f, fdm.body_ecef_rotvel);
stateSetBodyRates_f(&rates_f);
}
void imu_mpu_spi_event(void)
{
mpu60x0_spi_event(&imu_mpu_spi.mpu);
if (imu_mpu_spi.mpu.data_available) {
RATES_COPY(imu.gyro_unscaled, imu_mpu_spi.mpu.data_rates.rates);
VECT3_COPY(imu.accel_unscaled, imu_mpu_spi.mpu.data_accel.vect);
imu_mpu_spi.mpu.data_available = FALSE;
imu_mpu_spi.gyro_valid = TRUE;
imu_mpu_spi.accel_valid = TRUE;
}
}
static void feed_imu(int i) {
if (i>0) {
RATES_COPY(imu.gyro_prev, imu.gyro);
}
else {
RATES_BFP_OF_REAL(imu.gyro_prev, samples[0].gyro);
}
RATES_BFP_OF_REAL(imu.gyro, samples[i].gyro);
ACCELS_BFP_OF_REAL(imu.accel, samples[i].accel);
MAGS_BFP_OF_REAL(imu.mag, samples[i].mag);
}
void WEAK imu_scale_gyro(struct Imu *_imu)
{
#ifdef TREF
RATES_COPY(_imu->gyro_prev, _imu->gyro);
_imu->gyro.p = ((_imu->gyro_unscaled.p + (TREF-_imu->temp_unscaled)*DXG -_imu->gyro_neutral.p) * IMU_GYRO_P_SIGN *
IMU_GYRO_P_SENS_NUM) / IMU_GYRO_P_SENS_DEN;
_imu->gyro.q = ((_imu->gyro_unscaled.q + (TREF-_imu->temp_unscaled)*DYG - _imu->gyro_neutral.q) * IMU_GYRO_Q_SIGN *
IMU_GYRO_Q_SENS_NUM) / IMU_GYRO_Q_SENS_DEN;
_imu->gyro.r = ((_imu->gyro_unscaled.r + (TREF-_imu->temp_unscaled)*DZG - _imu->gyro_neutral.r) * IMU_GYRO_R_SIGN *
IMU_GYRO_R_SENS_NUM) / IMU_GYRO_R_SENS_DEN;
#else
RATES_COPY(_imu->gyro_prev, _imu->gyro);
_imu->gyro.p = ((_imu->gyro_unscaled.p - _imu->gyro_neutral.p) * IMU_GYRO_P_SIGN *
IMU_GYRO_P_SENS_NUM) / IMU_GYRO_P_SENS_DEN;
_imu->gyro.q = ((_imu->gyro_unscaled.q - _imu->gyro_neutral.q) * IMU_GYRO_Q_SIGN *
IMU_GYRO_Q_SENS_NUM) / IMU_GYRO_Q_SENS_DEN;
_imu->gyro.r = ((_imu->gyro_unscaled.r - _imu->gyro_neutral.r) * IMU_GYRO_R_SIGN *
IMU_GYRO_R_SENS_NUM) / IMU_GYRO_R_SENS_DEN;
#endif // TREF
}
void ahrs_align(void) {
#if USE_MAGNETOMETER
/* Compute an initial orientation from accel and mag directly as quaternion */
ahrs_int_get_quat_from_accel_mag(&ahrs_impl.ltp_to_imu_quat, &ahrs_aligner.lp_accel, &ahrs_aligner.lp_mag);
ahrs_impl.heading_aligned = TRUE;
#else
/* Compute an initial orientation from accel and just set heading to zero */
ahrs_int_get_quat_from_accel(&ahrs_impl.ltp_to_imu_quat, &ahrs_aligner.lp_accel);
ahrs_impl.heading_aligned = FALSE;
#endif
set_body_state_from_quat();
/* Use low passed gyro value as initial bias */
RATES_COPY( ahrs_impl.gyro_bias, ahrs_aligner.lp_gyro);
RATES_COPY( ahrs_impl.high_rez_bias, ahrs_aligner.lp_gyro);
INT_RATES_LSHIFT(ahrs_impl.high_rez_bias, ahrs_impl.high_rez_bias, 28);
ahrs.status = AHRS_RUNNING;
}
void ahrs_propagate(void) {
/* unbias gyro */
struct Int32Rates uf_rate;
RATES_DIFF(uf_rate, imu.gyro, ahrs_impl.gyro_bias);
#if USE_NOISE_CUT
static struct Int32Rates last_uf_rate = { 0, 0, 0 };
if (!cut_rates(uf_rate, last_uf_rate, RATE_CUT_THRESHOLD)) {
#endif
/* low pass rate */
#if USE_NOISE_FILTER
RATES_SUM_SCALED(ahrs.imu_rate, ahrs.imu_rate, uf_rate, NOISE_FILTER_GAIN);
RATES_SDIV(ahrs.imu_rate, ahrs.imu_rate, NOISE_FILTER_GAIN+1);
#else
RATES_ADD(ahrs.imu_rate, uf_rate);
RATES_SDIV(ahrs.imu_rate, ahrs.imu_rate, 2);
#endif
#if USE_NOISE_CUT
}
RATES_COPY(last_uf_rate, uf_rate);
#endif
/* integrate eulers */
struct Int32Eulers euler_dot;
INT32_EULERS_DOT_OF_RATES(euler_dot, ahrs.ltp_to_imu_euler, ahrs.imu_rate);
EULERS_ADD(ahrs_impl.hi_res_euler, euler_dot);
/* low pass measurement */
EULERS_ADD(ahrs_impl.measure, ahrs_impl.measurement);
EULERS_SDIV(ahrs_impl.measure, ahrs_impl.measure, 2);
/* compute residual */
EULERS_DIFF(ahrs_impl.residual, ahrs_impl.measure, ahrs_impl.hi_res_euler);
INTEG_EULER_NORMALIZE(ahrs_impl.residual.psi);
struct Int32Eulers correction;
/* compute a correction */
EULERS_SDIV(correction, ahrs_impl.residual, ahrs_impl.reinj_1);
/* correct estimation */
EULERS_ADD(ahrs_impl.hi_res_euler, correction);
INTEG_EULER_NORMALIZE(ahrs_impl.hi_res_euler.psi);
/* Compute LTP to IMU eulers */
EULERS_SDIV(ahrs.ltp_to_imu_euler, ahrs_impl.hi_res_euler, F_UPDATE);
compute_imu_quat_and_rmat_from_euler();
compute_body_orientation();
}
void imu_mpu_spi_event(void)
{
mpu60x0_spi_event(&imu_mpu_spi.mpu);
if (imu_mpu_spi.mpu.data_available) {
uint32_t now_ts = get_sys_time_usec();
RATES_COPY(imu.gyro_unscaled, imu_mpu_spi.mpu.data_rates.rates);
VECT3_COPY(imu.accel_unscaled, imu_mpu_spi.mpu.data_accel.vect);
imu_mpu_spi.mpu.data_available = false;
imu_scale_gyro(&imu);
imu_scale_accel(&imu);
AbiSendMsgIMU_GYRO_INT32(IMU_MPU6000_ID, now_ts, &imu.gyro);
AbiSendMsgIMU_ACCEL_INT32(IMU_MPU6000_ID, now_ts, &imu.accel);
}
}
void ahrs_align(void)
{
/* Compute an initial orientation from accel and mag directly as quaternion */
ahrs_float_get_quat_from_accel_mag(&ins_impl.state.quat, &ahrs_aligner.lp_accel, &ahrs_aligner.lp_mag);
/* use average gyro as initial value for bias */
struct FloatRates bias0;
RATES_COPY(bias0, ahrs_aligner.lp_gyro);
RATES_FLOAT_OF_BFP(ins_impl.state.bias, bias0);
// ins and ahrs are now running
ahrs.status = AHRS_RUNNING;
ins.status = INS_RUNNING;
}
void ahrs_align(void) {
/* Compute an initial orientation from accel and mag directly as quaternion */
ahrs_float_get_quat_from_accel_mag(&ahrs_impl.ltp_to_imu_quat, &ahrs_aligner.lp_accel, &ahrs_aligner.lp_mag);
/* set initial body orientation */
set_body_state_from_quat();
/* used averaged gyro as initial value for bias */
struct Int32Rates bias0;
RATES_COPY(bias0, ahrs_aligner.lp_gyro);
RATES_FLOAT_OF_BFP(ahrs_impl.gyro_bias, bias0);
ahrs.status = AHRS_RUNNING;
}
void ahrs_ice_propagate(struct Int32Rates *gyro)
{
/* unbias gyro */
struct Int32Rates uf_rate;
RATES_DIFF(uf_rate, *gyro, ahrs_ice.gyro_bias);
#if USE_NOISE_CUT
static struct Int32Rates last_uf_rate = { 0, 0, 0 };
if (!cut_rates(uf_rate, last_uf_rate, RATE_CUT_THRESHOLD)) {
#endif
/* low pass rate */
#if USE_NOISE_FILTER
RATES_SUM_SCALED(ahrs_ice.imu_rate, ahrs_ice.imu_rate, uf_rate, NOISE_FILTER_GAIN);
RATES_SDIV(ahrs_ice.imu_rate, ahrs_ice.imu_rate, NOISE_FILTER_GAIN + 1);
#else
RATES_ADD(ahrs_ice.imu_rate, uf_rate);
RATES_SDIV(ahrs_ice.imu_rate, ahrs_ice.imu_rate, 2);
#endif
#if USE_NOISE_CUT
}
RATES_COPY(last_uf_rate, uf_rate);
#endif
/* integrate eulers */
struct Int32Eulers euler_dot;
int32_eulers_dot_of_rates(&euler_dot, &ahrs_ice.ltp_to_imu_euler, &ahrs_ice.imu_rate);
EULERS_ADD(ahrs_ice.hi_res_euler, euler_dot);
/* low pass measurement */
EULERS_ADD(ahrs_ice.measure, ahrs_ice.measurement);
EULERS_SDIV(ahrs_ice.measure, ahrs_ice.measure, 2);
/* compute residual */
EULERS_DIFF(ahrs_ice.residual, ahrs_ice.measure, ahrs_ice.hi_res_euler);
INTEG_EULER_NORMALIZE(ahrs_ice.residual.psi);
struct Int32Eulers correction;
/* compute a correction */
EULERS_SDIV(correction, ahrs_ice.residual, ahrs_ice.reinj_1);
/* correct estimation */
EULERS_ADD(ahrs_ice.hi_res_euler, correction);
INTEG_EULER_NORMALIZE(ahrs_ice.hi_res_euler.psi);
/* Compute LTP to IMU eulers */
EULERS_SDIV(ahrs_ice.ltp_to_imu_euler, ahrs_ice.hi_res_euler, F_UPDATE);
}
void imu_mpu9250_event(void)
{
uint32_t now_ts = get_sys_time_usec();
// If the MPU9250 I2C transaction has succeeded: convert the data
mpu9250_i2c_event(&imu_mpu9250.mpu);
if (imu_mpu9250.mpu.data_available) {
// set channel order
struct Int32Vect3 accel = {
IMU_MPU9250_X_SIGN *(int32_t)(imu_mpu9250.mpu.data_accel.value[IMU_MPU9250_CHAN_X]),
IMU_MPU9250_Y_SIGN *(int32_t)(imu_mpu9250.mpu.data_accel.value[IMU_MPU9250_CHAN_Y]),
IMU_MPU9250_Z_SIGN *(int32_t)(imu_mpu9250.mpu.data_accel.value[IMU_MPU9250_CHAN_Z])
};
struct Int32Rates rates = {
IMU_MPU9250_X_SIGN *(int32_t)(imu_mpu9250.mpu.data_rates.value[IMU_MPU9250_CHAN_X]),
IMU_MPU9250_Y_SIGN *(int32_t)(imu_mpu9250.mpu.data_rates.value[IMU_MPU9250_CHAN_Y]),
IMU_MPU9250_Z_SIGN *(int32_t)(imu_mpu9250.mpu.data_rates.value[IMU_MPU9250_CHAN_Z])
};
// unscaled vector
VECT3_COPY(imu.accel_unscaled, accel);
RATES_COPY(imu.gyro_unscaled, rates);
imu_mpu9250.mpu.data_available = false;
imu_scale_gyro(&imu);
imu_scale_accel(&imu);
AbiSendMsgIMU_GYRO_INT32(IMU_MPU9250_ID, now_ts, &imu.gyro);
AbiSendMsgIMU_ACCEL_INT32(IMU_MPU9250_ID, now_ts, &imu.accel);
}
#if IMU_MPU9250_READ_MAG
// Test if mag data are updated
if (imu_mpu9250.mpu.akm.data_available) {
struct Int32Vect3 mag = {
IMU_MPU9250_X_SIGN *(int32_t)(imu_mpu9250.mpu.akm.data.value[IMU_MPU9250_CHAN_Y]),
IMU_MPU9250_Y_SIGN *(int32_t)(imu_mpu9250.mpu.akm.data.value[IMU_MPU9250_CHAN_X]),
-IMU_MPU9250_Z_SIGN *(int32_t)(imu_mpu9250.mpu.akm.data.value[IMU_MPU9250_CHAN_Z])
};
VECT3_COPY(imu.mag_unscaled, mag);
imu_mpu9250.mpu.akm.data_available = false;
imu_scale_mag(&imu);
AbiSendMsgIMU_MAG_INT32(IMU_MPU9250_ID, now_ts, &imu.mag);
}
#endif
}
void imu_mpu9250_event(void)
{
uint32_t now_ts = get_sys_time_usec();
// If the MPU9250 SPI transaction has succeeded: convert the data
mpu9250_spi_event(&imu_mpu9250.mpu);
if (imu_mpu9250.mpu.data_available) {
// set channel order
struct Int32Vect3 accel = {
IMU_MPU9250_X_SIGN * (int32_t)(imu_mpu9250.mpu.data_accel.value[IMU_MPU9250_CHAN_X]),
IMU_MPU9250_Y_SIGN * (int32_t)(imu_mpu9250.mpu.data_accel.value[IMU_MPU9250_CHAN_Y]),
IMU_MPU9250_Z_SIGN * (int32_t)(imu_mpu9250.mpu.data_accel.value[IMU_MPU9250_CHAN_Z])
};
struct Int32Rates rates = {
IMU_MPU9250_X_SIGN * (int32_t)(imu_mpu9250.mpu.data_rates.value[IMU_MPU9250_CHAN_X]),
IMU_MPU9250_Y_SIGN * (int32_t)(imu_mpu9250.mpu.data_rates.value[IMU_MPU9250_CHAN_Y]),
IMU_MPU9250_Z_SIGN * (int32_t)(imu_mpu9250.mpu.data_rates.value[IMU_MPU9250_CHAN_Z])
};
// unscaled vector
VECT3_COPY(imu.accel_unscaled, accel);
RATES_COPY(imu.gyro_unscaled, rates);
#if IMU_MPU9250_READ_MAG
if (!bit_is_set(imu_mpu9250.mpu.data_ext[6], 3)) { //mag valid just HOFL == 0
/** FIXME: assumes that we get new mag data each time instead of reading drdy bit */
struct Int32Vect3 mag;
mag.x = (IMU_MPU9250_X_SIGN) * Int16FromBuf(imu_mpu9250.mpu.data_ext, 2 * IMU_MPU9250_CHAN_Y);
mag.y = (IMU_MPU9250_Y_SIGN) * Int16FromBuf(imu_mpu9250.mpu.data_ext, 2 * IMU_MPU9250_CHAN_X);
mag.z = -(IMU_MPU9250_Z_SIGN) * Int16FromBuf(imu_mpu9250.mpu.data_ext, 2 * IMU_MPU9250_CHAN_Z);
VECT3_COPY(imu.mag_unscaled, mag);
imu_scale_mag(&imu);
AbiSendMsgIMU_MAG_INT32(IMU_MPU9250_ID, now_ts, &imu.mag);
}
#endif
imu_mpu9250.mpu.data_available = false;
imu_scale_gyro(&imu);
imu_scale_accel(&imu);
AbiSendMsgIMU_GYRO_INT32(IMU_MPU9250_ID, now_ts, &imu.gyro);
AbiSendMsgIMU_ACCEL_INT32(IMU_MPU9250_ID, now_ts, &imu.accel);
}
}
void ahrs_align(void) {
/* Compute an initial orientation using euler angles */
ahrs_float_get_euler_from_accel_mag(&ahrs_float.ltp_to_imu_euler, &ahrs_aligner.lp_accel, &ahrs_aligner.lp_mag);
/* Convert initial orientation in quaternion and rotation matrice representations. */
FLOAT_QUAT_OF_EULERS(ahrs_float.ltp_to_imu_quat, ahrs_float.ltp_to_imu_euler);
FLOAT_RMAT_OF_QUAT(ahrs_float.ltp_to_imu_rmat, ahrs_float.ltp_to_imu_quat);
/* Compute initial body orientation */
compute_body_orientation_and_rates();
/* used averaged gyro as initial value for bias */
struct Int32Rates bias0;
RATES_COPY(bias0, ahrs_aligner.lp_gyro);
RATES_FLOAT_OF_BFP(ahrs_impl.gyro_bias, bias0);
ahrs.status = AHRS_RUNNING;
}
void imu_drotek2_event(void)
{
uint32_t now_ts = get_sys_time_usec();
// If the MPU6050 I2C transaction has succeeded: convert the data
mpu60x0_i2c_event(&imu_drotek2.mpu);
if (imu_drotek2.mpu.data_available) {
#if IMU_DROTEK_2_ORIENTATION_IC_UP
/* change orientation, so if ICs face up, z-axis is down */
imu.gyro_unscaled.p = imu_drotek2.mpu.data_rates.rates.p;
imu.gyro_unscaled.q = -imu_drotek2.mpu.data_rates.rates.q;
imu.gyro_unscaled.r = -imu_drotek2.mpu.data_rates.rates.r;
imu.accel_unscaled.x = imu_drotek2.mpu.data_accel.vect.x;
imu.accel_unscaled.y = -imu_drotek2.mpu.data_accel.vect.y;
imu.accel_unscaled.z = -imu_drotek2.mpu.data_accel.vect.z;
#else
/* default orientation as should be printed on the pcb, z-down, ICs down */
RATES_COPY(imu.gyro_unscaled, imu_drotek2.mpu.data_rates.rates);
VECT3_COPY(imu.accel_unscaled, imu_drotek2.mpu.data_accel.vect);
#endif
imu_drotek2.mpu.data_available = false;
imu_scale_gyro(&imu);
imu_scale_accel(&imu);
AbiSendMsgIMU_GYRO_INT32(IMU_DROTEK_ID, now_ts, &imu.gyro);
AbiSendMsgIMU_ACCEL_INT32(IMU_DROTEK_ID, now_ts, &imu.accel);
}
/* HMC58XX event task */
hmc58xx_event(&imu_drotek2.hmc);
if (imu_drotek2.hmc.data_available) {
#if IMU_DROTEK_2_ORIENTATION_IC_UP
imu.mag_unscaled.x = imu_drotek2.hmc.data.vect.x;
imu.mag_unscaled.y = -imu_drotek2.hmc.data.vect.y;
imu.mag_unscaled.z = -imu_drotek2.hmc.data.vect.z;
#else
VECT3_COPY(imu.mag_unscaled, imu_drotek2.hmc.data.vect);
#endif
imu_drotek2.hmc.data_available = false;
imu_scale_mag(&imu);
AbiSendMsgIMU_MAG_INT32(IMU_DROTEK_ID, now_ts, &imu.mag);
}
}
