void orientationCalcEulers_i(struct OrientationReps* orientation) {
if (bit_is_set(orientation->status, ORREP_EULER_I))
return;
if (bit_is_set(orientation->status, ORREP_EULER_F)) {
EULERS_BFP_OF_REAL(orientation->eulers_i, orientation->eulers_f);
}
else if (bit_is_set(orientation->status, ORREP_RMAT_I)) {
INT32_EULERS_OF_RMAT(orientation->eulers_i, orientation->rmat_i);
}
else if (bit_is_set(orientation->status, ORREP_QUAT_I)) {
INT32_EULERS_OF_QUAT(orientation->eulers_i, orientation->quat_i);
}
else if (bit_is_set(orientation->status, ORREP_RMAT_F)) {
RMAT_BFP_OF_REAL(orientation->rmat_i, orientation->rmat_f);
SetBit(orientation->status, ORREP_RMAT_I);
INT32_EULERS_OF_RMAT(orientation->eulers_i, orientation->rmat_i);
}
else if (bit_is_set(orientation->status, ORREP_QUAT_F)) {
QUAT_BFP_OF_REAL(orientation->quat_i, orientation->quat_f);
SetBit(orientation->status, ORREP_QUAT_I);
INT32_EULERS_OF_QUAT(orientation->eulers_i, orientation->quat_i);
}
/* set bit to indicate this representation is computed */
SetBit(orientation->status, ORREP_EULER_I);
}
void ahrs_init(void) {
ahrs.status = AHRS_UNINIT;
ahrs_impl.ltp_vel_norm_valid = FALSE;
ahrs_impl.heading_aligned = FALSE;
/* set ltp_to_body to zero */
INT_EULERS_ZERO(ahrs.ltp_to_body_euler);
INT32_QUAT_ZERO(ahrs.ltp_to_body_quat);
INT32_RMAT_ZERO(ahrs.ltp_to_body_rmat);
INT_RATES_ZERO(ahrs.body_rate);
/* set ltp_to_imu so that body is zero */
QUAT_COPY(ahrs.ltp_to_imu_quat, imu.body_to_imu_quat);
RMAT_COPY(ahrs.ltp_to_imu_rmat, imu.body_to_imu_rmat);
INT32_EULERS_OF_RMAT(ahrs.ltp_to_imu_euler, ahrs.ltp_to_imu_rmat);
INT_RATES_ZERO(ahrs.imu_rate);
INT_RATES_ZERO(ahrs_impl.gyro_bias);
INT_RATES_ZERO(ahrs_impl.rate_correction);
INT_RATES_ZERO(ahrs_impl.high_rez_bias);
#if AHRS_GRAVITY_UPDATE_COORDINATED_TURN
ahrs_impl.correct_gravity = TRUE;
#else
ahrs_impl.correct_gravity = FALSE;
#endif
#if AHRS_GRAVITY_UPDATE_NORM_HEURISTIC
ahrs_impl.use_gravity_heuristic = TRUE;
#else
ahrs_impl.use_gravity_heuristic = FALSE;
#endif
}
__attribute__ ((always_inline)) static inline void compute_body_orientation(void) {
/* Compute LTP to BODY quaternion */
INT32_QUAT_COMP_INV(ahrs.ltp_to_body_quat, ahrs.ltp_to_imu_quat, imu.body_to_imu_quat);
/* Compute LTP to BODY rotation matrix */
INT32_RMAT_COMP_INV(ahrs.ltp_to_body_rmat, ahrs.ltp_to_imu_rmat, imu.body_to_imu_rmat);
/* compute LTP to BODY eulers */
INT32_EULERS_OF_RMAT(ahrs.ltp_to_body_euler, ahrs.ltp_to_body_rmat);
/* compute body rates */
INT32_RMAT_TRANSP_RATEMULT(ahrs.body_rate, imu.body_to_imu_rmat, ahrs.imu_rate);
}
float test_eulers_of_rmat(struct FloatRMat frm, int display) {
struct FloatEulers fe;
FLOAT_EULERS_OF_RMAT(fe, frm);
struct Int32RMat irm;
RMAT_BFP_OF_REAL(irm, frm);
struct Int32Eulers ie;
INT32_EULERS_OF_RMAT(ie, irm);
struct FloatEulers fe2;
EULERS_FLOAT_OF_BFP(fe2, ie);
EULERS_SUB(fe2, ie);
float norm_err = FLOAT_EULERS_NORM(fe2);
if (display) {
printf("euler of rmat\n");
// DISPLAY_FLOAT_RMAT("fr", fr);
DISPLAY_FLOAT_EULERS_DEG("fe", fe);
DISPLAY_INT32_EULERS("ie", ie);
DISPLAY_INT32_EULERS_AS_FLOAT_DEG("ieaf", ie);
}
return norm_err;
}
void ahrs_init(void) {
ahrs.status = AHRS_UNINIT;
/* set ltp_to_body to zero */
INT_EULERS_ZERO(ahrs.ltp_to_body_euler);
INT32_QUAT_ZERO(ahrs.ltp_to_body_quat);
INT32_RMAT_ZERO(ahrs.ltp_to_body_rmat);
INT_RATES_ZERO(ahrs.body_rate);
/* set ltp_to_imu so that body is zero */
QUAT_COPY(ahrs.ltp_to_imu_quat, imu.body_to_imu_quat);
RMAT_COPY(ahrs.ltp_to_imu_rmat, imu.body_to_imu_rmat);
INT32_EULERS_OF_RMAT(ahrs.ltp_to_imu_euler, ahrs.ltp_to_imu_rmat);
INT_RATES_ZERO(ahrs.imu_rate);
INT_RATES_ZERO(ahrs_impl.gyro_bias);
ahrs_impl.reinj_1 = FACE_REINJ_1;
#ifdef IMU_MAG_OFFSET
ahrs_mag_offset = IMU_MAG_OFFSET;
#else
ahrs_mag_offset = 0.;
#endif
}
static void test_3(void) {
/* Compute BODY to IMU eulers */
struct Int32Eulers b2i_e;
EULERS_ASSIGN(b2i_e, ANGLE_BFP_OF_REAL(RadOfDeg(10.66)), ANGLE_BFP_OF_REAL(RadOfDeg(-0.7)), ANGLE_BFP_OF_REAL(RadOfDeg(0.)));
DISPLAY_INT32_EULERS_AS_FLOAT_DEG("b2i_e", b2i_e);
/* Compute BODY to IMU quaternion */
struct Int32Quat b2i_q;
INT32_QUAT_OF_EULERS(b2i_q, b2i_e);
DISPLAY_INT32_QUAT_AS_EULERS_DEG("b2i_q", b2i_q);
// INT32_QUAT_NORMALIZE(b2i_q);
// DISPLAY_INT32_QUAT_AS_EULERS_DEG("b2i_q_n", b2i_q);
/* Compute BODY to IMU rotation matrix */
struct Int32RMat b2i_r;
INT32_RMAT_OF_EULERS(b2i_r, b2i_e);
// DISPLAY_INT32_RMAT("b2i_r", b2i_r);
DISPLAY_INT32_RMAT_AS_EULERS_DEG("b2i_r", b2i_r);
/* Compute LTP to IMU eulers */
struct Int32Eulers l2i_e;
EULERS_ASSIGN(l2i_e, ANGLE_BFP_OF_REAL(RadOfDeg(0.)), ANGLE_BFP_OF_REAL(RadOfDeg(20.)), ANGLE_BFP_OF_REAL(RadOfDeg(0.)));
DISPLAY_INT32_EULERS_AS_FLOAT_DEG("l2i_e", l2i_e);
/* Compute LTP to IMU quaternion */
struct Int32Quat l2i_q;
INT32_QUAT_OF_EULERS(l2i_q, l2i_e);
DISPLAY_INT32_QUAT_AS_EULERS_DEG("l2i_q", l2i_q);
/* Compute LTP to IMU rotation matrix */
struct Int32RMat l2i_r;
INT32_RMAT_OF_EULERS(l2i_r, l2i_e);
// DISPLAY_INT32_RMAT("l2i_r", l2i_r);
DISPLAY_INT32_RMAT_AS_EULERS_DEG("l2i_r", l2i_r);
/* again but from quaternion */
struct Int32RMat l2i_r2;
INT32_RMAT_OF_QUAT(l2i_r2, l2i_q);
// DISPLAY_INT32_RMAT("l2i_r2", l2i_r2);
DISPLAY_INT32_RMAT_AS_EULERS_DEG("l2i_r2", l2i_r2);
/* Compute LTP to BODY quaternion */
struct Int32Quat l2b_q;
INT32_QUAT_COMP_INV(l2b_q, b2i_q, l2i_q);
DISPLAY_INT32_QUAT_AS_EULERS_DEG("l2b_q", l2b_q);
/* Compute LTP to BODY rotation matrix */
struct Int32RMat l2b_r;
INT32_RMAT_COMP_INV(l2b_r, l2i_r, b2i_r);
// DISPLAY_INT32_RMAT("l2b_r", l2b_r);
DISPLAY_INT32_RMAT_AS_EULERS_DEG("l2b_r2", l2b_r);
/* again but from quaternion */
struct Int32RMat l2b_r2;
INT32_RMAT_OF_QUAT(l2b_r2, l2b_q);
// DISPLAY_INT32_RMAT("l2b_r2", l2b_r2);
DISPLAY_INT32_RMAT_AS_EULERS_DEG("l2b_r2", l2b_r2);
/* compute LTP to BODY eulers */
struct Int32Eulers l2b_e;
INT32_EULERS_OF_RMAT(l2b_e, l2b_r);
DISPLAY_INT32_EULERS_AS_FLOAT_DEG("l2b_e", l2b_e);
/* again but from quaternion */
struct Int32Eulers l2b_e2;
INT32_EULERS_OF_QUAT(l2b_e2, l2b_q);
DISPLAY_INT32_EULERS_AS_FLOAT_DEG("l2b_e2", l2b_e2);
}
