void orientationCalcEulers_f(struct OrientationReps* orientation) { if (bit_is_set(orientation->status, ORREP_EULER_F)) return; if (bit_is_set(orientation->status, ORREP_EULER_I)) { EULERS_FLOAT_OF_BFP(orientation->eulers_f, orientation->eulers_i); } else if (bit_is_set(orientation->status, ORREP_RMAT_F)) { FLOAT_EULERS_OF_RMAT(orientation->eulers_f, orientation->rmat_f); } else if (bit_is_set(orientation->status, ORREP_QUAT_F)) { FLOAT_EULERS_OF_QUAT(orientation->eulers_f, orientation->quat_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_EULERS_OF_RMAT(orientation->eulers_f, orientation->rmat_f); } else if (bit_is_set(orientation->status, ORREP_QUAT_I)) { QUAT_FLOAT_OF_BFP(orientation->quat_f, orientation->quat_i); SetBit(orientation->status, ORREP_QUAT_F); FLOAT_EULERS_OF_QUAT(orientation->eulers_f, orientation->quat_f); } /* set bit to indicate this representation is computed */ SetBit(orientation->status, ORREP_EULER_F); }
void quat_from_rpy_cmd_i(struct Int32Quat *quat, struct Int32Eulers *rpy) { struct FloatEulers rpy_f; EULERS_FLOAT_OF_BFP(rpy_f, *rpy); struct FloatQuat quat_f; quat_from_rpy_cmd_f(&quat_f, &rpy_f); QUAT_BFP_OF_REAL(*quat, quat_f); }
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 stabilization_attitude_set_rpy_setpoint_i(struct Int32Eulers *rpy) { // copy euler setpoint for debugging EULERS_FLOAT_OF_BFP(stab_att_sp_euler, *rpy); float_quat_of_eulers(&stab_att_sp_quat, &stab_att_sp_euler); }
gboolean timeout_callback(gpointer data) { for (int i=0; i<20; i++) { aos_compute_state(); aos_compute_sensors(); #ifndef DISABLE_PROPAGATE ahrs_propagate(); #endif #ifndef DISABLE_ACCEL_UPDATE ahrs_update_accel(); #endif #ifndef DISABLE_MAG_UPDATE if (!(i%5)) ahrs_update_mag(); #endif } #if AHRS_TYPE == AHRS_TYPE_ICE || AHRS_TYPE == AHRS_TYPE_ICQ EULERS_FLOAT_OF_BFP(ahrs_float.ltp_to_imu_euler, ahrs.ltp_to_imu_euler); #endif #if AHRS_TYPE == AHRS_TYPE_ICQ IvySendMsg("183 BOOZ_AHRS_BIAS %d %d %d", ahrs_impl.gyro_bias.p, ahrs_impl.gyro_bias.q, ahrs_impl.gyro_bias.r); #endif #if AHRS_TYPE == AHRS_TYPE_FLQ || AHRS_TYPE == AHRS_TYPE_FCR2 struct Int32Rates bias_i; RATES_BFP_OF_REAL(bias_i, ahrs_impl.gyro_bias); IvySendMsg("183 BOOZ_AHRS_BIAS %d %d %d", bias_i.p, bias_i.q, bias_i.r); #endif IvySendMsg("183 AHRS_EULER %f %f %f", ahrs_float.ltp_to_imu_euler.phi, ahrs_float.ltp_to_imu_euler.theta, ahrs_float.ltp_to_imu_euler.psi); IvySendMsg("183 BOOZ_SIM_RATE_ATTITUDE %f %f %f %f %f %f", DegOfRad(aos.imu_rates.p), DegOfRad(aos.imu_rates.q), DegOfRad(aos.imu_rates.r), DegOfRad(aos.ltp_to_imu_euler.phi), DegOfRad(aos.ltp_to_imu_euler.theta), DegOfRad(aos.ltp_to_imu_euler.psi)); IvySendMsg("183 BOOZ_SIM_GYRO_BIAS %f %f %f", DegOfRad(aos.gyro_bias.p), DegOfRad(aos.gyro_bias.q), DegOfRad(aos.gyro_bias.r)); return TRUE; }
void ahrs_update_fw_estimator(void) { struct FloatEulers att; // export results to estimator EULERS_FLOAT_OF_BFP(att,ahrs.ltp_to_imu_euler); estimator_phi = att.phi; estimator_theta = att.theta; estimator_psi = att.psi; //estimator_p = Omega_Vector[0]; }
static void store_filter_output(int i) { #ifdef OUTPUT_IN_BODY_FRAME QUAT_FLOAT_OF_BFP(output[i].quat_est, ahrs_impl.ltp_to_body_quat); RATES_FLOAT_OF_BFP(output[i].rate_est, ahrs_impl.body_rate); #else struct FloatEulers eul_f; EULERS_FLOAT_OF_BFP(eul_f, ahrs_impl.ltp_to_imu_euler); FLOAT_QUAT_OF_EULERS(output[i].quat_est, eul_f); RATES_FLOAT_OF_BFP(output[i].rate_est, ahrs_impl.imu_rate); #endif /* OUTPUT_IN_BODY_FRAME */ RATES_ASSIGN(output[i].bias_est, 0., 0., 0.); // memset(output[i].P, ahrs_impl.P, sizeof(ahrs_impl.P)); }
void ahrs_update_fw_estimator(void) { struct FloatEulers att; // export results to estimator EULERS_FLOAT_OF_BFP(att, ahrs.ltp_to_body_euler); estimator_phi = att.phi - ins_roll_neutral; estimator_theta = att.theta - ins_pitch_neutral; estimator_psi = att.psi; struct FloatRates rates; RATES_FLOAT_OF_BFP(rates, ahrs.body_rate); estimator_p = rates.p; estimator_q = rates.q; estimator_r = rates.r; }
float test_eulers_of_quat(struct FloatQuat fq, int display) { struct FloatEulers fe; FLOAT_EULERS_OF_QUAT(fe, fq); struct Int32Quat iq; QUAT_BFP_OF_REAL(iq, fq); struct Int32Eulers ie; INT32_EULERS_OF_QUAT(ie, iq); 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 quat\n"); DISPLAY_FLOAT_QUAT("fq", fq); DISPLAY_FLOAT_EULERS_DEG("fe", fe); DISPLAY_INT32_EULERS("ie", ie); DISPLAY_INT32_EULERS_AS_FLOAT_DEG("ieaf", ie); } return norm_err; }
void orientationCalcRMat_f(struct OrientationReps* orientation) { if (bit_is_set(orientation->status, ORREP_RMAT_F)) { return; } if (bit_is_set(orientation->status, ORREP_RMAT_I)) { RMAT_FLOAT_OF_BFP(orientation->rmat_f, orientation->rmat_i); } else if (bit_is_set(orientation->status, ORREP_QUAT_F)) { float_rmat_of_quat(&(orientation->rmat_f), &(orientation->quat_f)); } else if (bit_is_set(orientation->status, ORREP_EULER_F)) { float_rmat_of_eulers(&(orientation->rmat_f), &(orientation->eulers_f)); } else if (bit_is_set(orientation->status, ORREP_QUAT_I)) { QUAT_FLOAT_OF_BFP(orientation->quat_f, orientation->quat_i); SetBit(orientation->status, ORREP_QUAT_F); float_rmat_of_quat(&(orientation->rmat_f), &(orientation->quat_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_rmat_of_eulers(&(orientation->rmat_f), &(orientation->eulers_f)); } /* set bit to indicate this representation is computed */ SetBit(orientation->status, ORREP_RMAT_F); }
void stabilization_attitude_set_rpy_setpoint_i(struct Int32Eulers *rpy) { EULERS_FLOAT_OF_BFP(stab_att_sp_euler, *rpy); }
static void report(void) { int output_sensors = FALSE; int output_pos = FALSE; printf("%f ", aos.time); printf("%f %f %f ", DegOfRad(aos.ltp_to_imu_euler.phi), DegOfRad(aos.ltp_to_imu_euler.theta), DegOfRad(aos.ltp_to_imu_euler.psi)); printf("%f %f %f ", DegOfRad(aos.imu_rates.p), DegOfRad(aos.imu_rates.q), DegOfRad(aos.imu_rates.r)); printf("%f %f %f ", DegOfRad(aos.gyro_bias.p), DegOfRad(aos.gyro_bias.q), DegOfRad(aos.gyro_bias.r)); #if AHRS_TYPE == AHRS_TYPE_ICQ struct Int32Eulers ltp_to_imu_euler_i; int32_eulers_of_quat(<p_to_imu_euler_i, &ahrs_impl.ltp_to_imu_quat); struct FloatEulers ltp_to_imu_euler_f; EULERS_FLOAT_OF_BFP(ltp_to_imu_euler_f, ltp_to_imu_euler_i); printf("%f %f %f ", DegOfRad(ltp_to_imu_euler_f.phi), DegOfRad(ltp_to_imu_euler_f.theta), DegOfRad(ltp_to_imu_euler_f.psi)); struct FloatRates imu_rate_f; RATES_FLOAT_OF_BFP(imu_rate_f, ahrs_impl.imu_rate); printf("%f %f %f ", DegOfRad(imu_rate_f.p), DegOfRad(imu_rate_f.q), DegOfRad(imu_rate_f.r)); struct FloatRates bias; RATES_FLOAT_OF_BFP(bias, ahrs_impl.gyro_bias); printf("%f %f %f ", DegOfRad(bias.p), DegOfRad(bias.q), DegOfRad(bias.r)); #elif AHRS_TYPE == AHRS_TYPE_FCR2 || AHRS_TYPE == AHRS_TYPE_FCQ || AHRS_TYPE == AHRS_TYPE_FCR printf("%f %f %f ", DegOfRad(ahrs_impl.ltp_to_imu_euler.phi), DegOfRad(ahrs_impl.ltp_to_imu_euler.theta), DegOfRad(ahrs_impl.ltp_to_imu_euler.psi)); printf("%f %f %f ", DegOfRad(ahrs_impl.imu_rate.p), DegOfRad(ahrs_impl.imu_rate.q), DegOfRad(ahrs_impl.imu_rate.r)); printf("%f %f %f ", DegOfRad(ahrs_impl.gyro_bias.p), DegOfRad(ahrs_impl.gyro_bias.q), DegOfRad(ahrs_impl.gyro_bias.r)); #endif if (output_pos) { printf("%f %f %f ", aos.ltp_pos.x, aos.ltp_pos.y, aos.ltp_pos.z); } if (output_sensors) { } printf("\n"); }