/// 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); }