void ins_propagate() {
/* untilt accels */
struct Int32Vect3 accel_body;
INT32_RMAT_TRANSP_VMULT(accel_body, imu.body_to_imu_rmat, imu.accel);
struct Int32Vect3 accel_ltp;
INT32_RMAT_TRANSP_VMULT(accel_ltp, ahrs.ltp_to_body_rmat, accel_body);
float z_accel_float = ACCEL_FLOAT_OF_BFP(accel_ltp.z);
#ifdef USE_VFF
if (baro.status == BS_RUNNING && ins_baro_initialised) {
vff_propagate(z_accel_float);
ins_ltp_accel.z = ACCEL_BFP_OF_REAL(vff_zdotdot);
ins_ltp_speed.z = SPEED_BFP_OF_REAL(vff_zdot);
ins_ltp_pos.z = POS_BFP_OF_REAL(vff_z);
}
else { // feed accel from the sensors
ins_ltp_accel.z = ACCEL_BFP_OF_REAL(z_accel_float);
}
#else
ins_ltp_accel.z = ACCEL_BFP_OF_REAL(z_accel_float);
#endif /* USE_VFF */
#ifdef USE_HFF
/* propagate horizontal filter */
b2_hff_propagate();
#else
ins_ltp_accel.x = accel_ltp.x;
ins_ltp_accel.y = accel_ltp.y;
#endif /* USE_HFF */
INT32_VECT3_ENU_OF_NED(ins_enu_pos, ins_ltp_pos);
INT32_VECT3_ENU_OF_NED(ins_enu_speed, ins_ltp_speed);
INT32_VECT3_ENU_OF_NED(ins_enu_accel, ins_ltp_accel);
}
void ins_propagate() {
/* untilt accels */
struct Int32Vect3 accel_meas_body;
INT32_RMAT_TRANSP_VMULT(accel_meas_body, imu.body_to_imu_rmat, imu.accel);
struct Int32Vect3 accel_meas_ltp;
INT32_RMAT_TRANSP_VMULT(accel_meas_ltp, ahrs.ltp_to_body_rmat, accel_meas_body);
#if USE_VFF
float z_accel_meas_float = ACCEL_FLOAT_OF_BFP(accel_meas_ltp.z);
if (baro.status == BS_RUNNING && ins_baro_initialised) {
vff_propagate(z_accel_meas_float);
ins_ltp_accel.z = ACCEL_BFP_OF_REAL(vff_zdotdot);
ins_ltp_speed.z = SPEED_BFP_OF_REAL(vff_zdot);
ins_ltp_pos.z = POS_BFP_OF_REAL(vff_z);
}
else { // feed accel from the sensors
// subtract -9.81m/s2 (acceleration measured due to gravity, but vehivle not accelerating in ltp)
ins_ltp_accel.z = accel_meas_ltp.z + ACCEL_BFP_OF_REAL(9.81);
}
#else
ins_ltp_accel.z = accel_meas_ltp.z + ACCEL_BFP_OF_REAL(9.81);
#endif /* USE_VFF */
#if USE_HFF
/* propagate horizontal filter */
b2_hff_propagate();
#else
ins_ltp_accel.x = accel_meas_ltp.x;
ins_ltp_accel.y = accel_meas_ltp.y;
#endif /* USE_HFF */
INT32_VECT3_ENU_OF_NED(ins_enu_pos, ins_ltp_pos);
INT32_VECT3_ENU_OF_NED(ins_enu_speed, ins_ltp_speed);
INT32_VECT3_ENU_OF_NED(ins_enu_accel, ins_ltp_accel);
}
void ins_update_gps(void) {
if (gps_state.fix == GPS_FIX_3D) {
if (!ins.ltp_initialised) {
ltp_def_from_ecef_i(&ins.ltp_def, &gps_state.ecef_pos);
ins.ltp_initialised = TRUE;
}
ned_of_ecef_point_i(&ins.gps_pos_cm_ned, &ins.ltp_def, &gps_state.ecef_pos);
ned_of_ecef_vect_i(&ins.gps_speed_cm_s_ned, &ins.ltp_def, &gps_state.ecef_speed);
#ifdef USE_HFF
b2ins_update_gps();
VECT2_SDIV(ins.ltp_pos, (1<<(B2INS_POS_LTP_FRAC-INT32_POS_FRAC)), b2ins_pos_ltp);
VECT2_SDIV(ins.ltp_speed, (1<<(B2INS_SPEED_LTP_FRAC-INT32_SPEED_FRAC)), b2ins_speed_ltp);
#else
INT32_VECT3_SCALE_2(b2ins_meas_gps_pos_ned, ins.gps_pos_cm_ned,
INT32_POS_OF_CM_NUM, INT32_POS_OF_CM_DEN);
INT32_VECT3_SCALE_2(b2ins_meas_gps_speed_ned, ins.gps_speed_cm_s_ned,
INT32_SPEED_OF_CM_S_NUM, INT32_SPEED_OF_CM_S_DEN);
VECT3_COPY(ins.ltp_pos, b2ins_meas_gps_pos_ned);
VECT3_COPY(ins.ltp_speed, b2ins_meas_gps_speed_ned);
#endif
INT32_VECT3_ENU_OF_NED(ins.enu_pos, ins.ltp_pos);
INT32_VECT3_ENU_OF_NED(ins.enu_speed, ins.ltp_speed);
INT32_VECT3_ENU_OF_NED(ins.enu_accel, ins.ltp_accel);
}
}
void ins_update_gps(void) {
#ifdef USE_GPS
if (booz_gps_state.fix == BOOZ2_GPS_FIX_3D) {
if (!ins_ltp_initialised) {
ltp_def_from_ecef_i(&ins_ltp_def, &booz_gps_state.ecef_pos);
ins_ltp_def.lla.alt = booz_gps_state.lla_pos.alt;
ins_ltp_def.hmsl = booz_gps_state.hmsl;
ins_ltp_initialised = TRUE;
}
ned_of_ecef_point_i(&ins_gps_pos_cm_ned, &ins_ltp_def, &booz_gps_state.ecef_pos);
ned_of_ecef_vect_i(&ins_gps_speed_cm_s_ned, &ins_ltp_def, &booz_gps_state.ecef_vel);
#ifdef USE_HFF
VECT2_ASSIGN(ins_gps_pos_m_ned, ins_gps_pos_cm_ned.x, ins_gps_pos_cm_ned.y);
VECT2_SDIV(ins_gps_pos_m_ned, ins_gps_pos_m_ned, 100.);
VECT2_ASSIGN(ins_gps_speed_m_s_ned, ins_gps_speed_cm_s_ned.x, ins_gps_speed_cm_s_ned.y);
VECT2_SDIV(ins_gps_speed_m_s_ned, ins_gps_speed_m_s_ned, 100.);
if (ins_hf_realign) {
ins_hf_realign = FALSE;
#ifdef SITL
struct FloatVect2 true_pos, true_speed;
VECT2_COPY(true_pos, fdm.ltpprz_pos);
VECT2_COPY(true_speed, fdm.ltpprz_ecef_vel);
b2_hff_realign(true_pos, true_speed);
#else
const struct FloatVect2 zero = {0.0, 0.0};
b2_hff_realign(ins_gps_pos_m_ned, zero);
#endif
}
b2_hff_update_gps();
#ifndef USE_VFF /* vff not used */
ins_ltp_pos.z = (ins_gps_pos_cm_ned.z * INT32_POS_OF_CM_NUM) / INT32_POS_OF_CM_DEN;
ins_ltp_speed.z = (ins_gps_speed_cm_s_ned.z * INT32_SPEED_OF_CM_S_NUM) INT32_SPEED_OF_CM_S_DEN;
#endif /* vff not used */
#endif /* hff used */
#ifndef USE_HFF /* hff not used */
#ifndef USE_VFF /* neither hf nor vf used */
INT32_VECT3_SCALE_3(ins_ltp_pos, ins_gps_pos_cm_ned, INT32_POS_OF_CM_NUM, INT32_POS_OF_CM_DEN);
INT32_VECT3_SCALE_3(ins_ltp_speed, ins_gps_speed_cm_s_ned, INT32_SPEED_OF_CM_S_NUM, INT32_SPEED_OF_CM_S_DEN);
#else /* only vff used */
INT32_VECT2_SCALE_2(ins_ltp_pos, ins_gps_pos_cm_ned, INT32_POS_OF_CM_NUM, INT32_POS_OF_CM_DEN);
INT32_VECT2_SCALE_2(ins_ltp_speed, ins_gps_speed_cm_s_ned, INT32_SPEED_OF_CM_S_NUM, INT32_SPEED_OF_CM_S_DEN);
#endif
#ifdef USE_GPS_LAG_HACK
VECT2_COPY(d_pos, ins_ltp_speed);
INT32_VECT2_RSHIFT(d_pos, d_pos, 11);
VECT2_ADD(ins_ltp_pos, d_pos);
#endif
#endif /* hff not used */
INT32_VECT3_ENU_OF_NED(ins_enu_pos, ins_ltp_pos);
INT32_VECT3_ENU_OF_NED(ins_enu_speed, ins_ltp_speed);
INT32_VECT3_ENU_OF_NED(ins_enu_accel, ins_ltp_accel);
}
#endif /* USE_GPS */
}
void stateCalcSpeedEnu_i(void) {
if (bit_is_set(state.speed_status, SPEED_ENU_I))
return;
int errno = 0;
if (state.ned_initialized_i) {
if (bit_is_set(state.speed_status, SPEED_NED_I)) {
INT32_VECT3_ENU_OF_NED(state.enu_speed_i, state.ned_speed_i);
}
if (bit_is_set(state.speed_status, SPEED_ENU_F)) {
ENU_BFP_OF_REAL(state.enu_speed_i, state.enu_speed_f);
}
else if (bit_is_set(state.speed_status, SPEED_NED_F)) {
SPEEDS_BFP_OF_REAL(state.ned_speed_i, state.ned_speed_f);
SetBit(state.pos_status, SPEED_NED_I);
INT32_VECT3_ENU_OF_NED(state.enu_speed_i, state.ned_speed_i);
}
else if (bit_is_set(state.speed_status, SPEED_ECEF_I)) {
enu_of_ecef_vect_i(&state.enu_speed_i, &state.ned_origin_i, &state.ecef_speed_i);
}
else if (bit_is_set(state.speed_status, SPEED_ECEF_F)) {
/* transform ecef_f -> ecef_i -> enu_i , set status bits */
SPEEDS_BFP_OF_REAL(state.ecef_speed_i, state.ecef_speed_f);
SetBit(state.speed_status, SPEED_ECEF_I);
enu_of_ecef_vect_i(&state.enu_speed_i, &state.ned_origin_i, &state.ecef_speed_i);
}
else { /* could not get this representation, set errno */
errno = 1;
}
}
else if (state.utm_initialized_f) {
if (bit_is_set(state.speed_status, SPEED_NED_I)) {
INT32_VECT3_ENU_OF_NED(state.enu_speed_i, state.ned_speed_i);
}
if (bit_is_set(state.speed_status, SPEED_ENU_F)) {
ENU_BFP_OF_REAL(state.enu_speed_i, state.enu_speed_f);
}
else if (bit_is_set(state.speed_status, SPEED_NED_F)) {
SPEEDS_BFP_OF_REAL(state.ned_speed_i, state.ned_speed_f);
SetBit(state.pos_status, SPEED_NED_I);
INT32_VECT3_ENU_OF_NED(state.enu_speed_i, state.ned_speed_i);
}
else { /* could not get this representation, set errno */
errno = 2;
}
}
else { /* ned coordinate system not initialized, set errno */
errno = 3;
}
if (errno) {
//struct EnuCoor_i _enu_zero = {0};
//return _enu_zero;
}
/* set bit to indicate this representation is computed */
SetBit(state.speed_status, SPEED_ENU_I);
}
void stateCalcPositionEnu_i(void)
{
if (bit_is_set(state.pos_status, POS_ENU_I)) {
return;
}
int errno = 0;
if (state.ned_initialized_i) {
if (bit_is_set(state.pos_status, POS_NED_I)) {
INT32_VECT3_ENU_OF_NED(state.enu_pos_i, state.ned_pos_i);
} else if (bit_is_set(state.pos_status, POS_ENU_F)) {
ENU_BFP_OF_REAL(state.enu_pos_i, state.enu_pos_f);
} else if (bit_is_set(state.pos_status, POS_NED_F)) {
NED_BFP_OF_REAL(state.ned_pos_i, state.ned_pos_f);
SetBit(state.pos_status, POS_NED_I);
INT32_VECT3_ENU_OF_NED(state.enu_pos_i, state.ned_pos_i);
} else if (bit_is_set(state.pos_status, POS_ECEF_I)) {
enu_of_ecef_pos_i(&state.enu_pos_i, &state.ned_origin_i, &state.ecef_pos_i);
} else if (bit_is_set(state.pos_status, POS_ECEF_F)) {
/* transform ecef_f -> enu_f, set status bit, then convert to int */
enu_of_ecef_point_f(&state.enu_pos_f, &state.ned_origin_f, &state.ecef_pos_f);
SetBit(state.pos_status, POS_ENU_F);
ENU_BFP_OF_REAL(state.enu_pos_i, state.enu_pos_f);
} else if (bit_is_set(state.pos_status, POS_LLA_F)) {
/* transform lla_f -> ecef_f -> enu_f, set status bits, then convert to int */
ecef_of_lla_f(&state.ecef_pos_f, &state.lla_pos_f);
SetBit(state.pos_status, POS_ECEF_F);
enu_of_ecef_point_f(&state.enu_pos_f, &state.ned_origin_f, &state.ecef_pos_f);
SetBit(state.pos_status, POS_ENU_F);
ENU_BFP_OF_REAL(state.enu_pos_i, state.enu_pos_f);
} else if (bit_is_set(state.pos_status, POS_LLA_I)) {
enu_of_lla_point_i(&state.enu_pos_i, &state.ned_origin_i, &state.lla_pos_i);
} else { /* could not get this representation, set errno */
errno = 1;
}
} else if (state.utm_initialized_f) {
if (bit_is_set(state.pos_status, POS_ENU_F)) {
ENU_BFP_OF_REAL(state.enu_pos_i, state.enu_pos_f);
} else if (bit_is_set(state.pos_status, POS_NED_I)) {
INT32_VECT3_ENU_OF_NED(state.enu_pos_i, state.ned_pos_i);
} else if (bit_is_set(state.pos_status, POS_NED_F)) {
NED_BFP_OF_REAL(state.ned_pos_i, state.ned_pos_f);
SetBit(state.pos_status, POS_NED_I);
INT32_VECT3_ENU_OF_NED(state.enu_pos_i, state.ned_pos_i);
} else if (bit_is_set(state.pos_status, POS_UTM_F)) {
/* transform utm_f -> enu_f -> enu_i , set status bits */
ENU_OF_UTM_DIFF(state.enu_pos_f, state.utm_pos_f, state.utm_origin_f);
SetBit(state.pos_status, POS_ENU_F);
ENU_BFP_OF_REAL(state.enu_pos_i, state.enu_pos_f);
} else if (bit_is_set(state.pos_status, POS_LLA_F)) {
/* transform lla_f -> utm_f -> enu_f -> enu_i , set status bits */
utm_of_lla_f(&state.utm_pos_f, &state.lla_pos_f);
SetBit(state.pos_status, POS_UTM_F);
ENU_OF_UTM_DIFF(state.enu_pos_f, state.utm_pos_f, state.utm_origin_f);
SetBit(state.pos_status, POS_ENU_F);
ENU_BFP_OF_REAL(state.enu_pos_i, state.enu_pos_f);
} else if (bit_is_set(state.pos_status, POS_LLA_I)) {
/* transform lla_i -> lla_f -> utm_f -> enu_f -> enu_i , set status bits */
LLA_FLOAT_OF_BFP(state.lla_pos_f, state.lla_pos_i);
SetBit(state.pos_status, POS_LLA_F);
utm_of_lla_f(&state.utm_pos_f, &state.lla_pos_f);
SetBit(state.pos_status, POS_UTM_F);
ENU_OF_UTM_DIFF(state.enu_pos_f, state.utm_pos_f, state.utm_origin_f);
SetBit(state.pos_status, POS_ENU_F);
ENU_BFP_OF_REAL(state.enu_pos_i, state.enu_pos_f);
} else { /* could not get this representation, set errno */
errno = 2;
}
} else { /* ned coordinate system not initialized, set errno */
errno = 3;
}
if (errno) {
//struct EnuCoor_i _enu_zero = {0};
//return _enu_zero;
}
/* set bit to indicate this representation is computed */
SetBit(state.pos_status, POS_ENU_I);
}