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