static void store_filter_output(int i) {
#ifdef OUTPUT_IN_BODY_FRAME
QUAT_FLOAT_OF_BFP(output[i].quat_est, ahrs.ltp_to_body_quat);
RATES_FLOAT_OF_BFP(output[i].rate_est, ahrs.body_rate);
#else
QUAT_FLOAT_OF_BFP(output[i].quat_est, ahrs.ltp_to_imu_quat);
RATES_FLOAT_OF_BFP(output[i].rate_est, ahrs.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 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);
}
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));
}
float test_INT32_QUAT_OF_RMAT(struct FloatEulers *eul_f, bool display)
{
struct Int32Eulers eul321_i;
EULERS_BFP_OF_REAL(eul321_i, (*eul_f));
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_321(&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_321(&rmat_i, &eul321_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);
// dot product of two quaternions is 1 if they represent same rotation
float qi_dot_qf = qif.qi * qf.qi + qif.qx * qf.qx + qif.qy * qf.qy + qif.qz * qf.qz;
float err_norm = fabs(fabs(qi_dot_qf) - 1.);
if (display) { printf("err %f\n", err_norm); }
if (display) { printf("\n"); }
return err_norm;
}
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;
}
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);
}
