/// Utility function: converts lla (int) to local point (float)
bool_t mission_point_of_lla(struct EnuCoor_f *point, struct LlaCoor_i *lla)
{
// return FALSE if there is no valid local coordinate system
if (!state.ned_initialized_i) {
return FALSE;
}
// change geoid alt to ellipsoid alt
lla->alt = lla->alt - state.ned_origin_i.hmsl + state.ned_origin_i.lla.alt;
//Compute ENU components from LLA with respect to ltp origin
struct EnuCoor_i tmp_enu_point_i;
enu_of_lla_point_i(&tmp_enu_point_i, &state.ned_origin_i, lla);
struct EnuCoor_f tmp_enu_point_f;
ENU_FLOAT_OF_BFP(tmp_enu_point_f, tmp_enu_point_i);
//Bound the new waypoint with max distance from home
struct EnuCoor_f home;
ENU_FLOAT_OF_BFP(home, waypoints[WP_HOME]);
struct FloatVect2 vect_from_home;
VECT2_DIFF(vect_from_home, tmp_enu_point_f, home);
//Saturate the mission wp not to overflow max_dist_from_home
//including a buffer zone before limits
float dist_to_home = float_vect2_norm(&vect_from_home);
dist_to_home += BUFFER_ZONE_DIST;
if (dist_to_home > MAX_DIST_FROM_HOME) {
VECT2_SMUL(vect_from_home, vect_from_home, (MAX_DIST_FROM_HOME / dist_to_home));
}
// set new point
VECT2_SUM(*point, home, vect_from_home);
point->z = tmp_enu_point_f.z;
return TRUE;
}
void stateCalcSpeedEnu_f(void) {
if (bit_is_set(state.speed_status, SPEED_ENU_F))
return;
int errno = 0;
if (state.ned_initialized_f) {
if (bit_is_set(state.speed_status, SPEED_NED_F)) {
VECT3_ENU_OF_NED(state.enu_speed_f, state.ned_speed_f);
}
else if (bit_is_set(state.speed_status, SPEED_ENU_I)) {
ENU_FLOAT_OF_BFP(state.enu_speed_f, state.enu_speed_i);
}
else if (bit_is_set(state.speed_status, SPEED_NED_I)) {
SPEEDS_FLOAT_OF_BFP(state.ned_speed_f, state.ned_speed_i);
SetBit(state.pos_status, SPEED_NED_F);
VECT3_ENU_OF_NED(state.enu_speed_f, state.ned_speed_f);
}
else if (bit_is_set(state.speed_status, SPEED_ECEF_F)) {
enu_of_ecef_vect_f(&state.enu_speed_f, &state.ned_origin_f, &state.ecef_speed_f);
}
else if (bit_is_set(state.speed_status, SPEED_ECEF_I)) {
/* transform ecef_I -> ecef_f -> enu_f , set status bits */
SPEEDS_FLOAT_OF_BFP(state.ecef_speed_f, state.ecef_speed_i);
SetBit(state.speed_status, SPEED_ECEF_F);
enu_of_ecef_vect_f(&state.enu_speed_f, &state.ned_origin_f, &state.ecef_speed_f);
}
else { /* could not get this representation, set errno */
errno = 1;
}
}
else if (state.utm_initialized_f) {
if (bit_is_set(state.speed_status, SPEED_NED_F)) {
VECT3_ENU_OF_NED(state.enu_speed_f, state.ned_speed_f);
}
else if (bit_is_set(state.speed_status, SPEED_ENU_I)) {
ENU_FLOAT_OF_BFP(state.enu_speed_f, state.enu_speed_i);
}
else if (bit_is_set(state.speed_status, SPEED_NED_I)) {
SPEEDS_FLOAT_OF_BFP(state.ned_speed_f, state.ned_speed_i);
SetBit(state.pos_status, SPEED_NED_F);
VECT3_ENU_OF_NED(state.enu_speed_f, state.ned_speed_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};
//return _enu_zero;
}
/* set bit to indicate this representation is computed */
SetBit(state.speed_status, SPEED_ENU_F);
}
void stateCalcPositionUtm_f(void)
{
if (bit_is_set(state.pos_status, POS_UTM_F)) {
return;
}
if (bit_is_set(state.pos_status, POS_LLA_F)) {
utm_of_lla_f(&state.utm_pos_f, &state.lla_pos_f);
} else if (bit_is_set(state.pos_status, POS_LLA_I)) {
/* transform lla_i -> lla_f -> utm_f, 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);
} else if (state.utm_initialized_f) {
if (bit_is_set(state.pos_status, POS_ENU_F)) {
UTM_OF_ENU_ADD(state.utm_pos_f, state.enu_pos_f, state.utm_origin_f);
} else if (bit_is_set(state.pos_status, POS_ENU_I)) {
ENU_FLOAT_OF_BFP(state.enu_pos_f, state.enu_pos_i);
SetBit(state.pos_status, POS_ENU_F);
UTM_OF_ENU_ADD(state.utm_pos_f, state.enu_pos_f, state.utm_origin_f);
} else if (bit_is_set(state.pos_status, POS_NED_F)) {
UTM_OF_NED_ADD(state.utm_pos_f, state.ned_pos_f, state.utm_origin_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);
UTM_OF_NED_ADD(state.utm_pos_f, state.ned_pos_f, state.utm_origin_f);
}
} else {
/* could not get this representation, set errno */
//struct EcefCoor_f _ecef_zero = {0.0f};
//return _ecef_zero;
}
/* set bit to indicate this representation is computed */
SetBit(state.pos_status, POS_UTM_F);
}
void waypoint_set_enu_i(uint8_t wp_id, struct EnuCoor_i *enu)
{
if (wp_id < nb_waypoint) {
waypoints[wp_id].enu_i = *enu;
SetBit(waypoints[wp_id].flags, WP_FLAG_ENU_I);
ENU_FLOAT_OF_BFP(waypoints[wp_id].enu_f, waypoints[wp_id].enu_i);
SetBit(waypoints[wp_id].flags, WP_FLAG_ENU_F);
ClearBit(waypoints[wp_id].flags, WP_FLAG_LLA_I);
waypoint_globalize(wp_id);
}
}
/** update local ENU coordinates from its LLA coordinates */
void waypoint_localize(uint8_t wp_id)
{
if (state.ned_initialized_i) {
struct EnuCoor_i enu;
enu_of_lla_point_i(&enu, &state.ned_origin_i, &waypoints[wp_id].lla);
// convert ENU pos from cm to BFP with INT32_POS_FRAC
enu.x = POS_BFP_OF_REAL(enu.x) / 100;
enu.y = POS_BFP_OF_REAL(enu.y) / 100;
enu.z = POS_BFP_OF_REAL(enu.z) / 100;
waypoints[wp_id].enu_i = enu;
SetBit(waypoints[wp_id].flags, WP_FLAG_ENU_I);
ENU_FLOAT_OF_BFP(waypoints[wp_id].enu_f, waypoints[wp_id].enu_i);
SetBit(waypoints[wp_id].flags, WP_FLAG_ENU_F);
}
}
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);
}
