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