Paparazzi UAS  v5.15_devel-112-g521f3cf
Paparazzi is a free software Unmanned Aircraft System.
 All Data Structures Namespaces 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-2014 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 #ifdef DIGITAL_CAM
35 #include "modules/digital_cam/dc.h"
36 #endif
37 
38 #ifndef POLY_OSAM_DEFAULT_SIZE
39 #define POLY_OSAM_DEFAULT_SIZE 5
40 #endif
41 
42 #ifndef POLY_OSAM_DEFAULT_SWEEP
43 #define POLY_OSAM_DEFAULT_SWEEP 100
44 #endif
45 
47 #ifndef POLY_OSAM_ENTRY_RADIUS
48 #define POLY_OSAM_ENTRY_RADIUS 0
49 #endif
50 
52 #ifndef POLY_OSAM_MIN_RADIUS
53 #define POLY_OSAM_MIN_RADIUS 30
54 #endif
55 
57 #ifndef POLY_OSAM_FIRST_SWEEP_DISTANCE
58 #define POLY_OSAM_FIRST_SWEEP_DISTANCE 0
59 #endif
60 
62 #ifndef POLY_OSAM_POLYGONSIZE
63 #define POLY_OSAM_POLYGONSIZE 10
64 #endif
65 
66 #ifndef POLY_OSAM_USE_FULL_CIRCLE
67 #define POLY_OSAM_USE_FULL_CIRCLE TRUE
68 #endif
69 
70 // use half sweep at the end of polygon
71 #ifndef POLY_OSAM_HALF_SWEEP_ENABLED
72 #define POLY_OSAM_HALF_SWEEP_ENABLED TRUE
73 #endif
74 
80 
81 void nav_survey_poly_osam_setup_towards(uint8_t FirstWP, uint8_t Size, float Sweep, int SecondWP)
82 {
83  float dx = waypoints[SecondWP].x - waypoints[FirstWP].x;
84  float dy = waypoints[SecondWP].y - waypoints[FirstWP].y;
85  if (dx == 0.0f) { dx = 0.000000001; }
86  float ang = atan(dy / dx);
87  //if values passed, use it.
88  if (Size == 0) {Size = Poly_Size;}
89  if (Sweep == 0) {Sweep = Poly_Sweep;}
90  nav_survey_poly_osam_setup(FirstWP, Size, Sweep, DegOfRad(ang));
91 }
92 
93 struct Point2D {float x; float y;};
94 struct Line {float m; float b; float x;};
95 
96 static void TranslateAndRotateFromWorld(struct Point2D *p, float Zrot, float transX, float transY);
97 static void RotateAndTranslateToWorld(struct Point2D *p, float Zrot, float transX, float transY);
98 static void FindInterceptOfTwoLines(float *x, float *y, struct Line L1, struct Line L2);
99 static float EvaluateLineForX(float y, struct Line L);
100 
102 {
103  if (rst) {
105  Reset_Sweep = FALSE;
106  }
107 }
108 
109 #define PolygonSize POLY_OSAM_POLYGONSIZE
110 #define MaxFloat 1000000000
111 #define MinFloat -1000000000
112 
113 #ifndef LINE_START_FUNCTION
114 #define LINE_START_FUNCTION {}
115 #endif
116 #ifndef LINE_STOP_FUNCTION
117 #define LINE_STOP_FUNCTION {}
118 #endif
119 
120 /************** Polygon Survey **********************************************/
121 
127 static struct Point2D SmallestCorner;
128 static struct Line Edges[PolygonSize];
129 static float EdgeMaxY[PolygonSize];
130 static float EdgeMinY[PolygonSize];
131 static float SurveyTheta;
132 static float dSweep;
133 static float SurveyRadius;
134 static struct Point2D SurveyToWP;
135 static struct Point2D SurveyFromWP;
136 static struct Point2D SurveyCircle;
139 static float SurveyCircleQdr;
140 static float MaxY;
144 
145 void nav_survey_poly_osam_setup(uint8_t EntryWP, uint8_t Size, float sw, float Orientation)
146 {
147  SmallestCorner.x = 0;
148  SmallestCorner.y = 0;
149  int i = 0;
150  float ys = 0;
151  static struct Point2D EntryPoint;
152  float LeftYInt;
153  float RightYInt;
154  float temp;
155  float XIntercept1 = 0;
156  float XIntercept2 = 0;
157  float entry_distance;
158 
159  float PolySurveyEntryDistance = POLY_OSAM_FIRST_SWEEP_DISTANCE;
160  float PolySurveyEntryRadius = POLY_OSAM_ENTRY_RADIUS;
161 
162  if (PolySurveyEntryDistance == 0) {
163  entry_distance = sw / 2;
164  } else {
165  entry_distance = PolySurveyEntryDistance;
166  }
167 
168  if (PolySurveyEntryRadius == 0) {
169  EntryRadius = sw / 2;
170  } else {
171  EntryRadius = PolySurveyEntryRadius;
172  }
173 
174  SurveyTheta = RadOfDeg(Orientation);
175  PolySurveySweepNum = 0;
177 
178  SurveyEntryWP = EntryWP;
179  SurveySize = Size;
180 
181  struct Point2D Corners[PolygonSize];
182 
184 
185  if (Size == 0) {
186  return;
187  }
188 
189  //Don't initialize if Polygon is too big or if the orientation is not between 0 and 90
190  if (Size <= PolygonSize && Orientation >= -90 && Orientation <= 90) {
191  //Initialize Corners
192  for (i = 0; i < Size; i++) {
193  Corners[i].x = waypoints[i + EntryWP].x;
194  Corners[i].y = waypoints[i + EntryWP].y;
195  }
196 
197  //Rotate Corners so sweeps are parellel with x axis
198  for (i = 0; i < Size; i++) {
199  TranslateAndRotateFromWorld(&Corners[i], SurveyTheta, 0, 0);
200  }
201 
202  //Find min x and min y
203  SmallestCorner.y = Corners[0].y;
204  SmallestCorner.x = Corners[0].x;
205  for (i = 1; i < Size; i++) {
206  if (Corners[i].y < SmallestCorner.y) {
207  SmallestCorner.y = Corners[i].y;
208  }
209 
210  if (Corners[i].x < SmallestCorner.x) {
211  SmallestCorner.x = Corners[i].x;
212  }
213  }
214 
215  //Translate Corners all exist in quad #1
216  for (i = 0; i < Size; i++) {
218  }
219 
220  //Rotate and Translate Entry Point
221  EntryPoint.x = Corners[0].x;
222  EntryPoint.y = Corners[0].y;
223 
224  //Find max y
225  MaxY = Corners[0].y;
226  for (i = 1; i < Size; i++) {
227  if (Corners[i].y > MaxY) {
228  MaxY = Corners[i].y;
229  }
230  }
231 
232  //Find polygon edges
233  for (i = 0; i < Size; i++) {
234  if (i == 0)
235  if (Corners[Size - 1].x == Corners[i].x) { //Don't divide by zero!
236  Edges[i].m = MaxFloat;
237  } else {
238  Edges[i].m = ((Corners[Size - 1].y - Corners[i].y) / (Corners[Size - 1].x - Corners[i].x));
239  }
240  else if (Corners[i].x == Corners[i - 1].x) {
241  Edges[i].m = MaxFloat;
242  } else {
243  Edges[i].m = ((Corners[i].y - Corners[i - 1].y) / (Corners[i].x - Corners[i - 1].x));
244  }
245 
246  //Edges[i].m = MaxFloat;
247  Edges[i].b = (Corners[i].y - (Corners[i].x * Edges[i].m));
248  }
249 
250  //Find Min and Max y for each line
251  FindInterceptOfTwoLines(&temp, &LeftYInt, Edges[0], Edges[1]);
252  FindInterceptOfTwoLines(&temp, &RightYInt, Edges[0], Edges[Size - 1]);
253 
254  if (LeftYInt > RightYInt) {
255  EdgeMaxY[0] = LeftYInt;
256  EdgeMinY[0] = RightYInt;
257  } else {
258  EdgeMaxY[0] = RightYInt;
259  EdgeMinY[0] = LeftYInt;
260  }
261 
262  for (i = 1; i < Size - 1; i++) {
263  FindInterceptOfTwoLines(&temp, &LeftYInt, Edges[i], Edges[i + 1]);
264  FindInterceptOfTwoLines(&temp, &RightYInt, Edges[i], Edges[i - 1]);
265 
266  if (LeftYInt > RightYInt) {
267  EdgeMaxY[i] = LeftYInt;
268  EdgeMinY[i] = RightYInt;
269  } else {
270  EdgeMaxY[i] = RightYInt;
271  EdgeMinY[i] = LeftYInt;
272  }
273  }
274 
275  FindInterceptOfTwoLines(&temp, &LeftYInt, Edges[Size - 1], Edges[0]);
276  FindInterceptOfTwoLines(&temp, &RightYInt, Edges[Size - 1], Edges[Size - 2]);
277 
278  if (LeftYInt > RightYInt) {
279  EdgeMaxY[Size - 1] = LeftYInt;
280  EdgeMinY[Size - 1] = RightYInt;
281  } else {
282  EdgeMaxY[Size - 1] = RightYInt;
283  EdgeMinY[Size - 1] = LeftYInt;
284  }
285 
286  //Find amount to increment by every sweep
287  if (EntryPoint.y >= MaxY / 2) {
288  entry_distance = -entry_distance;
290  dSweep = -sw;
291  } else {
292  dSweep = sw;
293  }
294 
295  //CircleQdr tells the plane when to exit the circle
296  if (dSweep >= 0) {
297  SurveyCircleQdr = -DegOfRad(SurveyTheta);
298  } else {
299  SurveyCircleQdr = 180 - DegOfRad(SurveyTheta);
300  }
301 
302  //Find y value of the first sweep
303  ys = EntryPoint.y + entry_distance;
304 
305  //Find the edges which intercet the sweep line first
306  for (i = 0; i < SurveySize; i++) {
307  if (EdgeMinY[i] <= ys && EdgeMaxY[i] > ys) {
308  XIntercept2 = XIntercept1;
309  XIntercept1 = EvaluateLineForX(ys, Edges[i]);
310  }
311  }
312 
313  //Find point to come from and point to go to
314  if (fabs(EntryPoint.x - XIntercept2) <= fabs(EntryPoint.x - XIntercept1)) {
315  SurveyToWP.x = XIntercept1;
316  SurveyToWP.y = ys;
317 
318  SurveyFromWP.x = XIntercept2;
319  SurveyFromWP.y = ys;
320  } else {
321  SurveyToWP.x = XIntercept2;
322  SurveyToWP.y = ys;
323 
324  SurveyFromWP.x = XIntercept1;
325  SurveyFromWP.y = ys;
326  }
327 
328  //Find the direction to circle
329  if (ys > 0 && SurveyToWP.x > SurveyFromWP.x) {
331  } else if (ys < 0 && SurveyToWP.x < SurveyFromWP.x) {
333  } else {
335  }
336 
337  //Find the entry circle
339  SurveyCircle.y = EntryPoint.y + entry_distance - EntryRadius;
340 
341  //Go into entry circle state
342  CSurveyStatus = Entry;
344  }
345 }
346 
348 {
349  struct Point2D C;
350  struct Point2D ToP;
351  struct Point2D FromP;
352  float ys;
353  static struct Point2D LastPoint;
354  int i;
355  bool LastHalfSweep;
356  static bool HalfSweep = false;
357  float XIntercept1 = 0;
358  float XIntercept2 = 0;
359  float DInt1 = 0;
360  float DInt2 = 0;
361  float temp;
362  float min_radius = POLY_OSAM_MIN_RADIUS;
363 
364  if (SurveySize == 0) {
365  return false;
366  }
367 
368  NavVerticalAutoThrottleMode(0); /* No pitch */
370 
371  switch (CSurveyStatus) {
372  case Entry:
373  //Rotate and translate circle point into real world
374  C.x = SurveyCircle.x;
375  C.y = SurveyCircle.y;
378 
379  //follow the circle
380  nav_circle_XY(C.x, C.y, SurveyRadius);
381 
383  && stateGetPositionUtm_f()->alt > waypoints[SurveyEntryWP].a - 10) {
385  nav_init_stage();
386  //LINE_START_FUNCTION;
387  }
388  break;
389  case Sweep:
390  LastHalfSweep = HalfSweep;
391  ToP.x = SurveyToWP.x;
392  ToP.y = SurveyToWP.y;
393  FromP.x = SurveyFromWP.x;
394  FromP.y = SurveyFromWP.y;
395 
396  //Rotate and Translate de plane position to local world
397  C.x = stateGetPositionEnu_f()->x;
398  C.y = stateGetPositionEnu_f()->y;
401 
402 #ifdef DIGITAL_CAM
403  {
404  //calc distance from line start and plane position (use only X position because y can be far due to wind or other factor)
405  float dist = FromP.x - C.x;
406 
407  // verify if plane are less than 10 meter from line start
408  if ((dc_autoshoot == DC_AUTOSHOOT_STOP) && (fabs(dist) < 10)) {
410  }
411  }
412 #endif
413 
414  //Rotate and Translate Line points into real world
417 
419  RotateAndTranslateToWorld(&FromP, SurveyTheta, 0, 0);
420 
421  //follow the line
422  nav_route_xy(FromP.x, FromP.y, ToP.x, ToP.y);
423  if (nav_approaching_xy(ToP.x, ToP.y, FromP.x, FromP.y, 0)) {
424  LastPoint.x = SurveyToWP.x;
425  LastPoint.y = SurveyToWP.y;
426 
427  if (LastPoint.y + dSweep >= MaxY || LastPoint.y + dSweep <= 0) { //Your out of the Polygon so Sweep Back or Half Sweep
428 
429  if (LastPoint.y + (dSweep / 2) >= MaxY || LastPoint.y + (dSweep / 2) <= 0 || !Half_Sweep_Enabled) { //Sweep back
430  dSweep = -dSweep;
431  if (LastHalfSweep) {
432  HalfSweep = false;
433  ys = LastPoint.y + (dSweep);
434  } else {
435  HalfSweep = true;
436  ys = LastPoint.y + (dSweep / 2);
437  }
438 
439  if (dSweep >= 0) {
440  SurveyCircleQdr = -DegOfRad(SurveyTheta);
441  } else {
442  SurveyCircleQdr = 180 - DegOfRad(SurveyTheta);
443  }
445  } else { // Half Sweep forward
446  ys = LastPoint.y + (dSweep / 2);
447 
448  if (dSweep >= 0) {
449  SurveyCircleQdr = -DegOfRad(SurveyTheta);
450  } else {
451  SurveyCircleQdr = 180 - DegOfRad(SurveyTheta);
452  }
453  HalfSweep = true;
454  }
455 
456 
457  } else { // Normal sweep
458  //Find y value of the first sweep
459  HalfSweep = false;
460  ys = LastPoint.y + dSweep;
461  }
462 
463  //Find the edges which intercet the sweep line first
464  for (i = 0; i < SurveySize; i++) {
465  if (EdgeMinY[i] < ys && EdgeMaxY[i] >= ys) {
466  XIntercept2 = XIntercept1;
467  XIntercept1 = EvaluateLineForX(ys, Edges[i]);
468  }
469  }
470 
471  //Find point to come from and point to go to
472  DInt1 = XIntercept1 - LastPoint.x;
473  DInt2 = XIntercept2 - LastPoint.x;
474 
475  if (DInt1 * DInt2 >= 0) {
476  if (fabs(DInt2) <= fabs(DInt1)) {
477  SurveyToWP.x = XIntercept1;
478  SurveyToWP.y = ys;
479 
480  SurveyFromWP.x = XIntercept2;
481  SurveyFromWP.y = ys;
482  } else {
483  SurveyToWP.x = XIntercept2;
484  SurveyToWP.y = ys;
485 
486  SurveyFromWP.x = XIntercept1;
487  SurveyFromWP.y = ys;
488  }
489  } else {
490  if ((SurveyToWP.x - SurveyFromWP.x) > 0 && DInt2 > 0) {
491  SurveyToWP.x = XIntercept1;
492  SurveyToWP.y = ys;
493 
494  SurveyFromWP.x = XIntercept2;
495  SurveyFromWP.y = ys;
496  } else if ((SurveyToWP.x - SurveyFromWP.x) < 0 && DInt2 < 0) {
497  SurveyToWP.x = XIntercept1;
498  SurveyToWP.y = ys;
499 
500  SurveyFromWP.x = XIntercept2;
501  SurveyFromWP.y = ys;
502  } else {
503  SurveyToWP.x = XIntercept2;
504  SurveyToWP.y = ys;
505 
506  SurveyFromWP.x = XIntercept1;
507  SurveyFromWP.y = ys;
508  }
509  }
510 
511  //Find the radius to circle
512  if (!HalfSweep || use_full_circle) {
513  temp = dSweep / 2;
514  } else {
515  temp = dSweep / 4;
516  }
517 
518  //if less than min radius
519  if (fabs(temp) < min_radius) {
520  if (temp < 0) { temp = -min_radius; } else { temp = min_radius; }
521  }
522 
523  //Find the direction to circle
524  if (ys > 0 && SurveyToWP.x > SurveyFromWP.x) {
525  SurveyRadius = temp;
526  } else if (ys < 0 && SurveyToWP.x < SurveyFromWP.x) {
527  SurveyRadius = temp;
528  } else {
529  SurveyRadius = -temp;
530  }
531 
532  //find x position to circle
533  if (fabs(LastPoint.x - SurveyToWP.x) > fabs(SurveyFromWP.x - SurveyToWP.x)) {
534  SurveyCircle.x = LastPoint.x;
535  } else {
537  }
538 
539  //y position to circle
540  SurveyCircle.y = ys - temp;
541 
542  //Go into circle state
544  nav_init_stage();
547  }
548 
549  break;
550  case SweepCircle:
551  //Rotate and translate circle point into real world
552  C.x = SurveyCircle.x;
553  C.y = SurveyCircle.y;
556 
557  //follow the circle
558  nav_circle_XY(C.x, C.y, SurveyRadius);
559 
562  nav_init_stage();
563  //LINE_START_FUNCTION;
564  }
565  break;
566  case Init:
567  return false;
568  default:
569  return false;
570  }
571  return true;
572 }
573 
574 
575 /*
576  Translates point so (transX, transY) are (0,0) then rotates the point around z by Zrot
577 */
578 void TranslateAndRotateFromWorld(struct Point2D *p, float Zrot, float transX, float transY)
579 {
580  float temp;
581 
582  p->x = p->x - transX;
583  p->y = p->y - transY;
584 
585  temp = p->x;
586  p->x = p->x * cosf(Zrot) + p->y * sinf(Zrot);
587  p->y = -temp * sinf(Zrot) + p->y * cosf(Zrot);
588 }
589 
591 void RotateAndTranslateToWorld(struct Point2D *p, float Zrot, float transX, float transY)
592 {
593  float temp = p->x;
594 
595  p->x = p->x * cosf(Zrot) - p->y * sinf(Zrot);
596  p->y = temp * sinf(Zrot) + p->y * cosf(Zrot);
597 
598  p->x = p->x + transX;
599  p->y = p->y + transY;
600 }
601 
602 void FindInterceptOfTwoLines(float *x, float *y, struct Line L1, struct Line L2)
603 {
604  *x = ((L2.b - L1.b) / (L1.m - L2.m));
605  *y = L1.m * (*x) + L1.b;
606 }
607 
608 
609 float EvaluateLineForX(float y, struct Line L)
610 {
611  return ((y - L.b) / L.m);
612 }
unsigned short uint16_t
Definition: types.h:16
float x
Definition: common_nav.h:40
static struct EnuCoor_f * stateGetPositionEnu_f(void)
Get position in local ENU coordinates (float).
Definition: state.h:719
#define FALSE
Definition: std.h:5
float y
Definition: common_nav.h:41
Standard Digital Camera Control Interface.
float x
in meters
dc_autoshoot_type dc_autoshoot
Definition: dc.c:67
Core autopilot interface common to all firmwares.
static struct UtmCoor_f * stateGetPositionUtm_f(void)
Get position in UTM coordinates (float).
Definition: state.h:692
unsigned char uint8_t
Definition: types.h:14
API to get/set the generic vehicle states.
struct point waypoints[NB_WAYPOINT]
size == nb_waypoint, waypoint 0 is a dummy waypoint
Definition: common_nav.c:38
static float p[2][2]
float y
in meters