Paparazzi UAS  v5.2.2_stable-0-gd6b9f29
Paparazzi is a free software Unmanned Aircraft System.
 All Data Structures Files Functions Variables Typedefs Enumerations Enumerator Macros Modules Pages
nav_survey_poly_osam.c
Go to the documentation of this file.
1 /*
2  * Copyright (C) 2008-2013 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 
28 
30 #include "state.h"
31 #include "autopilot.h"
32 #include "generated/flight_plan.h"
33 
34 
35 struct Point2D {float x; float y;};
36 struct Line {float m;float b;float x;};
37 
38 static void TranslateAndRotateFromWorld(struct Point2D *p, float Zrot, float transX, float transY);
39 static void RotateAndTranslateToWorld(struct Point2D *p, float Zrot, float transX, float transY);
40 static void FindInterceptOfTwoLines(float *x, float *y, struct Line L1, struct Line L2);
41 static float EvaluateLineForX(float y, struct Line L);
42 
43 #define PolygonSize 10
44 #define MaxFloat 1000000000
45 #define MinFloat -1000000000
46 
47 #ifndef LINE_START_FUNCTION
48 #define LINE_START_FUNCTION {}
49 #endif
50 #ifndef LINE_STOP_FUNCTION
51 #define LINE_STOP_FUNCTION {}
52 #endif
53 
54 /************** Polygon Survey **********************************************/
55 
61 static struct Point2D SmallestCorner;
62 static struct Line Edges[PolygonSize];
63 static float EdgeMaxY[PolygonSize];
64 static float EdgeMinY[PolygonSize];
65 static float SurveyTheta;
66 static float dSweep;
67 static float SurveyRadius;
68 static struct Point2D SurveyToWP;
69 static struct Point2D SurveyFromWP;
70 static struct Point2D SurveyCircle;
73 static float SurveyCircleQdr;
74 static float MaxY;
77 
78 bool_t nav_survey_poly_osam_setup(uint8_t EntryWP, uint8_t Size, float sw, float Orientation)
79 {
80  SmallestCorner.x = 0;
81  SmallestCorner.y = 0;
82  int i = 0;
83  float ys = 0;
84  static struct Point2D EntryPoint;
85  float LeftYInt;
86  float RightYInt;
87  float temp;
88  float XIntercept1 = 0;
89  float XIntercept2 = 0;
90 
91  SurveyTheta = RadOfDeg(Orientation);
94 
95  SurveyEntryWP = EntryWP;
96  SurveySize = Size;
97 
98  struct Point2D Corners[PolygonSize];
99 
101 
102  if (Size == 0)
103  return TRUE;
104 
105  //Don't initialize if Polygon is too big or if the orientation is not between 0 and 90
106  if(Size <= PolygonSize && Orientation >= -90 && Orientation <= 90)
107  {
108  //Initialize Corners
109  for(i = 0; i < Size; i++)
110  {
111  Corners[i].x = waypoints[i+EntryWP].x;
112  Corners[i].y = waypoints[i+EntryWP].y;
113  }
114 
115  //Rotate Corners so sweeps are parellel with x axis
116  for(i = 0; i < Size; i++)
117  TranslateAndRotateFromWorld(&Corners[i], SurveyTheta, 0, 0);
118 
119  //Find min x and min y
120  SmallestCorner.y = Corners[0].y;
121  SmallestCorner.x = Corners[0].x;
122  for(i = 1; i < Size; i++)
123  {
124  if(Corners[i].y < SmallestCorner.y)
125  SmallestCorner.y = Corners[i].y;
126 
127  if(Corners[i].x < SmallestCorner.x)
128  SmallestCorner.x = Corners[i].x;
129  }
130 
131  //Translate Corners all exist in quad #1
132  for(i = 0; i < Size; i++)
134 
135  //Rotate and Translate Entry Point
136  EntryPoint.x = Corners[0].x;
137  EntryPoint.y = Corners[0].y;
138 
139  //Find max y
140  MaxY = Corners[0].y;
141  for(i = 1; i < Size; i++)
142  {
143  if(Corners[i].y > MaxY)
144  MaxY = Corners[i].y;
145  }
146 
147  //Find polygon edges
148  for(i = 0; i < Size; i++)
149  {
150  if(i == 0)
151  if(Corners[Size-1].x == Corners[i].x) //Don't divide by zero!
152  Edges[i].m = MaxFloat;
153  else
154  Edges[i].m = ((Corners[Size-1].y-Corners[i].y)/(Corners[Size-1].x-Corners[i].x));
155  else
156  if(Corners[i].x == Corners[i-1].x)
157  Edges[i].m = MaxFloat;
158  else
159  Edges[i].m = ((Corners[i].y-Corners[i-1].y)/(Corners[i].x-Corners[i-1].x));
160 
161  //Edges[i].m = MaxFloat;
162  Edges[i].b = (Corners[i].y - (Corners[i].x*Edges[i].m));
163  }
164 
165  //Find Min and Max y for each line
166  FindInterceptOfTwoLines(&temp, &LeftYInt, Edges[0], Edges[1]);
167  FindInterceptOfTwoLines(&temp, &RightYInt, Edges[0], Edges[Size-1]);
168 
169  if(LeftYInt > RightYInt)
170  {
171  EdgeMaxY[0] = LeftYInt;
172  EdgeMinY[0] = RightYInt;
173  }
174  else
175  {
176  EdgeMaxY[0] = RightYInt;
177  EdgeMinY[0] = LeftYInt;
178  }
179 
180  for(i = 1; i < Size-1; i++)
181  {
182  FindInterceptOfTwoLines(&temp, &LeftYInt, Edges[i], Edges[i+1]);
183  FindInterceptOfTwoLines(&temp, &RightYInt, Edges[i], Edges[i-1]);
184 
185  if(LeftYInt > RightYInt)
186  {
187  EdgeMaxY[i] = LeftYInt;
188  EdgeMinY[i] = RightYInt;
189  }
190  else
191  {
192  EdgeMaxY[i] = RightYInt;
193  EdgeMinY[i] = LeftYInt;
194  }
195  }
196 
197  FindInterceptOfTwoLines(&temp, &LeftYInt, Edges[Size-1], Edges[0]);
198  FindInterceptOfTwoLines(&temp, &RightYInt, Edges[Size-1], Edges[Size-2]);
199 
200  if(LeftYInt > RightYInt)
201  {
202  EdgeMaxY[Size-1] = LeftYInt;
203  EdgeMinY[Size-1] = RightYInt;
204  }
205  else
206  {
207  EdgeMaxY[Size-1] = RightYInt;
208  EdgeMinY[Size-1] = LeftYInt;
209  }
210 
211  //Find amount to increment by every sweep
212  if(EntryPoint.y >= MaxY/2)
213  dSweep = -sw;
214  else
215  dSweep = sw;
216 
217  //CircleQdr tells the plane when to exit the circle
218  if(dSweep >= 0)
219  SurveyCircleQdr = -DegOfRad(SurveyTheta);
220  else
221  SurveyCircleQdr = 180-DegOfRad(SurveyTheta);
222 
223  //Find y value of the first sweep
224  ys = EntryPoint.y+(dSweep/2);
225 
226  //Find the edges which intercet the sweep line first
227  for(i = 0; i < SurveySize; i++)
228  {
229  if(EdgeMinY[i] <= ys && EdgeMaxY[i] > ys)
230  {
231  XIntercept2 = XIntercept1;
232  XIntercept1 = EvaluateLineForX(ys, Edges[i]);
233  }
234  }
235 
236  //Find point to come from and point to go to
237  if(fabs(EntryPoint.x - XIntercept2) <= fabs(EntryPoint.x - XIntercept1))
238  {
239  SurveyToWP.x = XIntercept1;
240  SurveyToWP.y = ys;
241 
242  SurveyFromWP.x = XIntercept2;
243  SurveyFromWP.y = ys;
244  }
245  else
246  {
247  SurveyToWP.x = XIntercept2;
248  SurveyToWP.y = ys;
249 
250  SurveyFromWP.x = XIntercept1;
251  SurveyFromWP.y = ys;
252  }
253 
254  //Find the direction to circle
255  if(ys > 0 && SurveyToWP.x > SurveyFromWP.x)
256  SurveyRadius = dSweep/2;
257  else if(ys < 0 && SurveyToWP.x < SurveyFromWP.x)
258  SurveyRadius = dSweep/2;
259  else
260  SurveyRadius = -dSweep/2;
261 
262  //Find the entry circle
264  SurveyCircle.y = EntryPoint.y;
265 
266  //Go into entry circle state
267  CSurveyStatus = Entry;
269  }
270 
271  return FALSE;
272 }
273 
275 {
276  struct Point2D C;
277  struct Point2D ToP;
278  struct Point2D FromP;
279  float ys;
280  static struct Point2D LastPoint;
281  int i;
282  bool_t SweepingBack = FALSE;
283  float XIntercept1 = 0;
284  float XIntercept2 = 0;
285  float DInt1 = 0;
286  float DInt2 = 0;
287 
288  NavVerticalAutoThrottleMode(0); /* No pitch */
290 
291  switch(CSurveyStatus)
292  {
293  case Entry:
294  //Rotate and translate circle point into real world
295  C.x = SurveyCircle.x;
296  C.y = SurveyCircle.y;
299 
300  //follow the circle
301  nav_circle_XY(C.x, C.y, SurveyRadius);
302 
304  {
306  nav_init_stage();
308  }
309  break;
310  case Sweep:
311  //Rotate and Translate Line points into real world
312  ToP.x = SurveyToWP.x;
313  ToP.y = SurveyToWP.y;
314  FromP.x = SurveyFromWP.x;
315  FromP.y = SurveyFromWP.y;
316 
319 
321  RotateAndTranslateToWorld(&FromP, SurveyTheta, 0, 0);
322 
323  //follow the line
324  nav_route_xy(FromP.x,FromP.y,ToP.x,ToP.y);
325  if(nav_approaching_xy(ToP.x,ToP.y,FromP.x,FromP.y, 0))
326  {
327  LastPoint.x = SurveyToWP.x;
328  LastPoint.y = SurveyToWP.y;
329 
330  if(LastPoint.y+dSweep >= MaxY || LastPoint.y+dSweep <= 0) //Your out of the Polygon so Sweep Back
331  {
332  dSweep = -dSweep;
333  ys = LastPoint.y+(dSweep/2);
334 
335  if(dSweep >= 0)
336  SurveyCircleQdr = -DegOfRad(SurveyTheta);
337  else
338  SurveyCircleQdr = 180-DegOfRad(SurveyTheta);
339  SweepingBack = TRUE;
341  }
342  else
343  {
344  //Find y value of the first sweep
345  ys = LastPoint.y+dSweep;
346  }
347 
348  //Find the edges which intercet the sweep line first
349  for(i = 0; i < SurveySize; i++)
350  {
351  if(EdgeMinY[i] < ys && EdgeMaxY[i] >= ys)
352  {
353  XIntercept2 = XIntercept1;
354  XIntercept1 = EvaluateLineForX(ys, Edges[i]);
355  }
356  }
357 
358  //Find point to come from and point to go to
359  DInt1 = XIntercept1 - LastPoint.x;
360  DInt2 = XIntercept2 - LastPoint.x;
361 
362  if(DInt1 * DInt2 >= 0)
363  {
364  if(fabs(DInt2) <= fabs(DInt1))
365  {
366  SurveyToWP.x = XIntercept1;
367  SurveyToWP.y = ys;
368 
369  SurveyFromWP.x = XIntercept2;
370  SurveyFromWP.y = ys;
371  }
372  else
373  {
374  SurveyToWP.x = XIntercept2;
375  SurveyToWP.y = ys;
376 
377  SurveyFromWP.x = XIntercept1;
378  SurveyFromWP.y = ys;
379  }
380  }
381  else
382  {
383  if((SurveyToWP.x - SurveyFromWP.x) > 0 && DInt2 > 0)
384  {
385  SurveyToWP.x = XIntercept1;
386  SurveyToWP.y = ys;
387 
388  SurveyFromWP.x = XIntercept2;
389  SurveyFromWP.y = ys;
390  }
391  else if((SurveyToWP.x - SurveyFromWP.x) < 0 && DInt2 < 0)
392  {
393  SurveyToWP.x = XIntercept1;
394  SurveyToWP.y = ys;
395 
396  SurveyFromWP.x = XIntercept2;
397  SurveyFromWP.y = ys;
398  }
399  else
400  {
401  SurveyToWP.x = XIntercept2;
402  SurveyToWP.y = ys;
403 
404  SurveyFromWP.x = XIntercept1;
405  SurveyFromWP.y = ys;
406  }
407  }
408 
409 
410 
411  if(fabs(LastPoint.x-SurveyToWP.x) > fabs(SurveyFromWP.x-SurveyToWP.x))
412  SurveyCircle.x = LastPoint.x;
413  else
415 
416 
417  if(!SweepingBack)
418  SurveyCircle.y = LastPoint.y+(dSweep/2);
419  else
420  SurveyCircle.y = LastPoint.y;
421 
422  //Find the direction to circle
423  if(ys > 0 && SurveyToWP.x > SurveyFromWP.x)
424  SurveyRadius = dSweep/2;
425  else if(ys < 0 && SurveyToWP.x < SurveyFromWP.x)
426  SurveyRadius = dSweep/2;
427  else
428  SurveyRadius = -dSweep/2;
429 
430  //Go into circle state
432  nav_init_stage();
435  }
436 
437  break;
438  case SweepCircle:
439  //Rotate and translate circle point into real world
440  C.x = SurveyCircle.x;
441  C.y = SurveyCircle.y;
444 
445  //follow the circle
446  nav_circle_XY(C.x, C.y, SurveyRadius);
447 
449  {
451  nav_init_stage();
453  }
454  break;
455  case Init:
456  return FALSE;
457  default:
458  return FALSE;
459  }
460  return TRUE;
461 }
462 
463 
464 /*
465  Translates point so (transX, transY) are (0,0) then rotates the point around z by Zrot
466 */
467 void TranslateAndRotateFromWorld(struct Point2D *p, float Zrot, float transX, float transY)
468 {
469  float temp;
470 
471  p->x = p->x - transX;
472  p->y = p->y - transY;
473 
474  temp = p->x;
475  p->x = p->x*cosf(Zrot)+p->y*sinf(Zrot);
476  p->y = -temp*sinf(Zrot)+p->y*cosf(Zrot);
477 }
478 
480 void RotateAndTranslateToWorld(struct Point2D *p, float Zrot, float transX, float transY)
481 {
482  float temp = p->x;
483 
484  p->x = p->x*cosf(Zrot)-p->y*sinf(Zrot);
485  p->y = temp*sinf(Zrot)+p->y*cosf(Zrot);
486 
487  p->x = p->x + transX;
488  p->y = p->y + transY;
489 }
490 
491 void FindInterceptOfTwoLines(float *x, float *y, struct Line L1, struct Line L2)
492 {
493  *x = ((L2.b-L1.b)/(L1.m-L2.m));
494  *y = L1.m*(*x)+L1.b;
495 }
496 
497 
498 float EvaluateLineForX(float y, struct Line L)
499 {
500  return ((y-L.b)/L.m);
501 }
float y
Definition: nav_line_osam.c:44
unsigned short uint16_t
Definition: types.h:16
float x
Definition: nav_line_osam.c:44
#define FALSE
Definition: imu_chimu.h:141
#define TRUE
Definition: imu_chimu.h:144
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.
static float p[2][2]
int32_t y
North.
int32_t x
East.