Paparazzi UAS  v5.14.0_stable-0-g3f680d1
Paparazzi is a free software Unmanned Aircraft System.
 All Data Structures Namespaces 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 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  case FLInitialize:
79 
80  //Translate WPs so From_WP is origin
81  V.x = WaypointX(To_WP) - WaypointX(From_WP);
82  V.y = WaypointY(To_WP) - WaypointY(From_WP);
83 
84  //Record Aircraft Position
85  P.x = stateGetPositionEnu_f()->x;
86  P.y = stateGetPositionEnu_f()->y;
87 
88  //Rotate Aircraft Position so V is aligned with x axis
89  TranslateAndRotateFromWorld(&P, atan2f(V.y, V.x), WaypointX(From_WP), WaypointY(From_WP));
90 
91  //Find which side of the flight line the aircraft is on
92  if (P.y > 0) {
93  FLRadius = -radius;
94  } else {
95  FLRadius = radius;
96  }
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
115  FLQDR = atan2f(FLFROMWP.x - FLCircle.x, FLFROMWP.y - FLCircle.y);
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  CFLStatus = FLLine;
142  nav_init_stage();
143  }
144  break;
145 
146  case FLLine:
147 
148  NavVerticalAutoThrottleMode(0); /* No pitch */
149  NavVerticalAltitudeMode(waypoints[From_WP].a, 0);
150 
152 
153 
157  nav_init_stage();
158  }
159  break;
160 
161  case FLFinished:
163  nav_init_stage();
164  return false;
165  break;
166 
167  default:
168  break;
169  }
170  return true;
171 
172 }
173 
175 
176 bool nav_line_osam_block_run(uint8_t First_WP, uint8_t Last_WP, float radius, float Space_Before, float Space_After)
177 {
178  if (First_WP < Last_WP) {
179  nav_line_osam_run(First_WP + FLBlockCount, First_WP + FLBlockCount + 1, radius, Space_Before, Space_After);
180 
181  if (CFLStatus == FLInitialize) {
182  FLBlockCount++;
183  if (First_WP + FLBlockCount >= Last_WP) {
184  FLBlockCount = 0;
185  return false;
186  }
187  }
188  } else {
189  nav_line_osam_run(First_WP - FLBlockCount, First_WP - FLBlockCount - 1, radius, Space_Before, Space_After);
190 
191  if (CFLStatus == FLInitialize) {
192  FLBlockCount++;
193  if (First_WP - FLBlockCount <= Last_WP) {
194  FLBlockCount = 0;
195  return false;
196  }
197  }
198  }
199 
200  return true;
201 }
static struct EnuCoor_f * stateGetPositionEnu_f(void)
Get position in local ENU coordinates (float).
Definition: state.h:719
#define WaypointX(_wp)
Definition: common_nav.h:45
float x
in meters
#define WaypointY(_wp)
Definition: common_nav.h:46
Core autopilot interface common to all firmwares.
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
static float p[2][2]
float y
in meters