void ahrs_float_invariant_propagate(struct FloatRates* gyro, float dt) { // realign all the filter if needed // a complete init cycle is required if (ahrs_float_inv.reset) { ahrs_float_inv.reset = false; ahrs_float_inv.is_aligned = false; init_invariant_state(); } // fill command vector struct FloatRates gyro_meas_body; struct FloatRMat *body_to_imu_rmat = orientationGetRMat_f(&ahrs_float_inv.body_to_imu); float_rmat_transp_ratemult(&gyro_meas_body, body_to_imu_rmat, gyro); ahrs_float_inv.cmd.rates = gyro_meas_body; // update correction gains error_output(&ahrs_float_inv); // propagate model struct inv_state new_state; runge_kutta_4_float((float *)&new_state, (float *)&ahrs_float_inv.state, INV_STATE_DIM, (float *)&ahrs_float_inv.cmd, INV_COMMAND_DIM, invariant_model, dt); ahrs_float_inv.state = new_state; // normalize quaternion float_quat_normalize(&ahrs_float_inv.state.quat); //------------------------------------------------------------// #if SEND_INVARIANT_FILTER struct FloatEulers eulers; float foo = 0.f; float_eulers_of_quat(&eulers, &ahrs_float_inv.state.quat); RunOnceEvery(3, pprz_msg_send_INV_FILTER(&(DefaultChannel).trans_tx, &(DefaultDevice).device, AC_ID, &ahrs_float_inv.state.quat.qi, &eulers.phi, &eulers.theta, &eulers.psi, &foo, &foo, &foo, &foo, &foo, &foo, &ahrs_float_inv.state.bias.p, &ahrs_float_inv.state.bias.q, &ahrs_float_inv.state.bias.r, &ahrs_float_inv.state.as, &ahrs_float_inv.state.cs, &foo, &foo); );
void ahrs_fc_set_body_to_imu_quat(struct FloatQuat *q_b2i) { orientationSetQuat_f(&ahrs_fc.body_to_imu, q_b2i); if (!ahrs_fc.is_aligned) { /* Set ltp_to_imu so that body is zero */ ahrs_fc.ltp_to_imu_quat = *orientationGetQuat_f(&ahrs_fc.body_to_imu); ahrs_fc.ltp_to_imu_rmat = *orientationGetRMat_f(&ahrs_fc.body_to_imu); } }
/** * Read received data */ void ahrs_vectornav_propagate(void) { // Rates [rad/s] static struct FloatRates body_rate; float_rmat_ratemult(&body_rate, orientationGetRMat_f(&ahrs_vn.body_to_imu), &ahrs_vn.vn_data.gyro); // compute body rates stateSetBodyRates_f(&body_rate); // Set state [rad/s] // Attitude [deg] static struct FloatQuat imu_quat; // convert from euler to quat float_quat_of_eulers(&imu_quat, &ahrs_vn.vn_data.attitude); stateSetNedToBodyQuat_f(&imu_quat); }
/** * Compute body orientation and rates from imu orientation and rates */ static inline void compute_body_orientation_and_rates(void) { /* Compute LTP to BODY quaternion */ struct FloatQuat ltp_to_body_quat; struct FloatQuat *body_to_imu_quat = orientationGetQuat_f(&imu.body_to_imu); float_quat_comp_inv(<p_to_body_quat, &ahrs_impl.ltp_to_imu_quat, body_to_imu_quat); /* Set state */ stateSetNedToBodyQuat_f(<p_to_body_quat); /* compute body rates */ struct FloatRates body_rate; struct FloatRMat *body_to_imu_rmat = orientationGetRMat_f(&imu.body_to_imu); float_rmat_transp_ratemult(&body_rate, body_to_imu_rmat, &ahrs_impl.imu_rate); stateSetBodyRates_f(&body_rate); }
void ahrs_init(void) { ahrs.status = AHRS_UNINIT; /* Set ltp_to_imu so that body is zero */ memcpy(&ahrs_impl.ltp_to_imu_euler, orientationGetEulers_f(&imu.body_to_imu), sizeof(struct FloatEulers)); FLOAT_RATES_ZERO(ahrs_impl.imu_rate); /* set inital filter dcm */ set_dcm_matrix_from_rmat(orientationGetRMat_f(&imu.body_to_imu)); 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_dcm_init(void) { ahrs_dcm.status = AHRS_DCM_UNINIT; ahrs_dcm.is_aligned = FALSE; /* init ltp_to_imu euler with zero */ FLOAT_EULERS_ZERO(ahrs_dcm.ltp_to_imu_euler); FLOAT_RATES_ZERO(ahrs_dcm.imu_rate); /* set inital filter dcm */ set_dcm_matrix_from_rmat(orientationGetRMat_f(&ahrs_dcm.body_to_imu)); ahrs_dcm.gps_speed = 0; ahrs_dcm.gps_acceleration = 0; ahrs_dcm.gps_course = 0; ahrs_dcm.gps_course_valid = FALSE; ahrs_dcm.gps_age = 100; }
/* * Compute body orientation and rates from imu orientation and rates */ static inline void set_body_orientation_and_rates(void) { struct FloatRMat *body_to_imu_rmat = orientationGetRMat_f(&imu.body_to_imu); struct FloatRates body_rate; FLOAT_RMAT_TRANSP_RATEMULT(body_rate, *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, *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); }
void ahrs_init(void) { ahrs.status = AHRS_UNINIT; ahrs_impl.ltp_vel_norm_valid = FALSE; ahrs_impl.heading_aligned = FALSE; /* Set ltp_to_imu so that body is zero */ memcpy(&ahrs_impl.ltp_to_imu_quat, orientationGetQuat_f(&imu.body_to_imu), sizeof(struct FloatQuat)); memcpy(&ahrs_impl.ltp_to_imu_rmat, orientationGetRMat_f(&imu.body_to_imu), sizeof(struct FloatRMat)); 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); ahrs_impl.accel_cnt = 0; ahrs_impl.mag_cnt = 0; #if PERIODIC_TELEMETRY register_periodic_telemetry(DefaultPeriodic, "AHRS_EULER_INT", send_att); register_periodic_telemetry(DefaultPeriodic, "GEO_MAG", send_geo_mag); #endif }
/** * Propagate the received states into the vehicle * state machine */ void ins_vectornav_propagate() { // Acceleration [m/s^2] // in fixed point for sending as ABI and telemetry msgs ACCELS_BFP_OF_REAL(ins_vn.accel_i, ins_vn.accel); // Rates [rad/s] static struct FloatRates body_rate; // in fixed point for sending as ABI and telemetry msgs RATES_BFP_OF_REAL(ins_vn.gyro_i, ins_vn.gyro); float_rmat_ratemult(&body_rate, orientationGetRMat_f(&ins_vn.body_to_imu), &ins_vn.gyro); // compute body rates stateSetBodyRates_f(&body_rate); // Set state [rad/s] // Attitude [deg] ins_vectornav_yaw_pitch_roll_to_attitude(&ins_vn.attitude); // convert to correct units and axis [rad] static struct FloatQuat imu_quat; // convert from euler to quat float_quat_of_eulers(&imu_quat, &ins_vn.attitude); static struct FloatRMat imu_rmat; // convert from quat to rmat float_rmat_of_quat(&imu_rmat, &imu_quat); static struct FloatRMat ltp_to_body_rmat; // rotate to body frame float_rmat_comp(<p_to_body_rmat, &imu_rmat, orientationGetRMat_f(&ins_vn.body_to_imu)); stateSetNedToBodyRMat_f(<p_to_body_rmat); // set body states [rad] // NED (LTP) velocity [m/s] // North east down (NED), also known as local tangent plane (LTP), // is a geographical coordinate system for representing state vectors that is commonly used in aviation. // It consists of three numbers: one represents the position along the northern axis, // one along the eastern axis, and one represents vertical position. Down is chosen as opposed to // up in order to comply with the right-hand rule. // The origin of this coordinate system is usually chosen to be the aircraft's center of gravity. // x = North // y = East // z = Down stateSetSpeedNed_f(&ins_vn.vel_ned); // set state // NED (LTP) acceleration [m/s^2] static struct FloatVect3 accel_meas_ltp;// first we need to rotate linear acceleration from imu-frame to body-frame float_rmat_transp_vmult(&accel_meas_ltp, orientationGetRMat_f(&ins_vn.body_to_imu), &(ins_vn.lin_accel)); static struct NedCoor_f ltp_accel; // assign to NedCoord_f struct VECT3_ASSIGN(ltp_accel, accel_meas_ltp.x, accel_meas_ltp.y, accel_meas_ltp.z); stateSetAccelNed_f(<p_accel); // then set the states ins_vn.ltp_accel_f = ltp_accel; // LLA position [rad, rad, m] //static struct LlaCoor_f lla_pos; // convert from deg to rad, and from double to float ins_vn.lla_pos.lat = RadOfDeg((float)ins_vn.pos_lla[0]); // ins_impl.pos_lla[0] = lat ins_vn.lla_pos.lon = RadOfDeg((float)ins_vn.pos_lla[1]); // ins_impl.pos_lla[1] = lon ins_vn.lla_pos.alt = ((float)ins_vn.pos_lla[2]); // ins_impl.pos_lla[2] = alt LLA_BFP_OF_REAL(gps.lla_pos, ins_vn.lla_pos); stateSetPositionLla_i(&gps.lla_pos); // ECEF position struct LtpDef_f def; ltp_def_from_lla_f(&def, &ins_vn.lla_pos); struct EcefCoor_f ecef_vel; ecef_of_ned_point_f(&ecef_vel, &def, &ins_vn.vel_ned); ECEF_BFP_OF_REAL(gps.ecef_vel, ecef_vel); // ECEF velocity gps.ecef_pos.x = stateGetPositionEcef_i()->x; gps.ecef_pos.y = stateGetPositionEcef_i()->y; gps.ecef_pos.z = stateGetPositionEcef_i()->z; #if GPS_USE_LATLONG // GPS UTM /* Computes from (lat, long) in the referenced UTM zone */ struct UtmCoor_f utm_f; utm_f.zone = nav_utm_zone0; /* convert to utm */ //utm_of_lla_f(&utm_f, &lla_f); utm_of_lla_f(&utm_f, &ins_vn.lla_pos); /* copy results of utm conversion */ gps.utm_pos.east = (int32_t)(utm_f.east * 100); gps.utm_pos.north = (int32_t)(utm_f.north * 100); gps.utm_pos.alt = (int32_t)(utm_f.alt * 1000); gps.utm_pos.zone = (uint8_t)nav_utm_zone0; #endif // GPS Ground speed float speed = sqrt(ins_vn.vel_ned.x * ins_vn.vel_ned.x + ins_vn.vel_ned.y * ins_vn.vel_ned.y); gps.gspeed = ((uint16_t)(speed * 100)); // GPS course gps.course = (int32_t)(1e7 * (atan2(ins_vn.vel_ned.y, ins_vn.vel_ned.x))); // Because we have not HMSL data from Vectornav, we are using LLA-Altitude // as a workaround gps.hmsl = (uint32_t)(gps.lla_pos.alt); // set position uncertainty ins_vectornav_set_pacc(); // set velocity uncertainty ins_vectornav_set_sacc(); // check GPS status gps.last_msg_time = sys_time.nb_sec; gps.last_msg_ticks = sys_time.nb_sec_rem; if (gps.fix == GPS_FIX_3D) { gps.last_3dfix_time = sys_time.nb_sec; gps.last_3dfix_ticks = sys_time.nb_sec_rem; } // read INS status ins_vectornav_check_status(); // update internal states for telemetry purposes // TODO: directly convert vectornav output instead of using state interface // to support multiple INS running at the same time ins_vn.ltp_pos_i = *stateGetPositionNed_i(); ins_vn.ltp_speed_i = *stateGetSpeedNed_i(); ins_vn.ltp_accel_i = *stateGetAccelNed_i(); // send ABI messages uint32_t now_ts = get_sys_time_usec(); AbiSendMsgGPS(GPS_UBX_ID, now_ts, &gps); AbiSendMsgIMU_GYRO_INT32(IMU_ASPIRIN_ID, now_ts, &ins_vn.gyro_i); AbiSendMsgIMU_ACCEL_INT32(IMU_ASPIRIN_ID, now_ts, &ins_vn.accel_i); }
void ahrs_fc_update_accel(struct Int32Vect3 *accel, float dt) { // check if we had at least one propagation since last update if (ahrs_fc.accel_cnt == 0) { return; } /* last column of roation matrix = ltp z-axis in imu-frame */ struct FloatVect3 c2 = { RMAT_ELMT(ahrs_fc.ltp_to_imu_rmat, 0, 2), RMAT_ELMT(ahrs_fc.ltp_to_imu_rmat, 1, 2), RMAT_ELMT(ahrs_fc.ltp_to_imu_rmat, 2, 2) }; struct FloatVect3 imu_accel_float; ACCELS_FLOAT_OF_BFP(imu_accel_float, *accel); struct FloatVect3 residual; struct FloatVect3 pseudo_gravity_measurement; if (ahrs_fc.correct_gravity && ahrs_fc.ltp_vel_norm_valid) { /* * centrifugal acceleration in body frame * a_c_body = omega x (omega x r) * (omega x r) = tangential velocity in body frame * a_c_body = omega x vel_tangential_body * assumption: tangential velocity only along body x-axis */ const struct FloatVect3 vel_tangential_body = {ahrs_fc.ltp_vel_norm, 0.0, 0.0}; struct FloatRMat *body_to_imu_rmat = orientationGetRMat_f(&ahrs_fc.body_to_imu); struct FloatRates body_rate; float_rmat_transp_ratemult(&body_rate, body_to_imu_rmat, &ahrs_fc.imu_rate); struct FloatVect3 acc_c_body; VECT3_RATES_CROSS_VECT3(acc_c_body, body_rate, vel_tangential_body); /* convert centrifugal acceleration from body to imu frame */ struct FloatVect3 acc_c_imu; float_rmat_vmult(&acc_c_imu, body_to_imu_rmat, &acc_c_body); /* and subtract it from imu measurement to get a corrected measurement of the gravity vector */ VECT3_DIFF(pseudo_gravity_measurement, imu_accel_float, acc_c_imu); } else { VECT3_COPY(pseudo_gravity_measurement, imu_accel_float); } VECT3_CROSS_PRODUCT(residual, pseudo_gravity_measurement, c2); /* FIR filtered pseudo_gravity_measurement */ #define FIR_FILTER_SIZE 8 static struct FloatVect3 filtered_gravity_measurement = {0., 0., 0.}; VECT3_SMUL(filtered_gravity_measurement, filtered_gravity_measurement, FIR_FILTER_SIZE - 1); VECT3_ADD(filtered_gravity_measurement, pseudo_gravity_measurement); VECT3_SDIV(filtered_gravity_measurement, filtered_gravity_measurement, FIR_FILTER_SIZE); if (ahrs_fc.gravity_heuristic_factor) { /* heuristic on acceleration (gravity estimate) norm */ /* Factor how strongly to change the weight. * e.g. for gravity_heuristic_factor 30: * <0.66G = 0, 1G = 1.0, >1.33G = 0 */ const float g_meas_norm = float_vect3_norm(&filtered_gravity_measurement) / 9.81; ahrs_fc.weight = 1.0 - ahrs_fc.gravity_heuristic_factor * fabs(1.0 - g_meas_norm) / 10.0; Bound(ahrs_fc.weight, 0.15, 1.0); } else { ahrs_fc.weight = 1.0; } /* Complementary filter proportional gain. * Kp = 2 * zeta * omega * weight * ahrs_fc.accel_cnt * with ahrs_fc.accel_cnt beeing the number of propagations since last update */ const float gravity_rate_update_gain = -2 * ahrs_fc.accel_zeta * ahrs_fc.accel_omega * ahrs_fc.weight * ahrs_fc.accel_cnt / 9.81; RATES_ADD_SCALED_VECT(ahrs_fc.rate_correction, residual, gravity_rate_update_gain); // reset accel propagation counter ahrs_fc.accel_cnt = 0; /* Complementary filter integral gain * Correct the gyro bias. * Ki = (omega*weight)^2 * dt */ const float gravity_bias_update_gain = ahrs_fc.accel_omega * ahrs_fc.accel_omega * ahrs_fc.weight * ahrs_fc.weight * dt / 9.81; RATES_ADD_SCALED_VECT(ahrs_fc.gyro_bias, residual, gravity_bias_update_gain); /* FIXME: saturate bias */ }
void Normalize(void) { float error = 0; float temporary[3][3]; float renorm = 0; uint8_t problem = FALSE; // Find the non-orthogonality of X wrt Y error = -Vector_Dot_Product(&DCM_Matrix[0][0], &DCM_Matrix[1][0]) * .5; //eq.19 // Add half the XY error to X, and half to Y Vector_Scale(&temporary[0][0], &DCM_Matrix[1][0], error); //eq.19 Vector_Scale(&temporary[1][0], &DCM_Matrix[0][0], error); //eq.19 Vector_Add(&temporary[0][0], &temporary[0][0], &DCM_Matrix[0][0]); //eq.19 Vector_Add(&temporary[1][0], &temporary[1][0], &DCM_Matrix[1][0]); //eq.19 // The third axis is simply set perpendicular to the first 2. (there is not correction of XY based on Z) Vector_Cross_Product(&temporary[2][0], &temporary[0][0], &temporary[1][0]); // c= a x b //eq.20 // Normalize lenght of X renorm = Vector_Dot_Product(&temporary[0][0], &temporary[0][0]); // a) if norm is close to 1, use the fast 1st element from the tailer expansion of SQRT // b) if the norm is further from 1, use a real sqrt // c) norm is huge: disaster! reset! mayday! if (renorm < 1.5625f && renorm > 0.64f) { renorm = .5 * (3 - renorm); //eq.21 } else if (renorm < 100.0f && renorm > 0.01f) { renorm = 1. / sqrt(renorm); #if PERFORMANCE_REPORTING == 1 renorm_sqrt_count++; #endif } else { problem = TRUE; #if PERFORMANCE_REPORTING == 1 renorm_blowup_count++; #endif } Vector_Scale(&DCM_Matrix[0][0], &temporary[0][0], renorm); // Normalize lenght of Y renorm = Vector_Dot_Product(&temporary[1][0], &temporary[1][0]); if (renorm < 1.5625f && renorm > 0.64f) { renorm = .5 * (3 - renorm); //eq.21 } else if (renorm < 100.0f && renorm > 0.01f) { renorm = 1. / sqrt(renorm); #if PERFORMANCE_REPORTING == 1 renorm_sqrt_count++; #endif } else { problem = TRUE; #if PERFORMANCE_REPORTING == 1 renorm_blowup_count++; #endif } Vector_Scale(&DCM_Matrix[1][0], &temporary[1][0], renorm); // Normalize lenght of Z renorm = Vector_Dot_Product(&temporary[2][0], &temporary[2][0]); if (renorm < 1.5625f && renorm > 0.64f) { renorm = .5 * (3 - renorm); //eq.21 } else if (renorm < 100.0f && renorm > 0.01f) { renorm = 1. / sqrt(renorm); #if PERFORMANCE_REPORTING == 1 renorm_sqrt_count++; #endif } else { problem = TRUE; #if PERFORMANCE_REPORTING == 1 renorm_blowup_count++; #endif } Vector_Scale(&DCM_Matrix[2][0], &temporary[2][0], renorm); // Reset on trouble if (problem) { // Our solution is blowing up and we will force back to initial condition. Hope we are not upside down! set_dcm_matrix_from_rmat(orientationGetRMat_f(&ahrs_dcm.body_to_imu)); problem = FALSE; } }
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 }