void ahrs_update_gps(void) {
if (gps.fix == GPS_FIX_3D && ins.status == INS_RUNNING) {
ins_gps_fix_once = TRUE;
#if INS_UPDATE_FW_ESTIMATOR
if (state.utm_initialized_f) {
// position (local ned)
ins_impl.meas.pos_gps.x = (gps.utm_pos.north / 100.0f) - state.utm_origin_f.north;
ins_impl.meas.pos_gps.y = (gps.utm_pos.east / 100.0f) - state.utm_origin_f.east;
ins_impl.meas.pos_gps.z = state.utm_origin_f.alt - (gps.hmsl / 1000.0f);
// speed
ins_impl.meas.speed_gps.x = gps.ned_vel.x / 100.0f;
ins_impl.meas.speed_gps.y = gps.ned_vel.y / 100.0f;
ins_impl.meas.speed_gps.z = gps.ned_vel.z / 100.0f;
}
#else
if (state.ned_initialized_f) {
struct EcefCoor_f ecef_pos, ecef_vel;
ECEF_FLOAT_OF_BFP(ecef_pos, gps.ecef_pos);
ned_of_ecef_point_f(&ins_impl.meas.pos_gps, &state.ned_origin_f, &ecef_pos);
ECEF_FLOAT_OF_BFP(ecef_vel, gps.ecef_vel);
ned_of_ecef_vect_f(&ins_impl.meas.speed_gps, &state.ned_origin_f, &ecef_vel);
}
#endif
}
}
void stateCalcPositionEcef_f(void)
{
if (bit_is_set(state.pos_status, POS_ECEF_F)) {
return;
}
if (bit_is_set(state.pos_status, POS_ECEF_I)) {
ECEF_FLOAT_OF_BFP(state.ecef_pos_f, state.ecef_pos_i);
} else if (bit_is_set(state.pos_status, POS_NED_F) && state.ned_initialized_f) {
ecef_of_ned_point_f(&state.ecef_pos_f, &state.ned_origin_f, &state.ned_pos_f);
} else if (bit_is_set(state.pos_status, POS_NED_I) && state.ned_initialized_i) {
/* transform ned_i -> ecef_i -> ecef_f, set status bits */
ecef_of_ned_pos_i(&state.ecef_pos_i, &state.ned_origin_i, &state.ned_pos_i);
SetBit(state.pos_status, POS_ECEF_F);
ECEF_FLOAT_OF_BFP(state.ecef_pos_f, state.ecef_pos_i);
} else if (bit_is_set(state.pos_status, POS_LLA_F)) {
ecef_of_lla_f(&state.ecef_pos_f, &state.lla_pos_f);
} else if (bit_is_set(state.pos_status, POS_LLA_I)) {
LLA_FLOAT_OF_BFP(state.lla_pos_f, state.lla_pos_i);
SetBit(state.pos_status, POS_LLA_F);
ecef_of_lla_f(&state.ecef_pos_f, &state.lla_pos_f);
} else {
/* could not get this representation, set errno */
//struct EcefCoor_f _ecef_zero = {0.0f};
//return _ecef_zero;
}
/* set bit to indicate this representation is computed */
SetBit(state.pos_status, POS_ECEF_F);
}
void stateCalcPositionLla_f(void) {
if (bit_is_set(state.pos_status, POS_LLA_F))
return;
if (bit_is_set(state.pos_status, POS_LLA_I)) {
LLA_FLOAT_OF_BFP(state.lla_pos_f, state.lla_pos_f);
}
else if (bit_is_set(state.pos_status, POS_ECEF_F)) {
lla_of_ecef_f(&state.lla_pos_f, &state.ecef_pos_f);
}
else if (bit_is_set(state.pos_status, POS_ECEF_I)) {
/* transform ecef_i -> ecef_f -> lla_f, set status bits */
ECEF_FLOAT_OF_BFP(state.ecef_pos_f, state.ecef_pos_i);
SetBit(state.pos_status, POS_ECEF_F);
lla_of_ecef_f(&state.lla_pos_f, &state.ecef_pos_f);
}
else if (bit_is_set(state.pos_status, POS_NED_F)) {
/* transform ned_f -> ecef_f -> lla_f, set status bits */
ecef_of_ned_point_f(&state.ecef_pos_f, &state.ned_origin_f, &state.ned_pos_f);
SetBit(state.pos_status, POS_ECEF_F);
lla_of_ecef_f(&state.lla_pos_f, &state.ecef_pos_f);
}
else if (bit_is_set(state.pos_status, POS_NED_I)) {
/* transform ned_i -> ned_f -> ecef_f -> lla_f, set status bits */
NED_FLOAT_OF_BFP(state.ned_pos_f, state.ned_pos_i);
SetBit(state.pos_status, POS_NED_F);
ecef_of_ned_point_f(&state.ecef_pos_f, &state.ned_origin_f, &state.ned_pos_f);
SetBit(state.pos_status, POS_ECEF_F);
lla_of_ecef_f(&state.lla_pos_f, &state.ecef_pos_f);
}
else if (bit_is_set(state.pos_status, POS_UTM_F)) {
lla_of_utm_f(&state.lla_pos_f, &state.utm_pos_f);
}
else {
/* could not get this representation, set errno */
//struct LlaCoor_f _lla_zero = {0.0};
//return _lla_zero;
}
/* set bit to indicate this representation is computed */
SetBit(state.pos_status, POS_LLA_F);
}
/** Convert a ECEF to LLA.
* @param[out] out LLA in degrees*1e7 and mm above ellipsoid
* @param[in] in ECEF in cm
*/
void lla_of_ecef_i(struct LlaCoor_i *out, struct EcefCoor_i *in)
{
#if USE_SINGLE_PRECISION_LLA_ECEF
/* convert our input to floating point */
struct EcefCoor_f in_f;
ECEF_FLOAT_OF_BFP(in_f, *in);
/* calls the floating point transformation */
struct LlaCoor_f out_f;
lla_of_ecef_f(&out_f, &in_f);
/* convert the output to fixed point */
LLA_BFP_OF_REAL(*out, out_f);
#else // use double precision by default
/* convert our input to floating point */
struct EcefCoor_d in_d;
ECEF_DOUBLE_OF_BFP(in_d, *in);
/* calls the floating point transformation */
struct LlaCoor_d out_d;
lla_of_ecef_d(&out_d, &in_d);
/* convert the output to fixed point */
LLA_BFP_OF_REAL(*out, out_d);
#endif
}