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