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
}
