/**
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;
}
/**
initializes the variables needed for the survey to start
first_wp : the first Waypoint of the polygon
size : the number of points that make up the polygon
angle : angle in which to do the flyovers
sweep_width : distance between the sweeps
shot_dist : distance between the shots
min_rad : minimal radius when navigating
altitude : the altitude that must be reached before the flyover starts
**/
bool_t init_poly_survey_adv(uint8_t first_wp, uint8_t size, float angle, float sweep_width, float shot_dist, float min_rad, float altitude)
{
int i;
point2d small, sweep;
float divider, len, angle_rad = angle/180.0*M_PI;
if (angle < 0.0) angle += 360.0;
if (angle >= 360.0) angle -= 360.0;
poly_first = first_wp;
poly_count = size;
psa_sweep_width = sweep_width;
psa_min_rad = min_rad;
psa_shot_dist = shot_dist;
psa_altitude = altitude;
segment_angle = angle;
return_angle = angle+180;
if (return_angle > 359) return_angle -= 360;
if (angle <= 45.0 || angle >= 315.0) {
//north
dir_vec.y = 1.0;
dir_vec.x = 1.0*tanf(angle_rad);
sweep.x = 1.0;
sweep.y = - dir_vec.x / dir_vec.y;
}
else if (angle <= 135.0) {
//east
dir_vec.x = 1.0;
dir_vec.y = 1.0/tanf(angle_rad);
sweep.y = - 1.0;
sweep.x = dir_vec.y / dir_vec.x;
}
else if (angle <= 225.0) {
//south
dir_vec.y = -1.0;
dir_vec.x = -1.0*tanf(angle_rad);
sweep.x = -1.0;
sweep.y = dir_vec.x / dir_vec.y;
}
else {
//west
dir_vec.x = -1.0;
dir_vec.y = -1.0/tanf(angle_rad);
sweep.y = 1.0;
sweep.x = - dir_vec.y / dir_vec.x;
}
//normalize
len = sqrt(sweep.x*sweep.x+sweep.y*sweep.y);
sweep.x = sweep.x / len;
sweep.y = sweep.y / len;
rad_vec.x = sweep.x * psa_min_rad;
rad_vec.y = sweep.y * psa_min_rad;
sweep_vec.x = sweep.x * psa_sweep_width;
sweep_vec.y = sweep.y * psa_sweep_width;
//begin at leftmost position (relative to dir_vec)
small.x = waypoints[poly_first].x;
small.y = waypoints[poly_first].y;
divider = (sweep_vec.y*dir_vec.x) - (sweep_vec.x*dir_vec.y);
//cacluate the leftmost point if one sees the dir vec as going "up" and the sweep vec as going right
if (divider < 0.0) {
for(i=1;i<poly_count;i++)
if ((dir_vec.x*(waypoints[poly_first+i].y - small.y)) + (dir_vec.y*(small.x - waypoints[poly_first+i].x)) > 0.0) {
small.x = waypoints[poly_first+i].x;
small.y = waypoints[poly_first+i].y;
}
}
else
for(i=1;i<poly_count;i++)
if ((dir_vec.x*(waypoints[poly_first+i].y - small.y)) + (dir_vec.y*(small.x - waypoints[poly_first+i].x)) > 0.0) {
small.x = waypoints[poly_first+i].x;
small.y = waypoints[poly_first+i].y;
}
//calculate the line the defines the first flyover
seg_start.x = small.x + 0.5*sweep_vec.x;
seg_start.y = small.y + 0.5*sweep_vec.y;
VEC_CALC(seg_end, seg_start, dir_vec, +);
if (!get_two_intersects(&seg_start, &seg_end, seg_start, seg_end)) {
psa_stage = ERR;
return FALSE;
}
//center of the entry circle
entry_center.x = seg_start.x - rad_vec.x;
entry_center.y = seg_start.y - rad_vec.y;
//fast climbing to desired altitude
NavVerticalAutoThrottleMode(0.0);
NavVerticalAltitudeMode(psa_altitude, 0.0);
psa_stage = ENTRY;
return FALSE;
}
/**
* initializes the variables needed for the survey to start
* @param first_wp the first Waypoint of the polygon
* @param size the number of points that make up the polygon
* @param angle angle in which to do the flyovers
* @param sweep_width distance between the sweeps
* @param shot_dist distance between the shots
* @param min_rad minimal radius when navigating
* @param altitude the altitude that must be reached before the flyover starts
**/
void nav_survey_polygon_setup(uint8_t first_wp, uint8_t size, float angle, float sweep_width, float shot_dist,
float min_rad, float altitude)
{
int i;
struct FloatVect2 small, sweep;
float divider, angle_rad = angle / 180.0 * M_PI;
if (angle < 0.0) {
angle += 360.0;
}
if (angle >= 360.0) {
angle -= 360.0;
}
survey.poly_first = first_wp;
survey.poly_count = size;
survey.psa_sweep_width = sweep_width;
survey.psa_min_rad = min_rad;
survey.psa_shot_dist = shot_dist;
survey.psa_altitude = altitude;
survey.segment_angle = angle;
survey.return_angle = angle + 180;
if (survey.return_angle > 359) {
survey.return_angle -= 360;
}
if (angle <= 45.0 || angle >= 315.0) {
//north
survey.dir_vec.y = 1.0;
survey.dir_vec.x = 1.0 * tanf(angle_rad);
sweep.x = 1.0;
sweep.y = - survey.dir_vec.x / survey.dir_vec.y;
} else if (angle <= 135.0) {
//east
survey.dir_vec.x = 1.0;
survey.dir_vec.y = 1.0 / tanf(angle_rad);
sweep.y = - 1.0;
sweep.x = survey.dir_vec.y / survey.dir_vec.x;
} else if (angle <= 225.0) {
//south
survey.dir_vec.y = -1.0;
survey.dir_vec.x = -1.0 * tanf(angle_rad);
sweep.x = -1.0;
sweep.y = survey.dir_vec.x / survey.dir_vec.y;
} else {
//west
survey.dir_vec.x = -1.0;
survey.dir_vec.y = -1.0 / tanf(angle_rad);
sweep.y = 1.0;
sweep.x = - survey.dir_vec.y / survey.dir_vec.x;
}
//normalize
FLOAT_VECT2_NORMALIZE(sweep);
VECT2_SMUL(survey.rad_vec, sweep, survey.psa_min_rad);
VECT2_SMUL(survey.sweep_vec, sweep, survey.psa_sweep_width);
//begin at leftmost position (relative to survey.dir_vec)
VECT2_COPY(small, waypoints[survey.poly_first]);
divider = (survey.sweep_vec.y * survey.dir_vec.x) - (survey.sweep_vec.x * survey.dir_vec.y);
//calculate the leftmost point if one sees the dir vec as going "up" and the sweep vec as going right
if (divider < 0.0) {
for (i = 1; i < survey.poly_count; i++)
if ((survey.dir_vec.x * (waypoints[survey.poly_first + i].y - small.y)) + (survey.dir_vec.y *
(small.x - waypoints[survey.poly_first + i].x)) > 0.0) {
VECT2_COPY(small, waypoints[survey.poly_first + i]);
}
} else
for (i = 1; i < survey.poly_count; i++)
if ((survey.dir_vec.x * (waypoints[survey.poly_first + i].y - small.y)) + (survey.dir_vec.y *
(small.x - waypoints[survey.poly_first + i].x)) > 0.0) {
VECT2_COPY(small, waypoints[survey.poly_first + i]);
}
//calculate the line the defines the first flyover
survey.seg_start.x = small.x + 0.5 * survey.sweep_vec.x;
survey.seg_start.y = small.y + 0.5 * survey.sweep_vec.y;
VECT2_SUM(survey.seg_end, survey.seg_start, survey.dir_vec);
if (!get_two_intersects(&survey.seg_start, &survey.seg_end, survey.seg_start, survey.seg_end)) {
survey.stage = ERR;
return;
}
//center of the entry circle
VECT2_DIFF(survey.entry_center, survey.seg_start, survey.rad_vec);
//fast climbing to desired altitude
NavVerticalAutoThrottleMode(0.0);
NavVerticalAltitudeMode(survey.psa_altitude, 0.0);
survey.stage = ENTRY;
}
