static void test_2(void) { struct Int32Vect3 v1 = { 5000, 5000, 5000 }; DISPLAY_INT32_VECT3("v1", v1); struct FloatEulers euler_f = { RadOfDeg(45.), RadOfDeg(0.), RadOfDeg(0.)}; DISPLAY_FLOAT_EULERS("euler_f", euler_f); struct Int32Eulers euler_i; EULERS_BFP_OF_REAL(euler_i, euler_f); DISPLAY_INT32_EULERS("euler_i", euler_i); struct Int32Quat quat_i; int32_quat_of_eulers(&quat_i, &euler_i); DISPLAY_INT32_QUAT("quat_i", quat_i); int32_quat_normalize(&quat_i); DISPLAY_INT32_QUAT("quat_i_n", quat_i); struct Int32Vect3 v2; int32_quat_vmult(&v2, &quat_i, &v1); DISPLAY_INT32_VECT3("v2", v2); struct Int32RMat rmat_i; int32_rmat_of_quat(&rmat_i, &quat_i); DISPLAY_INT32_RMAT("rmat_i", rmat_i); struct Int32Vect3 v3; INT32_RMAT_VMULT(v3, rmat_i, v1); DISPLAY_INT32_VECT3("v3", v3); struct Int32RMat rmat_i2; int32_rmat_of_eulers(&rmat_i2, &euler_i); DISPLAY_INT32_RMAT("rmat_i2", rmat_i2); struct Int32Vect3 v4; INT32_RMAT_VMULT(v4, rmat_i2, v1); DISPLAY_INT32_VECT3("v4", v4); struct FloatQuat quat_f; float_quat_of_eulers(&quat_f, &euler_f); DISPLAY_FLOAT_QUAT("quat_f", quat_f); struct FloatVect3 v5; VECT3_COPY(v5, v1); DISPLAY_FLOAT_VECT3("v5", v5); struct FloatVect3 v6; float_quat_vmult(&v6, &quat_f, &v5); DISPLAY_FLOAT_VECT3("v6", v6); }
static void test_2(void) { struct Int32Vect3 v1 = { 5000, 5000, 5000 }; DISPLAY_INT32_VECT3("v1", v1); struct FloatEulers euler_f = { RadOfDeg(45.), RadOfDeg(0.), RadOfDeg(0.)}; DISPLAY_FLOAT_EULERS("euler_f", euler_f); struct Int32Eulers euler_i; EULERS_BFP_OF_REAL(euler_i, euler_f); DISPLAY_INT32_EULERS("euler_i", euler_i); struct Int32Quat quat_i; INT32_QUAT_OF_EULERS(quat_i, euler_i); DISPLAY_INT32_QUAT("quat_i", quat_i); INT32_QUAT_NORMALIZE(quat_i); DISPLAY_INT32_QUAT("quat_i_n", quat_i); struct Int32Vect3 v2; INT32_QUAT_VMULT(v2, quat_i, v1); DISPLAY_INT32_VECT3("v2", v2); struct Int32RMat rmat_i; INT32_RMAT_OF_QUAT(rmat_i, quat_i); DISPLAY_INT32_RMAT("rmat_i", rmat_i); struct Int32Vect3 v3; INT32_RMAT_VMULT(v3, rmat_i, v1); DISPLAY_INT32_VECT3("v3", v3); struct Int32RMat rmat_i2; INT32_RMAT_OF_EULERS(rmat_i2, euler_i); DISPLAY_INT32_RMAT("rmat_i2", rmat_i2); struct Int32Vect3 v4; INT32_RMAT_VMULT(v4, rmat_i2, v1); DISPLAY_INT32_VECT3("v4", v4); struct FloatQuat quat_f; FLOAT_QUAT_OF_EULERS(quat_f, euler_f); DISPLAY_FLOAT_QUAT("quat_f", quat_f); struct FloatVect3 v5; VECT3_COPY(v5, v1); DISPLAY_FLOAT_VECT3("v5", v5); struct FloatVect3 v6; FLOAT_QUAT_VMULT(v6, quat_f, v5); DISPLAY_FLOAT_VECT3("v6", v6); }
void test_of_axis_angle(void) { struct FloatVect3 axis = { 0., 1., 0.}; FLOAT_VECT3_NORMALIZE(axis); DISPLAY_FLOAT_VECT3("axis", axis); const float angle = RadOfDeg(30.); printf("angle %f\n", DegOfRad(angle)); struct FloatQuat my_q; FLOAT_QUAT_OF_AXIS_ANGLE(my_q, axis, angle); DISPLAY_FLOAT_QUAT_AS_EULERS_DEG("quat", my_q); struct FloatRMat my_r1; float_rmat_of_quat(&my_r1, &my_q); DISPLAY_FLOAT_RMAT_AS_EULERS_DEG("rmat1", my_r1); DISPLAY_FLOAT_RMAT("rmat1", my_r1); struct FloatRMat my_r; FLOAT_RMAT_OF_AXIS_ANGLE(my_r, axis, angle); DISPLAY_FLOAT_RMAT_AS_EULERS_DEG("rmat", my_r); DISPLAY_FLOAT_RMAT("rmat", my_r); printf("\n"); struct FloatEulers eul = {RadOfDeg(30.), RadOfDeg(30.), 0.}; struct FloatVect3 uz = { 0., 0., 1.}; struct FloatRMat r_yaw; FLOAT_RMAT_OF_AXIS_ANGLE(r_yaw, uz, eul.psi); struct FloatVect3 uy = { 0., 1., 0.}; struct FloatRMat r_pitch; FLOAT_RMAT_OF_AXIS_ANGLE(r_pitch, uy, eul.theta); struct FloatVect3 ux = { 1., 0., 0.}; struct FloatRMat r_roll; FLOAT_RMAT_OF_AXIS_ANGLE(r_roll, ux, eul.phi); struct FloatRMat r_yaw_pitch; float_rmat_comp(&r_yaw_pitch, &r_yaw, &r_pitch); struct FloatRMat r_yaw_pitch_roll; float_rmat_comp(&r_yaw_pitch_roll, &r_yaw_pitch, &r_roll); DISPLAY_FLOAT_RMAT_AS_EULERS_DEG("rmat", r_yaw_pitch_roll); DISPLAY_FLOAT_RMAT("rmat", r_yaw_pitch_roll); DISPLAY_FLOAT_EULERS_DEG("eul", eul); struct FloatRMat rmat1; float_rmat_of_eulers(&rmat1, &eul); DISPLAY_FLOAT_RMAT_AS_EULERS_DEG("rmat1", rmat1); DISPLAY_FLOAT_RMAT("rmat1", rmat1); }
float test_quat(void) { struct FloatVect3 u = { 1., 2., 3.}; FLOAT_VECT3_NORMALIZE(u); float angle = RadOfDeg(30.); struct FloatQuat q; FLOAT_QUAT_OF_AXIS_ANGLE(q, u, angle); DISPLAY_FLOAT_QUAT("q ", q); struct FloatQuat iq; FLOAT_QUAT_INVERT(iq, q); DISPLAY_FLOAT_QUAT("iq", iq); struct FloatQuat q1; FLOAT_QUAT_COMP(q1, q, iq); DISPLAY_FLOAT_QUAT("q1", q1); struct FloatQuat q2; FLOAT_QUAT_COMP(q2, q, iq); DISPLAY_FLOAT_QUAT("q2", q2); struct FloatQuat qe; QUAT_EXPLEMENTARY(qe, q); DISPLAY_FLOAT_QUAT("qe", qe); struct FloatVect3 a = { 2., 1., 3.}; DISPLAY_FLOAT_VECT3("a ", a); struct FloatVect3 a1; FLOAT_QUAT_VMULT(a1, q, a); DISPLAY_FLOAT_VECT3("a1", a1); struct FloatVect3 a2; FLOAT_QUAT_VMULT(a2, qe, a); DISPLAY_FLOAT_VECT3("a2", a2); return 0.; }
void ahrs_update_mag2(void) { const struct FloatVect3 expected_ltp = {AHRS_H_X, AHRS_H_Y, AHRS_H_Z}; struct FloatVect3 expected_imu; FLOAT_RMAT_VECT3_MUL(expected_imu, ahrs_float.ltp_to_imu_rmat, expected_ltp); struct FloatVect3 measured_imu; MAGS_FLOAT_OF_BFP(measured_imu, imu.mag); struct FloatVect3 residual; FLOAT_VECT3_CROSS_PRODUCT(residual, measured_imu, expected_imu); // FLOAT_VECT3_DIFF(residual, expected_imu, measured_imu); DISPLAY_FLOAT_VECT3(" expected", expected_imu); DISPLAY_FLOAT_VECT3(" measured", measured_imu); DISPLAY_FLOAT_VECT3("residual", residual); const float mag_rate_update_gain = 2.5; FLOAT_RATES_ADD_SCALED_VECT(ahrs_impl.rate_correction, residual, mag_rate_update_gain); }
void ahrs_update_mag(void) { /* project mag on local tangeant plane */ struct FloatVect3 magf; MAGS_FLOAT_OF_BFP(magf, imu.mag); const float cphi = cosf(ahrs_float.ltp_to_imu_euler.phi); const float sphi = sinf(ahrs_float.ltp_to_imu_euler.phi); const float ctheta = cosf(ahrs_float.ltp_to_imu_euler.theta); const float stheta = sinf(ahrs_float.ltp_to_imu_euler.theta); const float mn = ctheta * magf.x + sphi*stheta*magf.y + cphi*stheta*magf.z; const float me = 0. * magf.x + cphi *magf.y - sphi *magf.z; const float res_norm = -RMAT_ELMT(ahrs_float.ltp_to_imu_rmat, 0,0)*me + RMAT_ELMT(ahrs_float.ltp_to_imu_rmat, 1,0)*mn; printf("res norm %f\n", res_norm); struct FloatVect3* r2 = (struct FloatVect3*)(&RMAT_ELMT(ahrs_float.ltp_to_imu_rmat, 2,0)); DISPLAY_FLOAT_VECT3("r2", (*r2)); const float mag_rate_update_gain = 2.5; FLOAT_RATES_ADD_SCALED_VECT(ahrs_impl.rate_correction, (*r2), (mag_rate_update_gain*res_norm)); DISPLAY_FLOAT_RATES("corr", ahrs_impl.rate_correction); const float mag_bias_update_gain = -2.5e-4; FLOAT_RATES_ADD_SCALED_VECT(ahrs_impl.gyro_bias, (*r2), (mag_bias_update_gain*res_norm)); /* FIXME: saturate bias */ }