void test1234(void) {
struct FloatEulers eul = {RadOfDeg(33.), RadOfDeg(25.), RadOfDeg(26.)};
struct FloatVect3 uz = { 0., 0., 1.};
struct FloatMat33 r_yaw;
FLOAT_RMAT_OF_AXIS_ANGLE(r_yaw, uz, eul.psi);
struct FloatVect3 uy = { 0., 1., 0.};
struct FloatMat33 r_pitch;
FLOAT_RMAT_OF_AXIS_ANGLE(r_pitch, uy, eul.theta);
struct FloatVect3 ux = { 1., 0., 0.};
struct FloatMat33 r_roll;
FLOAT_RMAT_OF_AXIS_ANGLE(r_roll, ux, eul.phi);
struct FloatMat33 r_tmp;
FLOAT_RMAT_COMP(r_tmp, r_yaw, r_roll);
struct FloatMat33 r_att;
FLOAT_RMAT_COMP(r_att, r_tmp, r_pitch);
DISPLAY_FLOAT_RMAT("r_att_ref ", r_att);
FLOAT_RMAT_OF_EULERS_312(r_att, eul);
DISPLAY_FLOAT_RMAT("r_att312 ", r_att);
}
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 FloatMat33 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 FloatMat33 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 FloatMat33 r_yaw;
FLOAT_RMAT_OF_AXIS_ANGLE(r_yaw, uz, eul.psi);
struct FloatVect3 uy = { 0., 1., 0.};
struct FloatMat33 r_pitch;
FLOAT_RMAT_OF_AXIS_ANGLE(r_pitch, uy, eul.theta);
struct FloatVect3 ux = { 1., 0., 0.};
struct FloatMat33 r_roll;
FLOAT_RMAT_OF_AXIS_ANGLE(r_roll, ux, eul.phi);
struct FloatMat33 r_yaw_pitch;
FLOAT_RMAT_COMP(r_yaw_pitch, r_yaw, r_pitch);
struct FloatMat33 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 FloatMat33 rmat1;
FLOAT_RMAT_OF_EULERS(rmat1, eul);
DISPLAY_FLOAT_RMAT_AS_EULERS_DEG("rmat1", rmat1);
DISPLAY_FLOAT_RMAT("rmat1", rmat1);
}
void ahrs_propagate(void) {
/* converts gyro to floating point */
struct FloatRates gyro_float;
RATES_FLOAT_OF_BFP(gyro_float, imu.gyro_prev);
/* unbias measurement */
RATES_DIFF(ahrs_float.imu_rate, gyro_float, ahrs_impl.gyro_bias);
const float dt = 1./512.;
/* add correction */
struct FloatRates omega;
RATES_SUM(omega, ahrs_float.imu_rate, ahrs_impl.rate_correction);
// DISPLAY_FLOAT_RATES("omega ", omega);
/* and zeros it */
FLOAT_RATES_ZERO(ahrs_impl.rate_correction);
/* first order integration of rotation matrix */
struct FloatRMat exp_omega_dt = {
{ 1. , dt*omega.r, -dt*omega.q,
-dt*omega.r, 1. , dt*omega.p,
dt*omega.q, -dt*omega.p, 1. }};
struct FloatRMat R_tdt;
FLOAT_RMAT_COMP(R_tdt, ahrs_float.ltp_to_imu_rmat, exp_omega_dt);
memcpy(&ahrs_float.ltp_to_imu_rmat, &R_tdt, sizeof(R_tdt));
float_rmat_reorthogonalize(&ahrs_float.ltp_to_imu_rmat);
// struct FloatRMat test;
// FLOAT_RMAT_COMP_INV(test, ahrs_float.ltp_to_imu_rmat, ahrs_float.ltp_to_imu_rmat);
// DISPLAY_FLOAT_RMAT("foo", test);
compute_imu_quat_and_euler_from_rmat();
compute_body_orientation_and_rates();
}
/** in place first order integration of a rotation matrix */
void float_rmat_integrate_fi(struct FloatRMat *rm, struct FloatRates *omega, float dt)
{
struct FloatRMat exp_omega_dt = {
{
1. , dt *omega->r, -dt *omega->q,
-dt *omega->r, 1. , dt *omega->p,
dt *omega->q, -dt *omega->p, 1.
}
};
struct FloatRMat R_tdt;
FLOAT_RMAT_COMP(R_tdt, *rm, exp_omega_dt);
memcpy(rm, &R_tdt, sizeof(R_tdt));
}
float test_rmat_comp(struct FloatRMat ma2b_f, struct FloatRMat mb2c_f, int display) {
struct FloatRMat ma2c_f;
FLOAT_RMAT_COMP(ma2c_f, ma2b_f, mb2c_f);
struct Int32RMat ma2b_i;
RMAT_BFP_OF_REAL(ma2b_i, ma2b_f);
struct Int32RMat mb2c_i;
RMAT_BFP_OF_REAL(mb2c_i, mb2c_f);
struct Int32RMat ma2c_i;
INT32_RMAT_COMP(ma2c_i, ma2b_i, mb2c_i);
struct FloatRMat err;
RMAT_DIFF(err, ma2c_f, ma2c_i);
float norm_err = FLOAT_RMAT_NORM(err);
if (display) {
printf("rmap comp\n");
DISPLAY_FLOAT_RMAT_AS_EULERS_DEG("a2cf", ma2c_f);
DISPLAY_INT32_RMAT_AS_EULERS_DEG("a2ci", ma2c_i);
}
return norm_err;
}
void gx3_packet_read_message(void) {
ahrs_impl.gx3_accel.x = bef(&ahrs_impl.gx3_packet.msg_buf[1]);
ahrs_impl.gx3_accel.y = bef(&ahrs_impl.gx3_packet.msg_buf[5]);
ahrs_impl.gx3_accel.z = bef(&ahrs_impl.gx3_packet.msg_buf[9]);
ahrs_impl.gx3_rate.p = bef(&ahrs_impl.gx3_packet.msg_buf[13]);
ahrs_impl.gx3_rate.q = bef(&ahrs_impl.gx3_packet.msg_buf[17]);
ahrs_impl.gx3_rate.r = bef(&ahrs_impl.gx3_packet.msg_buf[21]);
ahrs_impl.gx3_rmat.m[0] = bef(&ahrs_impl.gx3_packet.msg_buf[25]);
ahrs_impl.gx3_rmat.m[1] = bef(&ahrs_impl.gx3_packet.msg_buf[29]);
ahrs_impl.gx3_rmat.m[2] = bef(&ahrs_impl.gx3_packet.msg_buf[33]);
ahrs_impl.gx3_rmat.m[3] = bef(&ahrs_impl.gx3_packet.msg_buf[37]);
ahrs_impl.gx3_rmat.m[4] = bef(&ahrs_impl.gx3_packet.msg_buf[41]);
ahrs_impl.gx3_rmat.m[5] = bef(&ahrs_impl.gx3_packet.msg_buf[45]);
ahrs_impl.gx3_rmat.m[6] = bef(&ahrs_impl.gx3_packet.msg_buf[49]);
ahrs_impl.gx3_rmat.m[7] = bef(&ahrs_impl.gx3_packet.msg_buf[53]);
ahrs_impl.gx3_rmat.m[8] = bef(&ahrs_impl.gx3_packet.msg_buf[57]);
ahrs_impl.gx3_time = (uint32_t)(ahrs_impl.gx3_packet.msg_buf[61] << 24 |
ahrs_impl.gx3_packet.msg_buf[62] << 16 | ahrs_impl.gx3_packet.msg_buf[63] << 8 | ahrs_impl.gx3_packet.msg_buf[64]);
ahrs_impl.gx3_chksm = GX3_CHKSM(ahrs_impl.gx3_packet.msg_buf);
ahrs_impl.gx3_freq = 62500.0 / (float)(ahrs_impl.gx3_time - ahrs_impl.gx3_ltime);
ahrs_impl.gx3_ltime = ahrs_impl.gx3_time;
// Acceleration
VECT3_SMUL(ahrs_impl.gx3_accel, ahrs_impl.gx3_accel, 9.80665); // Convert g into m/s2
ACCELS_BFP_OF_REAL(imu.accel, ahrs_impl.gx3_accel); // for backwards compatibility with fixed point interface
imuf.accel = ahrs_impl.gx3_accel;
// Rates
struct FloatRates body_rate;
imuf.gyro = ahrs_impl.gx3_rate;
/* compute body rates */
struct FloatRMat *body_to_imu_rmat = orientationGetRMat_f(&imuf.body_to_imu);
FLOAT_RMAT_TRANSP_RATEMULT(body_rate, *body_to_imu_rmat, imuf.gyro);
/* Set state */
stateSetBodyRates_f(&body_rate);
// Attitude
struct FloatRMat ltp_to_body_rmat;
FLOAT_RMAT_COMP(ltp_to_body_rmat, ahrs_impl.gx3_rmat, *body_to_imu_rmat);
#if AHRS_USE_GPS_HEADING && USE_GPS
struct FloatEulers ltp_to_body_eulers;
float_eulers_of_rmat(<p_to_body_eulers, <p_to_body_rmat);
float course_f = (float)DegOfRad(gps.course / 1e7);
if (course_f > 180.0) {
course_f -= 360.0;
}
ltp_to_body_eulers.psi = (float)RadOfDeg(course_f);
stateSetNedToBodyEulers_f(<p_to_body_eulers);
#else // !AHRS_USE_GPS_HEADING
#ifdef IMU_MAG_OFFSET
struct FloatEulers ltp_to_body_eulers;
float_eulers_of_rmat(<p_to_body_eulers, <p_to_body_rmat);
ltp_to_body_eulers.psi -= ahrs_impl.mag_offset;
stateSetNedToBodyEulers_f(<p_to_body_eulers);
#else
stateSetNedToBodyRMat_f(<p_to_body_rmat);
#endif // IMU_MAG_OFFSET
#endif // !AHRS_USE_GPS_HEADING
}
// Called after receipt of valid message to
void xsens_parse_msg( uint8_t xsens_id ) {
uint8_t buf_slot = xsens_msg_buf_ci[xsens_id];
if (msg_id[xsens_id][buf_slot] == XSENS_ReqOutputModeAck_ID) {
xsens_output_mode[xsens_id] = XSENS_ReqOutputModeAck_mode(xsens_msg_buf[xsens_id]);
}
else if (msg_id[xsens_id][buf_slot] == XSENS_Error_ID) {
xsens_errorcode[xsens_id] = XSENS_Error_errorcode(xsens_msg_buf[xsens_id]);
}
else if (msg_id[xsens_id][buf_slot] == XSENS_MTData_ID) {
uint8_t offset = 0;
// test RAW modes else calibrated modes
if (XSENS_MASK_RAWInertial(xsens_output_mode[xsens_id])){// || (XSENS_MASK_RAWGPS(xsens2_output_mode))) {
xsens_raw_accel_x[xsens_id] = XSENS_DATA_RAWInertial_accX(xsens_msg_buf[xsens_id][buf_slot],offset);
xsens_raw_accel_y[xsens_id] = XSENS_DATA_RAWInertial_accY(xsens_msg_buf[xsens_id][buf_slot],offset);
xsens_raw_accel_z[xsens_id] = XSENS_DATA_RAWInertial_accZ(xsens_msg_buf[xsens_id][buf_slot],offset);
xsens_raw_gyro_x[xsens_id] = XSENS_DATA_RAWInertial_gyrX(xsens_msg_buf[xsens_id][buf_slot],offset);
xsens_raw_gyro_y[xsens_id] = XSENS_DATA_RAWInertial_gyrY(xsens_msg_buf[xsens_id][buf_slot],offset);
xsens_raw_gyro_z[xsens_id] = XSENS_DATA_RAWInertial_gyrZ(xsens_msg_buf[xsens_id][buf_slot],offset);
xsens_raw_mag_x[xsens_id] = XSENS_DATA_RAWInertial_magX(xsens_msg_buf[xsens_id][buf_slot],offset);
xsens_raw_mag_y[xsens_id] = XSENS_DATA_RAWInertial_magY(xsens_msg_buf[xsens_id][buf_slot],offset);
xsens_raw_mag_z[xsens_id] = XSENS_DATA_RAWInertial_magZ(xsens_msg_buf[xsens_id][buf_slot],offset);
}
if (XSENS_MASK_Temp(xsens_output_mode[xsens_id])) {
offset += XSENS_DATA_Temp_LENGTH;
}
if (XSENS_MASK_Calibrated(xsens_output_mode[xsens_id])) {
if (XSENS_MASK_AccOut(xsens_output_settings[xsens_id])) {
xsens_accel_x[xsens_id] = XSENS_DATA_Calibrated_accX(xsens_msg_buf[xsens_id][buf_slot],offset);
xsens_accel_y[xsens_id] = XSENS_DATA_Calibrated_accY(xsens_msg_buf[xsens_id][buf_slot],offset);
xsens_accel_z[xsens_id] = XSENS_DATA_Calibrated_accZ(xsens_msg_buf[xsens_id][buf_slot],offset);
}
if (XSENS_MASK_GyrOut(xsens_output_settings[xsens_id])) {
xsens_gyro_x[xsens_id] = XSENS_DATA_Calibrated_gyrX(xsens_msg_buf[xsens_id][buf_slot],offset);
xsens_gyro_y[xsens_id] = XSENS_DATA_Calibrated_gyrY(xsens_msg_buf[xsens_id][buf_slot],offset);
xsens_gyro_z[xsens_id] = XSENS_DATA_Calibrated_gyrZ(xsens_msg_buf[xsens_id][buf_slot],offset);
}
if (XSENS_MASK_MagOut(xsens_output_settings[xsens_id])) {
xsens_mag_x[xsens_id] = XSENS_DATA_Calibrated_magX(xsens_msg_buf[xsens_id][buf_slot],offset);
xsens_mag_y[xsens_id] = XSENS_DATA_Calibrated_magY(xsens_msg_buf[xsens_id][buf_slot],offset);
xsens_mag_z[xsens_id] = XSENS_DATA_Calibrated_magZ(xsens_msg_buf[xsens_id][buf_slot],offset);
float pitch = xsens_phi[xsens_id];
float roll = -xsens_theta[xsens_id];
float tilt_comp_x = xsens_mag_x[xsens_id] * cos(pitch)
+ xsens_mag_y[xsens_id] * sin(roll) * sin(pitch)
- xsens_mag_z[xsens_id] * cos(roll) * sin(pitch);
float tilt_comp_y = xsens_mag_y[xsens_id] * cos(roll)
+ xsens_mag_z[xsens_id] * sin(roll);
xsens_mag_heading[xsens_id] = -atan2( tilt_comp_y, tilt_comp_x);
}
offset += XSENS_DATA_Calibrated_LENGTH;
}
if (XSENS_MASK_Orientation(xsens_output_mode[xsens_id])) {
if (XSENS_MASK_OrientationMode(xsens_output_settings[xsens_id]) == 0x0) {
offset += XSENS_DATA_Quaternion_LENGTH;
}
if (XSENS_MASK_OrientationMode(xsens_output_settings[xsens_id]) == 0x1) {
xsens_phi[xsens_id] = XSENS_DATA_Euler_roll(xsens_msg_buf[xsens_id][buf_slot],offset) * M_PI/180;
xsens_theta[xsens_id] =XSENS_DATA_Euler_pitch(xsens_msg_buf[xsens_id][buf_slot],offset) * M_PI/180;
xsens_psi[xsens_id] = XSENS_DATA_Euler_yaw(xsens_msg_buf[xsens_id][buf_slot],offset) * M_PI/180;
offset += XSENS_DATA_Euler_LENGTH;
}
if (XSENS_MASK_OrientationMode(xsens_output_settings[xsens_id]) == 0x2) {
xsens_rmat[xsens_id].m[0] = XSENS_DATA_Matrix_a(xsens_msg_buf[xsens_id][buf_slot],offset);
xsens_rmat[xsens_id].m[1] = XSENS_DATA_Matrix_b(xsens_msg_buf[xsens_id][buf_slot],offset);
xsens_rmat[xsens_id].m[2] = XSENS_DATA_Matrix_c(xsens_msg_buf[xsens_id][buf_slot],offset);
xsens_rmat[xsens_id].m[3] = XSENS_DATA_Matrix_d(xsens_msg_buf[xsens_id][buf_slot],offset);
xsens_rmat[xsens_id].m[4] = XSENS_DATA_Matrix_e(xsens_msg_buf[xsens_id][buf_slot],offset);
xsens_rmat[xsens_id].m[5] = XSENS_DATA_Matrix_f(xsens_msg_buf[xsens_id][buf_slot],offset);
xsens_rmat[xsens_id].m[6] = XSENS_DATA_Matrix_g(xsens_msg_buf[xsens_id][buf_slot],offset);
xsens_rmat[xsens_id].m[7] = XSENS_DATA_Matrix_h(xsens_msg_buf[xsens_id][buf_slot],offset);
xsens_rmat[xsens_id].m[8] = XSENS_DATA_Matrix_i(xsens_msg_buf[xsens_id][buf_slot],offset);
/* // FLOAT_RMAT_COMP_INV(xsens_rmat_adj[xsens_id], xsens_rmat_neutral[xsens_id], xsens_rmat[xsens_id]); */
struct FloatRMat xsens_rmat_temp[XSENS_COUNT];
FLOAT_RMAT_INV(xsens_rmat_temp[xsens_id], xsens_rmat[xsens_id]);
FLOAT_RMAT_COMP(xsens_rmat_adj[xsens_id], xsens_rmat_temp[xsens_id], xsens_rmat_neutral[xsens_id]);
xsens_phi[xsens_id] = -atan2(xsens_rmat_adj[xsens_id].m[7], xsens_rmat_adj[xsens_id].m[8]);
xsens_theta[xsens_id] = asin(xsens_rmat_adj[xsens_id].m[6]);
xsens_psi[xsens_id] = atan2(xsens_rmat_adj[xsens_id].m[3], xsens_rmat_adj[xsens_id].m[0]);
xsens_psi[xsens_id] -= RadOfDeg(xsens_psi_offset[xsens_id]);
/* FLOAT_RMAT_COMP(xsens_rmat_adj[xsens_id], xsens_rmat_neutral[xsens_id], xsens_rmat[xsens_id]); */
/* // Calculate roll, pitch, yaw from rotation matrix ( p 31-33 MTi-G USer Man and Tech Doc) */
/* xsens_phi[xsens_id] = -atan2 (xsens_rmat_adj[xsens_id].m[7], xsens_rmat_adj[xsens_id].m[8]); */
/* xsens_theta[xsens_id] = asin (xsens_rmat_adj[xsens_id].m[6]); */
/* xsens_psi[xsens_id] = atan2 (xsens_rmat_adj[xsens_id].m[3], xsens_rmat_adj[xsens_id].m[0]); */
offset += XSENS_DATA_Matrix_LENGTH;
}
}
if (XSENS_MASK_Status(xsens_output_mode[xsens_id])) {
xsens_msg_status[xsens_id] = XSENS_DATA_Status_status(xsens_msg_buf[xsens_id][buf_slot],offset);
xsens_mode[xsens_id] = xsens_msg_status[xsens_id];
offset += XSENS_DATA_Status_LENGTH;
}
if (XSENS_MASK_TimeStamp(xsens_output_settings[xsens_id])) {
uint16_t ts = XSENS_DATA_TimeStamp_ts(xsens_msg_buf[xsens_id][buf_slot],offset);
if (xsens_time_stamp[xsens_id] + 1 != ts)
//xsens_err_count[xsens_id]++;
xsens_time_stamp[xsens_id] = ts;
offset += XSENS_DATA_TimeStamp_LENGTH;
}
}
}
