/**
* Calculate LLA (int) from any other available representation.
* Note that since LLA in float has bad precision this is the last choice.
* So we mostly first convert to ECEF and then use lla_of_ecef_i
* which provides higher precision but is currently using the double function internally.
*/
void stateCalcPositionLla_i(void)
{
if (bit_is_set(state.pos_status, POS_LLA_I)) {
return;
}
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 -> ecef_i -> lla_i, set status bits */
ECEF_BFP_OF_REAL(state.ecef_pos_i, state.ecef_pos_f);
SetBit(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_NED_I) && state.ned_initialized_i) {
/* transform ned_i -> ecef_i -> lla_i, 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_I);
lla_of_ecef_i(&state.lla_pos_i, &state.ecef_pos_i);
} else if (bit_is_set(state.pos_status, POS_ENU_I) && state.ned_initialized_i) {
/* transform enu_i -> ecef_i -> lla_i, set status bits */
ecef_of_enu_pos_i(&state.ecef_pos_i, &state.ned_origin_i, &state.enu_pos_i);
SetBit(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_NED_F) && state.ned_initialized_i) {
/* transform ned_f -> ned_i -> ecef_i -> lla_i, set status bits */
NED_BFP_OF_REAL(state.ned_pos_i, state.ned_pos_f);
SetBit(state.pos_status, POS_NED_I);
ecef_of_ned_pos_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);
} else if (bit_is_set(state.pos_status, POS_ENU_F) && state.ned_initialized_i) {
/* transform enu_f -> enu_i -> ecef_i -> lla_i, set status bits */
ENU_BFP_OF_REAL(state.enu_pos_i, state.enu_pos_f);
SetBit(state.pos_status, POS_ENU_I);
ecef_of_enu_pos_i(&state.ecef_pos_i, &state.ned_origin_i, &state.enu_pos_i);
SetBit(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_LLA_F)) {
LLA_BFP_OF_REAL(state.lla_pos_i, state.lla_pos_f);
} 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 waypoint_globalize(uint8_t wp_id)
{
if (state.ned_initialized_i) {
struct EcefCoor_i ecef;
ecef_of_enu_pos_i(&ecef, &state.ned_origin_i, &waypoints[wp_id].enu_i);
lla_of_ecef_i(&waypoints[wp_id].lla, &ecef);
SetBit(waypoints[wp_id].flags, WP_FLAG_LLA_I);
}
}
/** Convert a local NED position to ECEF.
* @param[out] ecef ECEF position in cm
* @param[in] def local coordinate system definition
* @param[in] ned NED position in meter << #INT32_POS_FRAC
*/
void ecef_of_ned_pos_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_pos_i(ecef, def, &enu);
}
