Paparazzi UAS  v5.0.5_stable-7-g4b8bbb7
Paparazzi is a free software Unmanned Aircraft System.
 All Data Structures Files Functions Variables Typedefs Enumerations Enumerator Macros Modules Pages
poly_survey_adv.c
Go to the documentation of this file.
1 /*
2  * Copyright (C) 2011 The Paparazzi Team
3  *
4  * This file is part of paparazzi.
5  *
6  * paparazzi is free software; you can redistribute it and/or modify
7  * it under the terms of the GNU General Public License as published by
8  * the Free Software Foundation; either version 2, or (at your option)
9  * any later version.
10  *
11  * paparazzi is distributed in the hope that it will be useful,
12  * but WITHOUT ANY WARRANTY; without even the implied warranty of
13  * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the
14  * GNU General Public License for more details.
15  *
16  * You should have received a copy of the GNU General Public License
17  * along with paparazzi; see the file COPYING. If not, write to
18  * the Free Software Foundation, 59 Temple Place - Suite 330,
19  * Boston, MA 02111-1307, USA.
20  */
21 
29 #include "poly_survey_adv.h"
30 
31 #include "subsystems/nav.h"
32 #include "state.h"
33 #include "autopilot.h"
34 #include "generated/flight_plan.h"
35 
36 #ifdef DIGITAL_CAM
37 #include "modules/digital_cam/dc.h"
38 #endif
39 
40 
41 /*
42  The following variables are set by poly_survey_init and not changed later on
43 */
44 
45 // precomputed vectors to ease calculations
49 
50 //the polygon from the flightplan
53 
54 //desired properties of the flyover
59 
60 //direction for the flyover (0° == N)
63 
64 /*
65  The Following variables are dynamic, changed while navigating.
66 */
67 
68 /*
69  psa_stage starts at ENTRY and than circles trought the other
70  states until to polygon is completely covered
71  ENTRY : getting in the right position and height for the first flyover
72  SEG : fly from seg_start to seg_end and take pictures,
73  then calculate navigation points of next flyover
74  TURN1 : do a 180° turn around seg_center1
75  RET : fly from ret_start to ret_end
76  TURN2 : do a 180° turn around seg_center2
77 */
79 
80 // points for navigation
88 
89 
90 //helper functions and macro
91 #define VEC_CALC(A, B, C, OP) A.x = B.x OP C.x; A.y = B.y OP C.y;
92 
94 {
95  point2d tmp;
96  VEC_CALC(tmp, a, b, +);
97 
98  return tmp;
99 }
100 
101 static void nav_points(point2d start, point2d end)
102 {
103  nav_route_xy(start.x, start.y, end.x, end.y);
104 }
105 
114 static bool_t intercept_two_lines(point2d *p, point2d x, point2d y, float a1, float a2, float b1, float b2)
115 {
116  float divider, fac;
117 
118  divider = (((b2 - a2)*(y.x - x.x)) + ((x.y - y.y)*(b1 - a1)));
119  if (divider == 0) return FALSE;
120  fac = ((y.x*(x.y - a2)) + (x.x*(a2 - y.y)) + (a1*(y.y - x.y))) / divider;
121  if (fac > 1.0) return FALSE;
122  if (fac < 0.0) return FALSE;
123 
124  p->x = a1 + fac*(b1 - a1);
125  p->y = a2 + fac*(b2 - a2);
126 
127  return TRUE;
128 }
129 
136 static bool_t get_two_intersects(point2d *x, point2d *y, point2d a, point2d b)
137 {
138  int i, count = 0;
139  point2d tmp;
140 
141  for (i=0;i<poly_count-1;i++)
143  if (count == 0) {
144  *x = tmp;
145  count++;
146  }
147  else {
148  *y = tmp;
149  count++;
150  break;
151  }
152  }
153 
154  //wrapover first,last polygon waypoint
155  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)) {
156  *y = tmp;
157  count++;
158  }
159 
160  if (count != 2)
161  return FALSE;
162 
163  //change points
164  if (fabs(dir_vec.x) > fabs(dir_vec.y)) {
165  if ((y->x - x->x) / dir_vec.x < 0.0){
166  tmp = *x;
167  *x = *y;
168  *y = tmp;
169  }
170  }
171  else
172  if ((y->y - x->y) / dir_vec.y < 0.0) {
173  tmp = *x;
174  *x = *y;
175  *y = tmp;
176  }
177 
178  return TRUE;
179 }
180 
191 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)
192 {
193  int i;
194  point2d small, sweep;
195  float divider, len, angle_rad = angle/180.0*M_PI;
196 
197  if (angle < 0.0) angle += 360.0;
198  if (angle >= 360.0) angle -= 360.0;
199 
200  poly_first = first_wp;
201  poly_count = size;
202 
203  psa_sweep_width = sweep_width;
204  psa_min_rad = min_rad;
205  psa_shot_dist = shot_dist;
206  psa_altitude = altitude;
207 
208  segment_angle = angle;
209  return_angle = angle+180;
210  if (return_angle > 359) return_angle -= 360;
211 
212  if (angle <= 45.0 || angle >= 315.0) {
213  //north
214  dir_vec.y = 1.0;
215  dir_vec.x = 1.0*tanf(angle_rad);
216  sweep.x = 1.0;
217  sweep.y = - dir_vec.x / dir_vec.y;
218  }
219  else if (angle <= 135.0) {
220  //east
221  dir_vec.x = 1.0;
222  dir_vec.y = 1.0/tanf(angle_rad);
223  sweep.y = - 1.0;
224  sweep.x = dir_vec.y / dir_vec.x;
225  }
226  else if (angle <= 225.0) {
227  //south
228  dir_vec.y = -1.0;
229  dir_vec.x = -1.0*tanf(angle_rad);
230  sweep.x = -1.0;
231  sweep.y = dir_vec.x / dir_vec.y;
232  }
233  else {
234  //west
235  dir_vec.x = -1.0;
236  dir_vec.y = -1.0/tanf(angle_rad);
237  sweep.y = 1.0;
238  sweep.x = - dir_vec.y / dir_vec.x;
239  }
240 
241  //normalize
242  len = sqrt(sweep.x*sweep.x+sweep.y*sweep.y);
243  sweep.x = sweep.x / len;
244  sweep.y = sweep.y / len;
245 
246  rad_vec.x = sweep.x * psa_min_rad;
247  rad_vec.y = sweep.y * psa_min_rad;
248  sweep_vec.x = sweep.x * psa_sweep_width;
249  sweep_vec.y = sweep.y * psa_sweep_width;
250 
251  //begin at leftmost position (relative to dir_vec)
252  small.x = waypoints[poly_first].x;
253  small.y = waypoints[poly_first].y;
254 
255  divider = (sweep_vec.y*dir_vec.x) - (sweep_vec.x*dir_vec.y);
256 
257  //cacluate the leftmost point if one sees the dir vec as going "up" and the sweep vec as going right
258  if (divider < 0.0) {
259  for(i=1;i<poly_count;i++)
260  if ((dir_vec.x*(waypoints[poly_first+i].y - small.y)) + (dir_vec.y*(small.x - waypoints[poly_first+i].x)) > 0.0) {
261  small.x = waypoints[poly_first+i].x;
262  small.y = waypoints[poly_first+i].y;
263  }
264  }
265  else
266  for(i=1;i<poly_count;i++)
267  if ((dir_vec.x*(waypoints[poly_first+i].y - small.y)) + (dir_vec.y*(small.x - waypoints[poly_first+i].x)) > 0.0) {
268  small.x = waypoints[poly_first+i].x;
269  small.y = waypoints[poly_first+i].y;
270  }
271 
272  //calculate the line the defines the first flyover
273  seg_start.x = small.x + 0.5*sweep_vec.x;
274  seg_start.y = small.y + 0.5*sweep_vec.y;
275  VEC_CALC(seg_end, seg_start, dir_vec, +);
276 
277  if (!get_two_intersects(&seg_start, &seg_end, seg_start, seg_end)) {
278  psa_stage = ERR;
279  return FALSE;
280  }
281 
282  //center of the entry circle
283  entry_center.x = seg_start.x - rad_vec.x;
284  entry_center.y = seg_start.y - rad_vec.y;
285 
286  //fast climbing to desired altitude
289 
290  psa_stage = ENTRY;
291 
292  return FALSE;
293 }
294 
300 bool_t poly_survey_adv(void)
301 {
304 
305  //entry circle around entry-center until the desired altitude is reached
306  if (psa_stage == ENTRY) {
307  nav_circle_XY(entry_center.x, entry_center.y, -psa_min_rad);
309  && nav_approaching_xy(seg_start.x, seg_start.y, last_x, last_y, CARROT)
310  && fabs(stateGetPositionUtm_f()->alt - psa_altitude) <= 20) {
311  psa_stage = SEG;
312  nav_init_stage();
313 #ifdef DIGITAL_CAM
314  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);
315 #endif
316  }
317  }
318  //fly the segment until seg_end is reached
319  if (psa_stage == SEG) {
320  nav_points(seg_start, seg_end);
321  //calculate all needed points for the next flyover
322  if (nav_approaching_xy(seg_end.x, seg_end.y, seg_start.x, seg_start.y, 0)) {
323 #ifdef DIGITAL_CAM
324  dc_stop();
325 #endif
326  VEC_CALC(seg_center1, seg_end, rad_vec, -);
327  ret_start.x = seg_end.x - 2*rad_vec.x;
328  ret_start.y = seg_end.y - 2*rad_vec.y;
329 
330  //if we get no intersection the survey is finished
331  if (!get_two_intersects(&seg_start, &seg_end, vec_add(seg_start, sweep_vec), vec_add(seg_end, sweep_vec)))
332  return FALSE;
333 
334  ret_end.x = seg_start.x - sweep_vec.x - 2*rad_vec.x;
335  ret_end.y = seg_start.y - sweep_vec.y - 2*rad_vec.y;
336 
337  seg_center2.x = seg_start.x - 0.5*(2.0*rad_vec.x+sweep_vec.x);
338  seg_center2.y = seg_start.y - 0.5*(2.0*rad_vec.y+sweep_vec.y);
339 
340  psa_stage = TURN1;
341  nav_init_stage();
342  }
343  }
344  //turn from stage to return
345  else if (psa_stage == TURN1) {
346  nav_circle_XY(seg_center1.x, seg_center1.y, -psa_min_rad);
348  psa_stage = RET;
349  nav_init_stage();
350  }
351  //return
352  } else if (psa_stage == RET) {
353  nav_points(ret_start, ret_end);
354  if (nav_approaching_xy(ret_end.x, ret_end.y, ret_start.x, ret_start.y, 0)) {
355  psa_stage = TURN2;
356  nav_init_stage();
357  }
358  //turn from return to stage
359  } else if (psa_stage == TURN2) {
360  nav_circle_XY(seg_center2.x, seg_center2.y, -(2*psa_min_rad+psa_sweep_width)*0.5);
362  psa_stage = SEG;
363  nav_init_stage();
364 #ifdef DIGITAL_CAM
365  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);
366 #endif
367  }
368  }
369 
370  return TRUE;
371 }
Advanced polygon survey for fixedwings from Uni Stuttgart.
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
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
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
float psa_min_rad
point2d rad_vec
int segment_angle
#define TRUE
Definition: imu_chimu.h:144
point2d dir_vec
float psa_altitude
static struct UtmCoor_f * stateGetPositionUtm_f(void)
Get position in UTM coordinates (float).
Definition: state.h:651
unsigned char uint8_t
Definition: types.h:14
API to get/set the generic vehicle states.
point2d entry_center
uint8_t dc_stop(void)
Sets the dc control in inactive mode, stopping all current actions.
Definition: dc.c:153
static float p[2][2]
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
uint8_t poly_count
#define VEC_CALC(A, B, C, OP)
int32_t y
North.
int32_t x
East.
int return_angle