/** in place quaternion integration with constant rotational velocity */ void float_quat_integrate(struct FloatQuat *q, struct FloatRates *omega, float dt) { const float no = FLOAT_RATES_NORM(*omega); if (no > FLT_MIN) { const float a = 0.5 * no * dt; const float ca = cosf(a); const float sa_ov_no = sinf(a) / no; const float dp = sa_ov_no * omega->p; const float dq = sa_ov_no * omega->q; const float dr = sa_ov_no * omega->r; const float qi = q->qi; const float qx = q->qx; const float qy = q->qy; const float qz = q->qz; q->qi = ca * qi - dp * qx - dq * qy - dr * qz; q->qx = dp * qi + ca * qx + dr * qy - dq * qz; q->qy = dq * qi - dr * qx + ca * qy + dp * qz; q->qz = dr * qi + dq * qx - dp * qy + ca * qz; } }
static inline void propagate_ref(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 /* lowpass angular 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 /* propagate reference quaternion only if rate is non null */ const float no = FLOAT_RATES_NORM(ahrs_impl.imu_rate); if (no > FLT_MIN) { const float dt = 1. / (AHRS_PROPAGATE_FREQUENCY); const float a = 0.5*no*dt; const float ca = cosf(a); const float sa_ov_no = sinf(a)/no; const float dp = sa_ov_no*ahrs_impl.imu_rate.p; const float dq = sa_ov_no*ahrs_impl.imu_rate.q; const float dr = sa_ov_no*ahrs_impl.imu_rate.r; const float qi = ahrs_impl.ltp_to_imu_quat.qi; const float qx = ahrs_impl.ltp_to_imu_quat.qx; const float qy = ahrs_impl.ltp_to_imu_quat.qy; const float qz = ahrs_impl.ltp_to_imu_quat.qz; ahrs_impl.ltp_to_imu_quat.qi = ca*qi - dp*qx - dq*qy - dr*qz; ahrs_impl.ltp_to_imu_quat.qx = dp*qi + ca*qx + dr*qy - dq*qz; ahrs_impl.ltp_to_imu_quat.qy = dq*qi - dr*qx + ca*qy + dp*qz; ahrs_impl.ltp_to_imu_quat.qz = dr*qi + dq*qx - dp*qy + ca*qz; // printf("%f\n", ahrs_impl.ltp_to_imu_quat.qi); } }