Paparazzi UAS  v5.2.2_stable-0-gd6b9f29
Paparazzi is a free software Unmanned Aircraft System.
 All Data Structures Files Functions Variables Typedefs Enumerations Enumerator Macros Modules Pages
nav_line_osam.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 
31 
33 #include "state.h"
34 #include "autopilot.h"
35 #include "generated/flight_plan.h"
36 
37 #ifndef LINE_START_FUNCTION
38 #define LINE_START_FUNCTION {}
39 #endif
40 #ifndef LINE_STOP_FUNCTION
41 #define LINE_STOP_FUNCTION {}
42 #endif
43 
44 struct Point2D {float x; float y;};
45 
47 
49 static struct Point2D FLCircle;
50 static struct Point2D FLFROMWP;
51 static struct Point2D FLTOWP;
52 static float FLQDR;
53 static float FLRadius;
54 
55 /*
56  Translates point so (transX, transY) are (0,0) then rotates the point around z by Zrot
57 */
58 static void TranslateAndRotateFromWorld(struct Point2D *p, float Zrot, float transX, float transY)
59 {
60  float temp;
61 
62  p->x = p->x - transX;
63  p->y = p->y - transY;
64 
65  temp = p->x;
66  p->x = p->x*cosf(Zrot)+p->y*sinf(Zrot);
67  p->y = -temp*sinf(Zrot)+p->y*cosf(Zrot);
68 }
69 
70 
71 bool_t nav_line_osam_run(uint8_t From_WP, uint8_t To_WP, float radius, float Space_Before, float Space_After)
72 {
73  struct Point2D V;
74  struct Point2D P;
75  float dv;
76 
77  switch(CFLStatus)
78  {
79  case FLInitialize:
80 
81  //Translate WPs so From_WP is origin
82  V.x = WaypointX(To_WP) - WaypointX(From_WP);
83  V.y = WaypointY(To_WP) - WaypointY(From_WP);
84 
85  //Record Aircraft Position
86  P.x = stateGetPositionEnu_f()->x;
87  P.y = stateGetPositionEnu_f()->y;
88 
89  //Rotate Aircraft Position so V is aligned with x axis
90  TranslateAndRotateFromWorld(&P, atan2f(V.y,V.x), WaypointX(From_WP), WaypointY(From_WP));
91 
92  //Find which side of the flight line the aircraft is on
93  if(P.y > 0)
94  FLRadius = -radius;
95  else
96  FLRadius = radius;
97 
98  //Find unit vector of V
99  dv = sqrtf(V.x*V.x+V.y*V.y);
100  V.x = V.x / dv;
101  V.y = V.y / dv;
102 
103  //Find begin and end points of flight line
104  FLFROMWP.x = -V.x*Space_Before;
105  FLFROMWP.y = -V.y*Space_Before;
106 
107  FLTOWP.x = V.x*(dv+Space_After);
108  FLTOWP.y = V.y*(dv+Space_After);
109 
110  //Find center of circle
111  FLCircle.x = FLFROMWP.x + V.y * FLRadius;
112  FLCircle.y = FLFROMWP.y - V.x * FLRadius;
113 
114  //Find the angle to exit the circle
116 
117  //Translate back
118  FLFROMWP.x = FLFROMWP.x + WaypointX(From_WP);
119  FLFROMWP.y = FLFROMWP.y + WaypointY(From_WP);
120 
121  FLTOWP.x = FLTOWP.x + WaypointX(From_WP);
122  FLTOWP.y = FLTOWP.y + WaypointY(From_WP);
123 
124  FLCircle.x = FLCircle.x + WaypointX(From_WP);
125  FLCircle.y = FLCircle.y + WaypointY(From_WP);
126 
128  nav_init_stage();
129 
130  break;
131 
132  case FLCircleS:
133 
134  NavVerticalAutoThrottleMode(0); /* No pitch */
135  NavVerticalAltitudeMode(waypoints[From_WP].a, 0);
136 
138 
139  if(NavCircleCount() > 0.2 && NavQdrCloseTo(DegOfRad(FLQDR)))
140  {
141  CFLStatus = FLLine;
143  nav_init_stage();
144  }
145  break;
146 
147  case FLLine:
148 
149  NavVerticalAutoThrottleMode(0); /* No pitch */
150  NavVerticalAltitudeMode(waypoints[From_WP].a, 0);
151 
153 
154 
156  {
159  nav_init_stage();
160  }
161  break;
162 
163  case FLFinished:
165  nav_init_stage();
166  return FALSE;
167  break;
168 
169  default:
170  break;
171  }
172  return TRUE;
173 
174 }
175 
177 
178 bool_t nav_line_osam_block_run(uint8_t First_WP, uint8_t Last_WP, float radius, float Space_Before, float Space_After)
179 {
180  if(First_WP < Last_WP)
181  {
182  nav_line_osam_run(First_WP+FLBlockCount, First_WP+FLBlockCount+1, radius, Space_Before, Space_After);
183 
184  if(CFLStatus == FLInitialize)
185  {
186  FLBlockCount++;
187  if(First_WP+FLBlockCount >= Last_WP)
188  {
189  FLBlockCount = 0;
190  return FALSE;
191  }
192  }
193  }
194  else
195  {
196  nav_line_osam_run(First_WP-FLBlockCount, First_WP-FLBlockCount-1, radius, Space_Before, Space_After);
197 
198  if(CFLStatus == FLInitialize)
199  {
200  FLBlockCount++;
201  if(First_WP-FLBlockCount <= Last_WP)
202  {
203  FLBlockCount = 0;
204  return FALSE;
205  }
206  }
207  }
208 
209  return TRUE;
210 }
float y
Definition: nav_line_osam.c:44
static float radius
Definition: chemotaxis.c:15
float x
Definition: nav_line_osam.c:44
static struct EnuCoor_f * stateGetPositionEnu_f(void)
Get position in local ENU coordinates (float).
Definition: state.h:672
float y
in meters
#define FALSE
Definition: imu_chimu.h:141
float x
in meters
#define TRUE
Definition: imu_chimu.h:144
unsigned char uint8_t
Definition: types.h:14
API to get/set the generic vehicle states.
static float p[2][2]