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; }
static void test_4_int(void) { printf("euler to quat to euler - int\n"); /* initial euler angles */ struct Int32Eulers _e; EULERS_ASSIGN(_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("euler orig ", _e); /* transform to quaternion */ struct Int32Quat _q; INT32_QUAT_OF_EULERS(_q, _e); DISPLAY_INT32_QUAT_AS_EULERS_DEG("quat1 ", _q); // INT32_QUAT_NORMALIZE(_q); // DISPLAY_INT32_QUAT_2("_q_n", _q); /* back to eulers */ struct Int32Eulers _e2; INT32_EULERS_OF_QUAT(_e2, _q); DISPLAY_INT32_EULERS_AS_FLOAT_DEG("back to euler ", _e2); }
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; }
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); }