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); }