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
}
