Paparazzi UAS  v5.8.2_stable-0-g6260b7c
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 /*
28 #include <stdio.h>
29 #include "mcu_periph/uart.h"
30 #include "messages.h"
31 #include "subsystems/datalink/downlink.h"
32 */
33 
34 
35 
37 
39 #include "state.h"
40 #include "autopilot.h"
41 #include "generated/flight_plan.h"
42 
43 #ifdef DIGITAL_CAM
44 #include "modules/digital_cam/dc.h"
45 #endif
46 
47 #ifndef POLYSURVEY_DEFAULT_SIZE
48 #define POLYSURVEY_DEFAULT_SIZE 10
49 #endif
50 
51 #ifndef POLYSURVEY_DEFAULT_DISTANCE
52 #define POLYSURVEY_DEFAULT_DISTANCE 25
53 #endif
54 
56 #ifndef POLYSURVEY_ENTRY_DISTANCE
57 #define POLYSURVEY_ENTRY_DISTANCE 0
58 #endif
59 
61 #ifndef POLYSURVEY_MAX_POLYGONSIZE
62 #define POLYSURVEY_MAX_POLYGONSIZE 20
63 #endif
64 
67 
68 
69 bool_t nav_survey_poly_setup_towards(uint8_t FirstWP, uint8_t Size, float Sweep, int SecondWP)
70 {
71  float dx = waypoints[SecondWP].enu_f.x - waypoints[FirstWP].enu_f.x;
72  float dy = waypoints[SecondWP].enu_f.y - waypoints[FirstWP].enu_f.y;
73  if (dx == 0.0f) { dx = 0.000000001; }
74  float ang = atan(dy / dx);
75  ang = DegOfRad(ang);
76 
77  //if values passed, use it.
78  if (Size == 0) {Size = Poly_Size;}
79  if (Sweep == 0) {Sweep = Poly_Distance;}
80  return nav_survey_poly_setup(FirstWP, Size, Sweep, ang);
81 }
82 
83 struct Point2D {float x; float y;};
84 struct Line {float m; float b; float x;};
85 
86 static void TranslateAndRotateFromWorld(struct EnuCoor_f *p, float Zrot, float transX, float transY);
87 static void RotateAndTranslateToWorld(struct EnuCoor_f *p, float Zrot, float transX, float transY);
88 static void FindInterceptOfTwoLines(float *x, float *y, struct Line L1, struct Line L2);
89 static float EvaluateLineForX(float y, struct Line L);
90 
91 #define MaxPolygonSize POLYSURVEY_MAX_POLYGONSIZE
92 #define MaxFloat 1000000000
93 #define MinFloat -1000000000
94 
95 #ifndef LINE_START_FUNCTION
96 #define LINE_START_FUNCTION {}
97 #endif
98 #ifndef LINE_STOP_FUNCTION
99 #define LINE_STOP_FUNCTION {}
100 #endif
101 
102 /************** Polygon Survey **********************************************/
103 
109 static struct Point2D SmallestCorner;
110 static struct Line Edges[MaxPolygonSize];
111 static float EdgeMaxY[MaxPolygonSize];
112 static float EdgeMinY[MaxPolygonSize];
113 static float SurveyTheta;
114 static float dSweep;
115 static struct EnuCoor_f SurveyToWP;
116 static struct EnuCoor_f SurveyFromWP;
117 static struct EnuCoor_f SurveyEntry;
118 
119 //static struct EnuCoor_f survey_from, survey_to;
120 static struct EnuCoor_i survey_from_i, survey_to_i;
121 
124 //static float SurveyCircleQdr;
125 static float MaxY;
129 
130 //=========================================================================================================================
131 bool_t nav_survey_poly_setup(uint8_t EntryWP, uint8_t Size, float sw, float Orientation)
132 {
133  SmallestCorner.x = 0;
134  SmallestCorner.y = 0;
135  int i = 0;
136  float ys = 0;
137  static struct EnuCoor_f EntryPoint;
138  float LeftYInt;
139  float RightYInt;
140  float temp;
141  float XIntercept1 = 0;
142  float XIntercept2 = 0;
143  float entry_distance;
144 
145  float PolySurveyEntryDistance = POLYSURVEY_ENTRY_DISTANCE;
146 
147  if (PolySurveyEntryDistance == 0) {
148  entry_distance = sw / 2;
149  } else {
150  entry_distance = PolySurveyEntryDistance;
151  }
152 
153  SurveyTheta = RadOfDeg(Orientation);
154  PolySurveySweepNum = 0;
156 
157  SurveyEntryWP = EntryWP;
158  SurveySize = Size;
159 
160  struct EnuCoor_f Corners[MaxPolygonSize];
161 
163 
164  if (Size == 0) {
165  return TRUE;
166  }
167 
168  //Don't initialize if Polygon is too big or if the orientation is not between 0 and 90
169  if (Size <= MaxPolygonSize && Orientation >= -90 && Orientation <= 90) {
170  //Initialize Corners
171  for (i = 0; i < Size; i++) {
172  Corners[i].x = waypoints[i + EntryWP].enu_f.x;
173  Corners[i].y = waypoints[i + EntryWP].enu_f.y;
174  }
175 
176  //Rotate Corners so sweeps are parellel with x axis
177  for (i = 0; i < Size; i++) {
178  TranslateAndRotateFromWorld(&Corners[i], SurveyTheta, 0, 0);
179  }
180 
181  //Find min x and min y
182  SmallestCorner.y = Corners[0].y;
183  SmallestCorner.x = Corners[0].x;
184  for (i = 1; i < Size; i++) {
185  if (Corners[i].y < SmallestCorner.y) {
186  SmallestCorner.y = Corners[i].y;
187  }
188 
189  if (Corners[i].x < SmallestCorner.x) {
190  SmallestCorner.x = Corners[i].x;
191  }
192  }
193 
194  //Translate Corners all exist in quad #1
195  for (i = 0; i < Size; i++) {
197  }
198 
199  //Rotate and Translate Entry Point
200  EntryPoint.x = Corners[0].x;
201  EntryPoint.y = Corners[0].y;
202 
203  //Find max y
204  MaxY = Corners[0].y;
205  for (i = 1; i < Size; i++) {
206  if (Corners[i].y > MaxY) {
207  MaxY = Corners[i].y;
208  }
209  }
210 
211  //Find polygon edges
212  for (i = 0; i < Size; i++) {
213  if (i == 0)
214  if (Corners[Size - 1].x == Corners[i].x) { //Don't divide by zero!
215  Edges[i].m = MaxFloat;
216  } else {
217  Edges[i].m = ((Corners[Size - 1].y - Corners[i].y) / (Corners[Size - 1].x - Corners[i].x));
218  }
219  else if (Corners[i].x == Corners[i - 1].x) {
220  Edges[i].m = MaxFloat;
221  } else {
222  Edges[i].m = ((Corners[i].y - Corners[i - 1].y) / (Corners[i].x - Corners[i - 1].x));
223  }
224 
225  //Edges[i].m = MaxFloat;
226  Edges[i].b = (Corners[i].y - (Corners[i].x * Edges[i].m));
227  }
228 
229  //Find Min and Max y for each line
230  FindInterceptOfTwoLines(&temp, &LeftYInt, Edges[0], Edges[1]);
231  FindInterceptOfTwoLines(&temp, &RightYInt, Edges[0], Edges[Size - 1]);
232 
233  if (LeftYInt > RightYInt) {
234  EdgeMaxY[0] = LeftYInt;
235  EdgeMinY[0] = RightYInt;
236  } else {
237  EdgeMaxY[0] = RightYInt;
238  EdgeMinY[0] = LeftYInt;
239  }
240 
241  for (i = 1; i < Size - 1; i++) {
242  FindInterceptOfTwoLines(&temp, &LeftYInt, Edges[i], Edges[i + 1]);
243  FindInterceptOfTwoLines(&temp, &RightYInt, Edges[i], Edges[i - 1]);
244 
245  if (LeftYInt > RightYInt) {
246  EdgeMaxY[i] = LeftYInt;
247  EdgeMinY[i] = RightYInt;
248  } else {
249  EdgeMaxY[i] = RightYInt;
250  EdgeMinY[i] = LeftYInt;
251  }
252  }
253 
254  FindInterceptOfTwoLines(&temp, &LeftYInt, Edges[Size - 1], Edges[0]);
255  FindInterceptOfTwoLines(&temp, &RightYInt, Edges[Size - 1], Edges[Size - 2]);
256 
257  if (LeftYInt > RightYInt) {
258  EdgeMaxY[Size - 1] = LeftYInt;
259  EdgeMinY[Size - 1] = RightYInt;
260  } else {
261  EdgeMaxY[Size - 1] = RightYInt;
262  EdgeMinY[Size - 1] = LeftYInt;
263  }
264 
265  //Find amount to increment by every sweep
266  if (EntryPoint.y >= MaxY / 2) {
267  entry_distance = -entry_distance;
268  dSweep = -sw;
269  } else {
270  dSweep = sw;
271  }
272 
273  //Find y value of the first sweep
274  ys = EntryPoint.y + entry_distance;
275 
276  //Find the edges which intercet the sweep line first
277  for (i = 0; i < SurveySize; i++) {
278  if (EdgeMinY[i] <= ys && EdgeMaxY[i] > ys) {
279  XIntercept2 = XIntercept1;
280  XIntercept1 = EvaluateLineForX(ys, Edges[i]);
281  }
282  }
283 
284  //Find point to come from and point to go to
285  if (fabs(EntryPoint.x - XIntercept2) <= fabs(EntryPoint.x - XIntercept1)) {
286  SurveyToWP.x = XIntercept1;
287  SurveyToWP.y = ys;
288 
289  SurveyFromWP.x = XIntercept2;
290  SurveyFromWP.y = ys;
291  } else {
292  SurveyToWP.x = XIntercept2;
293  SurveyToWP.y = ys;
294 
295  SurveyFromWP.x = XIntercept1;
296  SurveyFromWP.y = ys;
297  }
298 
299  //Find the entry point
301  SurveyEntry.y = EntryPoint.y + entry_distance;
302 
303  //Go into entry state
304  CSurveyStatus = Entry;
305 
308  nav_set_heading_deg(-Orientation + 90.);
309 
310  }
311 
312  return FALSE;
313 }
314 
315 //=========================================================================================================================
317 {
318 
319  struct EnuCoor_f C;
320  struct EnuCoor_f ToP;
321  struct EnuCoor_f FromP;
322  float ys = 0;
323  static struct EnuCoor_f LastPoint;
324  int i;
325  bool_t LastHalfSweep;
326  static bool_t HalfSweep = FALSE;
327  float XIntercept1 = 0;
328  float XIntercept2 = 0;
329  float DInt1 = 0;
330  float DInt2 = 0;
331 
332  switch (CSurveyStatus) {
333  case Entry:
334  C = SurveyEntry;
337 
338  ENU_BFP_OF_REAL(survey_from_i, C);
340  VECT3_COPY(navigation_target, survey_from_i);
341 
342  if (((nav_approaching_from(&survey_from_i, NULL, 0))
343  && (fabsf(stateGetPositionEnu_f()->z - waypoints[SurveyEntryWP].enu_f.z)) < 1.)) {
345  nav_init_stage();
347  }
348  break;
349  case Sweep:
350  LastHalfSweep = HalfSweep;
351  ToP = SurveyToWP;
352  FromP = SurveyFromWP;
353 
354  //Rotate and Translate Line points into real world
357 
359  RotateAndTranslateToWorld(&FromP, SurveyTheta, 0, 0);
360 
361  //follow the line
363  ENU_BFP_OF_REAL(survey_from_i, FromP);
364 
366  nav_route(&survey_from_i, &survey_to_i);
367 
368  if (nav_approaching_from(&survey_to_i, 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  //fprintf(stderr,"Lastpoint:%f <= %f\n",( LastPoint.y + dSweep ), 0. );
382 
383 
384  if (LastPoint.y + dSweep >= MaxY || LastPoint.y + dSweep <= 0) { //Your out of the Polygon so Sweep Back or Half Sweep
385  //fprintf(stderr,"nao cabe interiro\n");
386 
387  if ((LastPoint.y + (dSweep / 2)) <= MaxY || LastPoint.y + (dSweep / 2) >= 0) { //Sweep back
388  //fprintf(stderr,"nao cabe meio\n");
389  dSweep = -dSweep;
390  } else {
391  //fprintf(stderr,"cabe meio\n");
392  }
393 
394  if (LastHalfSweep) {
395  HalfSweep = FALSE;
396  ys = LastPoint.y + (dSweep);
397  } else {
398  HalfSweep = TRUE;
399  ys = LastPoint.y + (dSweep / 2);
400  }
401 
402  } else { // Normal sweep
403  //fprintf(stderr,"cabe interiro\n");
404 
405  //Find y value of the first sweep
406  HalfSweep = FALSE;
407  ys = LastPoint.y + dSweep;
408  }
409 
410  //Find the edges which intercet the sweep line first
411  for (i = 0; i < SurveySize; i++) {
412  if (EdgeMinY[i] < ys && EdgeMaxY[i] >= ys) {
413  XIntercept2 = XIntercept1;
414  XIntercept1 = EvaluateLineForX(ys, Edges[i]);
415  }
416  }
417 
418  //Find point to come from and point to go to
419  DInt1 = XIntercept1 - LastPoint.x;
420  DInt2 = XIntercept2 - LastPoint.x;
421 
422  if (DInt1 * DInt2 >= 0) {
423  if (fabs(DInt2) <= fabs(DInt1)) {
424  SurveyToWP.x = XIntercept1;
425  SurveyToWP.y = ys;
426 
427  SurveyFromWP.x = XIntercept2;
428  SurveyFromWP.y = ys;
429  } else {
430  SurveyToWP.x = XIntercept2;
431  SurveyToWP.y = ys;
432 
433  SurveyFromWP.x = XIntercept1;
434  SurveyFromWP.y = ys;
435  }
436  } else {
437  if ((SurveyToWP.x - SurveyFromWP.x) > 0 && DInt2 > 0) {
438  SurveyToWP.x = XIntercept1;
439  SurveyToWP.y = ys;
440 
441  SurveyFromWP.x = XIntercept2;
442  SurveyFromWP.y = ys;
443  } else if ((SurveyToWP.x - SurveyFromWP.x) < 0 && DInt2 < 0) {
444  SurveyToWP.x = XIntercept1;
445  SurveyToWP.y = ys;
446 
447  SurveyFromWP.x = XIntercept2;
448  SurveyFromWP.y = ys;
449  } else {
450  SurveyToWP.x = XIntercept2;
451  SurveyToWP.y = ys;
452 
453  SurveyFromWP.x = XIntercept1;
454  SurveyFromWP.y = ys;
455  }
456  }
457 
458  //Go into Turn state
460  nav_init_stage();
462 
464  }
465 
466  break;
467  case Turn:
468  FromP = LastPoint;
469  ToP = SurveyFromWP;
470 
471  //Rotate and Translate Line points into real world
474 
476  RotateAndTranslateToWorld(&FromP, SurveyTheta, 0, 0);
477 
478  //follow the line
480  ENU_BFP_OF_REAL(survey_from_i, FromP);
481 
483  nav_route(&survey_from_i, &survey_to_i);
484 
485  if (nav_approaching_from(&survey_to_i, NULL, 0)) {
487  nav_init_stage();
489  }
490 
491  break;
492  case Init:
493  return FALSE;
494  default:
495  return FALSE;
496  }
497 
498  return TRUE;
499 
500 }
501 
502 //============================================================================================================================================
503 /*
504  Translates point so (transX, transY) are (0,0) then rotates the point around z by Zrot
505 */
506 void TranslateAndRotateFromWorld(struct EnuCoor_f *p, float Zrot, float transX, float transY)
507 {
508  float temp;
509 
510  p->x = p->x - transX;
511  p->y = p->y - transY;
512 
513  temp = p->x;
514  p->x = p->x * cosf(Zrot) + p->y * sinf(Zrot);
515  p->y = -temp * sinf(Zrot) + p->y * cosf(Zrot);
516 }
517 
519 void RotateAndTranslateToWorld(struct EnuCoor_f *p, float Zrot, float transX, float transY)
520 {
521  float temp = p->x;
522 
523  p->x = p->x * cosf(Zrot) - p->y * sinf(Zrot);
524  p->y = temp * sinf(Zrot) + p->y * cosf(Zrot);
525 
526  p->x = p->x + transX;
527  p->y = p->y + transY;
528 }
529 
530 void FindInterceptOfTwoLines(float *x, float *y, struct Line L1, struct Line L2)
531 {
532  *x = ((L2.b - L1.b) / (L1.m - L2.m));
533  *y = L1.m * (*x) + L1.b;
534 }
535 
536 
537 float EvaluateLineForX(float y, struct Line L)
538 {
539  return ((y - L.b) / L.m);
540 }
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:139
static struct EnuCoor_f * stateGetPositionEnu_f(void)
Get position in local ENU coordinates (float).
Definition: state.h:702
#define FALSE
Definition: std.h:5
#define TRUE
Definition: std.h:4
Definition: dc.h:100
float y
Definition: common_nav.h:41
Standard Digital Camera Control Interface.
float x
in meters
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
static float p[2][2]
#define ENU_BFP_OF_REAL(_o, _i)
float dc_distance_interval
AutoShoot photos on distance to last shot in meters.
Definition: dc.c:78
float y
in meters