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 FloatRMat 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 FloatRMat 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.;
}
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);
}
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;
}
void ahrs_fc_propagate(struct Int32Rates *gyro, float dt)
{
/* converts gyro to floating point */
struct FloatRates gyro_float;
RATES_FLOAT_OF_BFP(gyro_float, *gyro);
/* unbias measurement */
RATES_SUB(gyro_float, ahrs_fc.gyro_bias);
#ifdef AHRS_PROPAGATE_LOW_PASS_RATES
const float alpha = 0.1;
FLOAT_RATES_LIN_CMB(ahrs_fc.imu_rate, ahrs_fc.imu_rate, (1. - alpha), gyro_float, alpha);
#else
RATES_COPY(ahrs_fc.imu_rate, gyro_float);
#endif
/* add correction */
struct FloatRates omega;
RATES_SUM(omega, gyro_float, ahrs_fc.rate_correction);
/* and zeros it */
FLOAT_RATES_ZERO(ahrs_fc.rate_correction);
#if AHRS_PROPAGATE_RMAT
float_rmat_integrate_fi(&ahrs_fc.ltp_to_imu_rmat, &omega, dt);
float_rmat_reorthogonalize(&ahrs_fc.ltp_to_imu_rmat);
float_quat_of_rmat(&ahrs_fc.ltp_to_imu_quat, &ahrs_fc.ltp_to_imu_rmat);
#endif
#if AHRS_PROPAGATE_QUAT
float_quat_integrate(&ahrs_fc.ltp_to_imu_quat, &omega, dt);
float_quat_normalize(&ahrs_fc.ltp_to_imu_quat);
float_rmat_of_quat(&ahrs_fc.ltp_to_imu_rmat, &ahrs_fc.ltp_to_imu_quat);
#endif
// increase accel and mag propagation counters
ahrs_fc.accel_cnt++;
ahrs_fc.mag_cnt++;
}
