/** Convert a local ENU position to ECEF. * @param[out] ecef ECEF position in cm * @param[in] def local coordinate system definition * @param[in] enu ENU position in meter << #INT32_POS_FRAC */ void ecef_of_enu_pos_i(struct EcefCoor_i *ecef, struct LtpDef_i *def, struct EnuCoor_i *enu) { /* enu_cm = (enu * 100) >> INT32_POS_FRAC * to loose less range: * enu_cm = (enu * 25) >> (INT32_POS_FRAC-2) * which puts max enu input Q23.8 range to 8388km / 25 = 335km */ struct EnuCoor_i enu_cm; VECT3_SMUL(enu_cm, *enu, 25); INT32_VECT3_RSHIFT(enu_cm, enu_cm, INT32_POS_FRAC - 2); ecef_of_enu_vect_i(ecef, def, &enu_cm); VECT3_ADD(*ecef, def->ecef); }
void ecef_of_enu_point_i(struct EcefCoor_i* ecef, struct LtpDef_i* def, struct EnuCoor_i* enu) { ecef_of_enu_vect_i(ecef, def, enu); INT32_VECT3_ADD(*ecef, def->ecef); }
void ecef_of_ned_vect_i(struct EcefCoor_i* ecef, struct LtpDef_i* def, struct NedCoor_i* ned) { struct EnuCoor_i enu; ENU_OF_TO_NED(enu, *ned); ecef_of_enu_vect_i(ecef, def, &enu); }