/** Convert a ECEF position to local NED. * @param[out] ned NED position in meter << #INT32_POS_FRAC * @param[in] def local coordinate system definition * @param[in] ecef ECEF position in cm */ void ned_of_ecef_pos_i(struct NedCoor_i *ned, struct LtpDef_i *def, struct EcefCoor_i *ecef) { struct EnuCoor_i enu; enu_of_ecef_pos_i(&enu, def, ecef); ENU_OF_TO_NED(*ned, enu); }
void stateCalcPositionEnu_f(void) { if (bit_is_set(state.pos_status, POS_ENU_F)) { return; } int errno = 0; if (state.ned_initialized_f) { if (bit_is_set(state.pos_status, POS_NED_F)) { VECT3_ENU_OF_NED(state.enu_pos_f, state.ned_pos_f); } else if (bit_is_set(state.pos_status, POS_ENU_I)) { ENU_FLOAT_OF_BFP(state.enu_pos_f, state.enu_pos_i); } else if (bit_is_set(state.pos_status, POS_NED_I)) { NED_FLOAT_OF_BFP(state.ned_pos_f, state.ned_pos_i); SetBit(state.pos_status, POS_NED_F); VECT3_ENU_OF_NED(state.enu_pos_f, state.ned_pos_f); } else if (bit_is_set(state.pos_status, POS_ECEF_F)) { enu_of_ecef_point_f(&state.enu_pos_f, &state.ned_origin_f, &state.ecef_pos_f); } else if (bit_is_set(state.pos_status, POS_ECEF_I)) { /* transform ecef_i -> enu_i -> enu_f, set status bits */ enu_of_ecef_pos_i(&state.enu_pos_i, &state.ned_origin_i, &state.ecef_pos_i); SetBit(state.pos_status, POS_ENU_I); ENU_FLOAT_OF_BFP(state.enu_pos_f, state.enu_pos_i); } else if (bit_is_set(state.pos_status, POS_LLA_F)) { enu_of_lla_point_f(&state.enu_pos_f, &state.ned_origin_f, &state.lla_pos_f); } else if (bit_is_set(state.pos_status, POS_LLA_I)) { /* transform lla_i -> ecef_i -> enu_i -> enu_f, set status bits */ ecef_of_lla_i(&state.ecef_pos_i, &state.lla_pos_i); /* converts to doubles internally */ SetBit(state.pos_status, POS_ECEF_I); enu_of_ecef_pos_i(&state.enu_pos_i, &state.ned_origin_i, &state.ecef_pos_i); SetBit(state.pos_status, POS_ENU_I); ENU_FLOAT_OF_BFP(state.enu_pos_f, state.enu_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_I)) { ENU_FLOAT_OF_BFP(state.enu_pos_f, state.enu_pos_i); } else if (bit_is_set(state.pos_status, POS_NED_F)) { VECT3_ENU_OF_NED(state.enu_pos_f, state.ned_pos_f); } else if (bit_is_set(state.pos_status, POS_NED_I)) { NED_FLOAT_OF_BFP(state.ned_pos_f, state.ned_pos_i); SetBit(state.pos_status, POS_NED_F); VECT3_ENU_OF_NED(state.enu_pos_f, state.ned_pos_f); } else if (bit_is_set(state.pos_status, POS_UTM_F)) { ENU_OF_UTM_DIFF(state.enu_pos_f, state.utm_pos_f, state.utm_origin_f); } else if (bit_is_set(state.pos_status, POS_LLA_F)) { /* transform lla_f -> utm_f -> enu, 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); } else if (bit_is_set(state.pos_status, POS_LLA_I)) { /* transform lla_i -> lla_f -> utm_f -> enu, 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); } else { /* could not get this representation, set errno */ errno = 2; } } else { /* ned coordinate system not initialized, set errno */ errno = 3; } if (errno) { //struct EnuCoor_f _enu_zero = {0.0f}; //return _enu_zero; } /* set bit to indicate this representation is computed */ SetBit(state.pos_status, POS_ENU_F); }