//Function that converts target wp from float point versions to int bool_t mission_element_convert(struct _mission_element *el) { struct _mission_element tmp_element = *el; uint8_t i = 0; switch (tmp_element.type) { case MissionWP: ENU_BFP_OF_REAL(el->element.mission_wp.wp.wp_i, tmp_element.element.mission_wp.wp.wp_f); break; case MissionCircle: ENU_BFP_OF_REAL(el->element.mission_circle.center.center_i, tmp_element.element.mission_circle.center.center_f); break; case MissionSegment: ENU_BFP_OF_REAL(el->element.mission_segment.from.from_i, tmp_element.element.mission_segment.from.from_f); ENU_BFP_OF_REAL(el->element.mission_segment.to.to_i, tmp_element.element.mission_segment.to.to_f); break; case MissionPath: for (i = 0; i < 5; i++) { ENU_BFP_OF_REAL(el->element.mission_path.path.path_i[i], tmp_element.element.mission_path.path.path_f[i]); } break; default: // invalid element type return FALSE; break; } return TRUE; }
void stateCalcSpeedEnu_i(void) { if (bit_is_set(state.speed_status, SPEED_ENU_I)) return; int errno = 0; if (state.ned_initialized_i) { if (bit_is_set(state.speed_status, SPEED_NED_I)) { INT32_VECT3_ENU_OF_NED(state.enu_speed_i, state.ned_speed_i); } if (bit_is_set(state.speed_status, SPEED_ENU_F)) { ENU_BFP_OF_REAL(state.enu_speed_i, state.enu_speed_f); } else if (bit_is_set(state.speed_status, SPEED_NED_F)) { SPEEDS_BFP_OF_REAL(state.ned_speed_i, state.ned_speed_f); SetBit(state.pos_status, SPEED_NED_I); INT32_VECT3_ENU_OF_NED(state.enu_speed_i, state.ned_speed_i); } else if (bit_is_set(state.speed_status, SPEED_ECEF_I)) { enu_of_ecef_vect_i(&state.enu_speed_i, &state.ned_origin_i, &state.ecef_speed_i); } else if (bit_is_set(state.speed_status, SPEED_ECEF_F)) { /* transform ecef_f -> ecef_i -> enu_i , set status bits */ SPEEDS_BFP_OF_REAL(state.ecef_speed_i, state.ecef_speed_f); SetBit(state.speed_status, SPEED_ECEF_I); enu_of_ecef_vect_i(&state.enu_speed_i, &state.ned_origin_i, &state.ecef_speed_i); } 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_I)) { INT32_VECT3_ENU_OF_NED(state.enu_speed_i, state.ned_speed_i); } if (bit_is_set(state.speed_status, SPEED_ENU_F)) { ENU_BFP_OF_REAL(state.enu_speed_i, state.enu_speed_f); } else if (bit_is_set(state.speed_status, SPEED_NED_F)) { SPEEDS_BFP_OF_REAL(state.ned_speed_i, state.ned_speed_f); SetBit(state.pos_status, SPEED_NED_I); INT32_VECT3_ENU_OF_NED(state.enu_speed_i, state.ned_speed_i); } else { /* could not get this representation, set errno */ errno = 2; } } else { /* ned coordinate system not initialized, set errno */ errno = 3; } if (errno) { //struct EnuCoor_i _enu_zero = {0}; //return _enu_zero; } /* set bit to indicate this representation is computed */ SetBit(state.speed_status, SPEED_ENU_I); }
bool_t nav_survey_rectangle_rotorcraft_setup(uint8_t wp1, uint8_t wp2, float grid, survey_orientation_t so) { rectangle_survey_sweep_num = 0; nav_survey_west = Min(WaypointX(wp1), WaypointX(wp2)); nav_survey_east = Max(WaypointX(wp1), WaypointX(wp2)); nav_survey_south = Min(WaypointY(wp1), WaypointY(wp2)); nav_survey_north = Max(WaypointY(wp1), WaypointY(wp2)); survey_orientation = so; if (survey_orientation == NS) { if (fabsf(stateGetPositionEnu_f()->x - nav_survey_west) < fabsf(stateGetPositionEnu_f()->x - nav_survey_east)) { survey_from.x = survey_to.x = nav_survey_west + grid / 4.; } else { survey_from.x = survey_to.x = nav_survey_east - grid / 4.; grid = -grid; } if (fabsf(stateGetPositionEnu_f()->y - nav_survey_south) > fabsf(stateGetPositionEnu_f()->y - nav_survey_north)) { survey_to.y = nav_survey_south; survey_from.y = nav_survey_north; } else { survey_from.y = nav_survey_south; survey_to.y = nav_survey_north; } } else { /* survey_orientation == WE */ if (fabsf(stateGetPositionEnu_f()->y - nav_survey_south) < fabsf(stateGetPositionEnu_f()->y - nav_survey_north)) { survey_from.y = survey_to.y = nav_survey_south + grid / 4.; } else { survey_from.y = survey_to.y = nav_survey_north - grid / 4.; grid = -grid; } if (fabsf(stateGetPositionEnu_f()->x - nav_survey_west) > fabsf(stateGetPositionEnu_f()->x - nav_survey_east)) { survey_to.x = nav_survey_west; survey_from.x = nav_survey_east; } else { survey_from.x = nav_survey_west; survey_to.x = nav_survey_east; } } nav_survey_shift = grid; survey_uturn = FALSE; nav_survey_rectangle_active = FALSE; //go to start position ENU_BFP_OF_REAL(survey_from_i, survey_from); horizontal_mode = HORIZONTAL_MODE_ROUTE; VECT3_COPY(navigation_target, survey_from_i); LINE_STOP_FUNCTION; NavVerticalAltitudeMode(waypoints[wp1].enu_f.z, 0.); if (survey_orientation == NS) { nav_set_heading_deg(0); } else { nav_set_heading_deg(90); } return FALSE; }
void waypoint_set_enu(uint8_t wp_id, struct EnuCoor_f *enu) { if (wp_id < nb_waypoint) { waypoints[wp_id].enu_f = *enu; SetBit(waypoints[wp_id].flags, WP_FLAG_ENU_I); ENU_BFP_OF_REAL(waypoints[wp_id].enu_i, waypoints[wp_id].enu_f); SetBit(waypoints[wp_id].flags, WP_FLAG_ENU_F); ClearBit(waypoints[wp_id].flags, WP_FLAG_LLA_I); waypoint_globalize(wp_id); } }
/** * 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); }
bool_t nav_survey_rectangle_rotorcraft_run(uint8_t wp1, uint8_t wp2) { static bool_t is_last_half = FALSE; static float survey_radius; nav_survey_active = TRUE; /* entry scan */ // wait for start position and altitude be reached if (!nav_survey_rectangle_active && ((!nav_approaching_from(&survey_from_i, NULL, 0)) || (fabsf(stateGetPositionEnu_f()->z - waypoints[wp1].enu_f.z)) > 1.)) { } else { if (!nav_survey_rectangle_active) { nav_survey_rectangle_active = TRUE; LINE_START_FUNCTION; } nav_survey_west = Min(WaypointX(wp1), WaypointX(wp2)); nav_survey_east = Max(WaypointX(wp1), WaypointX(wp2)); nav_survey_south = Min(WaypointY(wp1), WaypointY(wp2)); nav_survey_north = Max(WaypointY(wp1), WaypointY(wp2)); /* Update the current segment from corners' coordinates*/ if (SurveyGoingNorth()) { survey_to.y = nav_survey_north; survey_from.y = nav_survey_south; } else if (SurveyGoingSouth()) { survey_to.y = nav_survey_south; survey_from.y = nav_survey_north; } else if (SurveyGoingEast()) { survey_to.x = nav_survey_east; survey_from.x = nav_survey_west; } else if (SurveyGoingWest()) { survey_to.x = nav_survey_west; survey_from.x = nav_survey_east; } if (!survey_uturn) { /* S-N, N-S, W-E or E-W straight route */ /* if you like to use position croos instead of approaching uncoment this line if ((stateGetPositionEnu_f()->y < nav_survey_north && SurveyGoingNorth()) || (stateGetPositionEnu_f()->y > nav_survey_south && SurveyGoingSouth()) || (stateGetPositionEnu_f()->x < nav_survey_east && SurveyGoingEast()) || (stateGetPositionEnu_f()->x > nav_survey_west && SurveyGoingWest())) { */ /* Continue ... */ ENU_BFP_OF_REAL(survey_to_i, survey_to); if (!nav_approaching_from(&survey_to_i, NULL, 0)) { ENU_BFP_OF_REAL(survey_from_i, survey_from); horizontal_mode = HORIZONTAL_MODE_ROUTE; nav_route(&survey_from_i, &survey_to_i); } else { if (survey_orientation == NS) { /* North or South limit reached, prepare turn and next leg */ float x0 = survey_from.x; /* Current longitude */ if ((x0 + nav_survey_shift < nav_survey_west) || (x0 + nav_survey_shift > nav_survey_east)) { // not room for full sweep if (((x0 + (nav_survey_shift / 2)) < nav_survey_west) || ((x0 + (nav_survey_shift / 2)) > nav_survey_east)) { //not room for half sweep if (is_last_half) {// was last sweep half? nav_survey_shift = -nav_survey_shift; if (interleave) { survey_radius = nav_survey_shift; }else { survey_radius = nav_survey_shift /2.; } is_last_half = FALSE; } else { // last sweep not half nav_survey_shift = -nav_survey_shift; if (interleave) { survey_radius = nav_survey_shift /2.; }else{ survey_radius = nav_survey_shift ; } } rectangle_survey_sweep_num ++; } else { //room for half sweep after survey_radius = nav_survey_shift / 2.; is_last_half = TRUE; } } else {// room for full sweep after survey_radius = nav_survey_shift; } x0 = x0 + survey_radius; /* Longitude of next leg */ survey_from.x = survey_to.x = x0; /* Swap South and North extremities */ float tmp = survey_from.y; survey_from.y = survey_to.y; survey_to.y = tmp; /** Do half a circle around WP 0 */ waypoints[0].enu_f.x = x0; waypoints[0].enu_f.y = survey_from.y; /* Computes the right direction */ if (SurveyGoingEast()) { survey_radius = -survey_radius; } } else { /* (survey_orientation == WE) */ /* East or West limit reached, prepare turn and next leg */ /* There is a y0 declared in math.h (for ARM) !!! */ float my_y0 = survey_from.y; /* Current latitude */ if (my_y0 + nav_survey_shift < nav_survey_south || my_y0 + nav_survey_shift > nav_survey_north) { // not room for full sweep if (((my_y0 + (nav_survey_shift / 2)) < nav_survey_south) || ((my_y0 + (nav_survey_shift / 2)) > nav_survey_north)) { //not room for half sweep if (is_last_half) {// was last sweep half? nav_survey_shift = -nav_survey_shift; if (interleave) { survey_radius = nav_survey_shift; }else { survey_radius = nav_survey_shift /2.; } is_last_half = FALSE; } else { // last sweep not half nav_survey_shift = -nav_survey_shift; if (interleave) { survey_radius = nav_survey_shift /2.; }else{ survey_radius = nav_survey_shift ; } } rectangle_survey_sweep_num ++; } else { //room for half sweep after survey_radius = nav_survey_shift / 2.; is_last_half = TRUE; } } else {// room for full sweep after survey_radius = nav_survey_shift; } my_y0 = my_y0 + survey_radius; /* latitude of next leg */ survey_from.y = survey_to.y = my_y0; /* Swap West and East extremities */ float tmp = survey_from.x; survey_from.x = survey_to.x; survey_to.x = tmp; /** Do half a circle around WP 0 */ waypoints[0].enu_f.x = survey_from.x; waypoints[0].enu_f.y = my_y0; /* Computes the right direction */ if (SurveyGoingNorth()) { survey_radius = -survey_radius; } } nav_in_segment = FALSE; survey_uturn = TRUE; LINE_STOP_FUNCTION; #ifdef DIGITAL_CAM float temp; if (survey_orientation == NS) { temp = fabsf(nav_survey_north - nav_survey_south) / dc_distance_interval; } else{ temp = fabsf(nav_survey_west - nav_survey_east) / dc_distance_interval; } double inteiro; double fract = modf (temp , &inteiro); if (fract > .5) { dc_send_command(DC_SHOOT); //if last shot is more than shot_distance/2 from the corner take a picture in the corner before go to the next sweep } #endif } } else { /* START turn */ static struct EnuCoor_f temp_f; if (survey_orientation == WE) { temp_f.x = waypoints[0].enu_f.x; temp_f.y = waypoints[0].enu_f.y - survey_radius; } else { temp_f.y = waypoints[0].enu_f.y; temp_f.x = waypoints[0].enu_f.x - survey_radius; } //detect when segment has done /* if you like to use position croos instead of approaching uncoment this line if ( (stateGetPositionEnu_f()->y > waypoints[0].enu_f.y && ((survey_orientation == WE) && (temp_f.y < waypoints[0].enu_f.y)) )|| (stateGetPositionEnu_f()->y < waypoints[0].enu_f.y && ((survey_orientation == WE) && (temp_f.y > waypoints[0].enu_f.y)) )|| (stateGetPositionEnu_f()->x < waypoints[0].enu_f.x && ((survey_orientation == NS) && (temp_f.x > waypoints[0].enu_f.x)) )|| (stateGetPositionEnu_f()->x > waypoints[0].enu_f.x && ((survey_orientation == NS) && (temp_f.x < waypoints[0].enu_f.x)) ) ) { */ if (survey_orientation == WE) { ENU_BFP_OF_REAL(survey_from_i, temp_f); ENU_BFP_OF_REAL(survey_to_i, waypoints[0].enu_f); } else { ENU_BFP_OF_REAL(survey_from_i, temp_f); ENU_BFP_OF_REAL(survey_to_i, waypoints[0].enu_f); } if (nav_approaching_from(&survey_to_i, NULL, 0)) { survey_uturn = FALSE; nav_in_circle = FALSE; LINE_START_FUNCTION; } else { if (survey_orientation == WE) { ENU_BFP_OF_REAL(survey_from_i, temp_f); ENU_BFP_OF_REAL(survey_to_i, waypoints[0].enu_f); } else { ENU_BFP_OF_REAL(survey_from_i, temp_f); ENU_BFP_OF_REAL(survey_to_i, waypoints[0].enu_f); } horizontal_mode = HORIZONTAL_MODE_ROUTE; nav_route(&survey_from_i, &survey_to_i); } } /* END turn */ } /* END entry scan */ return TRUE; }// /* END survey_retangle */
void stateCalcPositionNed_i(void) { if (bit_is_set(state.pos_status, POS_NED_I)) { return; } int errno = 0; if (state.ned_initialized_i) { if (bit_is_set(state.pos_status, POS_NED_F)) { NED_BFP_OF_REAL(state.ned_pos_i, state.ned_pos_f); } else if (bit_is_set(state.pos_status, POS_ENU_I)) { INT32_VECT3_NED_OF_ENU(state.ned_pos_i, state.enu_pos_i); } else if (bit_is_set(state.pos_status, POS_ENU_F)) { ENU_BFP_OF_REAL(state.enu_pos_i, state.enu_pos_f); SetBit(state.pos_status, POS_ENU_I); INT32_VECT3_NED_OF_ENU(state.ned_pos_i, state.enu_pos_i); } else if (bit_is_set(state.pos_status, POS_ECEF_I)) { ned_of_ecef_pos_i(&state.ned_pos_i, &state.ned_origin_i, &state.ecef_pos_i); } else if (bit_is_set(state.pos_status, POS_ECEF_F)) { /* transform ecef_f -> ned_f, set status bit, then convert to int */ ned_of_ecef_point_f(&state.ned_pos_f, &state.ned_origin_f, &state.ecef_pos_f); SetBit(state.pos_status, POS_NED_F); NED_BFP_OF_REAL(state.ned_pos_i, state.ned_pos_f); } else if (bit_is_set(state.pos_status, POS_LLA_F)) { /* transform lla_f -> ecef_f -> ned_f, set status bits, then convert to int */ ecef_of_lla_f(&state.ecef_pos_f, &state.lla_pos_f); SetBit(state.pos_status, POS_ECEF_F); ned_of_ecef_point_f(&state.ned_pos_f, &state.ned_origin_f, &state.ecef_pos_f); SetBit(state.pos_status, POS_NED_F); NED_BFP_OF_REAL(state.ned_pos_i, state.ned_pos_f); } else if (bit_is_set(state.pos_status, POS_LLA_I)) { ned_of_lla_point_i(&state.ned_pos_i, &state.ned_origin_i, &state.lla_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_NED_F)) { NED_BFP_OF_REAL(state.ned_pos_i, state.ned_pos_f); } else if (bit_is_set(state.pos_status, POS_ENU_I)) { INT32_VECT3_NED_OF_ENU(state.ned_pos_i, state.enu_pos_i); } else if (bit_is_set(state.pos_status, POS_ENU_F)) { ENU_BFP_OF_REAL(state.enu_pos_i, state.enu_pos_f); SetBit(state.pos_status, POS_ENU_I); INT32_VECT3_NED_OF_ENU(state.ned_pos_i, state.enu_pos_i); } else if (bit_is_set(state.pos_status, POS_UTM_F)) { /* transform utm_f -> ned_f -> ned_i, set status bits */ NED_OF_UTM_DIFF(state.ned_pos_f, state.utm_pos_f, state.utm_origin_f); SetBit(state.pos_status, POS_NED_F); NED_BFP_OF_REAL(state.ned_pos_i, state.ned_pos_f); } else if (bit_is_set(state.pos_status, POS_LLA_F)) { /* transform lla_f -> utm_f -> ned_f -> ned_i, set status bits */ utm_of_lla_f(&state.utm_pos_f, &state.lla_pos_f); SetBit(state.pos_status, POS_UTM_F); NED_OF_UTM_DIFF(state.ned_pos_f, state.utm_pos_f, state.utm_origin_f); SetBit(state.pos_status, POS_NED_F); NED_BFP_OF_REAL(state.ned_pos_i, state.ned_pos_f); } else if (bit_is_set(state.pos_status, POS_LLA_I)) { /* transform lla_i -> lla_f -> utm_f -> ned_f -> ned_i, 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); NED_OF_UTM_DIFF(state.ned_pos_f, state.utm_pos_f, state.utm_origin_f); SetBit(state.pos_status, POS_NED_F); NED_BFP_OF_REAL(state.ned_pos_i, state.ned_pos_f); } else { /* could not get this representation, set errno */ errno = 2; } } else { /* ned coordinate system not initialized, set errno */ errno = 3; } if (errno) { //struct NedCoor_i _ned_zero = {0}; //return _ned_zero; } /* set bit to indicate this representation is computed */ SetBit(state.pos_status, POS_NED_I); }