Paparazzi UAS  v7.0_unstable
Paparazzi is a free software Unmanned Aircraft System.
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 
101 static enum SurveyStatus CSurveyStatus;
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_f survey_from, survey_to;
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
298 
301  nav_set_heading_deg(-Orientation + 90.);
302 
303  }
304 }
305 
306 //=========================================================================================================================
308 {
309  // check if nav route is available
310  if (nav.nav_goto == NULL || nav.nav_route == NULL || nav.nav_approaching == NULL) {
311  return false;
312  }
313 
314  #ifdef NAV_SURVEY_POLY_DYNAMIC
316  #endif
317 
318  struct EnuCoor_f C;
319  struct EnuCoor_f ToP;
320  struct EnuCoor_f FromP;
321  float ys = 0;
322  static struct EnuCoor_f LastPoint;
323  int i;
324  bool LastHalfSweep;
325  static bool HalfSweep = false;
326  float XIntercept1 = 0;
327  float XIntercept2 = 0;
328  float DInt1 = 0;
329  float DInt2 = 0;
330 
331  if (SurveySize == 0) {
332  return false;
333  }
334 
335  switch (CSurveyStatus) {
336  case Entry:
337  C = SurveyEntry;
340 
343 
344  if (((nav.nav_approaching(&survey_from, NULL, 0))
345  && (fabsf(stateGetPositionEnu_f()->z - waypoints[SurveyEntryWP].enu_f.z)) < 1.)) {
347  nav_init_stage();
349  }
350  break;
351  case Sweep:
352  LastHalfSweep = HalfSweep;
353  ToP = SurveyToWP;
354  FromP = SurveyFromWP;
355 
356  //Rotate and Translate Line points into real world
359 
361  RotateAndTranslateToWorld(&FromP, SurveyTheta, 0, 0);
362 
363  //follow the line
364  VECT3_COPY(survey_to, ToP);
365  VECT3_COPY(survey_from, FromP);
367 
368  if (nav.nav_approaching(&survey_to, NULL, 0)) {
369  LastPoint = SurveyToWP;
370 
371 #ifdef DIGITAL_CAM
372  float line_length = fabsf((fabsf(FromP.x) - fabsf(ToP.x)));
373  double inteiro;
374  double fract = modf(line_length / dc_distance_interval, &inteiro);
375  if (fract > .5) {
376  //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
378  }
379 #endif
380 
381  if (LastPoint.y + dSweep >= MaxY || LastPoint.y + dSweep <= 0) { //Your out of the Polygon so Sweep Back or Half Sweep
382 
383  if ((LastPoint.y + (dSweep / 2)) <= MaxY || LastPoint.y + (dSweep / 2) >= 0 || !Half_Sweep_Enabled) { //Sweep back
384  dSweep = -dSweep;
385  } else {
386  }
387 
388  if (LastHalfSweep) {
389  HalfSweep = false;
390  ys = LastPoint.y + (dSweep);
391  } else {
392  HalfSweep = true;
393  ys = LastPoint.y + (dSweep / 2);
394  }
396  } else { // Normal sweep
397  //Find y value of the first sweep
398  HalfSweep = false;
399  ys = LastPoint.y + dSweep;
400  }
401 
402  //Find the edges which intercet the sweep line first
403  for (i = 0; i < SurveySize; i++) {
404  if (EdgeMinY[i] < ys && EdgeMaxY[i] >= ys) {
405  XIntercept2 = XIntercept1;
406  XIntercept1 = EvaluateLineForX(ys, Edges[i]);
407  }
408  }
409 
410  //Find point to come from and point to go to
411  DInt1 = XIntercept1 - LastPoint.x;
412  DInt2 = XIntercept2 - LastPoint.x;
413 
414  if (DInt1 *DInt2 >= 0) {
415  if (fabs(DInt2) <= fabs(DInt1)) {
416  SurveyToWP.x = XIntercept1;
417  SurveyToWP.y = ys;
418 
419  SurveyFromWP.x = XIntercept2;
420  SurveyFromWP.y = ys;
421  } else {
422  SurveyToWP.x = XIntercept2;
423  SurveyToWP.y = ys;
424 
425  SurveyFromWP.x = XIntercept1;
426  SurveyFromWP.y = ys;
427  }
428  } else {
429  if ((SurveyToWP.x - SurveyFromWP.x) > 0 && DInt2 > 0) {
430  SurveyToWP.x = XIntercept1;
431  SurveyToWP.y = ys;
432 
433  SurveyFromWP.x = XIntercept2;
434  SurveyFromWP.y = ys;
435  } else if ((SurveyToWP.x - SurveyFromWP.x) < 0 && DInt2 < 0) {
436  SurveyToWP.x = XIntercept1;
437  SurveyToWP.y = ys;
438 
439  SurveyFromWP.x = XIntercept2;
440  SurveyFromWP.y = ys;
441  } else {
442  SurveyToWP.x = XIntercept2;
443  SurveyToWP.y = ys;
444 
445  SurveyFromWP.x = XIntercept1;
446  SurveyFromWP.y = ys;
447  }
448  }
449 
450  //Go into Turn state
452  nav_init_stage();
454 
456  }
457 
458  break;
459  case Turn:
460  FromP = LastPoint;
461  ToP = SurveyFromWP;
462 
463  //Rotate and Translate Line points into real world
466 
468  RotateAndTranslateToWorld(&FromP, SurveyTheta, 0, 0);
469 
470  //follow the line
471  VECT3_COPY(survey_to, ToP);
472  VECT3_COPY(survey_from, FromP);
474 
475  if (nav.nav_approaching(&survey_to, NULL, 0)) {
477  nav_init_stage();
479  }
480 
481  break;
482  case Init:
483  return false;
484  default:
485  return false;
486  }
487 
488  return true;
489 
490 }
491 
492 //============================================================================================================================================
493 /*
494  Translates point so (transX, transY) are (0,0) then rotates the point around z by Zrot
495 */
496 void TranslateAndRotateFromWorld(struct EnuCoor_f *p, float Zrot, float transX, float transY)
497 {
498  float temp;
499 
500  p->x = p->x - transX;
501  p->y = p->y - transY;
502 
503  temp = p->x;
504  p->x = p->x * cosf(Zrot) + p->y * sinf(Zrot);
505  p->y = -temp * sinf(Zrot) + p->y * cosf(Zrot);
506 }
507 
509 void RotateAndTranslateToWorld(struct EnuCoor_f *p, float Zrot, float transX, float transY)
510 {
511  float temp = p->x;
512 
513  p->x = p->x * cosf(Zrot) - p->y * sinf(Zrot);
514  p->y = temp * sinf(Zrot) + p->y * cosf(Zrot);
515 
516  p->x = p->x + transX;
517  p->y = p->y + transY;
518 }
519 
520 void FindInterceptOfTwoLines(float *x, float *y, struct Line L1, struct Line L2)
521 {
522  *x = ((L2.b - L1.b) / (L1.m - L2.m));
523  *y = L1.m * (*x) + L1.b;
524 }
525 
526 
527 float EvaluateLineForX(float y, struct Line L)
528 {
529  return ((y - L.b) / L.m);
530 }
void dc_send_command(uint8_t cmd)
Send Command To Camera.
Core autopilot interface common to all firmwares.
struct point waypoints[NB_WAYPOINT]
size == nb_waypoint, waypoint 0 is a dummy waypoint
Definition: common_nav.c:39
float y
Definition: common_nav.h:41
float x
Definition: common_nav.h:40
float dc_distance_interval
AutoShoot photos on distance to last shot in meters.
Definition: dc.c:83
Standard Digital Camera Control Interface.
@ DC_SHOOT
Definition: dc.h:100
#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
static float p[2][2]
void nav_init_stage(void)
needs to be implemented by fixedwing and rotorcraft seperately
Definition: nav.c:92
float nav_survey_shift
Definition: nav.c:86
#define NavVerticalAltitudeMode(_alt, _pre_climb)
Set the vertical mode to altitude control with the specified altitude setpoint and climb pre-command.
Definition: nav.h:191
SurveyStatus
bool Half_Sweep_Enabled
float EntryRadius
static float MaxY
#define POLYSURVEY_DEFAULT_SIZE
static void RotateAndTranslateToWorld(struct EnuCoor_f *p, float Zrot, float transX, float transY)
Rotates point round z by -Zrot then translates so (0,0) becomes (transX,transY)
#define LINE_STOP_FUNCTION
float x
Definition: nav_launcher.c:92
static float SurveyTheta
static float EvaluateLineForX(float y, struct Line L)
static uint8_t SurveySize
bool nav_survey_poly_run(void)
Run polygon survey.
uint16_t PolySurveySweepNum
float Poly_Distance
static struct Point2D SmallestCorner
#define POLYSURVEY_ENTRY_DISTANCE
if 0 default to half sweep
static struct EnuCoor_f SurveyToWP
void nav_survey_poly_setup_towards(uint8_t FirstWP, uint8_t Size, float Sweep, int SecondWP)
Setup "dynamic" polygon survey with sweep orientation towards a waypoint.
static struct EnuCoor_f survey_from survey_to
#define MaxPolygonSize
static float EdgeMaxY[MaxPolygonSize]
#define MaxFloat
static void FindInterceptOfTwoLines(float *x, float *y, struct Line L1, struct Line L2)
#define LINE_START_FUNCTION
uint8_t Poly_Size
static struct Line Edges[MaxPolygonSize]
static void TranslateAndRotateFromWorld(struct EnuCoor_f *p, float Zrot, float transX, float transY)
#define POLYSURVEY_DEFAULT_DISTANCE
float y
Definition: nav_launcher.c:93
static struct EnuCoor_f SurveyFromWP
void nav_survey_poly_setup(uint8_t EntryWP, uint8_t Size, float sw, float Orientation)
Setup polygon survey.
static uint8_t SurveyEntryWP
uint16_t PolySurveySweepBackNum
static float dSweep
static struct EnuCoor_f SurveyEntry
#define POLY_OSAM_HALF_SWEEP_ENABLED
static float EdgeMinY[MaxPolygonSize]
static enum SurveyStatus CSurveyStatus
static struct point survey_from
float y
in meters
float x
in meters
float z
in meters
vector in East North Up coordinates Units: meters
struct RotorcraftNavigation nav
Definition: navigation.c:51
void nav_set_heading_deg(float deg)
Set nav_heading in degrees.
Definition: navigation.c:342
Rotorcraft navigation functions.
navigation_approaching nav_approaching
Definition: navigation.h:152
navigation_goto nav_goto
Definition: navigation.h:150
navigation_route nav_route
Definition: navigation.h:151
API to get/set the generic vehicle states.
unsigned short uint16_t
Typedef defining 16 bit unsigned short type.
Definition: vl53l1_types.h:88
unsigned char uint8_t
Typedef defining 8 bit unsigned char type.
Definition: vl53l1_types.h:98