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); }