Paparazzi UAS  v5.15_devel-230-gc96ce27
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 void nav_survey_poly_setup_towards(uint8_t FirstWP, uint8_t Size, float Sweep, int SecondWP)
64 {
65  float dx = waypoints[SecondWP].enu_f.x - waypoints[FirstWP].enu_f.x;
66  float dy = waypoints[SecondWP].enu_f.y - waypoints[FirstWP].enu_f.y;
67  if (dx == 0.0f) { dx = 0.000000001; }
68  float ang = atan(dy / dx);
69  ang = DegOfRad(ang);
70 
71  //if values passed, use it.
72  if (Size == 0) {Size = Poly_Size;}
73  if (Sweep == 0) {Sweep = Poly_Distance;}
74  nav_survey_poly_setup(FirstWP, Size, Sweep, ang);
75 }
76 
77 struct Point2D {float x; float y;};
78 struct Line {float m; float b; float x;};
79 
80 static void TranslateAndRotateFromWorld(struct EnuCoor_f *p, float Zrot, float transX, float transY);
81 static void RotateAndTranslateToWorld(struct EnuCoor_f *p, float Zrot, float transX, float transY);
82 static void FindInterceptOfTwoLines(float *x, float *y, struct Line L1, struct Line L2);
83 static float EvaluateLineForX(float y, struct Line L);
84 
85 #define MaxPolygonSize POLYSURVEY_MAX_POLYGONSIZE
86 #define MaxFloat 1000000000
87 #define MinFloat -1000000000
88 
89 #ifndef LINE_START_FUNCTION
90 #define LINE_START_FUNCTION {}
91 #endif
92 #ifndef LINE_STOP_FUNCTION
93 #define LINE_STOP_FUNCTION {}
94 #endif
95 
96 /************** Polygon Survey **********************************************/
97 
102 static struct Point2D SmallestCorner;
103 static struct Line Edges[MaxPolygonSize];
104 static float EdgeMaxY[MaxPolygonSize];
105 static float EdgeMinY[MaxPolygonSize];
106 static float SurveyTheta;
107 static float dSweep;
108 static struct EnuCoor_f SurveyToWP;
109 static struct EnuCoor_f SurveyFromWP;
110 static struct EnuCoor_f SurveyEntry;
111 
112 static struct EnuCoor_i survey_from_i, survey_to_i;
113 
116 static float MaxY;
121 
122 //=========================================================================================================================
123 void nav_survey_poly_setup(uint8_t EntryWP, uint8_t Size, float sw, float Orientation)
124 {
125  SmallestCorner.x = 0;
126  SmallestCorner.y = 0;
127  int i = 0;
128  float ys = 0;
129  static struct EnuCoor_f EntryPoint;
130  float LeftYInt;
131  float RightYInt;
132  float temp;
133  float XIntercept1 = 0;
134  float XIntercept2 = 0;
135  float entry_distance;
136 
137  float PolySurveyEntryDistance = POLYSURVEY_ENTRY_DISTANCE;
138 
139  if (PolySurveyEntryDistance == 0) {
140  entry_distance = sw / 2;
141  } else {
142  entry_distance = PolySurveyEntryDistance;
143  }
144 
145  SurveyTheta = RadOfDeg(Orientation);
146  PolySurveySweepNum = 0;
148 
149  SurveyEntryWP = EntryWP;
150  SurveySize = Size;
151  Poly_Distance = sw;
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  #ifdef NAV_SURVEY_POLY_DYNAMIC
312  #endif
313 
314  struct EnuCoor_f C;
315  struct EnuCoor_f ToP;
316  struct EnuCoor_f FromP;
317  float ys = 0;
318  static struct EnuCoor_f LastPoint;
319  int i;
320  bool LastHalfSweep;
321  static bool HalfSweep = false;
322  float XIntercept1 = 0;
323  float XIntercept2 = 0;
324  float DInt1 = 0;
325  float DInt2 = 0;
326 
327  if (SurveySize == 0) {
328  return false;
329  }
330 
331  switch (CSurveyStatus) {
332  case Entry:
333  C = SurveyEntry;
336 
337  ENU_BFP_OF_REAL(survey_from_i, C);
339  VECT3_COPY(navigation_target, survey_from_i);
340 
341  if (((nav_approaching_from(&survey_from_i, NULL, 0))
342  && (fabsf(stateGetPositionEnu_f()->z - waypoints[SurveyEntryWP].enu_f.z)) < 1.)) {
344  nav_init_stage();
346  }
347  break;
348  case Sweep:
349  LastHalfSweep = HalfSweep;
350  ToP = SurveyToWP;
351  FromP = SurveyFromWP;
352 
353  //Rotate and Translate Line points into real world
356 
358  RotateAndTranslateToWorld(&FromP, SurveyTheta, 0, 0);
359 
360  //follow the line
362  ENU_BFP_OF_REAL(survey_from_i, FromP);
363 
365  nav_route(&survey_from_i, &survey_to_i);
366 
367  if (nav_approaching_from(&survey_to_i, NULL, 0)) {
368  LastPoint = SurveyToWP;
369 
370 #ifdef DIGITAL_CAM
371  float line_length = fabsf((fabsf(FromP.x) - fabsf(ToP.x)));
372  double inteiro;
373  double fract = modf(line_length / dc_distance_interval, &inteiro);
374  if (fract > .5) {
375  //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
377  }
378 #endif
379 
380  if (LastPoint.y + dSweep >= MaxY || LastPoint.y + dSweep <= 0) { //Your out of the Polygon so Sweep Back or Half Sweep
381 
382  if ((LastPoint.y + (dSweep / 2)) <= MaxY || LastPoint.y + (dSweep / 2) >= 0 || !Half_Sweep_Enabled) { //Sweep back
383  dSweep = -dSweep;
384  } else {
385  }
386 
387  if (LastHalfSweep) {
388  HalfSweep = false;
389  ys = LastPoint.y + (dSweep);
390  } else {
391  HalfSweep = true;
392  ys = LastPoint.y + (dSweep / 2);
393  }
395  } else { // Normal sweep
396  //Find y value of the first sweep
397  HalfSweep = false;
398  ys = LastPoint.y + dSweep;
399  }
400 
401  //Find the edges which intercet the sweep line first
402  for (i = 0; i < SurveySize; i++) {
403  if (EdgeMinY[i] < ys && EdgeMaxY[i] >= ys) {
404  XIntercept2 = XIntercept1;
405  XIntercept1 = EvaluateLineForX(ys, Edges[i]);
406  }
407  }
408 
409  //Find point to come from and point to go to
410  DInt1 = XIntercept1 - LastPoint.x;
411  DInt2 = XIntercept2 - LastPoint.x;
412 
413  if (DInt1 *DInt2 >= 0) {
414  if (fabs(DInt2) <= fabs(DInt1)) {
415  SurveyToWP.x = XIntercept1;
416  SurveyToWP.y = ys;
417 
418  SurveyFromWP.x = XIntercept2;
419  SurveyFromWP.y = ys;
420  } else {
421  SurveyToWP.x = XIntercept2;
422  SurveyToWP.y = ys;
423 
424  SurveyFromWP.x = XIntercept1;
425  SurveyFromWP.y = ys;
426  }
427  } else {
428  if ((SurveyToWP.x - SurveyFromWP.x) > 0 && DInt2 > 0) {
429  SurveyToWP.x = XIntercept1;
430  SurveyToWP.y = ys;
431 
432  SurveyFromWP.x = XIntercept2;
433  SurveyFromWP.y = ys;
434  } else if ((SurveyToWP.x - SurveyFromWP.x) < 0 && DInt2 < 0) {
435  SurveyToWP.x = XIntercept1;
436  SurveyToWP.y = ys;
437 
438  SurveyFromWP.x = XIntercept2;
439  SurveyFromWP.y = ys;
440  } else {
441  SurveyToWP.x = XIntercept2;
442  SurveyToWP.y = ys;
443 
444  SurveyFromWP.x = XIntercept1;
445  SurveyFromWP.y = ys;
446  }
447  }
448 
449  //Go into Turn state
451  nav_init_stage();
453 
455  }
456 
457  break;
458  case Turn:
459  FromP = LastPoint;
460  ToP = SurveyFromWP;
461 
462  //Rotate and Translate Line points into real world
465 
467  RotateAndTranslateToWorld(&FromP, SurveyTheta, 0, 0);
468 
469  //follow the line
471  ENU_BFP_OF_REAL(survey_from_i, FromP);
472 
474  nav_route(&survey_from_i, &survey_to_i);
475 
476  if (nav_approaching_from(&survey_to_i, NULL, 0)) {
478  nav_init_stage();
480  }
481 
482  break;
483  case Init:
484  return false;
485  default:
486  return false;
487  }
488 
489  return true;
490 
491 }
492 
493 //============================================================================================================================================
494 /*
495  Translates point so (transX, transY) are (0,0) then rotates the point around z by Zrot
496 */
497 void TranslateAndRotateFromWorld(struct EnuCoor_f *p, float Zrot, float transX, float transY)
498 {
499  float temp;
500 
501  p->x = p->x - transX;
502  p->y = p->y - transY;
503 
504  temp = p->x;
505  p->x = p->x * cosf(Zrot) + p->y * sinf(Zrot);
506  p->y = -temp * sinf(Zrot) + p->y * cosf(Zrot);
507 }
508 
510 void RotateAndTranslateToWorld(struct EnuCoor_f *p, float Zrot, float transX, float transY)
511 {
512  float temp = p->x;
513 
514  p->x = p->x * cosf(Zrot) - p->y * sinf(Zrot);
515  p->y = temp * sinf(Zrot) + p->y * cosf(Zrot);
516 
517  p->x = p->x + transX;
518  p->y = p->y + transY;
519 }
520 
521 void FindInterceptOfTwoLines(float *x, float *y, struct Line L1, struct Line L2)
522 {
523  *x = ((L2.b - L1.b) / (L1.m - L2.m));
524  *y = L1.m * (*x) + L1.b;
525 }
526 
527 
528 float EvaluateLineForX(float y, struct Line L)
529 {
530  return ((y - L.b) / L.m);
531 }
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