void ahrs_fc_update_mag_2d_dumb(struct Int32Vect3 *mag) { /* project mag on local tangeant plane */ struct FloatEulers ltp_to_imu_euler; float_eulers_of_rmat(<p_to_imu_euler, &ahrs_fc.ltp_to_imu_rmat); struct FloatVect3 magf; MAGS_FLOAT_OF_BFP(magf, *mag); const float cphi = cosf(ltp_to_imu_euler.phi); const float sphi = sinf(ltp_to_imu_euler.phi); const float ctheta = cosf(ltp_to_imu_euler.theta); const float stheta = sinf(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_fc.ltp_to_imu_rmat, 0, 0) * me + RMAT_ELMT(ahrs_fc.ltp_to_imu_rmat, 1, 0) * mn; const struct FloatVect3 r2 = {RMAT_ELMT(ahrs_fc.ltp_to_imu_rmat, 2, 0), RMAT_ELMT(ahrs_fc.ltp_to_imu_rmat, 2, 1), RMAT_ELMT(ahrs_fc.ltp_to_imu_rmat, 2, 2) }; const float mag_rate_update_gain = 2.5; RATES_ADD_SCALED_VECT(ahrs_fc.rate_correction, r2, (mag_rate_update_gain * res_norm)); const float mag_bias_update_gain = -2.5e-4; RATES_ADD_SCALED_VECT(ahrs_fc.gyro_bias, r2, (mag_bias_update_gain * res_norm)); }
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; }
void orientationCalcEulers_f(struct OrientationReps* orientation) { if (bit_is_set(orientation->status, ORREP_EULER_F)) { return; } if (bit_is_set(orientation->status, ORREP_EULER_I)) { EULERS_FLOAT_OF_BFP(orientation->eulers_f, orientation->eulers_i); } else if (bit_is_set(orientation->status, ORREP_RMAT_F)) { float_eulers_of_rmat(&(orientation->eulers_f), &(orientation->rmat_f)); } else if (bit_is_set(orientation->status, ORREP_QUAT_F)) { float_eulers_of_quat(&(orientation->eulers_f), &(orientation->quat_f)); } else if (bit_is_set(orientation->status, ORREP_RMAT_I)) { RMAT_FLOAT_OF_BFP(orientation->rmat_f, orientation->rmat_i); SetBit(orientation->status, ORREP_RMAT_F); float_eulers_of_rmat(&(orientation->eulers_f), &(orientation->rmat_f)); } else if (bit_is_set(orientation->status, ORREP_QUAT_I)) { QUAT_FLOAT_OF_BFP(orientation->quat_f, orientation->quat_i); SetBit(orientation->status, ORREP_QUAT_F); float_eulers_of_quat(&(orientation->eulers_f), &(orientation->quat_f)); } /* set bit to indicate this representation is computed */ SetBit(orientation->status, ORREP_EULER_F); }
float test_quat_of_rmat(void) { // struct FloatEulers eul = {-0.280849, 0.613423, -1.850440}; struct FloatEulers eul = {RadOfDeg(0.131579), RadOfDeg(-62.397659), RadOfDeg(-110.470299)}; // struct FloatEulers eul = {RadOfDeg(0.13), RadOfDeg(180.), RadOfDeg(-61.)}; struct FloatRMat rm; float_rmat_of_eulers(&rm, &eul); DISPLAY_FLOAT_RMAT_AS_EULERS_DEG("rmat", rm); struct FloatQuat q; float_quat_of_rmat(&q, &rm); DISPLAY_FLOAT_QUAT("q_of_rm ", q); DISPLAY_FLOAT_QUAT_AS_EULERS_DEG("q_of_rm ", q); struct FloatQuat qref; float_quat_of_eulers(&qref, &eul); DISPLAY_FLOAT_QUAT("q_of_euler", qref); DISPLAY_FLOAT_QUAT_AS_EULERS_DEG("q_of_euler", qref); printf("\n\n\n"); struct FloatRMat r_att; struct FloatEulers e312 = { eul.phi, eul.theta, eul.psi }; float_rmat_of_eulers_312(&r_att, &e312); DISPLAY_FLOAT_RMAT("r_att ", r_att); DISPLAY_FLOAT_RMAT_AS_EULERS_DEG("r_att ", r_att); struct FloatQuat q_att; float_quat_of_rmat(&q_att, &r_att); struct FloatEulers e_att; float_eulers_of_rmat(&e_att, &r_att); DISPLAY_FLOAT_EULERS_DEG("of rmat", e_att); float_eulers_of_quat(&e_att, &q_att); DISPLAY_FLOAT_EULERS_DEG("of quat", e_att); return 0.; }
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 }