diff --git a/conf/autopilot/subsystems/fixedwing/navigation_extra.makefile b/conf/autopilot/subsystems/fixedwing/navigation_extra.makefile index 8ec111c216..60cc01c1af 100644 --- a/conf/autopilot/subsystems/fixedwing/navigation_extra.makefile +++ b/conf/autopilot/subsystems/fixedwing/navigation_extra.makefile @@ -13,4 +13,5 @@ $(TARGET).srcs += $(SRC_SUBSYSTEMS)/navigation/discsurvey.c $(TARGET).srcs += $(SRC_SUBSYSTEMS)/navigation/OSAMNav.c $(TARGET).srcs += $(SRC_SUBSYSTEMS)/navigation/snav.c $(TARGET).srcs += $(SRC_SUBSYSTEMS)/navigation/spiral.c +$(TARGET).srcs += $(SRC_SUBSYSTEMS)/navigation/poly_survey_adv.c diff --git a/sw/airborne/subsystems/navigation/poly_survey_adv.c b/sw/airborne/subsystems/navigation/poly_survey_adv.c new file mode 100644 index 0000000000..b56ff6f579 --- /dev/null +++ b/sw/airborne/subsystems/navigation/poly_survey_adv.c @@ -0,0 +1,332 @@ +#include "poly_survey_adv.h" + +#include "subsystems/nav.h" +#include "estimator.h" +#include "autopilot.h" +#include "generated/flight_plan.h" +//#include "modules/digital_cam/dc.h" + + +/** +The following variables are set by poly_survey_init and not changed later on +**/ + +// precomputed vectors to ease calculations +point2d dir_vec; +point2d sweep_vec; +point2d rad_vec; + +//the polygon from the flightplan +uint8_t poly_first; +uint8_t poly_count; + +//desired properties of the flyover +float psa_min_rad; +float psa_sweep_width; +float psa_shot_dist; +float psa_altitude; + +//direction for the flyover (0° == N) +int segment_angle; +int return_angle; + +/** +The Following variables are dynamic, changed while navigating. +**/ + +/* +psa_stage starts at ENTRY and than circles trought the other +states until to polygon is completely covered +ENTRY : getting in the right position and height for the first flyover +SEG : fly from seg_start to seg_end and take pictures, + then calculate navigation points of next flyover +TURN1 : do a 180° turn around seg_center1 +RET : fly from ret_start to ret_end +TURN2 : do a 180° turn around seg_center2 +*/ +survey_stage psa_stage; + +// points for navigation +point2d seg_start; +point2d seg_end; +point2d seg_center1; +point2d seg_center2; +point2d entry_center; +point2d ret_start; +point2d ret_end; + + +//helper functions and macro +#define VEC_CALC(A, B, C, OP) A.x = B.x OP C.x; A.y = B.y OP C.y; + +static point2d vec_add(point2d a, point2d b) +{ + point2d tmp; + VEC_CALC(tmp, a, b, +); + + return tmp; +} + +static void nav_points(point2d start, point2d end) +{ + nav_route_xy(start.x, start.y, end.x, end.y); +} + +/** + intercept two lines and give back the point of intersection + returns : FALSE if no intersection can be found or intersection does not lie between points a and b + else TRUE + p : returns intersection + x, y : first line is defined by point x and y (goes through this points) + a1, a2, b1, b2 : second line by coordinates a1/a2, b1/b2 +**/ +static bool_t intercept_two_lines(point2d *p, point2d x, point2d y, float a1, float a2, float b1, float b2) +{ + float div, fac; + + div = (((b2 - a2)*(y.x - x.x)) + ((x.y - y.y)*(b1 - a1))); + if (div == 0) return FALSE; + fac = ((y.x*(x.y - a2)) + (x.x*(a2 - y.y)) + (a1*(y.y - x.y))) / div; + if (fac > 1.0) return FALSE; + if (fac < 0.0) return FALSE; + + p->x = a1 + fac*(b1 - a1); + p->y = a2 + fac*(b2 - a2); + + return TRUE; +} + +/** + intersects a line with the polygon and gives back the two intersection points + returns : TRUE if two intersection can be found, else FALSE + x, y : intersection points + a, b : define the line to intersection +**/ +static bool_t get_two_intersects(point2d *x, point2d *y, point2d a, point2d b) +{ + int i, count = 0; + point2d tmp; + + for (i=0;i fabs(dir_vec.y)) { + if ((y->x - x->x) / dir_vec.x < 0.0){ + tmp = *x; + *x = *y; + *y = tmp; + } + } + else + if ((y->y - x->y) / dir_vec.y < 0.0) { + tmp = *x; + *x = *y; + *y = tmp; + } + + 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 div, 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; + + div = (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 (div < 0.0) { + for(i=1;i 0.0) { + small.x = waypoints[poly_first+i].x; + small.y = waypoints[poly_first+i].y; + } + } + else + for(i=1;i 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(100.0); + NavVerticalAltitudeMode(psa_altitude, 0.0); + + psa_stage = ENTRY; + + return FALSE; +} + +/** + 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) +{ + //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; + NavVerticalAutoThrottleMode(0.0); + nav_init_stage(); + //dc_distance(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); + } + } + //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)) { + //dc_stop(); + 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(); + } + } + //turn from stage to return + else if (psa_stage == TURN1) { + nav_circle_XY(seg_center1.x, seg_center1.y, -psa_min_rad); + if (NavCourseCloseTo(return_angle)) { + psa_stage = RET; + nav_init_stage(); + } + //return + } else if (psa_stage == RET) { + nav_points(ret_start, ret_end); + if (nav_approaching_xy(ret_end.x, ret_end.y, ret_start.x, ret_start.y, 0)) { + psa_stage = TURN2; + nav_init_stage(); + } + //turn from return to stage + } else if (psa_stage == TURN2) { + nav_circle_XY(seg_center2.x, seg_center2.y, -(2*psa_min_rad+psa_sweep_width)*0.5); + if (NavCourseCloseTo(segment_angle)) { + psa_stage = SEG; + nav_init_stage(); + //dc_distance(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); + } + } + + return TRUE; +} diff --git a/sw/airborne/subsystems/navigation/poly_survey_adv.h b/sw/airborne/subsystems/navigation/poly_survey_adv.h new file mode 100644 index 0000000000..90b0164565 --- /dev/null +++ b/sw/airborne/subsystems/navigation/poly_survey_adv.h @@ -0,0 +1,13 @@ +#ifndef POLY_ADV_H +#define POLY_ADV_H + +#include "std.h" + +typedef struct {float x; float y;} point2d; + +typedef enum {ERR, ENTRY, SEG, TURN1, RET, TURN2} survey_stage; + +extern 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); +extern bool_t poly_survey_adv(void); + +#endif