static void test_6(void) {
printf("\n");
struct FloatEulers ea2b;
EULERS_ASSIGN(ea2b, RadOfDeg(45.), RadOfDeg(22.), RadOfDeg(0.));
DISPLAY_FLOAT_EULERS_DEG("ea2b", ea2b);
struct FloatEulers eb2c;
EULERS_ASSIGN(eb2c, RadOfDeg(0.), RadOfDeg(0.), RadOfDeg(90.));
DISPLAY_FLOAT_EULERS_DEG("eb2c", eb2c);
struct FloatRMat fa2b;
FLOAT_RMAT_OF_EULERS(fa2b, ea2b);
struct FloatRMat fb2c;
FLOAT_RMAT_OF_EULERS(fb2c, eb2c);
printf("\n");
test_rmat_comp(fa2b, fb2c, 1);
struct FloatQuat qa2b;
FLOAT_QUAT_OF_EULERS(qa2b, ea2b);
struct FloatQuat qb2c;
FLOAT_QUAT_OF_EULERS(qb2c, eb2c);
printf("\n");
test_quat_comp(qa2b, qb2c, 1);
printf("\n");
}
void orientationCalcRMat_f(struct OrientationReps* orientation) {
if (bit_is_set(orientation->status, ORREP_RMAT_F))
return;
if (bit_is_set(orientation->status, ORREP_RMAT_I)) {
RMAT_FLOAT_OF_BFP(orientation->rmat_f, orientation->rmat_i);
}
else if (bit_is_set(orientation->status, ORREP_QUAT_F)) {
FLOAT_RMAT_OF_QUAT(orientation->rmat_f, orientation->quat_f);
}
else if (bit_is_set(orientation->status, ORREP_EULER_F)) {
FLOAT_RMAT_OF_EULERS(orientation->rmat_f, orientation->eulers_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_RMAT_OF_QUAT(orientation->rmat_f, orientation->quat_f);
}
else if (bit_is_set(orientation->status, ORREP_EULER_I)) {
EULERS_FLOAT_OF_BFP(orientation->eulers_f, orientation->eulers_i);
SetBit(orientation->status, ORREP_EULER_F);
FLOAT_RMAT_OF_EULERS(orientation->rmat_f, orientation->eulers_f);
}
/* set bit to indicate this representation is computed */
SetBit(orientation->status, ORREP_RMAT_F);
}
static void test_7(void) {
printf("\n");
struct FloatEulers ea2c;
EULERS_ASSIGN(ea2c, RadOfDeg(29.742755), RadOfDeg(-40.966522), RadOfDeg(69.467265));
DISPLAY_FLOAT_EULERS_DEG("ea2c", ea2c);
struct FloatEulers eb2c;
EULERS_ASSIGN(eb2c, RadOfDeg(0.), RadOfDeg(0.), RadOfDeg(90.));
DISPLAY_FLOAT_EULERS_DEG("eb2c", eb2c);
struct FloatRMat fa2c;
FLOAT_RMAT_OF_EULERS(fa2c, ea2c);
struct FloatRMat fb2c;
FLOAT_RMAT_OF_EULERS(fb2c, eb2c);
printf("\n");
test_rmat_comp_inv(fa2c, fb2c, 1);
struct FloatQuat qa2c;
FLOAT_QUAT_OF_EULERS(qa2c, ea2c);
struct FloatQuat qb2c;
FLOAT_QUAT_OF_EULERS(qb2c, eb2c);
printf("\n");
test_quat_comp_inv(qa2c, qb2c, 1);
printf("\n");
}
void ahrs_init(void) {
ahrs.status = AHRS_UNINIT;
ahrs_impl.ltp_vel_norm_valid = FALSE;
ahrs_impl.heading_aligned = FALSE;
/* Initialises IMU alignement */
struct FloatEulers body_to_imu_euler =
{IMU_BODY_TO_IMU_PHI, IMU_BODY_TO_IMU_THETA, IMU_BODY_TO_IMU_PSI};
FLOAT_QUAT_OF_EULERS(ahrs_impl.body_to_imu_quat, body_to_imu_euler);
FLOAT_RMAT_OF_EULERS(ahrs_impl.body_to_imu_rmat, body_to_imu_euler);
/* Set ltp_to_imu so that body is zero */
QUAT_COPY(ahrs_impl.ltp_to_imu_quat, ahrs_impl.body_to_imu_quat);
RMAT_COPY(ahrs_impl.ltp_to_imu_rmat, ahrs_impl.body_to_imu_rmat);
FLOAT_RATES_ZERO(ahrs_impl.imu_rate);
#if AHRS_GRAVITY_UPDATE_COORDINATED_TURN
ahrs_impl.correct_gravity = TRUE;
#else
ahrs_impl.correct_gravity = FALSE;
#endif
VECT3_ASSIGN(ahrs_impl.mag_h, AHRS_H_X, AHRS_H_Y, AHRS_H_Z);
}
static inline void compute_ahrs_representations(void) {
#if (OUTPUTMODE==2) // Only accelerometer info (debugging purposes)
ahrs_float.ltp_to_imu_euler.phi = atan2(accel_float.y,accel_float.z); // atan2(acc_y,acc_z)
ahrs_float.ltp_to_imu_euler.theta = -asin((accel_float.x)/GRAVITY); // asin(acc_x)
ahrs_float.ltp_to_imu_euler.psi = 0;
#else
ahrs_float.ltp_to_imu_euler.phi = atan2(DCM_Matrix[2][1],DCM_Matrix[2][2]);
ahrs_float.ltp_to_imu_euler.theta = -asin(DCM_Matrix[2][0]);
ahrs_float.ltp_to_imu_euler.psi = atan2(DCM_Matrix[1][0],DCM_Matrix[0][0]);
ahrs_float.ltp_to_imu_euler.psi += M_PI; // Rotating the angle 180deg to fit for PPRZ
#endif
/* set quaternion and rotation matrix representations as well */
FLOAT_QUAT_OF_EULERS(ahrs_float.ltp_to_imu_quat, ahrs_float.ltp_to_imu_euler);
FLOAT_RMAT_OF_EULERS(ahrs_float.ltp_to_imu_rmat, ahrs_float.ltp_to_imu_euler);
compute_body_orientation_and_rates();
/*
RunOnceEvery(6,DOWNLINK_SEND_RMAT_DEBUG(DefaultChannel, DefaultDevice,
&(DCM_Matrix[0][0]),
&(DCM_Matrix[0][1]),
&(DCM_Matrix[0][2]),
&(DCM_Matrix[1][0]),
&(DCM_Matrix[1][1]),
&(DCM_Matrix[1][2]),
&(DCM_Matrix[2][0]),
&(DCM_Matrix[2][1]),
&(DCM_Matrix[2][2])
));
*/
}
void ahrs_init(void) {
ahrs_float.status = AHRS_UNINIT;
/*
* Initialises our IMU alignement variables
* This should probably done in the IMU code instead
*/
struct FloatEulers body_to_imu_euler =
{IMU_BODY_TO_IMU_PHI, IMU_BODY_TO_IMU_THETA, IMU_BODY_TO_IMU_PSI};
FLOAT_QUAT_OF_EULERS(ahrs_impl.body_to_imu_quat, body_to_imu_euler);
FLOAT_RMAT_OF_EULERS(ahrs_impl.body_to_imu_rmat, body_to_imu_euler);
/* set ltp_to_body to zero */
FLOAT_QUAT_ZERO(ahrs_float.ltp_to_body_quat);
FLOAT_EULERS_ZERO(ahrs_float.ltp_to_body_euler);
FLOAT_RMAT_ZERO(ahrs_float.ltp_to_body_rmat);
FLOAT_RATES_ZERO(ahrs_float.body_rate);
/* set ltp_to_imu so that body is zero */
QUAT_COPY(ahrs_float.ltp_to_imu_quat, ahrs_impl.body_to_imu_quat);
RMAT_COPY(ahrs_float.ltp_to_imu_rmat, ahrs_impl.body_to_imu_rmat);
EULERS_COPY(ahrs_float.ltp_to_imu_euler, body_to_imu_euler);
FLOAT_RATES_ZERO(ahrs_float.imu_rate);
/* set inital filter dcm */
set_dcm_matrix_from_rmat(&ahrs_float.ltp_to_imu_rmat);
}
void ahrs_init(void) {
ahrs.status = AHRS_UNINIT;
/*
* Initialises our IMU alignement variables
* This should probably done in the IMU code instead
*/
struct FloatEulers body_to_imu_euler =
{IMU_BODY_TO_IMU_PHI, IMU_BODY_TO_IMU_THETA, IMU_BODY_TO_IMU_PSI};
FLOAT_QUAT_OF_EULERS(ahrs_impl.body_to_imu_quat, body_to_imu_euler);
FLOAT_RMAT_OF_EULERS(ahrs_impl.body_to_imu_rmat, body_to_imu_euler);
/* Set ltp_to_imu so that body is zero */
QUAT_COPY(ahrs_impl.ltp_to_imu_quat, ahrs_impl.body_to_imu_quat);
FLOAT_RATES_ZERO(ahrs_impl.imu_rate);
VECT3_ASSIGN(ahrs_impl.mag_h, AHRS_H_X, AHRS_H_Y, AHRS_H_Z);
/*
* Initialises our state
*/
FLOAT_RATES_ZERO(ahrs_impl.gyro_bias);
const float P0_a = 1.;
const float P0_b = 1e-4;
float P0[6][6] = {{ P0_a, 0., 0., 0., 0., 0. },
{ 0., P0_a, 0., 0., 0., 0. },
{ 0., 0., P0_a, 0., 0., 0. },
{ 0., 0., 0., P0_b, 0., 0. },
{ 0., 0., 0., 0., P0_b, 0. },
{ 0., 0., 0., 0., 0., P0_b}};
memcpy(ahrs_impl.P, P0, sizeof(P0));
}
void imu_float_init(void) {
/*
Compute quaternion and rotation matrix
for conversions between body and imu frame
*/
EULERS_ASSIGN(imuf.body_to_imu_eulers,
IMU_BODY_TO_IMU_PHI, IMU_BODY_TO_IMU_THETA, IMU_BODY_TO_IMU_PSI);
FLOAT_QUAT_OF_EULERS(imuf.body_to_imu_quat, imuf.body_to_imu_eulers);
FLOAT_QUAT_NORMALIZE(imuf.body_to_imu_quat);
FLOAT_RMAT_OF_EULERS(imuf.body_to_imu_rmat, imuf.body_to_imu_eulers);
}
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 update_ahrs_from_sim(void) {
ahrs_float.ltp_to_imu_euler.phi = sim_phi;
ahrs_float.ltp_to_imu_euler.theta = sim_theta;
ahrs_float.ltp_to_imu_euler.psi = sim_psi;
ahrs_float.imu_rate.p = sim_p;
ahrs_float.imu_rate.q = sim_q;
/* set quaternion and rotation matrix representations as well */
FLOAT_QUAT_OF_EULERS(ahrs_float.ltp_to_imu_quat, ahrs_float.ltp_to_imu_euler);
FLOAT_RMAT_OF_EULERS(ahrs_float.ltp_to_imu_rmat, ahrs_float.ltp_to_imu_euler);
compute_body_orientation_and_rates();
}
static void test_5(void) {
struct FloatEulers fe;
EULERS_ASSIGN(fe, RadOfDeg(-10.66), RadOfDeg(-0.7), RadOfDeg(0.));
DISPLAY_FLOAT_EULERS_DEG("fe", fe);
printf("\n");
struct FloatQuat fq;
FLOAT_QUAT_OF_EULERS(fq, fe);
test_eulers_of_quat(fq, 1);
printf("\n");
struct FloatRMat frm;
FLOAT_RMAT_OF_EULERS(frm, fe);
DISPLAY_FLOAT_RMAT_AS_EULERS_DEG("frm", frm);
test_eulers_of_rmat(frm, 1);
printf("\n");
}
void imu_float_init(struct ImuFloat* imuf) {
/*
Compute quaternion and rotation matrix
for conversions between body and imu frame
*/
#if defined IMU_BODY_TO_IMU_PHI && defined IMU_BODY_TO_IMU_THETA & defined IMU_BODY_TO_IMU_PSI
EULERS_ASSIGN(imuf->body_to_imu_eulers,
IMU_BODY_TO_IMU_PHI, IMU_BODY_TO_IMU_THETA, IMU_BODY_TO_IMU_PSI);
FLOAT_QUAT_OF_EULERS(imuf->body_to_imu_quat, imuf->body_to_imu_eulers);
FLOAT_QUAT_NORMALISE(imuf->body_to_imu_quat);
FLOAT_RMAT_OF_EULERS(imuf->body_to_imu_rmat, imuf->body_to_imu_eulers);
#else
EULERS_ASSIGN(imuf->body_to_imu_eulers, 0., 0., 0.);
FLOAT_QUAT_ZERO(imuf->body_to_imu_quat);
FLOAT_RMAT_ZERO(imuf->body_to_imu_rmat);
#endif
}
void ahrs_update_fw_estimator( void )
{
#if (OUTPUTMODE==2) // Only accelerometer info (debugging purposes)
ahrs_float.ltp_to_imu_euler.phi = atan2(accel_float.y,accel_float.z); // atan2(acc_y,acc_z)
ahrs_float.ltp_to_imu_euler.theta = -asin((accel_float.x)/GRAVITY); // asin(acc_x)
ahrs_float.ltp_to_imu_euler.psi = 0;
#else
ahrs_float.ltp_to_imu_euler.phi = atan2(DCM_Matrix[2][1],DCM_Matrix[2][2]);
ahrs_float.ltp_to_imu_euler.theta = -asin(DCM_Matrix[2][0]);
ahrs_float.ltp_to_imu_euler.psi = atan2(DCM_Matrix[1][0],DCM_Matrix[0][0]);
ahrs_float.ltp_to_imu_euler.psi += M_PI; // Rotating the angle 180deg to fit for PPRZ
#endif
/* set quaternion and rotation matrix representations as well */
FLOAT_QUAT_OF_EULERS(ahrs_float.ltp_to_imu_quat, ahrs_float.ltp_to_imu_euler);
FLOAT_RMAT_OF_EULERS(ahrs_float.ltp_to_imu_rmat, ahrs_float.ltp_to_imu_euler);
compute_body_orientation_and_rates();
// export results to estimator
estimator_phi = ahrs_float.ltp_to_body_euler.phi - ins_roll_neutral;
estimator_theta = ahrs_float.ltp_to_body_euler.theta - ins_pitch_neutral;
estimator_psi = ahrs_float.ltp_to_body_euler.psi;
estimator_p = ahrs_float.body_rate.p;
estimator_q = ahrs_float.body_rate.q;
/*
RunOnceEvery(6,DOWNLINK_SEND_RMAT_DEBUG(DefaultChannel,
&(DCM_Matrix[0][0]),
&(DCM_Matrix[0][1]),
&(DCM_Matrix[0][2]),
&(DCM_Matrix[1][0]),
&(DCM_Matrix[1][1]),
&(DCM_Matrix[1][2]),
&(DCM_Matrix[2][0]),
&(DCM_Matrix[2][1]),
&(DCM_Matrix[2][2])
));
*/
}
/*
* Compute body orientation and rates from imu orientation and rates
*/
static inline void set_body_orientation_and_rates(void) {
struct FloatRates body_rate;
FLOAT_RMAT_TRANSP_RATEMULT(body_rate, ahrs_impl.body_to_imu_rmat, ahrs_impl.imu_rate);
stateSetBodyRates_f(&body_rate);
struct FloatRMat ltp_to_imu_rmat, ltp_to_body_rmat;
FLOAT_RMAT_OF_EULERS(ltp_to_imu_rmat, ahrs_impl.ltp_to_imu_euler);
FLOAT_RMAT_COMP_INV(ltp_to_body_rmat, ltp_to_imu_rmat, ahrs_impl.body_to_imu_rmat);
// Some stupid lines of code for neutrals
struct FloatEulers ltp_to_body_euler;
FLOAT_EULERS_OF_RMAT(ltp_to_body_euler, ltp_to_body_rmat);
ltp_to_body_euler.phi -= ins_roll_neutral;
ltp_to_body_euler.theta -= ins_pitch_neutral;
stateSetNedToBodyEulers_f(<p_to_body_euler);
// should be replaced at the end by:
// stateSetNedToBodyRMat_f(<p_to_body_rmat);
}
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 FloatMat33 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 FloatMat33 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 ahrs_align(void)
{
/* Compute an initial orientation using euler angles */
ahrs_float_get_euler_from_accel_mag(&ahrs_impl.ltp_to_imu_euler, &ahrs_aligner.lp_accel, &ahrs_aligner.lp_mag);
/* Convert initial orientation in quaternion and rotation matrice representations. */
struct FloatRMat ltp_to_imu_rmat;
FLOAT_RMAT_OF_EULERS(ltp_to_imu_rmat, ahrs_impl.ltp_to_imu_euler);
/* set filter dcm */
set_dcm_matrix_from_rmat(<p_to_imu_rmat);
/* Set initial body orientation */
set_body_orientation_and_rates();
/* use averaged gyro as initial value for bias */
struct Int32Rates bias0;
RATES_COPY(bias0, ahrs_aligner.lp_gyro);
RATES_FLOAT_OF_BFP(ahrs_impl.gyro_bias, bias0);
ahrs.status = AHRS_RUNNING;
}
void ahrs_init(void) {
ahrs.status = AHRS_UNINIT;
ahrs_impl.ltp_vel_norm_valid = FALSE;
ahrs_impl.heading_aligned = FALSE;
/* Initialises IMU alignement */
struct FloatEulers body_to_imu_euler =
{IMU_BODY_TO_IMU_PHI, IMU_BODY_TO_IMU_THETA, IMU_BODY_TO_IMU_PSI};
FLOAT_QUAT_OF_EULERS(ahrs_impl.body_to_imu_quat, body_to_imu_euler);
FLOAT_RMAT_OF_EULERS(ahrs_impl.body_to_imu_rmat, body_to_imu_euler);
/* Set ltp_to_imu so that body is zero */
QUAT_COPY(ahrs_impl.ltp_to_imu_quat, ahrs_impl.body_to_imu_quat);
RMAT_COPY(ahrs_impl.ltp_to_imu_rmat, ahrs_impl.body_to_imu_rmat);
FLOAT_RATES_ZERO(ahrs_impl.imu_rate);
/* set default filter cut-off frequency and damping */
ahrs_impl.accel_omega = AHRS_ACCEL_OMEGA;
ahrs_impl.accel_zeta = AHRS_ACCEL_ZETA;
ahrs_impl.mag_omega = AHRS_MAG_OMEGA;
ahrs_impl.mag_zeta = AHRS_MAG_ZETA;
#if AHRS_GRAVITY_UPDATE_COORDINATED_TURN
ahrs_impl.correct_gravity = TRUE;
#else
ahrs_impl.correct_gravity = FALSE;
#endif
ahrs_impl.gravity_heuristic_factor = AHRS_GRAVITY_HEURISTIC_FACTOR;
VECT3_ASSIGN(ahrs_impl.mag_h, AHRS_H_X, AHRS_H_Y, AHRS_H_Z);
#if PERIODIC_TELEMETRY
register_periodic_telemetry(DefaultPeriodic, "AHRS_EULER_INT", send_att);
register_periodic_telemetry(DefaultPeriodic, "GEO_MAG", send_geo_mag);
#endif
}
void ahrs_init(void) {
ahrs.status = AHRS_UNINIT;
/*
* Initialises our IMU alignement variables
* This should probably done in the IMU code instead
*/
struct FloatEulers body_to_imu_euler =
{IMU_BODY_TO_IMU_PHI, IMU_BODY_TO_IMU_THETA, IMU_BODY_TO_IMU_PSI};
FLOAT_QUAT_OF_EULERS(ahrs_impl.body_to_imu_quat, body_to_imu_euler);
FLOAT_RMAT_OF_EULERS(ahrs_impl.body_to_imu_rmat, body_to_imu_euler);
/* set ltp_to_body to zero */
FLOAT_QUAT_ZERO(ahrs_float.ltp_to_body_quat);
FLOAT_EULERS_ZERO(ahrs_float.ltp_to_body_euler);
FLOAT_RMAT_ZERO(ahrs_float.ltp_to_body_rmat);
FLOAT_RATES_ZERO(ahrs_float.body_rate);
/* set ltp_to_imu so that body is zero */
QUAT_COPY(ahrs_float.ltp_to_imu_quat, ahrs_impl.body_to_imu_quat);
RMAT_COPY(ahrs_float.ltp_to_imu_rmat, ahrs_impl.body_to_imu_rmat);
EULERS_COPY(ahrs_float.ltp_to_imu_euler, body_to_imu_euler);
FLOAT_RATES_ZERO(ahrs_float.imu_rate);
/* set inital filter dcm */
set_dcm_matrix_from_rmat(&ahrs_float.ltp_to_imu_rmat);
#if USE_HIGH_ACCEL_FLAG
high_accel_done = FALSE;
high_accel_flag = FALSE;
#endif
ahrs_impl.gps_speed = 0;
ahrs_impl.gps_acceleration = 0;
ahrs_impl.gps_course = 0;
ahrs_impl.gps_course_valid = FALSE;
ahrs_impl.gps_age = 100;
}
void ahrs_init(void) {
ahrs.status = AHRS_UNINIT;
/* Initialises IMU alignement */
struct FloatEulers body_to_imu_euler =
{IMU_BODY_TO_IMU_PHI, IMU_BODY_TO_IMU_THETA, IMU_BODY_TO_IMU_PSI};
FLOAT_QUAT_OF_EULERS(ahrs_impl.body_to_imu_quat, body_to_imu_euler);
FLOAT_RMAT_OF_EULERS(ahrs_impl.body_to_imu_rmat, body_to_imu_euler);
/* Set ltp_to_body to zero */
FLOAT_QUAT_ZERO(ahrs_float.ltp_to_body_quat);
FLOAT_EULERS_ZERO(ahrs_float.ltp_to_body_euler);
FLOAT_RMAT_ZERO(ahrs_float.ltp_to_body_rmat);
FLOAT_RATES_ZERO(ahrs_float.body_rate);
/* Set ltp_to_imu so that body is zero */
QUAT_COPY(ahrs_float.ltp_to_imu_quat, ahrs_impl.body_to_imu_quat);
RMAT_COPY(ahrs_float.ltp_to_imu_rmat, ahrs_impl.body_to_imu_rmat);
EULERS_COPY(ahrs_float.ltp_to_imu_euler, body_to_imu_euler);
FLOAT_RATES_ZERO(ahrs_float.imu_rate);
}
void ahrs_init(void) {
ahrs.status = AHRS_UNINIT;
/*
* Initialises our IMU alignement variables
* This should probably done in the IMU code instead
*/
struct FloatEulers body_to_imu_euler =
{IMU_BODY_TO_IMU_PHI, IMU_BODY_TO_IMU_THETA, IMU_BODY_TO_IMU_PSI};
FLOAT_RMAT_OF_EULERS(ahrs_impl.body_to_imu_rmat, body_to_imu_euler);
EULERS_COPY(ahrs_impl.ltp_to_imu_euler, body_to_imu_euler);
FLOAT_RATES_ZERO(ahrs_impl.imu_rate);
/* set inital filter dcm */
set_dcm_matrix_from_rmat(&ahrs_impl.body_to_imu_rmat);
ahrs_impl.gps_speed = 0;
ahrs_impl.gps_acceleration = 0;
ahrs_impl.gps_course = 0;
ahrs_impl.gps_course_valid = FALSE;
ahrs_impl.gps_age = 100;
}
static inline void update_ref_quat_from_eulers(void) {
struct FloatRMat ref_rmat;
FLOAT_RMAT_OF_EULERS(ref_rmat, stab_att_ref_euler);
FLOAT_QUAT_OF_RMAT(stab_att_ref_quat, ref_rmat);
FLOAT_QUAT_WRAP_SHORTEST(stab_att_ref_quat);
}
