Paparazzi UAS  v4.2.2_stable-4-gcc32f65
Paparazzi is a free software Unmanned Aircraft System.
 All Data Structures Files Functions Variables Typedefs Enumerations Enumerator Macros Pages
poly_survey_adv.c
Go to the documentation of this file.
1 #include "poly_survey_adv.h"
2 
3 #include "subsystems/nav.h"
4 #include "estimator.h"
5 #include "autopilot.h"
6 #include "generated/flight_plan.h"
7 
8 #ifdef DIGITAL_CAM
10 #endif
11 
12 
17 // precomputed vectors to ease calculations
21 
22 //the polygon from the flightplan
25 
26 //desired properties of the flyover
31 
32 //direction for the flyover (0° == N)
35 
40 /*
41 psa_stage starts at ENTRY and than circles trought the other
42 states until to polygon is completely covered
43 ENTRY : getting in the right position and height for the first flyover
44 SEG : fly from seg_start to seg_end and take pictures,
45  then calculate navigation points of next flyover
46 TURN1 : do a 180° turn around seg_center1
47 RET : fly from ret_start to ret_end
48 TURN2 : do a 180° turn around seg_center2
49 */
51 
52 // points for navigation
60 
61 
62 //helper functions and macro
63 #define VEC_CALC(A, B, C, OP) A.x = B.x OP C.x; A.y = B.y OP C.y;
64 
66 {
67  point2d tmp;
68  VEC_CALC(tmp, a, b, +);
69 
70  return tmp;
71 }
72 
73 static void nav_points(point2d start, point2d end)
74 {
75  nav_route_xy(start.x, start.y, end.x, end.y);
76 }
77 
86 static bool_t intercept_two_lines(point2d *p, point2d x, point2d y, float a1, float a2, float b1, float b2)
87 {
88  float divider, fac;
89 
90  divider = (((b2 - a2)*(y.x - x.x)) + ((x.y - y.y)*(b1 - a1)));
91  if (divider == 0) return FALSE;
92  fac = ((y.x*(x.y - a2)) + (x.x*(a2 - y.y)) + (a1*(y.y - x.y))) / divider;
93  if (fac > 1.0) return FALSE;
94  if (fac < 0.0) return FALSE;
95 
96  p->x = a1 + fac*(b1 - a1);
97  p->y = a2 + fac*(b2 - a2);
98 
99  return TRUE;
100 }
101 
108 static bool_t get_two_intersects(point2d *x, point2d *y, point2d a, point2d b)
109 {
110  int i, count = 0;
111  point2d tmp;
112 
113  for (i=0;i<poly_count-1;i++)
115  if (count == 0) {
116  *x = tmp;
117  count++;
118  }
119  else {
120  *y = tmp;
121  count++;
122  break;
123  }
124  }
125 
126  //wrapover first,last polygon waypoint
127  if (count == 1 && intercept_two_lines(&tmp,a,b,waypoints[poly_first+poly_count-1].x,waypoints[poly_first+poly_count-1].y,waypoints[poly_first].x,waypoints[poly_first].y)) {
128  *y = tmp;
129  count++;
130  }
131 
132  if (count != 2)
133  return FALSE;
134 
135  //change points
136  if (fabs(dir_vec.x) > fabs(dir_vec.y)) {
137  if ((y->x - x->x) / dir_vec.x < 0.0){
138  tmp = *x;
139  *x = *y;
140  *y = tmp;
141  }
142  }
143  else
144  if ((y->y - x->y) / dir_vec.y < 0.0) {
145  tmp = *x;
146  *x = *y;
147  *y = tmp;
148  }
149 
150  return TRUE;
151 }
152 
163 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)
164 {
165  int i;
166  point2d small, sweep;
167  float divider, len, angle_rad = angle/180.0*M_PI;
168 
169  if (angle < 0.0) angle += 360.0;
170  if (angle >= 360.0) angle -= 360.0;
171 
172  poly_first = first_wp;
173  poly_count = size;
174 
175  psa_sweep_width = sweep_width;
176  psa_min_rad = min_rad;
177  psa_shot_dist = shot_dist;
178  psa_altitude = altitude;
179 
180  segment_angle = angle;
181  return_angle = angle+180;
182  if (return_angle > 359) return_angle -= 360;
183 
184  if (angle <= 45.0 || angle >= 315.0) {
185  //north
186  dir_vec.y = 1.0;
187  dir_vec.x = 1.0*tanf(angle_rad);
188  sweep.x = 1.0;
189  sweep.y = - dir_vec.x / dir_vec.y;
190  }
191  else if (angle <= 135.0) {
192  //east
193  dir_vec.x = 1.0;
194  dir_vec.y = 1.0/tanf(angle_rad);
195  sweep.y = - 1.0;
196  sweep.x = dir_vec.y / dir_vec.x;
197  }
198  else if (angle <= 225.0) {
199  //south
200  dir_vec.y = -1.0;
201  dir_vec.x = -1.0*tanf(angle_rad);
202  sweep.x = -1.0;
203  sweep.y = dir_vec.x / dir_vec.y;
204  }
205  else {
206  //west
207  dir_vec.x = -1.0;
208  dir_vec.y = -1.0/tanf(angle_rad);
209  sweep.y = 1.0;
210  sweep.x = - dir_vec.y / dir_vec.x;
211  }
212 
213  //normalize
214  len = sqrt(sweep.x*sweep.x+sweep.y*sweep.y);
215  sweep.x = sweep.x / len;
216  sweep.y = sweep.y / len;
217 
218  rad_vec.x = sweep.x * psa_min_rad;
219  rad_vec.y = sweep.y * psa_min_rad;
220  sweep_vec.x = sweep.x * psa_sweep_width;
221  sweep_vec.y = sweep.y * psa_sweep_width;
222 
223  //begin at leftmost position (relative to dir_vec)
224  small.x = waypoints[poly_first].x;
225  small.y = waypoints[poly_first].y;
226 
227  divider = (sweep_vec.y*dir_vec.x) - (sweep_vec.x*dir_vec.y);
228 
229  //cacluate the leftmost point if one sees the dir vec as going "up" and the sweep vec as going right
230  if (divider < 0.0) {
231  for(i=1;i<poly_count;i++)
232  if ((dir_vec.x*(waypoints[poly_first+i].y - small.y)) + (dir_vec.y*(small.x - waypoints[poly_first+i].x)) > 0.0) {
233  small.x = waypoints[poly_first+i].x;
234  small.y = waypoints[poly_first+i].y;
235  }
236  }
237  else
238  for(i=1;i<poly_count;i++)
239  if ((dir_vec.x*(waypoints[poly_first+i].y - small.y)) + (dir_vec.y*(small.x - waypoints[poly_first+i].x)) > 0.0) {
240  small.x = waypoints[poly_first+i].x;
241  small.y = waypoints[poly_first+i].y;
242  }
243 
244  //calculate the line the defines the first flyover
245  seg_start.x = small.x + 0.5*sweep_vec.x;
246  seg_start.y = small.y + 0.5*sweep_vec.y;
247  VEC_CALC(seg_end, seg_start, dir_vec, +);
248 
249  if (!get_two_intersects(&seg_start, &seg_end, seg_start, seg_end)) {
250  psa_stage = ERR;
251  return FALSE;
252  }
253 
254  //center of the entry circle
255  entry_center.x = seg_start.x - rad_vec.x;
256  entry_center.y = seg_start.y - rad_vec.y;
257 
258  //fast climbing to desired altitude
261 
262  psa_stage = ENTRY;
263 
264  return FALSE;
265 }
266 
272 bool_t poly_survey_adv(void)
273 {
276 
277  //entry circle around entry-center until the desired altitude is reached
278  if (psa_stage == ENTRY) {
279  nav_circle_XY(entry_center.x, entry_center.y, -psa_min_rad);
281  && nav_approaching_xy(seg_start.x, seg_start.y, last_x, last_y, CARROT)
282  && fabs(estimator_z - psa_altitude) <= 20) {
283  psa_stage = SEG;
284  nav_init_stage();
285 #ifdef DIGITAL_CAM
286  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);
287 #endif
288  }
289  }
290  //fly the segment until seg_end is reached
291  if (psa_stage == SEG) {
292  nav_points(seg_start, seg_end);
293  //calculate all needed points for the next flyover
294  if (nav_approaching_xy(seg_end.x, seg_end.y, seg_start.x, seg_start.y, 0)) {
295 #ifdef DIGITAL_CAM
296  dc_stop();
297 #endif
298  VEC_CALC(seg_center1, seg_end, rad_vec, -);
299  ret_start.x = seg_end.x - 2*rad_vec.x;
300  ret_start.y = seg_end.y - 2*rad_vec.y;
301 
302  //if we get no intersection the survey is finished
303  if (!get_two_intersects(&seg_start, &seg_end, vec_add(seg_start, sweep_vec), vec_add(seg_end, sweep_vec)))
304  return FALSE;
305 
306  ret_end.x = seg_start.x - sweep_vec.x - 2*rad_vec.x;
307  ret_end.y = seg_start.y - sweep_vec.y - 2*rad_vec.y;
308 
309  seg_center2.x = seg_start.x - 0.5*(2.0*rad_vec.x+sweep_vec.x);
310  seg_center2.y = seg_start.y - 0.5*(2.0*rad_vec.y+sweep_vec.y);
311 
312  psa_stage = TURN1;
313  nav_init_stage();
314  }
315  }
316  //turn from stage to return
317  else if (psa_stage == TURN1) {
318  nav_circle_XY(seg_center1.x, seg_center1.y, -psa_min_rad);
320  psa_stage = RET;
321  nav_init_stage();
322  }
323  //return
324  } else if (psa_stage == RET) {
325  nav_points(ret_start, ret_end);
326  if (nav_approaching_xy(ret_end.x, ret_end.y, ret_start.x, ret_start.y, 0)) {
327  psa_stage = TURN2;
328  nav_init_stage();
329  }
330  //turn from return to stage
331  } else if (psa_stage == TURN2) {
332  nav_circle_XY(seg_center2.x, seg_center2.y, -(2*psa_min_rad+psa_sweep_width)*0.5);
334  psa_stage = SEG;
335  nav_init_stage();
336 #ifdef DIGITAL_CAM
337  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);
338 #endif
339  }
340  }
341 
342  return TRUE;
343 }
static bool_t get_two_intersects(point2d *x, point2d *y, point2d a, point2d b)
intersects a line with the polygon and gives back the two intersection points returns : TRUE if two i...
static void nav_points(point2d start, point2d end)
point2d sweep_vec
point2d seg_start
point2d ret_start
point2d seg_center1
static point2d vec_add(point2d a, point2d b)
point2d seg_end
float psa_sweep_width
#define FALSE
Definition: imu_chimu.h:141
uint8_t dc_survey(float interval, float x, float y)
Sets the dc control in distance mode.
Definition: dc.c:133
bool_t poly_survey_adv(void)
main navigation routine.
point2d seg_center2
survey_stage
float psa_shot_dist
survey_stage psa_stage
The Following variables are dynamic, changed while navigating.
point2d ret_end
Standard Digital Camera Control Interface.
uint8_t poly_first
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)
initializes the variables needed for the survey to start first_wp : the first Waypoint of the polygon...
float psa_min_rad
point2d rad_vec
int segment_angle
#define TRUE
Definition: imu_chimu.h:144
point2d dir_vec
The following variables are set by poly_survey_init and not changed later on.
float psa_altitude
unsigned char uint8_t
Definition: types.h:14
float estimator_z
altitude above MSL in meters
Definition: estimator.c:44
point2d entry_center
uint8_t dc_stop(void)
Sets the dc control in inactive mode, stopping all current actions.
Definition: dc.c:153
State estimation, fusioning sensors.
static bool_t intercept_two_lines(point2d *p, point2d x, point2d y, float a1, float a2, float b1, float b2)
intercept two lines and give back the point of intersection returns : FALSE if no intersection can be...
uint8_t poly_count
#define VEC_CALC(A, B, C, OP)
int return_angle