/** main navigation routine. This is called periodically evaluates the current Position and stage and navigates accordingly. Returns True until the survey is finished **/ bool_t poly_survey_adv(void) { NavVerticalAutoThrottleMode(0.0); NavVerticalAltitudeMode(psa_altitude, 0.0); //entry circle around entry-center until the desired altitude is reached if (psa_stage == ENTRY) { nav_circle_XY(entry_center.x, entry_center.y, -psa_min_rad); if (NavCourseCloseTo(segment_angle) && nav_approaching_xy(seg_start.x, seg_start.y, last_x, last_y, CARROT) && fabs(estimator_z - psa_altitude) <= 20) { psa_stage = SEG; nav_init_stage(); #ifdef DIGITAL_CAM dc_survey(psa_shot_dist, seg_start.x - dir_vec.x*psa_shot_dist*0.5, seg_start.y - dir_vec.y*psa_shot_dist*0.5); #endif } } //fly the segment until seg_end is reached if (psa_stage == SEG) { nav_points(seg_start, seg_end); //calculate all needed points for the next flyover if (nav_approaching_xy(seg_end.x, seg_end.y, seg_start.x, seg_start.y, 0)) { #ifdef DIGITAL_CAM dc_stop(); #endif VEC_CALC(seg_center1, seg_end, rad_vec, -); ret_start.x = seg_end.x - 2*rad_vec.x; ret_start.y = seg_end.y - 2*rad_vec.y; //if we get no intersection the survey is finished if (!get_two_intersects(&seg_start, &seg_end, vec_add(seg_start, sweep_vec), vec_add(seg_end, sweep_vec))) return FALSE; ret_end.x = seg_start.x - sweep_vec.x - 2*rad_vec.x; ret_end.y = seg_start.y - sweep_vec.y - 2*rad_vec.y; seg_center2.x = seg_start.x - 0.5*(2.0*rad_vec.x+sweep_vec.x); seg_center2.y = seg_start.y - 0.5*(2.0*rad_vec.y+sweep_vec.y); psa_stage = TURN1; nav_init_stage(); }
/** * main navigation routine. * This is called periodically evaluates the current * Position and stage and navigates accordingly. * * @returns TRUE until the survey is finished */ bool_t nav_survey_zamboni_run(void) { // retain altitude NavVerticalAutoThrottleMode(0.0); NavVerticalAltitudeMode(zs.altitude, 0.0); //go from center of field to end of field - (before starting the syrvey) if (zs.stage == Z_ENTRY) { nav_route_xy(zs.wp_center.x, zs.wp_center.y, zs.seg_end.x, zs.seg_end.y); if (nav_approaching_xy(zs.seg_end.x, zs.seg_end.y, zs.wp_center.x, zs.wp_center.y, CARROT)) { zs.stage = Z_TURN1; NavVerticalAutoThrottleMode(0.0); nav_init_stage(); } } //Turn from stage to return else if (zs.stage == Z_TURN1) { nav_circle_XY(zs.turn_center1.x, zs.turn_center1.y, zs.turnradius1); if (NavCourseCloseTo(zs.return_angle + zs.pre_leave_angle)){ // && nav_approaching_xy(zs.seg_end.x, zs.seg_end.y, zs.seg_start.x, zs.seg_start.y, CARROT //calculate SEG-points for the next flyover VECT2_ADD(zs.seg_start, zs.sweep_width); VECT2_ADD(zs.seg_end, zs.sweep_width); zs.stage = Z_RET; nav_init_stage(); #ifdef DIGITAL_CAM LINE_START_FUNCTION; #endif } } //fly the segment until seg_end is reached else if (zs.stage == Z_RET) { nav_route_xy(zs.ret_start.x, zs.ret_start.y, zs.ret_end.x, zs.ret_end.y); if (nav_approaching_xy(zs.ret_end.x, zs.ret_end.y, zs.ret_start.x, zs.ret_start.y, 0)) { zs.current_laps = zs.current_laps + 1; #ifdef DIGITAL_CAM //dc_stop(); LINE_STOP_FUNCTION; #endif zs.stage = Z_TURN2; } } //turn from stage to return else if (zs.stage == Z_TURN2) { nav_circle_XY(zs.turn_center2.x, zs.turn_center2.y, zs.turnradius2); if (NavCourseCloseTo(zs.flight_angle + zs.pre_leave_angle)) { //zs.current_laps = zs.current_laps + 1; zs.stage = Z_SEG; nav_init_stage(); #ifdef DIGITAL_CAM LINE_START_FUNCTION; #endif } //return } else if (zs.stage == Z_SEG) { nav_route_xy(zs.seg_start.x, zs.seg_start.y, zs.seg_end.x, zs.seg_end.y); if (nav_approaching_xy(zs.seg_end.x, zs.seg_end.y, zs.seg_start.x, zs.seg_start.y, 0)) { // calculate the rest of the points for the next fly-over VECT2_ADD(zs.ret_start, zs.sweep_width); VECT2_ADD(zs.ret_end, zs.sweep_width); zs.turn_center1.x = (zs.seg_end.x + zs.ret_start.x)/2; zs.turn_center1.y = (zs.seg_end.y + zs.ret_start.y)/2; zs.turn_center2.x = (zs.seg_start.x + zs.ret_end.x + zs.sweep_width.x) / 2; zs.turn_center2.y = (zs.seg_start.y + zs.ret_end.y + zs.sweep_width.y) / 2; zs.stage = Z_TURN1; nav_init_stage(); #ifdef DIGITAL_CAM //dc_stop(); LINE_STOP_FUNCTION; #endif } } if (zs.current_laps >= zs.total_laps) { #ifdef DIGITAL_CAM LINE_STOP_FUNCTION; #endif return FALSE; } else { return TRUE; } }
/** * main navigation routine. This is called periodically evaluates the current * Position and stage and navigates accordingly. * @returns True until the survey is finished */ bool nav_survey_polygon_run(void) { NavVerticalAutoThrottleMode(0.0); NavVerticalAltitudeMode(survey.psa_altitude, 0.0); //entry circle around entry-center until the desired altitude is reached if (survey.stage == ENTRY) { nav_circle_XY(survey.entry_center.x, survey.entry_center.y, -survey.psa_min_rad); if (NavCourseCloseTo(survey.segment_angle) && nav_approaching_xy(survey.seg_start.x, survey.seg_start.y, last_x, last_y, CARROT) && fabs(stateGetPositionUtm_f()->alt - survey.psa_altitude) <= 20) { survey.stage = SEG; nav_init_stage(); #ifdef DIGITAL_CAM dc_survey(survey.psa_shot_dist, survey.seg_start.x - survey.dir_vec.x * survey.psa_shot_dist * 0.5, survey.seg_start.y - survey.dir_vec.y * survey.psa_shot_dist * 0.5); #endif } } //fly the segment until seg_end is reached if (survey.stage == SEG) { nav_points(survey.seg_start, survey.seg_end); //calculate all needed points for the next flyover if (nav_approaching_xy(survey.seg_end.x, survey.seg_end.y, survey.seg_start.x, survey.seg_start.y, 0)) { #ifdef DIGITAL_CAM dc_stop(); #endif VECT2_DIFF(survey.seg_center1, survey.seg_end, survey.rad_vec); survey.ret_start.x = survey.seg_end.x - 2 * survey.rad_vec.x; survey.ret_start.y = survey.seg_end.y - 2 * survey.rad_vec.y; //if we get no intersection the survey is finished static struct FloatVect2 sum_start_sweep; static struct FloatVect2 sum_end_sweep; VECT2_SUM(sum_start_sweep, survey.seg_start, survey.sweep_vec); VECT2_SUM(sum_end_sweep, survey.seg_end, survey.sweep_vec); if (!get_two_intersects(&survey.seg_start, &survey.seg_end, sum_start_sweep, sum_end_sweep)) { return false; } survey.ret_end.x = survey.seg_start.x - survey.sweep_vec.x - 2 * survey.rad_vec.x; survey.ret_end.y = survey.seg_start.y - survey.sweep_vec.y - 2 * survey.rad_vec.y; survey.seg_center2.x = survey.seg_start.x - 0.5 * (2.0 * survey.rad_vec.x + survey.sweep_vec.x); survey.seg_center2.y = survey.seg_start.y - 0.5 * (2.0 * survey.rad_vec.y + survey.sweep_vec.y); survey.stage = TURN1; nav_init_stage(); } } //turn from stage to return else if (survey.stage == TURN1) { nav_circle_XY(survey.seg_center1.x, survey.seg_center1.y, -survey.psa_min_rad); if (NavCourseCloseTo(survey.return_angle)) { survey.stage = RET; nav_init_stage(); } //return } else if (survey.stage == RET) { nav_points(survey.ret_start, survey.ret_end); if (nav_approaching_xy(survey.ret_end.x, survey.ret_end.y, survey.ret_start.x, survey.ret_start.y, 0)) { survey.stage = TURN2; nav_init_stage(); } //turn from return to stage } else if (survey.stage == TURN2) { nav_circle_XY(survey.seg_center2.x, survey.seg_center2.y, -(2 * survey.psa_min_rad + survey.psa_sweep_width) * 0.5); if (NavCourseCloseTo(survey.segment_angle)) { survey.stage = SEG; nav_init_stage(); #ifdef DIGITAL_CAM dc_survey(survey.psa_shot_dist, survey.seg_start.x - survey.dir_vec.x * survey.psa_shot_dist * 0.5, survey.seg_start.y - survey.dir_vec.y * survey.psa_shot_dist * 0.5); #endif } } return true; }
void nav_survey_rectangle(uint8_t wp1, uint8_t wp2) { static float survey_radius; nav_survey_active = TRUE; 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 ((estimator_y < nav_survey_north && SurveyGoingNorth()) || (estimator_y > nav_survey_south && SurveyGoingSouth()) || (estimator_x < nav_survey_east && SurveyGoingEast()) || (estimator_x > nav_survey_west && SurveyGoingWest())) { /* Continue ... */ nav_route_xy(survey_from.x, survey_from.y, survey_to.x, survey_to.y); } else { if (survey_orientation == NS) { /* North or South limit reached, prepare U-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) { x0 += nav_survey_shift / 2; nav_survey_shift = -nav_survey_shift; } x0 = x0 + nav_survey_shift; /* 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].x = x0 - nav_survey_shift/2.; waypoints[0].y = survey_from.y; /* Computes the right direction for the circle */ survey_radius = nav_survey_shift / 2.; if (SurveyGoingNorth()) { survey_radius = -survey_radius; } } else { /* (survey_orientation == WE) */ /* East or West limit reached, prepare U-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) { my_y0 += nav_survey_shift / 2; nav_survey_shift = -nav_survey_shift; } my_y0 = my_y0 + nav_survey_shift; /* Longitude 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].x = survey_from.x; waypoints[0].y = my_y0 - nav_survey_shift/2.; /* Computes the right direction for the circle */ survey_radius = nav_survey_shift / 2.; if (SurveyGoingWest()) { survey_radius = -survey_radius; } } nav_in_segment = FALSE; survey_uturn = TRUE; LINE_STOP_FUNCTION; } } else { /* U-turn */ if ((SurveyGoingNorth() && NavCourseCloseTo(0)) || (SurveyGoingSouth() && NavCourseCloseTo(180)) || (SurveyGoingEast() && NavCourseCloseTo(90)) || (SurveyGoingWest() && NavCourseCloseTo(270))) { /* U-turn finished, back on a segment */ survey_uturn = FALSE; nav_in_circle = FALSE; LINE_START_FUNCTION; } else { NavCircleWaypoint(0, survey_radius); } } NavVerticalAutoThrottleMode(0.); /* No pitch */ NavVerticalAltitudeMode(WaypointAlt(wp1), 0.); /* No preclimb */ }