static inline void update_state_interface(void) { // Send to Estimator (Control) #if XSENS_BACKWARDS struct FloatEulers att = { ins_phi + ins_roll_neutral, -ins_theta + ins_pitch_neutral, -ins_psi + RadOfDeg(180) }; struct FloatRates rates = { ins_p, -ins_q, -ins_r }; #else struct FloatEulers att = { -ins_phi + ins_roll_neutral, ins_theta + ins_pitch_neutral, -ins_psi }; struct FloatRates rates = { -ins_p, ins_q, -ins_r }; #endif stateSetNedToBodyEulers_f(&att); stateSetBodyRates_f(&rates); }
static void update_state_interface(void) { // Send to Estimator (Control) #ifdef XSENS_BACKWARDS struct FloatEulers att = { -xsens.euler.phi + ins_roll_neutral, -xsens.euler.theta + ins_pitch_neutral, xsens.euler.psi + RadOfDeg(180) }; struct FloatEulerstRates rates = { -xsens.gyro.p, -xsens.gyro.q, xsens.gyro.r }; #else struct FloatEulers att = { xsens.euler.phi + ins_roll_neutral, xsens.euler.theta + ins_pitch_neutral, xsens.euler.psi }; struct FloatRates rates = xsens.gyro; #endif stateSetNedToBodyEulers_f(&att); stateSetBodyRates_f(&rates); }
void update_ahrs_from_sim(void) { struct FloatEulers ltp_to_imu_euler = { sim_phi, sim_theta, sim_psi }; struct FloatRates imu_rate = { sim_p, sim_q, sim_r }; /* set ltp_to_body to same as ltp_to_imu, currently no difference simulated */ stateSetNedToBodyEulers_f(<p_to_imu_euler); stateSetBodyRates_f(&imu_rate); }
void sim_overwrite_ahrs(void) { struct FloatQuat quat_f; QUAT_COPY(quat_f, fdm.ltp_to_body_quat); stateSetNedToBodyQuat_f(&quat_f); struct FloatRates rates_f; RATES_COPY(rates_f, fdm.body_ecef_rotvel); stateSetBodyRates_f(&rates_f); }
void ahrs_propagate(void) { struct FloatRates body_rate = { 0., 0., 0. }; #ifdef ADC_CHANNEL_GYRO_P body_rate.p = RATE_FLOAT_OF_BFP(imu.gyro.p); #endif #ifdef ADC_CHANNEL_GYRO_Q body_rate.q = RATE_FLOAT_OF_BFP(imu.gyro.q); #endif #ifdef ADC_CHANNEL_GYRO_R body_rate.r = RATE_FLOAT_OF_BFP(imu.gyro.r); #endif stateSetBodyRates_f(&body_rate); }
void update_ahrs_from_sim(void) { struct FloatEulers ltp_to_imu_euler = { sim_phi, sim_theta, sim_psi }; #ifdef AHRS_UPDATE_FW_ESTIMATOR ltp_to_imu_euler.phi -= ins_roll_neutral; ltp_to_imu_euler.theta -= ins_pitch_neutral; #endif struct FloatRates imu_rate = { sim_p, sim_q, sim_r }; /* set ltp_to_body to same as ltp_to_imu, currently no difference simulated */ stateSetNedToBodyEulers_f(<p_to_imu_euler); stateSetBodyRates_f(&imu_rate); }
/** * 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); }
/** * Compute body orientation and rates from imu orientation and rates */ static inline void set_body_state_from_quat(void) { /* Compute LTP to BODY quaternion */ struct FloatQuat ltp_to_body_quat; FLOAT_QUAT_COMP_INV(ltp_to_body_quat, ahrs_impl.ltp_to_imu_quat, ahrs_impl.body_to_imu_quat); /* Set in state interface */ stateSetNedToBodyQuat_f(<p_to_body_quat); /* compute body rates */ struct FloatRates body_rate; FLOAT_RMAT_TRANSP_RATEMULT(body_rate, ahrs_impl.body_to_imu_rmat, ahrs_impl.imu_rate); /* Set state */ stateSetBodyRates_f(&body_rate); }
void ArduIMU_event(void) { // Handle INS I2C event if (ardu_ins_trans.status == I2CTransSuccess) { // received data from I2C transaction recievedData[0] = (ardu_ins_trans.buf[1] << 8) | ardu_ins_trans.buf[0]; recievedData[1] = (ardu_ins_trans.buf[3] << 8) | ardu_ins_trans.buf[2]; recievedData[2] = (ardu_ins_trans.buf[5] << 8) | ardu_ins_trans.buf[4]; recievedData[3] = (ardu_ins_trans.buf[7] << 8) | ardu_ins_trans.buf[6]; recievedData[4] = (ardu_ins_trans.buf[9] << 8) | ardu_ins_trans.buf[8]; recievedData[5] = (ardu_ins_trans.buf[11] << 8) | ardu_ins_trans.buf[10]; recievedData[6] = (ardu_ins_trans.buf[13] << 8) | ardu_ins_trans.buf[12]; recievedData[7] = (ardu_ins_trans.buf[15] << 8) | ardu_ins_trans.buf[14]; recievedData[8] = (ardu_ins_trans.buf[17] << 8) | ardu_ins_trans.buf[16]; // Update ArduIMU data arduimu_eulers.phi = ANGLE_FLOAT_OF_BFP(recievedData[0]) - ins_roll_neutral; arduimu_eulers.theta = ANGLE_FLOAT_OF_BFP(recievedData[1]) - ins_pitch_neutral; arduimu_eulers.psi = ANGLE_FLOAT_OF_BFP(recievedData[2]); arduimu_rates.p = RATE_FLOAT_OF_BFP(recievedData[3]); arduimu_rates.q = RATE_FLOAT_OF_BFP(recievedData[4]); arduimu_rates.r = RATE_FLOAT_OF_BFP(recievedData[5]); arduimu_accel.x = ACCEL_FLOAT_OF_BFP(recievedData[6]); arduimu_accel.y = ACCEL_FLOAT_OF_BFP(recievedData[7]); arduimu_accel.z = ACCEL_FLOAT_OF_BFP(recievedData[8]); // Update estimator stateSetNedToBodyEulers_f(&arduimu_eulers); stateSetBodyRates_f(&arduimu_rates); stateSetAccelNed_f(&((struct NedCoor_f)arduimu_accel)); ardu_ins_trans.status = I2CTransDone; #ifdef ARDUIMU_SYNC_SEND // uint8_t arduimu_id = 102; //RunOnceEvery(15, DOWNLINK_SEND_AHRS_EULER(DefaultChannel, DefaultDevice, &arduimu_eulers.phi, &arduimu_eulers.theta, &arduimu_eulers.psi, &arduimu_id)); RunOnceEvery(15, DOWNLINK_SEND_IMU_GYRO(DefaultChannel, DefaultDevice, &arduimu_rates.p, &arduimu_rates.q, &arduimu_rates.r)); RunOnceEvery(15, DOWNLINK_SEND_IMU_ACCEL(DefaultChannel, DefaultDevice, &arduimu_accel.x, &arduimu_accel.y, &arduimu_accel.z)); #endif } else if (ardu_ins_trans.status == I2CTransFailed) { ardu_ins_trans.status = I2CTransDone; } // Handle GPS I2C event if (ardu_gps_trans.status == I2CTransSuccess || ardu_gps_trans.status == I2CTransFailed) { ardu_gps_trans.status = I2CTransDone; } }
void parse_ins_msg(void) { struct link_device *dev = InsLinkDevice; while (dev->char_available(dev->periph)) { uint8_t ch = dev->get_byte(dev->periph); if (CHIMU_Parse(ch, 0, &CHIMU_DATA)) { RunOnceEvery(25, LED_TOGGLE(3)); if (CHIMU_DATA.m_MsgID == CHIMU_Msg_3_IMU_Attitude) { // TODO: remove this flag as it doesn't work with multiple AHRS new_ins_attitude = 1; if (CHIMU_DATA.m_attitude.euler.phi > M_PI) { CHIMU_DATA.m_attitude.euler.phi -= 2 * M_PI; } //FIXME ahrs_chimu.is_aligned = true; if (ahrs_chimu.is_enabled) { struct FloatEulers att = { CHIMU_DATA.m_attitude.euler.phi, CHIMU_DATA.m_attitude.euler.theta, CHIMU_DATA.m_attitude.euler.psi }; stateSetNedToBodyEulers_f(&att); struct FloatRates rates = { CHIMU_DATA.m_sensor.rate[0], CHIMU_DATA.m_attrates.euler.theta, 0. }; // FIXME rate r stateSetBodyRates_f(&rates); } } else if (CHIMU_DATA.m_MsgID == 0x02) { #if CHIMU_DOWNLINK_IMMEDIATE RunOnceEvery(25, DOWNLINK_SEND_AHRS_EULER(DefaultChannel, DefaultDevice, &CHIMU_DATA.m_sensor.rate[0], &CHIMU_DATA.m_sensor.rate[1], &CHIMU_DATA.m_sensor.rate[2])); #endif } } } }
/** * 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; FLOAT_QUAT_COMP_INV(ltp_to_body_quat, ahrs_impl.ltp_to_imu_quat, ahrs_impl.body_to_imu_quat); /* Set state */ #ifdef AHRS_UPDATE_FW_ESTIMATOR struct FloatEulers neutrals_to_body_eulers = { ins_roll_neutral, ins_pitch_neutral, 0. }; struct FloatQuat neutrals_to_body_quat, ltp_to_neutrals_quat; FLOAT_QUAT_OF_EULERS(neutrals_to_body_quat, neutrals_to_body_eulers); FLOAT_QUAT_NORMALIZE(neutrals_to_body_quat); FLOAT_QUAT_COMP_INV(ltp_to_neutrals_quat, ltp_to_body_quat, neutrals_to_body_quat); stateSetNedToBodyQuat_f(<p_to_neutrals_quat); #else stateSetNedToBodyQuat_f(<p_to_body_quat); #endif /* compute body rates */ struct FloatRates body_rate; FLOAT_RMAT_TRANSP_RATEMULT(body_rate, ahrs_impl.body_to_imu_rmat, ahrs_impl.imu_rate); stateSetBodyRates_f(&body_rate); }
/* * 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); }
void vn100_event_task(void) { if (vn100_trans.status == SPITransSuccess) { parse_ins_msg(); #ifndef INS_VN100_READ_ONLY // Update estimator // FIXME Use a proper rotation matrix here struct FloatEulers att = { ins_eulers.phi - ins_roll_neutral, ins_eulers.theta - ins_pitch_neutral, ins_eulers.psi }; stateSetNedToBodyEulers_f(&att); stateSetBodyRates_f(&ins_rates); #endif //uint8_t s = 4+VN100_REG_QMR_SIZE; //DOWNLINK_SEND_DEBUG(DefaultChannel, DefaultDevice,s,spi_buffer_input); vn100_trans.status = SPITransDone; } if (vn100_trans.status == SPITransFailed) { vn100_trans.status = SPITransDone; // FIXME retry config if not done ? } }
void parse_ins_msg(void) { while (InsLink(ChAvailable())) { uint8_t ch = InsLink(Getch()); if (CHIMU_Parse(ch, 0, &CHIMU_DATA)) { RunOnceEvery(25, LED_TOGGLE(3)); if (CHIMU_DATA.m_MsgID == CHIMU_Msg_3_IMU_Attitude) { new_ins_attitude = 1; if (CHIMU_DATA.m_attitude.euler.phi > M_PI) { CHIMU_DATA.m_attitude.euler.phi -= 2 * M_PI; } struct FloatEulers att = { CHIMU_DATA.m_attitude.euler.phi, CHIMU_DATA.m_attitude.euler.theta, CHIMU_DATA.m_attitude.euler.psi }; stateSetNedToBodyEulers_f(&att); struct FloatRates rates = { CHIMU_DATA.m_sensor.rate[0], CHIMU_DATA.m_attrates.euler.theta, 0. }; // FIXME rate r stateSetBodyRates_f(&rates); //FIXME ahrs_chimu.is_aligned = TRUE; } else if (CHIMU_DATA.m_MsgID == 0x02) { #if CHIMU_DOWNLINK_IMMEDIATE RunOnceEvery(25, DOWNLINK_SEND_AHRS_EULER(DefaultChannel, DefaultDevice, &CHIMU_DATA.m_sensor.rate[0], &CHIMU_DATA.m_sensor.rate[1], &CHIMU_DATA.m_sensor.rate[2])); #endif } } } }
void ahrs_propagate(void) { struct NedCoor_f accel; struct FloatRates body_rates; struct FloatEulers eulers; // realign all the filter if needed // a complete init cycle is required if (ins_impl.reset) { ins_impl.reset = FALSE; ins.status = INS_UNINIT; ahrs.status = AHRS_UNINIT; init_invariant_state(); } // fill command vector struct Int32Rates gyro_meas_body; INT32_RMAT_TRANSP_RATEMULT(gyro_meas_body, imu.body_to_imu_rmat, imu.gyro); RATES_FLOAT_OF_BFP(ins_impl.cmd.rates, gyro_meas_body); struct Int32Vect3 accel_meas_body; INT32_RMAT_TRANSP_VMULT(accel_meas_body, imu.body_to_imu_rmat, imu.accel); ACCELS_FLOAT_OF_BFP(ins_impl.cmd.accel, accel_meas_body); // update correction gains error_output(&ins_impl); // propagate model struct inv_state new_state; runge_kutta_4_float((float*)&new_state, (float*)&ins_impl.state, INV_STATE_DIM, (float*)&ins_impl.cmd, INV_COMMAND_DIM, invariant_model, dt); ins_impl.state = new_state; // normalize quaternion FLOAT_QUAT_NORMALIZE(ins_impl.state.quat); // set global state FLOAT_EULERS_OF_QUAT(eulers, ins_impl.state.quat); #if INS_UPDATE_FW_ESTIMATOR // Some stupid lines of code for neutrals eulers.phi -= ins_roll_neutral; eulers.theta -= ins_pitch_neutral; stateSetNedToBodyEulers_f(&eulers); #else stateSetNedToBodyQuat_f(&ins_impl.state.quat); #endif RATES_DIFF(body_rates, ins_impl.cmd.rates, ins_impl.state.bias); stateSetBodyRates_f(&body_rates); stateSetPositionNed_f(&ins_impl.state.pos); stateSetSpeedNed_f(&ins_impl.state.speed); // untilt accel and remove gravity FLOAT_QUAT_RMAT_B2N(accel, ins_impl.state.quat, ins_impl.cmd.accel); FLOAT_VECT3_SMUL(accel, accel, 1. / (ins_impl.state.as)); FLOAT_VECT3_ADD(accel, A); stateSetAccelNed_f(&accel); //------------------------------------------------------------// RunOnceEvery(3,{ DOWNLINK_SEND_INV_FILTER(DefaultChannel, DefaultDevice, &ins_impl.state.quat.qi, &eulers.phi, &eulers.theta, &eulers.psi, &ins_impl.state.speed.x, &ins_impl.state.speed.y, &ins_impl.state.speed.z, &ins_impl.state.pos.x, &ins_impl.state.pos.y, &ins_impl.state.pos.z, &ins_impl.state.bias.p, &ins_impl.state.bias.q, &ins_impl.state.bias.r, &ins_impl.state.as, &ins_impl.state.hb, &ins_impl.meas.baro_alt, &ins_impl.meas.pos_gps.z) }); #if LOG_INVARIANT_FILTER if (pprzLogFile.fs != NULL) { if (!log_started) { // log file header sdLogWriteLog(&pprzLogFile, "p q r ax ay az gx gy gz gvx gvy gvz mx my mz b qi qx qy qz bp bq br vx vy vz px py pz hb as\n"); log_started = TRUE; } else { sdLogWriteLog(&pprzLogFile, "%.3f %.3f %.3f %.3f %.3f %.3f %.3f %.3f %.3f %.3f %.3f %.3f %.3f %.3f %.3f %.3f %.3f %.3f %.3f %.3f %.3f %.3f %.3f %.3f %.3f %.3f %.3f %.3f %.3f %.3f %.3f\n", ins_impl.cmd.rates.p, ins_impl.cmd.rates.q, ins_impl.cmd.rates.r, ins_impl.cmd.accel.x, ins_impl.cmd.accel.y, ins_impl.cmd.accel.z, ins_impl.meas.pos_gps.x, ins_impl.meas.pos_gps.y, ins_impl.meas.pos_gps.z, ins_impl.meas.speed_gps.x, ins_impl.meas.speed_gps.y, ins_impl.meas.speed_gps.z, ins_impl.meas.mag.x, ins_impl.meas.mag.y, ins_impl.meas.mag.z, ins_impl.meas.baro_alt, ins_impl.state.quat.qi, ins_impl.state.quat.qx, ins_impl.state.quat.qy, ins_impl.state.quat.qz, ins_impl.state.bias.p, ins_impl.state.bias.q, ins_impl.state.bias.r, ins_impl.state.speed.x, ins_impl.state.speed.y, ins_impl.state.speed.z, ins_impl.state.pos.x, ins_impl.state.pos.y, ins_impl.state.pos.z, ins_impl.state.hb, ins_impl.state.as); } } #endif }
void ahrs_propagate(float dt) { struct FloatVect3 accel; struct FloatRates body_rates; // realign all the filter if needed // a complete init cycle is required if (ins_impl.reset) { ins_impl.reset = FALSE; ins.status = INS_UNINIT; ahrs.status = AHRS_UNINIT; init_invariant_state(); } // fill command vector struct Int32Rates gyro_meas_body; struct Int32RMat *body_to_imu_rmat = orientationGetRMat_i(&imu.body_to_imu); int32_rmat_transp_ratemult(&gyro_meas_body, body_to_imu_rmat, &imu.gyro); RATES_FLOAT_OF_BFP(ins_impl.cmd.rates, gyro_meas_body); struct Int32Vect3 accel_meas_body; int32_rmat_transp_vmult(&accel_meas_body, body_to_imu_rmat, &imu.accel); ACCELS_FLOAT_OF_BFP(ins_impl.cmd.accel, accel_meas_body); // update correction gains error_output(&ins_impl); // propagate model struct inv_state new_state; runge_kutta_4_float((float *)&new_state, (float *)&ins_impl.state, INV_STATE_DIM, (float *)&ins_impl.cmd, INV_COMMAND_DIM, invariant_model, dt); ins_impl.state = new_state; // normalize quaternion FLOAT_QUAT_NORMALIZE(ins_impl.state.quat); // set global state stateSetNedToBodyQuat_f(&ins_impl.state.quat); RATES_DIFF(body_rates, ins_impl.cmd.rates, ins_impl.state.bias); stateSetBodyRates_f(&body_rates); stateSetPositionNed_f(&ins_impl.state.pos); stateSetSpeedNed_f(&ins_impl.state.speed); // untilt accel and remove gravity struct FloatQuat q_b2n; float_quat_invert(&q_b2n, &ins_impl.state.quat); float_quat_vmult(&accel, &q_b2n, &ins_impl.cmd.accel); VECT3_SMUL(accel, accel, 1. / (ins_impl.state.as)); VECT3_ADD(accel, A); stateSetAccelNed_f((struct NedCoor_f *)&accel); //------------------------------------------------------------// #if SEND_INVARIANT_FILTER struct FloatEulers eulers; FLOAT_EULERS_OF_QUAT(eulers, ins_impl.state.quat); RunOnceEvery(3, { pprz_msg_send_INV_FILTER(trans, dev, AC_ID, &ins_impl.state.quat.qi, &eulers.phi, &eulers.theta, &eulers.psi, &ins_impl.state.speed.x, &ins_impl.state.speed.y, &ins_impl.state.speed.z, &ins_impl.state.pos.x, &ins_impl.state.pos.y, &ins_impl.state.pos.z, &ins_impl.state.bias.p, &ins_impl.state.bias.q, &ins_impl.state.bias.r, &ins_impl.state.as, &ins_impl.state.hb, &ins_impl.meas.baro_alt, &ins_impl.meas.pos_gps.z) }); #endif #if LOG_INVARIANT_FILTER if (pprzLogFile.fs != NULL) { if (!log_started) { // log file header sdLogWriteLog(&pprzLogFile, "p q r ax ay az gx gy gz gvx gvy gvz mx my mz b qi qx qy qz bp bq br vx vy vz px py pz hb as\n"); log_started = TRUE; } else { sdLogWriteLog(&pprzLogFile, "%.3f %.3f %.3f %.3f %.3f %.3f %.3f %.3f %.3f %.3f %.3f %.3f %.3f %.3f %.3f %.3f %.3f %.3f %.3f %.3f %.3f %.3f %.3f %.3f %.3f %.3f %.3f %.3f %.3f %.3f %.3f\n", ins_impl.cmd.rates.p, ins_impl.cmd.rates.q, ins_impl.cmd.rates.r, ins_impl.cmd.accel.x, ins_impl.cmd.accel.y, ins_impl.cmd.accel.z, ins_impl.meas.pos_gps.x, ins_impl.meas.pos_gps.y, ins_impl.meas.pos_gps.z, ins_impl.meas.speed_gps.x, ins_impl.meas.speed_gps.y, ins_impl.meas.speed_gps.z, ins_impl.meas.mag.x, ins_impl.meas.mag.y, ins_impl.meas.mag.z, ins_impl.meas.baro_alt, ins_impl.state.quat.qi, ins_impl.state.quat.qx, ins_impl.state.quat.qy, ins_impl.state.quat.qz, ins_impl.state.bias.p, ins_impl.state.bias.q, ins_impl.state.bias.r, ins_impl.state.speed.x, ins_impl.state.speed.y, ins_impl.state.speed.z, ins_impl.state.pos.x, ins_impl.state.pos.y, ins_impl.state.pos.z, ins_impl.state.hb, ins_impl.state.as); } } #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 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 }