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; in_f.x = M_OF_CM((float)in->x); in_f.y = M_OF_CM((float)in->y); in_f.z = M_OF_CM((float)in->z); /* calls the floating point transformation */ struct LlaCoor_f out_f; lla_of_ecef_f(&out_f, &in_f); /* convert the output to fixed point */ out->lon = (int32_t)rint(EM7DEG_OF_RAD(out_f.lon)); out->lat = (int32_t)rint(EM7DEG_OF_RAD(out_f.lat)); out->alt = (int32_t)MM_OF_M(out_f.alt); #else // use double precision by default /* convert our input to floating point */ struct EcefCoor_d in_d; in_d.x = M_OF_CM((double)in->x); in_d.y = M_OF_CM((double)in->y); in_d.z = M_OF_CM((double)in->z); /* calls the floating point transformation */ struct LlaCoor_d out_d; lla_of_ecef_d(&out_d, &in_d); /* convert the output to fixed point */ out->lon = (int32_t)rint(EM7DEG_OF_RAD(out_d.lon)); out->lat = (int32_t)rint(EM7DEG_OF_RAD(out_d.lat)); out->alt = (int32_t)MM_OF_M(out_d.alt); #endif }
void stateCalcPositionLla_i(void) { if (bit_is_set(state.pos_status, POS_LLA_I)) return; if (bit_is_set(state.pos_status, POS_LLA_F)) { LLA_BFP_OF_REAL(state.lla_pos_i, state.lla_pos_f); } else if (bit_is_set(state.pos_status, POS_ECEF_I)) { lla_of_ecef_i(&state.lla_pos_i, &state.ecef_pos_i); } else if (bit_is_set(state.pos_status, POS_ECEF_F)) { /* transform ecef_f -> lla_f, set status bit, then convert to int */ lla_of_ecef_f(&state.lla_pos_f, &state.ecef_pos_f); SetBit(state.pos_status, POS_LLA_F); LLA_BFP_OF_REAL(state.lla_pos_i, state.lla_pos_f); } else if (bit_is_set(state.pos_status, POS_NED_F)) { /* transform ned_f -> ecef_f -> lla_f -> lla_i, 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); SetBit(state.pos_status, POS_LLA_F); LLA_BFP_OF_REAL(state.lla_pos_i, state.lla_pos_f); } else if (bit_is_set(state.pos_status, POS_NED_I)) { /* transform ned_i -> ecef_i -> lla_i, set status bits */ /// @todo check if resolution is enough ecef_of_ned_point_i(&state.ecef_pos_i, &state.ned_origin_i, &state.ned_pos_i); SetBit(state.pos_status, POS_ECEF_I); lla_of_ecef_i(&state.lla_pos_i, &state.ecef_pos_i); /* uses double version internally */ } else if (bit_is_set(state.pos_status, POS_UTM_F)) { /* transform utm_f -> lla_f -> lla_i, set status bits */ lla_of_utm_f(&state.lla_pos_f, &state.utm_pos_f); SetBit(state.pos_status, POS_LLA_F); LLA_BFP_OF_REAL(state.lla_pos_i, state.lla_pos_f); } else { /* could not get this representation, set errno */ //struct LlaCoor_i _lla_zero = {0}; //return _lla_zero; } /* set bit to indicate this representation is computed */ SetBit(state.pos_status, POS_LLA_I); }
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); }
void ltp_def_from_ecef_f(struct LtpDef_f* def, struct EcefCoor_f* ecef) { /* store the origin of the tangeant plane */ VECT3_COPY(def->ecef, *ecef); /* compute the lla representation of the origin */ lla_of_ecef_f(&def->lla, &def->ecef); /* store the rotation matrix */ const float sin_lat = sinf(def->lla.lat); const float cos_lat = cosf(def->lla.lat); const float sin_lon = sinf(def->lla.lon); const float cos_lon = cosf(def->lla.lon); def->ltp_of_ecef.m[0] = -sin_lon; def->ltp_of_ecef.m[1] = cos_lon; def->ltp_of_ecef.m[2] = 0.; /* this element is always zero http://en.wikipedia.org/wiki/Geodetic_system#From_ECEF_to_ENU */ def->ltp_of_ecef.m[3] = -sin_lat*cos_lon; def->ltp_of_ecef.m[4] = -sin_lat*sin_lon; def->ltp_of_ecef.m[5] = cos_lat; def->ltp_of_ecef.m[6] = cos_lat*cos_lon; def->ltp_of_ecef.m[7] = cos_lat*sin_lon; def->ltp_of_ecef.m[8] = sin_lat; }
/** 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 }