float test_rmat_comp_inv(struct FloatRMat ma2c_f, struct FloatRMat mb2c_f, int display)
{
struct FloatRMat ma2b_f;
float_rmat_comp_inv(&ma2b_f, &ma2c_f, &mb2c_f);
struct Int32RMat ma2c_i;
RMAT_BFP_OF_REAL(ma2c_i, ma2c_f);
struct Int32RMat mb2c_i;
RMAT_BFP_OF_REAL(mb2c_i, mb2c_f);
struct Int32RMat ma2b_i;
int32_rmat_comp_inv(&ma2b_i, &ma2c_i, &mb2c_i);
struct FloatRMat err;
RMAT_DIFF(err, ma2b_f, ma2b_i);
float norm_err = FLOAT_RMAT_NORM(err);
if (display) {
printf("rmap comp_inv\n");
DISPLAY_FLOAT_RMAT_AS_EULERS_DEG("a2cf", ma2b_f);
DISPLAY_INT32_RMAT_AS_EULERS_DEG("a2ci", ma2b_i);
}
return norm_err;
}
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_rmat_of_eulers_312(void) {
struct FloatEulers eul312_f;
EULERS_ASSIGN(eul312_f, RadOfDeg(45.), RadOfDeg(22.), RadOfDeg(0.));
DISPLAY_FLOAT_EULERS_DEG("eul312_f", eul312_f);
struct Int32Eulers eul312_i;
EULERS_BFP_OF_REAL(eul312_i, eul312_f);
DISPLAY_INT32_EULERS("eul312_i", eul312_i);
struct FloatRMat rmat_f;
FLOAT_RMAT_OF_EULERS_312(rmat_f, eul312_f);
DISPLAY_FLOAT_RMAT_AS_EULERS_DEG("rmat float", rmat_f);
struct Int32RMat rmat_i;
INT32_RMAT_OF_EULERS_312(rmat_i, eul312_i);
DISPLAY_INT32_RMAT_AS_EULERS_DEG("rmat int", rmat_i);
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 void test_3(void) {
/* Compute BODY to IMU eulers */
struct Int32Eulers b2i_e;
EULERS_ASSIGN(b2i_e, ANGLE_BFP_OF_REAL(RadOfDeg(10.66)), ANGLE_BFP_OF_REAL(RadOfDeg(-0.7)), ANGLE_BFP_OF_REAL(RadOfDeg(0.)));
DISPLAY_INT32_EULERS_AS_FLOAT_DEG("b2i_e", b2i_e);
/* Compute BODY to IMU quaternion */
struct Int32Quat b2i_q;
INT32_QUAT_OF_EULERS(b2i_q, b2i_e);
DISPLAY_INT32_QUAT_AS_EULERS_DEG("b2i_q", b2i_q);
// INT32_QUAT_NORMALIZE(b2i_q);
// DISPLAY_INT32_QUAT_AS_EULERS_DEG("b2i_q_n", b2i_q);
/* Compute BODY to IMU rotation matrix */
struct Int32RMat b2i_r;
INT32_RMAT_OF_EULERS(b2i_r, b2i_e);
// DISPLAY_INT32_RMAT("b2i_r", b2i_r);
DISPLAY_INT32_RMAT_AS_EULERS_DEG("b2i_r", b2i_r);
/* Compute LTP to IMU eulers */
struct Int32Eulers l2i_e;
EULERS_ASSIGN(l2i_e, ANGLE_BFP_OF_REAL(RadOfDeg(0.)), ANGLE_BFP_OF_REAL(RadOfDeg(20.)), ANGLE_BFP_OF_REAL(RadOfDeg(0.)));
DISPLAY_INT32_EULERS_AS_FLOAT_DEG("l2i_e", l2i_e);
/* Compute LTP to IMU quaternion */
struct Int32Quat l2i_q;
INT32_QUAT_OF_EULERS(l2i_q, l2i_e);
DISPLAY_INT32_QUAT_AS_EULERS_DEG("l2i_q", l2i_q);
/* Compute LTP to IMU rotation matrix */
struct Int32RMat l2i_r;
INT32_RMAT_OF_EULERS(l2i_r, l2i_e);
// DISPLAY_INT32_RMAT("l2i_r", l2i_r);
DISPLAY_INT32_RMAT_AS_EULERS_DEG("l2i_r", l2i_r);
/* again but from quaternion */
struct Int32RMat l2i_r2;
INT32_RMAT_OF_QUAT(l2i_r2, l2i_q);
// DISPLAY_INT32_RMAT("l2i_r2", l2i_r2);
DISPLAY_INT32_RMAT_AS_EULERS_DEG("l2i_r2", l2i_r2);
/* Compute LTP to BODY quaternion */
struct Int32Quat l2b_q;
INT32_QUAT_COMP_INV(l2b_q, b2i_q, l2i_q);
DISPLAY_INT32_QUAT_AS_EULERS_DEG("l2b_q", l2b_q);
/* Compute LTP to BODY rotation matrix */
struct Int32RMat l2b_r;
INT32_RMAT_COMP_INV(l2b_r, l2i_r, b2i_r);
// DISPLAY_INT32_RMAT("l2b_r", l2b_r);
DISPLAY_INT32_RMAT_AS_EULERS_DEG("l2b_r2", l2b_r);
/* again but from quaternion */
struct Int32RMat l2b_r2;
INT32_RMAT_OF_QUAT(l2b_r2, l2b_q);
// DISPLAY_INT32_RMAT("l2b_r2", l2b_r2);
DISPLAY_INT32_RMAT_AS_EULERS_DEG("l2b_r2", l2b_r2);
/* compute LTP to BODY eulers */
struct Int32Eulers l2b_e;
INT32_EULERS_OF_RMAT(l2b_e, l2b_r);
DISPLAY_INT32_EULERS_AS_FLOAT_DEG("l2b_e", l2b_e);
/* again but from quaternion */
struct Int32Eulers l2b_e2;
INT32_EULERS_OF_QUAT(l2b_e2, l2b_q);
DISPLAY_INT32_EULERS_AS_FLOAT_DEG("l2b_e2", l2b_e2);
}
static void test_3(void)
{
/* Compute BODY to IMU eulers */
struct Int32Eulers b2i_e;
EULERS_ASSIGN(b2i_e, ANGLE_BFP_OF_REAL(RadOfDeg(10.66)), ANGLE_BFP_OF_REAL(RadOfDeg(-0.7)),
ANGLE_BFP_OF_REAL(RadOfDeg(0.)));
DISPLAY_INT32_EULERS_AS_FLOAT_DEG("b2i_e", b2i_e);
/* Compute BODY to IMU quaternion */
struct Int32Quat b2i_q;
int32_quat_of_eulers(&b2i_q, &b2i_e);
DISPLAY_INT32_QUAT_AS_EULERS_DEG("b2i_q", b2i_q);
// int32_quat_normalize(&b2i_q);
// DISPLAY_INT32_QUAT_AS_EULERS_DEG("b2i_q_n", b2i_q);
/* Compute BODY to IMU rotation matrix */
struct Int32RMat b2i_r;
int32_rmat_of_eulers(&b2i_r, &b2i_e);
// DISPLAY_INT32_RMAT("b2i_r", b2i_r);
DISPLAY_INT32_RMAT_AS_EULERS_DEG("b2i_r", b2i_r);
/* Compute LTP to IMU eulers */
struct Int32Eulers l2i_e;
EULERS_ASSIGN(l2i_e, ANGLE_BFP_OF_REAL(RadOfDeg(0.)), ANGLE_BFP_OF_REAL(RadOfDeg(20.)),
ANGLE_BFP_OF_REAL(RadOfDeg(0.)));
DISPLAY_INT32_EULERS_AS_FLOAT_DEG("l2i_e", l2i_e);
/* Compute LTP to IMU quaternion */
struct Int32Quat l2i_q;
int32_quat_of_eulers(&l2i_q, &l2i_e);
DISPLAY_INT32_QUAT_AS_EULERS_DEG("l2i_q", l2i_q);
/* Compute LTP to IMU rotation matrix */
struct Int32RMat l2i_r;
int32_rmat_of_eulers(&l2i_r, &l2i_e);
// DISPLAY_INT32_RMAT("l2i_r", l2i_r);
DISPLAY_INT32_RMAT_AS_EULERS_DEG("l2i_r", l2i_r);
/* again but from quaternion */
struct Int32RMat l2i_r2;
int32_rmat_of_quat(&l2i_r2, &l2i_q);
// DISPLAY_INT32_RMAT("l2i_r2", l2i_r2);
DISPLAY_INT32_RMAT_AS_EULERS_DEG("l2i_r2", l2i_r2);
/* Compute LTP to BODY quaternion */
struct Int32Quat l2b_q;
int32_quat_comp_inv(&l2b_q, &b2i_q, &l2i_q);
DISPLAY_INT32_QUAT_AS_EULERS_DEG("l2b_q", l2b_q);
/* Compute LTP to BODY rotation matrix */
struct Int32RMat l2b_r;
int32_rmat_comp_inv(&l2b_r, &l2i_r, &b2i_r);
// DISPLAY_INT32_RMAT("l2b_r", l2b_r);
DISPLAY_INT32_RMAT_AS_EULERS_DEG("l2b_r2", l2b_r);
/* again but from quaternion */
struct Int32RMat l2b_r2;
int32_rmat_of_quat(&l2b_r2, &l2b_q);
// DISPLAY_INT32_RMAT("l2b_r2", l2b_r2);
DISPLAY_INT32_RMAT_AS_EULERS_DEG("l2b_r2", l2b_r2);
/* compute LTP to BODY eulers */
struct Int32Eulers l2b_e;
int32_eulers_of_rmat(&l2b_e, &l2b_r);
DISPLAY_INT32_EULERS_AS_FLOAT_DEG("l2b_e", l2b_e);
/* again but from quaternion */
struct Int32Eulers l2b_e2;
int32_eulers_of_quat(&l2b_e2, &l2b_q);
DISPLAY_INT32_EULERS_AS_FLOAT_DEG("l2b_e2", l2b_e2);
}
