float test_quat_comp_inv(struct FloatQuat qa2c_f, struct FloatQuat qb2c_f, int display) { struct FloatQuat qa2b_f; float_quat_comp_inv(&qa2b_f, &qa2c_f, &qb2c_f); struct Int32Quat qa2c_i; QUAT_BFP_OF_REAL(qa2c_i, qa2c_f); struct Int32Quat qb2c_i; QUAT_BFP_OF_REAL(qb2c_i, qb2c_f); struct Int32Quat qa2b_i; int32_quat_comp_inv(&qa2b_i, &qa2c_i, &qb2c_i); struct FloatQuat err; QUAT_DIFF(err, qa2b_f, qa2b_i); float norm_err = FLOAT_QUAT_NORM(err); if (display) { printf("quat comp_inv\n"); DISPLAY_FLOAT_QUAT_AS_EULERS_DEG("a2bf", qa2b_f); DISPLAY_INT32_QUAT_AS_EULERS_DEG("a2bi", qa2b_i); } 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); }
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); }