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
}
