Paparazzi UAS  v5.17_devel-24-g2ae834f
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 
146 void nav_survey_poly_osam_setup(uint8_t EntryWP, uint8_t Size, float sw, float Orientation)
147 {
148  SmallestCorner.x = 0;
149  SmallestCorner.y = 0;
150  int i = 0;
151  float ys = 0;
152  static struct Point2D EntryPoint;
153  float LeftYInt;
154  float RightYInt;
155  float temp;
156  float XIntercept1 = 0;
157  float XIntercept2 = 0;
158  float entry_distance;
159 
160  float PolySurveyEntryDistance = POLY_OSAM_FIRST_SWEEP_DISTANCE;
161  float PolySurveyEntryRadius = POLY_OSAM_ENTRY_RADIUS;
162 
163  if (PolySurveyEntryDistance == 0) {
164  entry_distance = sw / 2;
165  } else {
166  entry_distance = PolySurveyEntryDistance;
167  }
168 
169  if (PolySurveyEntryRadius == 0) {
170  EntryRadius = sw / 2;
171  } else {
172  EntryRadius = PolySurveyEntryRadius;
173  }
174 
175  SurveyTheta = RadOfDeg(Orientation);
176  PolySurveySweepNum = 0;
178 
179  SurveyEntryWP = EntryWP;
180  SurveySize = Size;
181  Poly_Sweep = sw;
182 
183  struct Point2D Corners[PolygonSize];
184 
186 
187  if (Size == 0) {
188  return;
189  }
190 
191  //Don't initialize if Polygon is too big or if the orientation is not between 0 and 90
192  if (Size <= PolygonSize && Orientation >= -90 && Orientation <= 90) {
193  //Initialize Corners
194  for (i = 0; i < Size; i++) {
195  Corners[i].x = waypoints[i + EntryWP].x;
196  Corners[i].y = waypoints[i + EntryWP].y;
197  }
198 
199  //Rotate Corners so sweeps are parellel with x axis
200  for (i = 0; i < Size; i++) {
201  TranslateAndRotateFromWorld(&Corners[i], SurveyTheta, 0, 0);
202  }
203 
204  //Find min x and min y
205  SmallestCorner.y = Corners[0].y;
206  SmallestCorner.x = Corners[0].x;
207  for (i = 1; i < Size; i++) {
208  if (Corners[i].y < SmallestCorner.y) {
209  SmallestCorner.y = Corners[i].y;
210  }
211 
212  if (Corners[i].x < SmallestCorner.x) {
213  SmallestCorner.x = Corners[i].x;
214  }
215  }
216 
217  //Translate Corners all exist in quad #1
218  for (i = 0; i < Size; i++) {
220  }
221 
222  //Rotate and Translate Entry Point
223  EntryPoint.x = Corners[0].x;
224  EntryPoint.y = Corners[0].y;
225 
226  //Find max y
227  MaxY = Corners[0].y;
228  for (i = 1; i < Size; i++) {
229  if (Corners[i].y > MaxY) {
230  MaxY = Corners[i].y;
231  }
232  }
233 
234  //Find polygon edges
235  for (i = 0; i < Size; i++) {
236  if (i == 0)
237  if (Corners[Size - 1].x == Corners[i].x) { //Don't divide by zero!
238  Edges[i].m = MaxFloat;
239  } else {
240  Edges[i].m = ((Corners[Size - 1].y - Corners[i].y) / (Corners[Size - 1].x - Corners[i].x));
241  }
242  else if (Corners[i].x == Corners[i - 1].x) {
243  Edges[i].m = MaxFloat;
244  } else {
245  Edges[i].m = ((Corners[i].y - Corners[i - 1].y) / (Corners[i].x - Corners[i - 1].x));
246  }
247 
248  //Edges[i].m = MaxFloat;
249  Edges[i].b = (Corners[i].y - (Corners[i].x * Edges[i].m));
250  }
251 
252  //Find Min and Max y for each line
253  FindInterceptOfTwoLines(&temp, &LeftYInt, Edges[0], Edges[1]);
254  FindInterceptOfTwoLines(&temp, &RightYInt, Edges[0], Edges[Size - 1]);
255 
256  if (LeftYInt > RightYInt) {
257  EdgeMaxY[0] = LeftYInt;
258  EdgeMinY[0] = RightYInt;
259  } else {
260  EdgeMaxY[0] = RightYInt;
261  EdgeMinY[0] = LeftYInt;
262  }
263 
264  for (i = 1; i < Size - 1; i++) {
265  FindInterceptOfTwoLines(&temp, &LeftYInt, Edges[i], Edges[i + 1]);
266  FindInterceptOfTwoLines(&temp, &RightYInt, Edges[i], Edges[i - 1]);
267 
268  if (LeftYInt > RightYInt) {
269  EdgeMaxY[i] = LeftYInt;
270  EdgeMinY[i] = RightYInt;
271  } else {
272  EdgeMaxY[i] = RightYInt;
273  EdgeMinY[i] = LeftYInt;
274  }
275  }
276 
277  FindInterceptOfTwoLines(&temp, &LeftYInt, Edges[Size - 1], Edges[0]);
278  FindInterceptOfTwoLines(&temp, &RightYInt, Edges[Size - 1], Edges[Size - 2]);
279 
280  if (LeftYInt > RightYInt) {
281  EdgeMaxY[Size - 1] = LeftYInt;
282  EdgeMinY[Size - 1] = RightYInt;
283  } else {
284  EdgeMaxY[Size - 1] = RightYInt;
285  EdgeMinY[Size - 1] = LeftYInt;
286  }
287 
288  //Find amount to increment by every sweep
289  if (EntryPoint.y >= MaxY / 2) {
290  entry_distance = -entry_distance;
292  dSweep = -sw;
293  } else {
294  dSweep = sw;
295  }
296 
297  //CircleQdr tells the plane when to exit the circle
298  if (dSweep >= 0) {
299  SurveyCircleQdr = -DegOfRad(SurveyTheta);
300  } else {
301  SurveyCircleQdr = 180 - DegOfRad(SurveyTheta);
302  }
303 
304  //Find y value of the first sweep
305  ys = EntryPoint.y + entry_distance;
306 
307  //Find the edges which intercet the sweep line first
308  for (i = 0; i < SurveySize; i++) {
309  if (EdgeMinY[i] <= ys && EdgeMaxY[i] > ys) {
310  XIntercept2 = XIntercept1;
311  XIntercept1 = EvaluateLineForX(ys, Edges[i]);
312  }
313  }
314 
315  //Find point to come from and point to go to
316  if (fabs(EntryPoint.x - XIntercept2) <= fabs(EntryPoint.x - XIntercept1)) {
317  SurveyToWP.x = XIntercept1;
318  SurveyToWP.y = ys;
319 
320  SurveyFromWP.x = XIntercept2;
321  SurveyFromWP.y = ys;
322  } else {
323  SurveyToWP.x = XIntercept2;
324  SurveyToWP.y = ys;
325 
326  SurveyFromWP.x = XIntercept1;
327  SurveyFromWP.y = ys;
328  }
329 
330  //Find the direction to circle
331  if (ys > 0 && SurveyToWP.x > SurveyFromWP.x) {
333  } else if (ys < 0 && SurveyToWP.x < SurveyFromWP.x) {
335  } else {
337  }
338 
339  //Find the entry circle
341  SurveyCircle.y = EntryPoint.y + entry_distance - EntryRadius;
342 
343  //Go into entry circle state
344  CSurveyStatus = Entry;
346  }
347 }
348 
350 {
351  #ifdef NAV_SURVEY_POLY_OSAM_DYNAMIC
353  #endif
354 
355  struct Point2D C;
356  struct Point2D ToP;
357  struct Point2D FromP;
358  float ys;
359  static struct Point2D LastPoint;
360  int i;
361  bool LastHalfSweep;
362  static bool HalfSweep = false;
363  float XIntercept1 = 0;
364  float XIntercept2 = 0;
365  float DInt1 = 0;
366  float DInt2 = 0;
367  float temp;
368  float min_radius = POLY_OSAM_MIN_RADIUS;
369 
370  if (SurveySize == 0) {
371  return false;
372  }
373 
374  NavVerticalAutoThrottleMode(0); /* No pitch */
376 
377  switch (CSurveyStatus) {
378  case Entry:
379  //Rotate and translate circle point into real world
380  C.x = SurveyCircle.x;
381  C.y = SurveyCircle.y;
384 
385  //follow the circle
386  nav_circle_XY(C.x, C.y, SurveyRadius);
387 
389  && stateGetPositionUtm_f()->alt > waypoints[SurveyEntryWP].a - 10) {
391  nav_init_stage();
392  //LINE_START_FUNCTION;
393  }
394  break;
395  case Sweep:
396  LastHalfSweep = HalfSweep;
397  ToP.x = SurveyToWP.x;
398  ToP.y = SurveyToWP.y;
399  FromP.x = SurveyFromWP.x;
400  FromP.y = SurveyFromWP.y;
401 
402  //Rotate and Translate de plane position to local world
403  C.x = stateGetPositionEnu_f()->x;
404  C.y = stateGetPositionEnu_f()->y;
407 
408 #ifdef DIGITAL_CAM
409  {
410  //calc distance from line start and plane position (use only X position because y can be far due to wind or other factor)
411  float dist = FromP.x - C.x;
412 
413  // verify if plane are less than 10 meter from line start
414  if ((dc_autoshoot == DC_AUTOSHOOT_STOP) && (fabs(dist) < 10)) {
416  }
417  }
418 #endif
419 
420  //Rotate and Translate Line points into real world
423 
425  RotateAndTranslateToWorld(&FromP, SurveyTheta, 0, 0);
426 
427  //follow the line
428  nav_route_xy(FromP.x, FromP.y, ToP.x, ToP.y);
429  if (nav_approaching_xy(ToP.x, ToP.y, FromP.x, FromP.y, 0)) {
430  LastPoint.x = SurveyToWP.x;
431  LastPoint.y = SurveyToWP.y;
432 
433  if (LastPoint.y + dSweep >= MaxY || LastPoint.y + dSweep <= 0) { //Your out of the Polygon so Sweep Back or Half Sweep
434 
435  if (LastPoint.y + (dSweep / 2) >= MaxY || LastPoint.y + (dSweep / 2) <= 0 || !Half_Sweep_Enabled) { //Sweep back
436  dSweep = -dSweep;
437  if (LastHalfSweep) {
438  HalfSweep = false;
439  ys = LastPoint.y + (dSweep);
440  } else {
441  HalfSweep = true;
442  ys = LastPoint.y + (dSweep / 2);
443  }
444 
445  if (dSweep >= 0) {
446  SurveyCircleQdr = -DegOfRad(SurveyTheta);
447  } else {
448  SurveyCircleQdr = 180 - DegOfRad(SurveyTheta);
449  }
451  } else { // Half Sweep forward
452  ys = LastPoint.y + (dSweep / 2);
453 
454  if (dSweep >= 0) {
455  SurveyCircleQdr = -DegOfRad(SurveyTheta);
456  } else {
457  SurveyCircleQdr = 180 - DegOfRad(SurveyTheta);
458  }
459  HalfSweep = true;
460  }
461 
462 
463  } else { // Normal sweep
464  //Find y value of the first sweep
465  HalfSweep = false;
466  ys = LastPoint.y + dSweep;
467  }
468 
469  //Find the edges which intercet the sweep line first
470  for (i = 0; i < SurveySize; i++) {
471  if (EdgeMinY[i] < ys && EdgeMaxY[i] >= ys) {
472  XIntercept2 = XIntercept1;
473  XIntercept1 = EvaluateLineForX(ys, Edges[i]);
474  }
475  }
476 
477  //Find point to come from and point to go to
478  DInt1 = XIntercept1 - LastPoint.x;
479  DInt2 = XIntercept2 - LastPoint.x;
480 
481  if (DInt1 * DInt2 >= 0) {
482  if (fabs(DInt2) <= fabs(DInt1)) {
483  SurveyToWP.x = XIntercept1;
484  SurveyToWP.y = ys;
485 
486  SurveyFromWP.x = XIntercept2;
487  SurveyFromWP.y = ys;
488  } else {
489  SurveyToWP.x = XIntercept2;
490  SurveyToWP.y = ys;
491 
492  SurveyFromWP.x = XIntercept1;
493  SurveyFromWP.y = ys;
494  }
495  } else {
496  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 if ((SurveyToWP.x - SurveyFromWP.x) < 0 && DInt2 < 0) {
503  SurveyToWP.x = XIntercept1;
504  SurveyToWP.y = ys;
505 
506  SurveyFromWP.x = XIntercept2;
507  SurveyFromWP.y = ys;
508  } else {
509  SurveyToWP.x = XIntercept2;
510  SurveyToWP.y = ys;
511 
512  SurveyFromWP.x = XIntercept1;
513  SurveyFromWP.y = ys;
514  }
515  }
516 
517  //Find the radius to circle
518  if (!HalfSweep || use_full_circle) {
519  temp = dSweep / 2;
520  } else {
521  temp = dSweep / 4;
522  }
523 
524  //if less than min radius
525  if (fabs(temp) < min_radius) {
526  if (temp < 0) { temp = -min_radius; } else { temp = min_radius; }
527  }
528 
529  //Find the direction to circle
530  if (ys > 0 && SurveyToWP.x > SurveyFromWP.x) {
531  SurveyRadius = temp;
532  } else if (ys < 0 && SurveyToWP.x < SurveyFromWP.x) {
533  SurveyRadius = temp;
534  } else {
535  SurveyRadius = -temp;
536  }
537 
538  //find x position to circle
539  if (fabs(LastPoint.x - SurveyToWP.x) > fabs(SurveyFromWP.x - SurveyToWP.x)) {
540  SurveyCircle.x = LastPoint.x;
541  } else {
543  }
544 
545  //y position to circle
546  SurveyCircle.y = ys - temp;
547 
548  //Go into circle state
550  nav_init_stage();
553  }
554 
555  break;
556  case SweepCircle:
557  //Rotate and translate circle point into real world
558  C.x = SurveyCircle.x;
559  C.y = SurveyCircle.y;
562 
563  //follow the circle
564  nav_circle_XY(C.x, C.y, SurveyRadius);
565 
568  nav_init_stage();
569  //LINE_START_FUNCTION;
570  }
571  break;
572  case Init:
573  return false;
574  default:
575  return false;
576  }
577  return true;
578 }
579 
580 
581 /*
582  Translates point so (transX, transY) are (0,0) then rotates the point around z by Zrot
583 */
584 void TranslateAndRotateFromWorld(struct Point2D *p, float Zrot, float transX, float transY)
585 {
586  float temp;
587 
588  p->x = p->x - transX;
589  p->y = p->y - transY;
590 
591  temp = p->x;
592  p->x = p->x * cosf(Zrot) + p->y * sinf(Zrot);
593  p->y = -temp * sinf(Zrot) + p->y * cosf(Zrot);
594 }
595 
597 void RotateAndTranslateToWorld(struct Point2D *p, float Zrot, float transX, float transY)
598 {
599  float temp = p->x;
600 
601  p->x = p->x * cosf(Zrot) - p->y * sinf(Zrot);
602  p->y = temp * sinf(Zrot) + p->y * cosf(Zrot);
603 
604  p->x = p->x + transX;
605  p->y = p->y + transY;
606 }
607 
608 void FindInterceptOfTwoLines(float *x, float *y, struct Line L1, struct Line L2)
609 {
610  *x = ((L2.b - L1.b) / (L1.m - L2.m));
611  *y = L1.m * (*x) + L1.b;
612 }
613 
614 
615 float EvaluateLineForX(float y, struct Line L)
616 {
617  return ((y - L.b) / L.m);
618 }
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
uint16_t f
Camera baseline, in meters (i.e. horizontal distance between the two cameras of the stereo setup) ...
Definition: wedgebug.c:204
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