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_rotorcraft.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 
27 
29 #include "state.h"
30 #include "autopilot.h"
31 #include "generated/flight_plan.h"
32 
33 #ifdef DIGITAL_CAM
34 #include "modules/digital_cam/dc.h"
35 #endif
36 
37 #ifndef POLYSURVEY_DEFAULT_SIZE
38 #define POLYSURVEY_DEFAULT_SIZE 10
39 #endif
40 
41 #ifndef POLYSURVEY_DEFAULT_DISTANCE
42 #define POLYSURVEY_DEFAULT_DISTANCE 25
43 #endif
44 
46 #ifndef POLYSURVEY_ENTRY_DISTANCE
47 #define POLYSURVEY_ENTRY_DISTANCE 0
48 #endif
49 
51 #ifndef POLYSURVEY_MAX_POLYGONSIZE
52 #define POLYSURVEY_MAX_POLYGONSIZE 20
53 #endif
54 
55 // use half sweep at the end of polygon
56 #ifndef POLY_OSAM_HALF_SWEEP_ENABLED
57 #define POLY_OSAM_HALF_SWEEP_ENABLED TRUE
58 #endif
59 
62 
63 
64 void nav_survey_poly_setup_towards(uint8_t FirstWP, uint8_t Size, float Sweep, int SecondWP)
65 {
66  float dx = waypoints[SecondWP].enu_f.x - waypoints[FirstWP].enu_f.x;
67  float dy = waypoints[SecondWP].enu_f.y - waypoints[FirstWP].enu_f.y;
68  if (dx == 0.0f) { dx = 0.000000001; }
69  float ang = atan(dy / dx);
70  ang = DegOfRad(ang);
71 
72  //if values passed, use it.
73  if (Size == 0) {Size = Poly_Size;}
74  if (Sweep == 0) {Sweep = Poly_Distance;}
75  nav_survey_poly_setup(FirstWP, Size, Sweep, ang);
76 }
77 
78 struct Point2D {float x; float y;};
79 struct Line {float m; float b; float x;};
80 
81 static void TranslateAndRotateFromWorld(struct EnuCoor_f *p, float Zrot, float transX, float transY);
82 static void RotateAndTranslateToWorld(struct EnuCoor_f *p, float Zrot, float transX, float transY);
83 static void FindInterceptOfTwoLines(float *x, float *y, struct Line L1, struct Line L2);
84 static float EvaluateLineForX(float y, struct Line L);
85 
86 #define MaxPolygonSize POLYSURVEY_MAX_POLYGONSIZE
87 #define MaxFloat 1000000000
88 #define MinFloat -1000000000
89 
90 #ifndef LINE_START_FUNCTION
91 #define LINE_START_FUNCTION {}
92 #endif
93 #ifndef LINE_STOP_FUNCTION
94 #define LINE_STOP_FUNCTION {}
95 #endif
96 
97 /************** Polygon Survey **********************************************/
98 
103 static struct Point2D SmallestCorner;
104 static struct Line Edges[MaxPolygonSize];
105 static float EdgeMaxY[MaxPolygonSize];
106 static float EdgeMinY[MaxPolygonSize];
107 static float SurveyTheta;
108 static float dSweep;
109 static struct EnuCoor_f SurveyToWP;
110 static struct EnuCoor_f SurveyFromWP;
111 static struct EnuCoor_f SurveyEntry;
112 
113 static struct EnuCoor_i survey_from_i, survey_to_i;
114 
117 static float MaxY;
122 
123 //=========================================================================================================================
124 void nav_survey_poly_setup(uint8_t EntryWP, uint8_t Size, float sw, float Orientation)
125 {
126  SmallestCorner.x = 0;
127  SmallestCorner.y = 0;
128  int i = 0;
129  float ys = 0;
130  static struct EnuCoor_f EntryPoint;
131  float LeftYInt;
132  float RightYInt;
133  float temp;
134  float XIntercept1 = 0;
135  float XIntercept2 = 0;
136  float entry_distance;
137 
138  float PolySurveyEntryDistance = POLYSURVEY_ENTRY_DISTANCE;
139 
140  if (PolySurveyEntryDistance == 0) {
141  entry_distance = sw / 2;
142  } else {
143  entry_distance = PolySurveyEntryDistance;
144  }
145 
146  SurveyTheta = RadOfDeg(Orientation);
147  PolySurveySweepNum = 0;
149 
150  SurveyEntryWP = EntryWP;
151  SurveySize = Size;
152 
153  struct EnuCoor_f Corners[MaxPolygonSize];
154 
156 
157  if (Size == 0) {
158  return;
159  }
160 
161  //Don't initialize if Polygon is too big or if the orientation is not between 0 and 90
162  if (Size <= MaxPolygonSize && Orientation >= -90 && Orientation <= 90) {
163  //Initialize Corners
164  for (i = 0; i < Size; i++) {
165  Corners[i].x = waypoints[i + EntryWP].enu_f.x;
166  Corners[i].y = waypoints[i + EntryWP].enu_f.y;
167  }
168 
169  //Rotate Corners so sweeps are parellel with x axis
170  for (i = 0; i < Size; i++) {
171  TranslateAndRotateFromWorld(&Corners[i], SurveyTheta, 0, 0);
172  }
173 
174  //Find min x and min y
175  SmallestCorner.y = Corners[0].y;
176  SmallestCorner.x = Corners[0].x;
177  for (i = 1; i < Size; i++) {
178  if (Corners[i].y < SmallestCorner.y) {
179  SmallestCorner.y = Corners[i].y;
180  }
181 
182  if (Corners[i].x < SmallestCorner.x) {
183  SmallestCorner.x = Corners[i].x;
184  }
185  }
186 
187  //Translate Corners all exist in quad #1
188  for (i = 0; i < Size; i++) {
190  }
191 
192  //Rotate and Translate Entry Point
193  EntryPoint.x = Corners[0].x;
194  EntryPoint.y = Corners[0].y;
195 
196  //Find max y
197  MaxY = Corners[0].y;
198  for (i = 1; i < Size; i++) {
199  if (Corners[i].y > MaxY) {
200  MaxY = Corners[i].y;
201  }
202  }
203 
204  //Find polygon edges
205  for (i = 0; i < Size; i++) {
206  if (i == 0)
207  if (Corners[Size - 1].x == Corners[i].x) { //Don't divide by zero!
208  Edges[i].m = MaxFloat;
209  } else {
210  Edges[i].m = ((Corners[Size - 1].y - Corners[i].y) / (Corners[Size - 1].x - Corners[i].x));
211  }
212  else if (Corners[i].x == Corners[i - 1].x) {
213  Edges[i].m = MaxFloat;
214  } else {
215  Edges[i].m = ((Corners[i].y - Corners[i - 1].y) / (Corners[i].x - Corners[i - 1].x));
216  }
217 
218  //Edges[i].m = MaxFloat;
219  Edges[i].b = (Corners[i].y - (Corners[i].x * Edges[i].m));
220  }
221 
222  //Find Min and Max y for each line
223  FindInterceptOfTwoLines(&temp, &LeftYInt, Edges[0], Edges[1]);
224  FindInterceptOfTwoLines(&temp, &RightYInt, Edges[0], Edges[Size - 1]);
225 
226  if (LeftYInt > RightYInt) {
227  EdgeMaxY[0] = LeftYInt;
228  EdgeMinY[0] = RightYInt;
229  } else {
230  EdgeMaxY[0] = RightYInt;
231  EdgeMinY[0] = LeftYInt;
232  }
233 
234  for (i = 1; i < Size - 1; i++) {
235  FindInterceptOfTwoLines(&temp, &LeftYInt, Edges[i], Edges[i + 1]);
236  FindInterceptOfTwoLines(&temp, &RightYInt, Edges[i], Edges[i - 1]);
237 
238  if (LeftYInt > RightYInt) {
239  EdgeMaxY[i] = LeftYInt;
240  EdgeMinY[i] = RightYInt;
241  } else {
242  EdgeMaxY[i] = RightYInt;
243  EdgeMinY[i] = LeftYInt;
244  }
245  }
246 
247  FindInterceptOfTwoLines(&temp, &LeftYInt, Edges[Size - 1], Edges[0]);
248  FindInterceptOfTwoLines(&temp, &RightYInt, Edges[Size - 1], Edges[Size - 2]);
249 
250  if (LeftYInt > RightYInt) {
251  EdgeMaxY[Size - 1] = LeftYInt;
252  EdgeMinY[Size - 1] = RightYInt;
253  } else {
254  EdgeMaxY[Size - 1] = RightYInt;
255  EdgeMinY[Size - 1] = LeftYInt;
256  }
257 
258  //Find amount to increment by every sweep
259  if (EntryPoint.y >= MaxY / 2) {
260  entry_distance = -entry_distance;
261  dSweep = -sw;
262  } else {
263  dSweep = sw;
264  }
265 
266  //Find y value of the first sweep
267  ys = EntryPoint.y + entry_distance;
268 
269  //Find the edges which intercet the sweep line first
270  for (i = 0; i < SurveySize; i++) {
271  if (EdgeMinY[i] <= ys && EdgeMaxY[i] > ys) {
272  XIntercept2 = XIntercept1;
273  XIntercept1 = EvaluateLineForX(ys, Edges[i]);
274  }
275  }
276 
277  //Find point to come from and point to go to
278  if (fabs(EntryPoint.x - XIntercept2) <= fabs(EntryPoint.x - XIntercept1)) {
279  SurveyToWP.x = XIntercept1;
280  SurveyToWP.y = ys;
281 
282  SurveyFromWP.x = XIntercept2;
283  SurveyFromWP.y = ys;
284  } else {
285  SurveyToWP.x = XIntercept2;
286  SurveyToWP.y = ys;
287 
288  SurveyFromWP.x = XIntercept1;
289  SurveyFromWP.y = ys;
290  }
291 
292  //Find the entry point
294  SurveyEntry.y = EntryPoint.y + entry_distance;
295 
296  //Go into entry state
297  CSurveyStatus = Entry;
298 
301  nav_set_heading_deg(-Orientation + 90.);
302 
303  }
304 }
305 
306 //=========================================================================================================================
308 {
309 
310  struct EnuCoor_f C;
311  struct EnuCoor_f ToP;
312  struct EnuCoor_f FromP;
313  float ys = 0;
314  static struct EnuCoor_f LastPoint;
315  int i;
316  bool LastHalfSweep;
317  static bool HalfSweep = false;
318  float XIntercept1 = 0;
319  float XIntercept2 = 0;
320  float DInt1 = 0;
321  float DInt2 = 0;
322 
323  if (SurveySize == 0) {
324  return false;
325  }
326 
327  switch (CSurveyStatus) {
328  case Entry:
329  C = SurveyEntry;
332 
333  ENU_BFP_OF_REAL(survey_from_i, C);
335  VECT3_COPY(navigation_target, survey_from_i);
336 
337  if (((nav_approaching_from(&survey_from_i, NULL, 0))
338  && (fabsf(stateGetPositionEnu_f()->z - waypoints[SurveyEntryWP].enu_f.z)) < 1.)) {
340  nav_init_stage();
342  }
343  break;
344  case Sweep:
345  LastHalfSweep = HalfSweep;
346  ToP = SurveyToWP;
347  FromP = SurveyFromWP;
348 
349  //Rotate and Translate Line points into real world
352 
354  RotateAndTranslateToWorld(&FromP, SurveyTheta, 0, 0);
355 
356  //follow the line
358  ENU_BFP_OF_REAL(survey_from_i, FromP);
359 
361  nav_route(&survey_from_i, &survey_to_i);
362 
363  if (nav_approaching_from(&survey_to_i, NULL, 0)) {
364  LastPoint = SurveyToWP;
365 
366 #ifdef DIGITAL_CAM
367  float line_length = fabsf((fabsf(FromP.x) - fabsf(ToP.x)));
368  double inteiro;
369  double fract = modf(line_length / dc_distance_interval, &inteiro);
370  if (fract > .5) {
371  //if last shot is more than shot_distance/2 from the corner then take a picture in the corner before go to the next sweep
373  }
374 #endif
375 
376  if (LastPoint.y + dSweep >= MaxY || LastPoint.y + dSweep <= 0) { //Your out of the Polygon so Sweep Back or Half Sweep
377 
378  if ((LastPoint.y + (dSweep / 2)) <= MaxY || LastPoint.y + (dSweep / 2) >= 0 || !Half_Sweep_Enabled) { //Sweep back
379  dSweep = -dSweep;
380  } else {
381  }
382 
383  if (LastHalfSweep) {
384  HalfSweep = false;
385  ys = LastPoint.y + (dSweep);
386  } else {
387  HalfSweep = true;
388  ys = LastPoint.y + (dSweep / 2);
389  }
391  } else { // Normal sweep
392  //Find y value of the first sweep
393  HalfSweep = false;
394  ys = LastPoint.y + dSweep;
395  }
396 
397  //Find the edges which intercet the sweep line first
398  for (i = 0; i < SurveySize; i++) {
399  if (EdgeMinY[i] < ys && EdgeMaxY[i] >= ys) {
400  XIntercept2 = XIntercept1;
401  XIntercept1 = EvaluateLineForX(ys, Edges[i]);
402  }
403  }
404 
405  //Find point to come from and point to go to
406  DInt1 = XIntercept1 - LastPoint.x;
407  DInt2 = XIntercept2 - LastPoint.x;
408 
409  if (DInt1 *DInt2 >= 0) {
410  if (fabs(DInt2) <= fabs(DInt1)) {
411  SurveyToWP.x = XIntercept1;
412  SurveyToWP.y = ys;
413 
414  SurveyFromWP.x = XIntercept2;
415  SurveyFromWP.y = ys;
416  } else {
417  SurveyToWP.x = XIntercept2;
418  SurveyToWP.y = ys;
419 
420  SurveyFromWP.x = XIntercept1;
421  SurveyFromWP.y = ys;
422  }
423  } else {
424  if ((SurveyToWP.x - SurveyFromWP.x) > 0 && DInt2 > 0) {
425  SurveyToWP.x = XIntercept1;
426  SurveyToWP.y = ys;
427 
428  SurveyFromWP.x = XIntercept2;
429  SurveyFromWP.y = ys;
430  } else if ((SurveyToWP.x - SurveyFromWP.x) < 0 && DInt2 < 0) {
431  SurveyToWP.x = XIntercept1;
432  SurveyToWP.y = ys;
433 
434  SurveyFromWP.x = XIntercept2;
435  SurveyFromWP.y = ys;
436  } else {
437  SurveyToWP.x = XIntercept2;
438  SurveyToWP.y = ys;
439 
440  SurveyFromWP.x = XIntercept1;
441  SurveyFromWP.y = ys;
442  }
443  }
444 
445  //Go into Turn state
447  nav_init_stage();
449 
451  }
452 
453  break;
454  case Turn:
455  FromP = LastPoint;
456  ToP = SurveyFromWP;
457 
458  //Rotate and Translate Line points into real world
461 
463  RotateAndTranslateToWorld(&FromP, SurveyTheta, 0, 0);
464 
465  //follow the line
467  ENU_BFP_OF_REAL(survey_from_i, FromP);
468 
470  nav_route(&survey_from_i, &survey_to_i);
471 
472  if (nav_approaching_from(&survey_to_i, NULL, 0)) {
474  nav_init_stage();
476  }
477 
478  break;
479  case Init:
480  return false;
481  default:
482  return false;
483  }
484 
485  return true;
486 
487 }
488 
489 //============================================================================================================================================
490 /*
491  Translates point so (transX, transY) are (0,0) then rotates the point around z by Zrot
492 */
493 void TranslateAndRotateFromWorld(struct EnuCoor_f *p, float Zrot, float transX, float transY)
494 {
495  float temp;
496 
497  p->x = p->x - transX;
498  p->y = p->y - transY;
499 
500  temp = p->x;
501  p->x = p->x * cosf(Zrot) + p->y * sinf(Zrot);
502  p->y = -temp * sinf(Zrot) + p->y * cosf(Zrot);
503 }
504 
506 void RotateAndTranslateToWorld(struct EnuCoor_f *p, float Zrot, float transX, float transY)
507 {
508  float temp = p->x;
509 
510  p->x = p->x * cosf(Zrot) - p->y * sinf(Zrot);
511  p->y = temp * sinf(Zrot) + p->y * cosf(Zrot);
512 
513  p->x = p->x + transX;
514  p->y = p->y + transY;
515 }
516 
517 void FindInterceptOfTwoLines(float *x, float *y, struct Line L1, struct Line L2)
518 {
519  *x = ((L2.b - L1.b) / (L1.m - L2.m));
520  *y = L1.m * (*x) + L1.b;
521 }
522 
523 
524 float EvaluateLineForX(float y, struct Line L)
525 {
526  return ((y - L.b) / L.m);
527 }
struct EnuCoor_i navigation_target
Definition: navigation.c:87
unsigned short uint16_t
Definition: types.h:16
float x
Definition: common_nav.h:40
vector in East North Up coordinates Units: meters
#define VECT3_COPY(_a, _b)
Definition: pprz_algebra.h:140
static struct EnuCoor_f * stateGetPositionEnu_f(void)
Get position in local ENU coordinates (float).
Definition: state.h:719
Definition: dc.h:100
float y
Definition: common_nav.h:41
Standard Digital Camera Control Interface.
float x
in meters
Core autopilot interface common to all firmwares.
Rotorcraft navigation functions.
void dc_send_command(uint8_t cmd)
Send Command To Camera.
vector in East North Up coordinates
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
float z
in meters
void nav_set_heading_deg(float deg)
Set nav_heading in degrees.
Definition: navigation.c:491
static float p[2][2]
#define ENU_BFP_OF_REAL(_o, _i)
void nav_route(struct EnuCoor_i *wp_start, struct EnuCoor_i *wp_end)
Definition: navigation.c:584
float dc_distance_interval
AutoShoot photos on distance to last shot in meters.
Definition: dc.c:83
float y
in meters
bool nav_approaching_from(struct EnuCoor_i *wp, struct EnuCoor_i *from, int16_t approaching_time)
Proximity tests on approaching a wp.
Definition: navigation.c:257