Paparazzi UAS  v4.2.2_stable-4-gcc32f65
Paparazzi is a free software Unmanned Aircraft System.
 All Data Structures Files Functions Variables Typedefs Enumerations Enumerator Macros Pages
OSAMNav.c
Go to the documentation of this file.
2 
3 #include "subsystems/nav.h"
4 #include "estimator.h"
5 #include "autopilot.h"
6 #include "generated/flight_plan.h"
7 
8 /************** Flower Navigation **********************************************/
9 
17 static float CircleX;
18 static float CircleY;
19 static float Fly2X;
20 static float Fly2Y;
21 static float FlyFromX;
22 static float FlyFromY;
23 static float TransCurrentX;
24 static float TransCurrentY;
25 static float EdgeCurrentX;
26 static float EdgeCurrentY;
27 static float DistanceFromCenter;
28 static float FlowerTheta;
29 static float Flowerradius;
30 static uint8_t Center;
31 static uint8_t Edge;
32 
33 #ifndef LINE_START_FUNCTION
34 #define LINE_START_FUNCTION {}
35 #endif
36 #ifndef LINE_STOP_FUNCTION
37 #define LINE_STOP_FUNCTION {}
38 #endif
39 
40 bool_t InitializeFlower(uint8_t CenterWP, uint8_t EdgeWP)
41 {
42  Center = CenterWP;
43  Edge = EdgeWP;
44 
47 
49 
53 
54  FlowerTheta = atan2(TransCurrentY,TransCurrentX);
59 
62  else
64 
65  CircleX = 0;
66  CircleY = 0;
67  return FALSE;
68 }
69 
70 bool_t FlowerNav(void)
71 {
75 
76  bool_t InCircle = TRUE;
77  float CircleTheta;
78 
80  InCircle = FALSE;
81 
82  NavVerticalAutoThrottleMode(0); /* No pitch */
84 
85  switch(CFlowerStatus)
86  {
87  case Outside:
89  if(InCircle)
90  {
92  FlowerTheta = atan2(TransCurrentY,TransCurrentX);
98  }
99  break;
100  case FlowerLine:
102  if(!InCircle)
103  {
105  CircleTheta = nav_radius/Flowerradius;
106  CircleX = Flowerradius*cos(FlowerTheta+3.14-CircleTheta)+WaypointX(Center);
107  CircleY = Flowerradius*sin(FlowerTheta+3.14-CircleTheta)+WaypointY(Center);
108  nav_init_stage();
109  }
110  break;
111  case Circle:
113  if(InCircle)
114  {
120  else
122  FlowerTheta = atan2(TransCurrentY,TransCurrentX);
127  nav_init_stage();
128  }
129  break;
130 
131  default:
132  break;
133  }
134  return TRUE;
135 }
136 
137 /************** Bungee Takeoff **********************************************/
138 
154 #ifndef Takeoff_Distance
155 #define Takeoff_Distance 10
156 #endif
157 #ifndef Takeoff_Height
158 #define Takeoff_Height 30
159 #endif
160 #ifndef Takeoff_Speed
161 #define Takeoff_Speed 15
162 #endif
163 #ifndef Takeoff_MinSpeed
164 #define Takeoff_MinSpeed 5
165 #endif
166 
169 static float throttlePx;
170 static float throttlePy;
171 static float initialx;
172 static float initialy;
173 static float ThrottleSlope;
174 static bool_t AboveLine;
175 static float BungeeAlt;
176 static float TDistance;
178 
180 {
181  float ThrottleB;
182 
185 
186  BungeeWaypoint = BungeeWP;
187 
188  //Takeoff_Distance can only be positive
189  TDistance = fabs(Takeoff_Distance);
190 
191  //Translate initial position so that the position of the bungee is (0,0)
192  float Currentx = initialx-(WaypointX(BungeeWaypoint));
193  float Currenty = initialy-(WaypointY(BungeeWaypoint));
194 
195  //Record bungee alt (which should be the ground alt at that point)
197 
198  //Find Launch line slope and Throttle line slope
199  float MLaunch = Currenty/Currentx;
200 
201  //Find Throttle Point (the point where the throttle line and launch line intersect)
202  if(Currentx < 0)
203  throttlePx = TDistance/sqrt(MLaunch*MLaunch+1);
204  else
205  throttlePx = -(TDistance/sqrt(MLaunch*MLaunch+1));
206 
207  if(Currenty < 0)
209  else
210  throttlePy = -sqrt((TDistance*TDistance)-(throttlePx*throttlePx));
211 
212  //Find ThrottleLine
213  ThrottleSlope = tan(atan2(Currenty,Currentx)+(3.14/2));
214  ThrottleB = (throttlePy - (ThrottleSlope*throttlePx));
215 
216  //Determine whether the UAV is below or above the throttle line
217  if(Currenty > ((ThrottleSlope*Currentx)+ThrottleB))
218  AboveLine = TRUE;
219  else
220  AboveLine = FALSE;
221 
222  //Enable Launch Status and turn kill throttle on
224  kill_throttle = 1;
225 
226  //Translate the throttle point back
227  throttlePx = throttlePx+(WaypointX(BungeeWP));
228  throttlePy = throttlePy+(WaypointY(BungeeWP));
229 
230  return FALSE;
231 }
232 
233 bool_t BungeeTakeoff(void)
234 {
235  //Translate current position so Throttle point is (0,0)
236  float Currentx = estimator_x-throttlePx;
237  float Currenty = estimator_y-throttlePy;
238  bool_t CurrentAboveLine;
239  float ThrottleB;
240 
241  switch(CTakeoffStatus)
242  {
243  case Launch:
244  //Follow Launch Line
247  nav_route_xy(initialx,initialy,throttlePx,throttlePy);
248 
249  kill_throttle = 1;
250 
251  //recalculate lines if below min speed
253  {
256 
257  //Translate initial position so that the position of the bungee is (0,0)
258  Currentx = initialx-(WaypointX(BungeeWaypoint));
259  Currenty = initialy-(WaypointY(BungeeWaypoint));
260 
261  //Find Launch line slope
262  float MLaunch = Currenty/Currentx;
263 
264  //Find Throttle Point (the point where the throttle line and launch line intersect)
265  if(Currentx < 0)
266  throttlePx = TDistance/sqrt(MLaunch*MLaunch+1);
267  else
268  throttlePx = -(TDistance/sqrt(MLaunch*MLaunch+1));
269 
270  if(Currenty < 0)
271  throttlePy = sqrt((TDistance*TDistance)-(throttlePx*throttlePx));
272  else
273  throttlePy = -sqrt((TDistance*TDistance)-(throttlePx*throttlePx));
274 
275  //Find ThrottleLine
276  ThrottleSlope = tan(atan2(Currenty,Currentx)+(3.14/2));
277  ThrottleB = (throttlePy - (ThrottleSlope*throttlePx));
278 
279  //Determine whether the UAV is below or above the throttle line
280  if(Currenty > ((ThrottleSlope*Currentx)+ThrottleB))
281  AboveLine = TRUE;
282  else
283  AboveLine = FALSE;
284 
285  //Translate the throttle point back
286  throttlePx = throttlePx+(WaypointX(BungeeWaypoint));
287  throttlePy = throttlePy+(WaypointY(BungeeWaypoint));
288  }
289 
290  //Find out if the UAV is currently above the line
291  if(Currenty > (ThrottleSlope*Currentx))
292  CurrentAboveLine = TRUE;
293  else
294  CurrentAboveLine = FALSE;
295 
296  //Find out if UAV has crossed the line
297  if(AboveLine != CurrentAboveLine && estimator_hspeed_mod > Takeoff_MinSpeed)
298  {
300  kill_throttle = 0;
301  nav_init_stage();
302  }
303  break;
304  case Throttle:
305  //Follow Launch Line
306  NavVerticalAutoThrottleMode(AGR_CLIMB_PITCH);
307  NavVerticalThrottleMode(9600*(1));
308  nav_route_xy(initialx,initialy,throttlePx,throttlePy);
309  kill_throttle = 0;
310 
312  {
314  return FALSE;
315  }
316  else
317  {
318  return TRUE;
319  }
320  break;
321  default:
322  break;
323  }
324  return TRUE;
325 }
326 
327 /************** Polygon Survey **********************************************/
328 
334 static struct Point2D SmallestCorner;
335 static struct Line Edges[PolygonSize];
336 static float EdgeMaxY[PolygonSize];
337 static float EdgeMinY[PolygonSize];
338 static float SurveyTheta;
339 static float dSweep;
340 static float SurveyRadius;
341 static struct Point2D SurveyToWP;
342 static struct Point2D SurveyFromWP;
343 static struct Point2D SurveyCircle;
346 static float SurveyCircleQdr;
347 static float MaxY;
350 
351 bool_t InitializePolygonSurvey(uint8_t EntryWP, uint8_t Size, float sw, float Orientation)
352 {
353  SmallestCorner.x = 0;
354  SmallestCorner.y = 0;
355  int i = 0;
356  float ys = 0;
357  static struct Point2D EntryPoint;
358  float LeftYInt;
359  float RightYInt;
360  float temp;
361  float XIntercept1 = 0;
362  float XIntercept2 = 0;
363 
364  SurveyTheta = RadOfDeg(Orientation);
365  PolySurveySweepNum = 0;
367 
368  SurveyEntryWP = EntryWP;
369  SurveySize = Size;
370 
371  struct Point2D Corners[PolygonSize];
372 
374 
375  if (Size == 0)
376  return TRUE;
377 
378  //Don't initialize if Polygon is too big or if the orientation is not between 0 and 90
379  if(Size <= PolygonSize && Orientation >= -90 && Orientation <= 90)
380  {
381  //Initialize Corners
382  for(i = 0; i < Size; i++)
383  {
384  Corners[i].x = waypoints[i+EntryWP].x;
385  Corners[i].y = waypoints[i+EntryWP].y;
386  }
387 
388  //Rotate Corners so sweeps are parellel with x axis
389  for(i = 0; i < Size; i++)
390  TranslateAndRotateFromWorld(&Corners[i], SurveyTheta, 0, 0);
391 
392  //Find min x and min y
393  SmallestCorner.y = Corners[0].y;
394  SmallestCorner.x = Corners[0].x;
395  for(i = 1; i < Size; i++)
396  {
397  if(Corners[i].y < SmallestCorner.y)
398  SmallestCorner.y = Corners[i].y;
399 
400  if(Corners[i].x < SmallestCorner.x)
401  SmallestCorner.x = Corners[i].x;
402  }
403 
404  //Translate Corners all exist in quad #1
405  for(i = 0; i < Size; i++)
407 
408  //Rotate and Translate Entry Point
409  EntryPoint.x = Corners[0].x;
410  EntryPoint.y = Corners[0].y;
411 
412  //Find max y
413  MaxY = Corners[0].y;
414  for(i = 1; i < Size; i++)
415  {
416  if(Corners[i].y > MaxY)
417  MaxY = Corners[i].y;
418  }
419 
420  //Find polygon edges
421  for(i = 0; i < Size; i++)
422  {
423  if(i == 0)
424  if(Corners[Size-1].x == Corners[i].x) //Don't divide by zero!
425  Edges[i].m = MaxFloat;
426  else
427  Edges[i].m = ((Corners[Size-1].y-Corners[i].y)/(Corners[Size-1].x-Corners[i].x));
428  else
429  if(Corners[i].x == Corners[i-1].x)
430  Edges[i].m = MaxFloat;
431  else
432  Edges[i].m = ((Corners[i].y-Corners[i-1].y)/(Corners[i].x-Corners[i-1].x));
433 
434  //Edges[i].m = MaxFloat;
435  Edges[i].b = (Corners[i].y - (Corners[i].x*Edges[i].m));
436  }
437 
438  //Find Min and Max y for each line
439  FindInterceptOfTwoLines(&temp, &LeftYInt, Edges[0], Edges[1]);
440  FindInterceptOfTwoLines(&temp, &RightYInt, Edges[0], Edges[Size-1]);
441 
442  if(LeftYInt > RightYInt)
443  {
444  EdgeMaxY[0] = LeftYInt;
445  EdgeMinY[0] = RightYInt;
446  }
447  else
448  {
449  EdgeMaxY[0] = RightYInt;
450  EdgeMinY[0] = LeftYInt;
451  }
452 
453  for(i = 1; i < Size-1; i++)
454  {
455  FindInterceptOfTwoLines(&temp, &LeftYInt, Edges[i], Edges[i+1]);
456  FindInterceptOfTwoLines(&temp, &RightYInt, Edges[i], Edges[i-1]);
457 
458  if(LeftYInt > RightYInt)
459  {
460  EdgeMaxY[i] = LeftYInt;
461  EdgeMinY[i] = RightYInt;
462  }
463  else
464  {
465  EdgeMaxY[i] = RightYInt;
466  EdgeMinY[i] = LeftYInt;
467  }
468  }
469 
470  FindInterceptOfTwoLines(&temp, &LeftYInt, Edges[Size-1], Edges[0]);
471  FindInterceptOfTwoLines(&temp, &RightYInt, Edges[Size-1], Edges[Size-2]);
472 
473  if(LeftYInt > RightYInt)
474  {
475  EdgeMaxY[Size-1] = LeftYInt;
476  EdgeMinY[Size-1] = RightYInt;
477  }
478  else
479  {
480  EdgeMaxY[Size-1] = RightYInt;
481  EdgeMinY[Size-1] = LeftYInt;
482  }
483 
484  //Find amount to increment by every sweep
485  if(EntryPoint.y >= MaxY/2)
486  dSweep = -sw;
487  else
488  dSweep = sw;
489 
490  //CircleQdr tells the plane when to exit the circle
491  if(dSweep >= 0)
492  SurveyCircleQdr = -DegOfRad(SurveyTheta);
493  else
494  SurveyCircleQdr = 180-DegOfRad(SurveyTheta);
495 
496  //Find y value of the first sweep
497  ys = EntryPoint.y+(dSweep/2);
498 
499  //Find the edges which intercet the sweep line first
500  for(i = 0; i < SurveySize; i++)
501  {
502  if(EdgeMinY[i] <= ys && EdgeMaxY[i] > ys)
503  {
504  XIntercept2 = XIntercept1;
505  XIntercept1 = EvaluateLineForX(ys, Edges[i]);
506  }
507  }
508 
509  //Find point to come from and point to go to
510  if(fabs(EntryPoint.x - XIntercept2) <= fabs(EntryPoint.x - XIntercept1))
511  {
512  SurveyToWP.x = XIntercept1;
513  SurveyToWP.y = ys;
514 
515  SurveyFromWP.x = XIntercept2;
516  SurveyFromWP.y = ys;
517  }
518  else
519  {
520  SurveyToWP.x = XIntercept2;
521  SurveyToWP.y = ys;
522 
523  SurveyFromWP.x = XIntercept1;
524  SurveyFromWP.y = ys;
525  }
526 
527  //Find the direction to circle
528  if(ys > 0 && SurveyToWP.x > SurveyFromWP.x)
529  SurveyRadius = dSweep/2;
530  else if(ys < 0 && SurveyToWP.x < SurveyFromWP.x)
531  SurveyRadius = dSweep/2;
532  else
533  SurveyRadius = -dSweep/2;
534 
535  //Find the entry circle
537  SurveyCircle.y = EntryPoint.y;
538 
539  //Go into entry circle state
540  CSurveyStatus = Entry;
542  }
543 
544  return FALSE;
545 }
546 
547 bool_t PolygonSurvey(void)
548 {
549  struct Point2D C;
550  struct Point2D ToP;
551  struct Point2D FromP;
552  float ys;
553  static struct Point2D LastPoint;
554  int i;
555  bool_t SweepingBack = FALSE;
556  float XIntercept1 = 0;
557  float XIntercept2 = 0;
558  float DInt1 = 0;
559  float DInt2 = 0;
560 
561  NavVerticalAutoThrottleMode(0); /* No pitch */
563 
564  switch(CSurveyStatus)
565  {
566  case Entry:
567  //Rotate and translate circle point into real world
568  C.x = SurveyCircle.x;
569  C.y = SurveyCircle.y;
572 
573  //follow the circle
574  nav_circle_XY(C.x, C.y, SurveyRadius);
575 
577  {
579  nav_init_stage();
581  }
582  break;
583  case Sweep:
584  //Rotate and Translate Line points into real world
585  ToP.x = SurveyToWP.x;
586  ToP.y = SurveyToWP.y;
587  FromP.x = SurveyFromWP.x;
588  FromP.y = SurveyFromWP.y;
589 
592 
594  RotateAndTranslateToWorld(&FromP, SurveyTheta, 0, 0);
595 
596  //follow the line
597  nav_route_xy(FromP.x,FromP.y,ToP.x,ToP.y);
598  if(nav_approaching_xy(ToP.x,ToP.y,FromP.x,FromP.y, 0))
599  {
600  LastPoint.x = SurveyToWP.x;
601  LastPoint.y = SurveyToWP.y;
602 
603  if(LastPoint.y+dSweep >= MaxY || LastPoint.y+dSweep <= 0) //Your out of the Polygon so Sweep Back
604  {
605  dSweep = -dSweep;
606  ys = LastPoint.y+(dSweep/2);
607 
608  if(dSweep >= 0)
609  SurveyCircleQdr = -DegOfRad(SurveyTheta);
610  else
611  SurveyCircleQdr = 180-DegOfRad(SurveyTheta);
612  SweepingBack = TRUE;
614  }
615  else
616  {
617  //Find y value of the first sweep
618  ys = LastPoint.y+dSweep;
619  }
620 
621  //Find the edges which intercet the sweep line first
622  for(i = 0; i < SurveySize; i++)
623  {
624  if(EdgeMinY[i] < ys && EdgeMaxY[i] >= ys)
625  {
626  XIntercept2 = XIntercept1;
627  XIntercept1 = EvaluateLineForX(ys, Edges[i]);
628  }
629  }
630 
631  //Find point to come from and point to go to
632  DInt1 = XIntercept1 - LastPoint.x;
633  DInt2 = XIntercept2 - LastPoint.x;
634 
635  if(DInt1 * DInt2 >= 0)
636  {
637  if(fabs(DInt2) <= fabs(DInt1))
638  {
639  SurveyToWP.x = XIntercept1;
640  SurveyToWP.y = ys;
641 
642  SurveyFromWP.x = XIntercept2;
643  SurveyFromWP.y = ys;
644  }
645  else
646  {
647  SurveyToWP.x = XIntercept2;
648  SurveyToWP.y = ys;
649 
650  SurveyFromWP.x = XIntercept1;
651  SurveyFromWP.y = ys;
652  }
653  }
654  else
655  {
656  if((SurveyToWP.x - SurveyFromWP.x) > 0 && DInt2 > 0)
657  {
658  SurveyToWP.x = XIntercept1;
659  SurveyToWP.y = ys;
660 
661  SurveyFromWP.x = XIntercept2;
662  SurveyFromWP.y = ys;
663  }
664  else if((SurveyToWP.x - SurveyFromWP.x) < 0 && DInt2 < 0)
665  {
666  SurveyToWP.x = XIntercept1;
667  SurveyToWP.y = ys;
668 
669  SurveyFromWP.x = XIntercept2;
670  SurveyFromWP.y = ys;
671  }
672  else
673  {
674  SurveyToWP.x = XIntercept2;
675  SurveyToWP.y = ys;
676 
677  SurveyFromWP.x = XIntercept1;
678  SurveyFromWP.y = ys;
679  }
680  }
681 
682 
683 
684  if(fabs(LastPoint.x-SurveyToWP.x) > fabs(SurveyFromWP.x-SurveyToWP.x))
685  SurveyCircle.x = LastPoint.x;
686  else
688 
689 
690  if(!SweepingBack)
691  SurveyCircle.y = LastPoint.y+(dSweep/2);
692  else
693  SurveyCircle.y = LastPoint.y;
694 
695  //Find the direction to circle
696  if(ys > 0 && SurveyToWP.x > SurveyFromWP.x)
697  SurveyRadius = dSweep/2;
698  else if(ys < 0 && SurveyToWP.x < SurveyFromWP.x)
699  SurveyRadius = dSweep/2;
700  else
701  SurveyRadius = -dSweep/2;
702 
703  //Go into circle state
705  nav_init_stage();
708  }
709 
710  break;
711  case SweepCircle:
712  //Rotate and translate circle point into real world
713  C.x = SurveyCircle.x;
714  C.y = SurveyCircle.y;
717 
718  //follow the circle
719  nav_circle_XY(C.x, C.y, SurveyRadius);
720 
722  {
724  nav_init_stage();
726  }
727  break;
728  case Init:
729  return FALSE;
730  default:
731  return FALSE;
732  }
733  return TRUE;
734 }
735 
736 /************** Vertical Raster **********************************************/
737 
743 
744 bool_t InitializeVerticalRaster( void ) {
745  line_status = LR12;
746  return FALSE;
747 }
748 
749 bool_t VerticalRaster(uint8_t l1, uint8_t l2, float radius, float AltSweep) {
750  radius = fabs(radius);
751  float alt = waypoints[l1].a;
752  waypoints[l2].a = alt;
753 
754  float l2_l1_x = WaypointX(l1) - WaypointX(l2);
755  float l2_l1_y = WaypointY(l1) - WaypointY(l2);
756  float d = sqrt(l2_l1_x*l2_l1_x+l2_l1_y*l2_l1_y);
757 
758  /* Unit vector from l1 to l2 */
759  float u_x = l2_l1_x / d;
760  float u_y = l2_l1_y / d;
761 
762  /* The half circle centers and the other leg */
763  struct point l2_c1 = { WaypointX(l1) + radius * u_y,
764  WaypointY(l1) + radius * -u_x,
765  alt };
766  struct point l2_c2 = { WaypointX(l1) + 1.732*radius * u_x,
767  WaypointY(l1) + 1.732*radius * u_y,
768  alt };
769  struct point l2_c3 = { WaypointX(l1) + radius * -u_y,
770  WaypointY(l1) + radius * u_x,
771  alt };
772 
773  struct point l1_c1 = { WaypointX(l2) + radius * -u_y,
774  WaypointY(l2) + radius * u_x,
775  alt };
776  struct point l1_c2 = { WaypointX(l2) +1.732*radius * -u_x,
777  WaypointY(l2) + 1.732*radius * -u_y,
778  alt };
779  struct point l1_c3 = { WaypointX(l2) + radius * u_y,
780  WaypointY(l2) + radius * -u_x,
781  alt };
782  float qdr_out_2_1 = M_PI/3. - atan2(u_y, u_x);
783 
784  float qdr_out_2_2 = -M_PI/3. - atan2(u_y, u_x);
785  float qdr_out_2_3 = M_PI - atan2(u_y, u_x);
786 
787  /* Vertical target */
788  NavVerticalAutoThrottleMode(0); /* No pitch */
790 
791  switch (line_status) {
792  case LR12: /* From wp l2 to wp l1 */
793  NavSegment(l2, l1);
794  if (NavApproachingFrom(l1, l2, CARROT)) {
795  line_status = LQC21;
796  waypoints[l1].a = waypoints[l1].a+AltSweep;
797  nav_init_stage();
798  }
799  break;
800  case LQC21:
801  nav_circle_XY(l2_c1.x, l2_c1.y, radius);
802  if (NavQdrCloseTo(DegOfRad(qdr_out_2_1)-10)) {
803  line_status = LTC2;
804  nav_init_stage();
805  }
806  break;
807  case LTC2:
808  nav_circle_XY(l2_c2.x, l2_c2.y, -radius);
809  if (NavQdrCloseTo(DegOfRad(qdr_out_2_2)+10) && estimator_z >= (waypoints[l1].a-10)) {
810  line_status = LQC22;
811  nav_init_stage();
812  }
813  break;
814  case LQC22:
815  nav_circle_XY(l2_c3.x, l2_c3.y, radius);
816  if (NavQdrCloseTo(DegOfRad(qdr_out_2_3)-10)) {
817  line_status = LR21;
818  nav_init_stage();
819  }
820  break;
821  case LR21: /* From wp l1 to wp l2 */
822  NavSegment(l1, l2);
823  if (NavApproachingFrom(l2, l1, CARROT)) {
824  line_status = LQC12;
825  waypoints[l1].a = waypoints[l1].a+AltSweep;
826  nav_init_stage();
827  }
828  break;
829  case LQC12:
830  nav_circle_XY(l1_c1.x, l1_c1.y, radius);
831  if (NavQdrCloseTo(DegOfRad(qdr_out_2_1 + M_PI)-10)) {
832  line_status = LTC1;
833  nav_init_stage();
834  }
835  break;
836  case LTC1:
837  nav_circle_XY(l1_c2.x, l1_c2.y, -radius);
838  if (NavQdrCloseTo(DegOfRad(qdr_out_2_2 + M_PI)+10) && estimator_z >= (waypoints[l1].a-5)) {
839  line_status = LQC11;
840  nav_init_stage();
841  }
842  break;
843  case LQC11:
844  nav_circle_XY(l1_c3.x, l1_c3.y, radius);
845  if (NavQdrCloseTo(DegOfRad(qdr_out_2_3 + M_PI)-10)) {
846  line_status = LR12;
847  nav_init_stage();
848  }
849  default:
850  break;
851  }
852  return TRUE; /* This pattern never ends */
853 }
854 
855 /************** SkidLanding **********************************************/
856 /*
857 Landing Routine
858 
859 
860  <section name="Landing" prefix="Landing_">
861  <define name="AFHeight" value="50" unit="m"/>
862  <define name="FinalHeight" value="5" unit="m"/>
863  <define name="FinalStageTime" value="5" unit="s"/>
864  </section>
865 
866  */
867 
868 #ifndef Landing_AFHeight
869 #define Landing_AFHeight 50
870 #endif
871 #ifndef Landing_FinalHeight
872 #define Landing_FinalHeight 5
873 #endif
874 #ifndef Landing_FinalStageTime
875 #define Landing_FinalStageTime 5
876 #endif
877 
882 static float LandRadius;
883 static struct Point2D LandCircle;
884 static float LandAppAlt;
885 static float LandCircleQDR;
886 static float ApproachQDR;
887 static float FinalLandAltitude;
889 
890 bool_t InitializeSkidLanding(uint8_t AFWP, uint8_t TDWP, float radius)
891 {
892  AFWaypoint = AFWP;
893  TDWaypoint = TDWP;
895  LandRadius = radius;
898  FinalLandCount = 1;
900 
901  float x_0 = WaypointX(TDWaypoint) - WaypointX(AFWaypoint);
902  float y_0 = WaypointY(TDWaypoint) - WaypointY(AFWaypoint);
903 
904  /* Unit vector from AF to TD */
905  float d = sqrt(x_0*x_0+y_0*y_0);
906  float x_1 = x_0 / d;
907  float y_1 = y_0 / d;
908 
911 
913 
914  if(LandRadius > 0)
915  {
916  ApproachQDR = LandCircleQDR-RadOfDeg(90);
917  LandCircleQDR = LandCircleQDR-RadOfDeg(45);
918  }
919  else
920  {
921  ApproachQDR = LandCircleQDR+RadOfDeg(90);
922  LandCircleQDR = LandCircleQDR+RadOfDeg(45);
923  }
924 
925 
926  return FALSE;
927 }
928 
929 bool_t SkidLanding(void)
930 {
931  switch(CLandingStatus)
932  {
933  case CircleDown:
934  NavVerticalAutoThrottleMode(0); /* No pitch */
935 
936  if(NavCircleCount() < .1)
937  {
939  }
940  else
942 
944 
945  if(estimator_z < waypoints[AFWaypoint].a + 5)
946  {
948  nav_init_stage();
949  }
950 
951  break;
952 
953  case LandingWait:
954  NavVerticalAutoThrottleMode(0); /* No pitch */
957 
958  if(NavCircleCount() > 0.5 && NavQdrCloseTo(DegOfRad(ApproachQDR)))
959  {
961  nav_init_stage();
962  }
963  break;
964 
965  case Approach:
966  kill_throttle = 1;
967  NavVerticalAutoThrottleMode(0); /* No pitch */
970 
971  if(NavQdrCloseTo(DegOfRad(LandCircleQDR)))
972  {
974  nav_init_stage();
975  }
976  break;
977 
978  case Final:
979  kill_throttle = 1;
984  {
986  FinalLandCount++;
987  }
988  break;
989 
990  default:
991 
992  break;
993  }
994  return TRUE;
995 }
996 
999 static struct Point2D FLCircle;
1000 static struct Point2D FLFROMWP;
1001 static struct Point2D FLTOWP;
1002 static float FLQDR;
1003 static float FLRadius;
1004 
1005 bool_t FlightLine(uint8_t From_WP, uint8_t To_WP, float radius, float Space_Before, float Space_After)
1006 {
1007  struct Point2D V;
1008  struct Point2D P;
1009  float dv;
1010 
1011  switch(CFLStatus)
1012  {
1013  case FLInitialize:
1014 
1015  //Translate WPs so From_WP is origin
1016  V.x = WaypointX(To_WP) - WaypointX(From_WP);
1017  V.y = WaypointY(To_WP) - WaypointY(From_WP);
1018 
1019  //Record Aircraft Position
1020  P.x = estimator_x;
1021  P.y = estimator_y;
1022 
1023  //Rotate Aircraft Position so V is aligned with x axis
1024  TranslateAndRotateFromWorld(&P, atan2(V.y,V.x), WaypointX(From_WP), WaypointY(From_WP));
1025 
1026  //Find which side of the flight line the aircraft is on
1027  if(P.y > 0)
1028  FLRadius = -radius;
1029  else
1030  FLRadius = radius;
1031 
1032  //Find unit vector of V
1033  dv = sqrt(V.x*V.x+V.y*V.y);
1034  V.x = V.x / dv;
1035  V.y = V.y / dv;
1036 
1037  //Find begin and end points of flight line
1038  FLFROMWP.x = -V.x*Space_Before;
1039  FLFROMWP.y = -V.y*Space_Before;
1040 
1041  FLTOWP.x = V.x*(dv+Space_After);
1042  FLTOWP.y = V.y*(dv+Space_After);
1043 
1044  //Find center of circle
1045  FLCircle.x = FLFROMWP.x + V.y * FLRadius;
1046  FLCircle.y = FLFROMWP.y - V.x * FLRadius;
1047 
1048  //Find the angle to exit the circle
1050 
1051  //Translate back
1052  FLFROMWP.x = FLFROMWP.x + WaypointX(From_WP);
1053  FLFROMWP.y = FLFROMWP.y + WaypointY(From_WP);
1054 
1055  FLTOWP.x = FLTOWP.x + WaypointX(From_WP);
1056  FLTOWP.y = FLTOWP.y + WaypointY(From_WP);
1057 
1058  FLCircle.x = FLCircle.x + WaypointX(From_WP);
1059  FLCircle.y = FLCircle.y + WaypointY(From_WP);
1060 
1061  CFLStatus = FLCircleS;
1062  nav_init_stage();
1063 
1064  break;
1065 
1066  case FLCircleS:
1067 
1068  NavVerticalAutoThrottleMode(0); /* No pitch */
1069  NavVerticalAltitudeMode(waypoints[From_WP].a, 0);
1070 
1072 
1073  if(NavCircleCount() > 0.2 && NavQdrCloseTo(DegOfRad(FLQDR)))
1074  {
1075  CFLStatus = FLLine;
1077  nav_init_stage();
1078  }
1079  break;
1080 
1081  case FLLine:
1082 
1083  NavVerticalAutoThrottleMode(0); /* No pitch */
1084  NavVerticalAltitudeMode(waypoints[From_WP].a, 0);
1085 
1087 
1088 
1090  {
1093  nav_init_stage();
1094  }
1095  break;
1096 
1097  case FLFinished:
1099  nav_init_stage();
1100  return FALSE;
1101  break;
1102 
1103  default:
1104  break;
1105  }
1106  return TRUE;
1107 
1108 }
1109 
1111 
1112 bool_t FlightLineBlock(uint8_t First_WP, uint8_t Last_WP, float radius, float Space_Before, float Space_After)
1113 {
1114  if(First_WP < Last_WP)
1115  {
1116  FlightLine(First_WP+FLBlockCount, First_WP+FLBlockCount+1, radius, Space_Before, Space_After);
1117 
1118  if(CFLStatus == FLInitialize)
1119  {
1120  FLBlockCount++;
1121  if(First_WP+FLBlockCount >= Last_WP)
1122  {
1123  FLBlockCount = 0;
1124  return FALSE;
1125  }
1126  }
1127  }
1128  else
1129  {
1130  FlightLine(First_WP-FLBlockCount, First_WP-FLBlockCount-1, radius, Space_Before, Space_After);
1131 
1132  if(CFLStatus == FLInitialize)
1133  {
1134  FLBlockCount++;
1135  if(First_WP-FLBlockCount <= Last_WP)
1136  {
1137  FLBlockCount = 0;
1138  return FALSE;
1139  }
1140  }
1141  }
1142 
1143  return TRUE;
1144 }
1145 
1146 
1147 /*
1148 Translates point so (transX, transY) are (0,0) then rotates the point around z by Zrot
1149 */
1150 void TranslateAndRotateFromWorld(struct Point2D *p, float Zrot, float transX, float transY)
1151 {
1152  float temp;
1153 
1154  p->x = p->x - transX;
1155  p->y = p->y - transY;
1156 
1157  temp = p->x;
1158  p->x = p->x*cos(Zrot)+p->y*sin(Zrot);
1159  p->y = -temp*sin(Zrot)+p->y*cos(Zrot);
1160 }
1161 
1162 /*
1163 Rotates point round z by -Zrot then translates so (0,0) becomes (transX,transY)
1164 */
1165 void RotateAndTranslateToWorld(struct Point2D *p, float Zrot, float transX, float transY)
1166 {
1167  float temp = p->x;
1168 
1169  p->x = p->x*cos(Zrot)-p->y*sin(Zrot);
1170  p->y = temp*sin(Zrot)+p->y*cos(Zrot);
1171 
1172  p->x = p->x + transX;
1173  p->y = p->y + transY;
1174 }
1175 
1176 void FindInterceptOfTwoLines(float *x, float *y, struct Line L1, struct Line L2)
1177 {
1178  *x = ((L2.b-L1.b)/(L1.m-L2.m));
1179  *y = L1.m*(*x)+L1.b;
1180 }
1181 
1182 float EvaluateLineForY(float x, struct Line L)
1183 {
1184  return (L.m*x)+L.b;
1185 }
1186 
1187 float EvaluateLineForX(float y, struct Line L)
1188 {
1189  return ((y-L.b)/L.m);
1190 }
1191 
1192 float DistanceEquation(struct Point2D p1,struct Point2D p2)
1193 {
1194  return sqrt((p1.x-p2.x)*(p1.x-p2.x)+(p1.y-p2.y)*(p1.y-p2.y));
1195 }
float y
Definition: OSAMNav.h:7
#define PolygonSize
Definition: OSAMNav.h:19
unsigned short uint16_t
Definition: types.h:16
#define Landing_FinalHeight
Definition: OSAMNav.c:872
bool_t FlightLine(uint8_t From_WP, uint8_t To_WP, float radius, float Space_Before, float Space_After)
Definition: OSAMNav.c:1005
Definition: OSAMNav.c:741
static float FLQDR
Definition: OSAMNav.c:1002
void RotateAndTranslateToWorld(struct Point2D *p, float Zrot, float transX, float transY)
Definition: OSAMNav.c:1165
#define LINE_STOP_FUNCTION
Definition: OSAMNav.c:37
#define Takeoff_MinSpeed
Definition: OSAMNav.c:164
void TranslateAndRotateFromWorld(struct Point2D *p, float Zrot, float transX, float transY)
Definition: OSAMNav.c:1150
float estimator_hspeed_mod
module of horizontal ground speed in m/s
Definition: estimator.c:64
bool_t FlowerNav(void)
Definition: OSAMNav.c:70
static enum TakeoffStatus CTakeoffStatus
Definition: OSAMNav.c:168
static float initialy
Definition: OSAMNav.c:172
#define Landing_FinalStageTime
Definition: OSAMNav.c:875
static float MaxY
Definition: OSAMNav.c:347
Definition: OSAMNav.c:332
float a
Definition: common_nav.h:39
static float SurveyTheta
Definition: OSAMNav.c:338
Definition: OSAMNav.c:741
float EvaluateLineForX(float y, struct Line L)
Definition: OSAMNav.c:1187
static float Fly2Y
Definition: OSAMNav.c:20
static float radius
Definition: chemotaxis.c:15
static float throttlePy
Definition: OSAMNav.c:170
Definition: OSAMNav.c:741
float estimator_y
north position in meters
Definition: estimator.c:43
static struct Line Edges[PolygonSize]
Definition: OSAMNav.c:335
static struct Point2D FLTOWP
Definition: OSAMNav.c:1001
#define Takeoff_Distance
Takeoff functions for bungee takeoff.
Definition: OSAMNav.c:155
bool_t VerticalRaster(uint8_t l1, uint8_t l2, float radius, float AltSweep)
Definition: OSAMNav.c:749
static float initialx
Definition: OSAMNav.c:171
static float Fly2X
Definition: OSAMNav.c:19
float x
Definition: OSAMNav.h:7
float DistanceEquation(struct Point2D p1, struct Point2D p2)
Definition: OSAMNav.c:1192
static float ThrottleSlope
Definition: OSAMNav.c:173
static float TDistance
Definition: OSAMNav.c:176
bool_t SkidLanding(void)
Definition: OSAMNav.c:929
static float TransCurrentX
Definition: OSAMNav.c:23
bool_t InitializeSkidLanding(uint8_t AFWP, uint8_t TDWP, float radius)
Definition: OSAMNav.c:890
bool_t FlightLineBlock(uint8_t First_WP, uint8_t Last_WP, float radius, float Space_Before, float Space_After)
Definition: OSAMNav.c:1112
static float LandCircleQDR
Definition: OSAMNav.c:885
float b
Definition: OSAMNav.h:8
static float LandRadius
Definition: OSAMNav.c:882
FLStatus
Definition: OSAMNav.c:997
#define FALSE
Definition: imu_chimu.h:141
Definition: OSAMNav.c:741
Definition: OSAMNav.c:332
static float LandAppAlt
Definition: OSAMNav.c:884
static float DistanceFromCenter
Definition: OSAMNav.c:27
static float BungeeAlt
Definition: OSAMNav.c:175
Definition: OSAMNav.c:741
Definition: OSAMNav.c:878
bool_t InitializeBungeeTakeoff(uint8_t BungeeWP)
Definition: OSAMNav.c:179
static struct Point2D FLCircle
Definition: OSAMNav.c:999
Definition: OSAMNav.c:332
static float SurveyCircleQdr
Definition: OSAMNav.c:346
void FindInterceptOfTwoLines(float *x, float *y, struct Line L1, struct Line L2)
Definition: OSAMNav.c:1176
static float TransCurrentY
Definition: OSAMNav.c:24
static uint8_t TDWaypoint
Definition: OSAMNav.c:881
static uint8_t Edge
Definition: OSAMNav.c:31
static float throttlePx
Definition: OSAMNav.c:169
static float EdgeCurrentX
Definition: OSAMNav.c:25
static uint8_t AFWaypoint
Definition: OSAMNav.c:880
static struct Point2D SmallestCorner
Definition: OSAMNav.c:334
static struct Point2D FLFROMWP
Definition: OSAMNav.c:1000
bool_t kill_throttle
Definition: main_ap.c:120
static float ApproachQDR
Definition: OSAMNav.c:886
static float EdgeCurrentY
Definition: OSAMNav.c:26
static float CircleX
Definition: OSAMNav.c:17
float estimator_x
east position in meters
Definition: estimator.c:42
SurveyStatus
This routine will cover the enitre area of any Polygon defined in the flightplan which is a convex po...
Definition: OSAMNav.c:332
Definition: OSAMNav.h:8
static uint8_t BungeeWaypoint
Definition: OSAMNav.c:177
static enum SurveyStatus CSurveyStatus
Definition: OSAMNav.c:333
uint16_t stage_time
In s.
#define Takeoff_Height
Definition: OSAMNav.c:158
LandingStatus
Definition: OSAMNav.c:878
Definition: OSAMNav.c:741
#define TRUE
Definition: imu_chimu.h:144
#define LINE_START_FUNCTION
Definition: OSAMNav.c:34
static struct Point2D LandCircle
Definition: OSAMNav.c:883
Definition: OSAMNav.c:741
static uint8_t FinalLandCount
Definition: OSAMNav.c:888
TakeoffStatus
Definition: OSAMNav.c:167
unsigned char uint8_t
Definition: types.h:14
float estimator_z
altitude above MSL in meters
Definition: estimator.c:44
line_status
Copy of nav line.
Definition: OSAMNav.c:741
#define Takeoff_Speed
Definition: OSAMNav.c:161
static struct Point2D SurveyToWP
Definition: OSAMNav.c:341
bool_t InitializeFlower(uint8_t CenterWP, uint8_t EdgeWP)
Definition: OSAMNav.c:40
static struct Point2D SurveyFromWP
Definition: OSAMNav.c:342
float y
Definition: common_nav.h:38
static float FlowerTheta
Definition: OSAMNav.c:28
Definition: OSAMNav.h:7
#define Landing_AFHeight
Definition: OSAMNav.c:869
static float dSweep
Definition: OSAMNav.c:339
State estimation, fusioning sensors.
bool_t BungeeTakeoff(void)
Definition: OSAMNav.c:233
static struct Point2D SurveyCircle
Definition: OSAMNav.c:343
static float EdgeMaxY[PolygonSize]
Definition: OSAMNav.c:336
static float FinalLandAltitude
Definition: OSAMNav.c:887
Definition: OSAMNav.c:15
FlowerStatus
Makes a flower pattern.
Definition: OSAMNav.c:15
uint16_t PolySurveySweepBackNum
Definition: OSAMNav.c:349
static float SurveyRadius
Definition: OSAMNav.c:340
static uint8_t SurveyEntryWP
Definition: OSAMNav.c:344
#define MaxFloat
Definition: OSAMNav.h:20
static float FlyFromY
Definition: OSAMNav.c:22
static float FlyFromX
Definition: OSAMNav.c:21
float m
Definition: OSAMNav.h:8
static float CircleY
Definition: OSAMNav.c:18
static float FLRadius
Definition: OSAMNav.c:1003
uint16_t PolySurveySweepNum
Definition: OSAMNav.c:348
static uint8_t Center
Definition: OSAMNav.c:30
static enum FLStatus CFLStatus
Definition: OSAMNav.c:998
static enum LandingStatus CLandingStatus
Definition: OSAMNav.c:879
static bool_t AboveLine
Definition: OSAMNav.c:174
bool_t PolygonSurvey(void)
Definition: OSAMNav.c:547
Definition: OSAMNav.c:741
static float EdgeMinY[PolygonSize]
Definition: OSAMNav.c:337
static float Flowerradius
Definition: OSAMNav.c:29
bool_t InitializeVerticalRaster(void)
Definition: OSAMNav.c:744
float x
Definition: common_nav.h:37
static uint8_t FLBlockCount
Definition: OSAMNav.c:1110
static uint8_t SurveySize
Definition: OSAMNav.c:345
bool_t InitializePolygonSurvey(uint8_t EntryWP, uint8_t Size, float sw, float Orientation)
Definition: OSAMNav.c:351
static enum FlowerStatus CFlowerStatus
Definition: OSAMNav.c:16
float EvaluateLineForY(float x, struct Line L)
Definition: OSAMNav.c:1182