void orientationCalcQuat_f(struct OrientationReps* orientation) { if (bit_is_set(orientation->status, ORREP_QUAT_F)) return; if (bit_is_set(orientation->status, ORREP_QUAT_I)) { QUAT_FLOAT_OF_BFP(orientation->quat_f, orientation->quat_i); } else if (bit_is_set(orientation->status, ORREP_RMAT_F)) { FLOAT_QUAT_OF_RMAT(orientation->quat_f, orientation->rmat_f); } else if (bit_is_set(orientation->status, ORREP_EULER_F)) { FLOAT_QUAT_OF_EULERS(orientation->quat_f, orientation->eulers_f); } else if (bit_is_set(orientation->status, ORREP_RMAT_I)) { RMAT_FLOAT_OF_BFP(orientation->rmat_f, orientation->rmat_i); SetBit(orientation->status, ORREP_RMAT_F); FLOAT_QUAT_OF_RMAT(orientation->quat_f, orientation->rmat_f); } else if (bit_is_set(orientation->status, ORREP_EULER_I)) { EULERS_FLOAT_OF_BFP(orientation->eulers_f, orientation->eulers_i); SetBit(orientation->status, ORREP_EULER_F); FLOAT_QUAT_OF_EULERS(orientation->quat_f, orientation->eulers_f); } /* set bit to indicate this representation is computed */ SetBit(orientation->status, ORREP_QUAT_F); }
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(); }
static void update_ref_quat_from_eulers(void) { struct FloatRMat ref_rmat; #ifdef STICKS_RMAT312 FLOAT_RMAT_OF_EULERS_312(ref_rmat, stab_att_ref_euler); #else FLOAT_RMAT_OF_EULERS_321(ref_rmat, stab_att_ref_euler); #endif FLOAT_QUAT_OF_RMAT(stab_att_ref_quat, ref_rmat); FLOAT_QUAT_WRAP_SHORTEST(stab_att_ref_quat); }
float test_quat_of_rmat(void) { // struct FloatEulers eul = {-0.280849, 0.613423, -1.850440}; struct FloatEulers eul = {RadOfDeg(0.131579), RadOfDeg(-62.397659), RadOfDeg(-110.470299)}; // struct FloatEulers eul = {RadOfDeg(0.13), RadOfDeg(180.), RadOfDeg(-61.)}; struct FloatMat33 rm; FLOAT_RMAT_OF_EULERS(rm, eul); DISPLAY_FLOAT_RMAT_AS_EULERS_DEG("rmat", rm); struct FloatQuat q; FLOAT_QUAT_OF_RMAT(q, rm); DISPLAY_FLOAT_QUAT("q_of_rm ", q); DISPLAY_FLOAT_QUAT_AS_EULERS_DEG("q_of_rm ", q); struct FloatQuat qref; FLOAT_QUAT_OF_EULERS(qref, eul); DISPLAY_FLOAT_QUAT("q_of_euler", qref); DISPLAY_FLOAT_QUAT_AS_EULERS_DEG("q_of_euler", qref); printf("\n\n\n"); struct FloatMat33 r_att; struct FloatEulers e312 = { eul.phi, eul.theta, eul.psi }; FLOAT_RMAT_OF_EULERS_312(r_att, e312); DISPLAY_FLOAT_RMAT("r_att ", r_att); DISPLAY_FLOAT_RMAT_AS_EULERS_DEG("r_att ", r_att); struct FloatQuat q_att; FLOAT_QUAT_OF_RMAT(q_att, r_att); struct FloatEulers e_att; FLOAT_EULERS_OF_RMAT(e_att, r_att); DISPLAY_FLOAT_EULERS_DEG("of rmat", e_att); FLOAT_EULERS_OF_QUAT(e_att, q_att); DISPLAY_FLOAT_EULERS_DEG("of quat", e_att); return 0.; }
float test_INT32_QUAT_OF_RMAT(struct FloatEulers* eul_f, bool_t display) { struct Int32Eulers eul312_i; EULERS_BFP_OF_REAL(eul312_i, (*eul_f)); if (display) DISPLAY_INT32_EULERS("eul312_i", eul312_i); struct FloatRMat rmat_f; FLOAT_RMAT_OF_EULERS_312(rmat_f, (*eul_f)); if (display) DISPLAY_FLOAT_RMAT_AS_EULERS_DEG("rmat float", rmat_f); if (display) DISPLAY_FLOAT_RMAT("rmat float", rmat_f); struct Int32RMat rmat_i; INT32_RMAT_OF_EULERS_312(rmat_i, eul312_i); if (display) DISPLAY_INT32_RMAT_AS_EULERS_DEG("rmat int", rmat_i); if (display) DISPLAY_INT32_RMAT("rmat int", rmat_i); if (display) DISPLAY_INT32_RMAT_AS_FLOAT("rmat int", rmat_i); struct FloatQuat qf; FLOAT_QUAT_OF_RMAT(qf, rmat_f); FLOAT_QUAT_WRAP_SHORTEST(qf); if (display) DISPLAY_FLOAT_QUAT("qf", qf); struct Int32Quat qi; INT32_QUAT_OF_RMAT(qi, rmat_i); INT32_QUAT_WRAP_SHORTEST(qi); if (display) DISPLAY_INT32_QUAT("qi", qi); if (display) DISPLAY_INT32_QUAT_2("qi", qi); struct FloatQuat qif; QUAT_FLOAT_OF_BFP(qif, qi); struct FloatQuat qerr; QUAT_DIFF(qerr, qif, qf); float err_norm = FLOAT_QUAT_NORM(qerr); if (display) printf("err %f\n", err_norm); if (display) printf("\n"); return err_norm; }
static inline void update_ref_quat_from_eulers(void) { struct FloatRMat ref_rmat; FLOAT_RMAT_OF_EULERS(ref_rmat, stab_att_ref_euler); FLOAT_QUAT_OF_RMAT(stab_att_ref_quat, ref_rmat); FLOAT_QUAT_WRAP_SHORTEST(stab_att_ref_quat); }
static void print_estimator_state(double time) { #if FILTER_OUTPUT_IN_NED struct EcefCoor_d pos_ecef, cur_pos_ecef, cur_vel_ecef; struct NedCoor_d pos_ned, vel_ned; struct DoubleQuat q_ecef2body, q_ecef2enu, q_enu2body, q_ned2enu, q_ned2body; VECTOR_AS_VECT3(pos_ecef,pos_0_ecef); VECTOR_AS_VECT3(cur_pos_ecef,ins.avg_state.position); VECTOR_AS_VECT3(cur_vel_ecef,ins.avg_state.velocity); ned_of_ecef_point_d(&pos_ned, ¤t_ltp, &cur_pos_ecef); ned_of_ecef_vect_d(&vel_ned, ¤t_ltp, &cur_vel_ecef); int32_t xdd = 0; int32_t ydd = 0; int32_t zdd = 0; int32_t xd = vel_ned.x/0.0000019073; int32_t yd = vel_ned.y/0.0000019073; int32_t zd = vel_ned.z/0.0000019073; int32_t x = pos_ned.x/0.0039; int32_t y = pos_ned.y/0.0039; int32_t z = pos_ned.z/0.0039; fprintf(ins_logfile, "%f %d BOOZ2_INS2 %d %d %d %d %d %d %d %d %d\n", time, AC_ID, xdd, ydd, zdd, xd, yd, zd, x, y, z); #if 0 QUAT_ASSIGN(q_ecef2body, ins.avg_state.orientation.w(), -ins.avg_state.orientation.x(), -ins.avg_state.orientation.y(), -ins.avg_state.orientation.z()); QUAT_ASSIGN(q_ned2enu, 0, M_SQRT1_2, M_SQRT1_2, 0); FLOAT_QUAT_OF_RMAT(q_ecef2enu, current_ltp.ltp_of_ecef); FLOAT_QUAT_INV_COMP(q_enu2body, q_ecef2enu, q_ecef2body); // q_enu2body = q_ecef2body * (q_ecef2enu)^* FLOAT_QUAT_COMP(q_ned2body, q_ned2enu, q_enu2body); // q_ned2body = q_enu2body * q_ned2enu #else /* if 0 */ QUATERNIOND_AS_DOUBLEQUAT(q_ecef2body, ins.avg_state.orientation); DOUBLE_QUAT_OF_RMAT(q_ecef2enu, current_ltp.ltp_of_ecef); FLOAT_QUAT_INV_COMP(q_enu2body, q_ecef2enu, q_ecef2body); QUAT_ENU_FROM_TO_NED(q_enu2body, q_ned2body); #endif /* if 0 */ struct FloatEulers e; FLOAT_EULERS_OF_QUAT(e, q_ned2body); #if PRINT_EULER_NED printf("EULER % 6.1f % 6.1f % 6.1f\n", e.phi*180*M_1_PI, e.theta*180*M_1_PI, e.psi*180*M_1_PI); #endif /* PRINT_EULER_NED */ fprintf(ins_logfile, "%f %d AHRS_EULER %f %f %f\n", time, AC_ID, e.phi, e.theta, e.psi); fprintf(ins_logfile, "%f %d DEBUG_COVARIANCE %f %f %f %f %f %f %f %f %f %f %f %f\n", time, AC_ID, sqrt(ins.cov( 0, 0)), sqrt(ins.cov( 1, 1)), sqrt(ins.cov( 2, 2)), sqrt(ins.cov( 3, 3)), sqrt(ins.cov( 4, 4)), sqrt(ins.cov( 5, 5)), sqrt(ins.cov( 6, 6)), sqrt(ins.cov( 7, 7)), sqrt(ins.cov( 8, 8)), sqrt(ins.cov( 9, 9)), sqrt(ins.cov(10,10)), sqrt(ins.cov(11,11))); fprintf(ins_logfile, "%f %d BOOZ_SIM_GYRO_BIAS %f %f %f\n", time, AC_ID, ins.avg_state.gyro_bias(0), ins.avg_state.gyro_bias(1), ins.avg_state.gyro_bias(2)); #else /* FILTER_OUTPUT_IN_ECEF */ int32_t xdd = 0; int32_t ydd = 0; int32_t zdd = 0; int32_t xd = ins.avg_state.velocity(0)/0.0000019073; int32_t yd = ins.avg_state.velocity(1)/0.0000019073; int32_t zd = ins.avg_state.velocity(2)/0.0000019073; int32_t x = ins.avg_state.position(0)/0.0039; int32_t y = ins.avg_state.position(1)/0.0039; int32_t z = ins.avg_state.position(2)/0.0039; fprintf(ins_logfile, "%f %d BOOZ2_INS2 %d %d %d %d %d %d %d %d %d\n", time, AC_ID, xdd, ydd, zdd, xd, yd, zd, x, y, z); struct FloatQuat q_ecef2body; QUAT_ASSIGN(q_ecef2body, ins.avg_state.orientation.w(), ins.avg_state.orientation.x(), ins.avg_state.orientation.y(), ins.avg_state.orientation.z()); struct FloatEulers e_ecef2body; FLOAT_EULERS_OF_QUAT(e_ecef2body, q_ecef2body); fprintf(ins_logfile, "%f %d AHRS_EULER %f %f %f\n", time, AC_ID, e_ecef2body.phi, e_ecef2body.theta, e_ecef2body.psi); fprintf(ins_logfile, "%f %d DEBUG_COVARIANCE %f %f %f %f %f %f %f %f %f %f %f %f\n", time, AC_ID, sqrt(ins.cov( 0, 0)), sqrt(ins.cov( 1, 1)), sqrt(ins.cov( 2, 2)), sqrt(ins.cov( 3, 3)), sqrt(ins.cov( 4, 4)), sqrt(ins.cov( 5, 5)), sqrt(ins.cov( 6, 6)), sqrt(ins.cov( 7, 7)), sqrt(ins.cov( 8, 8)), sqrt(ins.cov( 9, 9)), sqrt(ins.cov(10,10)), sqrt(ins.cov(11,11))); fprintf(ins_logfile, "%f %d BOOZ_SIM_GYRO_BIAS %f %f %f\n", time, AC_ID, ins.avg_state.gyro_bias(0), ins.avg_state.gyro_bias(1), ins.avg_state.gyro_bias(2)); #endif /* FILTER_OUTPUT_IN_NED / ECEF */ }
/* * Compute ltp to imu rotation in euler angles and quaternion representations * from the rotation matrice representation */ static inline void compute_imu_quat_and_euler_from_rmat(void) { FLOAT_QUAT_OF_RMAT(ahrs_float.ltp_to_imu_quat, ahrs_float.ltp_to_imu_rmat); FLOAT_EULERS_OF_RMAT(ahrs_float.ltp_to_imu_euler, ahrs_float.ltp_to_imu_rmat); }