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_bungee_takeoff.c
Go to the documentation of this file.
1 /*
2  * Copyright (C) 2008-2013 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 
30 
32 #include "state.h"
33 #include "autopilot.h"
34 #include "generated/flight_plan.h"
35 
36 /************** Bungee Takeoff **********************************************/
37 
54 #ifndef Takeoff_Distance
55 #define Takeoff_Distance 10
56 #endif
57 #ifndef Takeoff_Height
58 #define Takeoff_Height 30
59 #endif
60 #ifndef Takeoff_Speed
61 #define Takeoff_Speed 15
62 #endif
63 #ifndef Takeoff_MinSpeed
64 #define Takeoff_MinSpeed 5
65 #endif
66 
69 static float throttlePx;
70 static float throttlePy;
71 static float initialx;
72 static float initialy;
73 static float ThrottleSlope;
74 static bool_t AboveLine;
75 static float BungeeAlt;
76 static float TDistance;
78 
80 {
81  float ThrottleB;
82 
85 
86  BungeeWaypoint = BungeeWP;
87 
88  //Takeoff_Distance can only be positive
90 
91  //Translate initial position so that the position of the bungee is (0,0)
92  float Currentx = initialx-(WaypointX(BungeeWaypoint));
93  float Currenty = initialy-(WaypointY(BungeeWaypoint));
94 
95  //Record bungee alt (which should be the ground alt at that point)
97 
98  //Find Launch line slope and Throttle line slope
99  float MLaunch = Currenty/Currentx;
100 
101  //Find Throttle Point (the point where the throttle line and launch line intersect)
102  if(Currentx < 0)
103  throttlePx = TDistance/sqrt(MLaunch*MLaunch+1);
104  else
105  throttlePx = -(TDistance/sqrt(MLaunch*MLaunch+1));
106 
107  if(Currenty < 0)
109  else
110  throttlePy = -sqrt((TDistance*TDistance)-(throttlePx*throttlePx));
111 
112  //Find ThrottleLine
113  ThrottleSlope = tan(atan2(Currenty,Currentx)+(3.14/2));
114  ThrottleB = (throttlePy - (ThrottleSlope*throttlePx));
115 
116  //Determine whether the UAV is below or above the throttle line
117  if(Currenty > ((ThrottleSlope*Currentx)+ThrottleB))
118  AboveLine = TRUE;
119  else
120  AboveLine = FALSE;
121 
122  //Enable Launch Status and turn kill throttle on
124  kill_throttle = 1;
125 
126  //Translate the throttle point back
127  throttlePx = throttlePx+(WaypointX(BungeeWP));
128  throttlePy = throttlePy+(WaypointY(BungeeWP));
129 
130  return FALSE;
131 }
132 
134 {
135  //Translate current position so Throttle point is (0,0)
136  float Currentx = stateGetPositionEnu_f()->x-throttlePx;
137  float Currenty = stateGetPositionEnu_f()->y-throttlePy;
138  bool_t CurrentAboveLine;
139  float ThrottleB;
140 
141  switch(CTakeoffStatus)
142  {
143  case Launch:
144  //Follow Launch Line
148 
149  kill_throttle = 1;
150 
151  //recalculate lines if below min speed
153  {
156 
157  //Translate initial position so that the position of the bungee is (0,0)
158  Currentx = initialx-(WaypointX(BungeeWaypoint));
159  Currenty = initialy-(WaypointY(BungeeWaypoint));
160 
161  //Find Launch line slope
162  float MLaunch = Currenty/Currentx;
163 
164  //Find Throttle Point (the point where the throttle line and launch line intersect)
165  if(Currentx < 0)
166  throttlePx = TDistance/sqrt(MLaunch*MLaunch+1);
167  else
168  throttlePx = -(TDistance/sqrt(MLaunch*MLaunch+1));
169 
170  if(Currenty < 0)
172  else
173  throttlePy = -sqrt((TDistance*TDistance)-(throttlePx*throttlePx));
174 
175  //Find ThrottleLine
176  ThrottleSlope = tan(atan2(Currenty,Currentx)+(3.14/2));
177  ThrottleB = (throttlePy - (ThrottleSlope*throttlePx));
178 
179  //Determine whether the UAV is below or above the throttle line
180  if(Currenty > ((ThrottleSlope*Currentx)+ThrottleB))
181  AboveLine = TRUE;
182  else
183  AboveLine = FALSE;
184 
185  //Translate the throttle point back
186  throttlePx = throttlePx+(WaypointX(BungeeWaypoint));
187  throttlePy = throttlePy+(WaypointY(BungeeWaypoint));
188  }
189 
190  //Find out if the UAV is currently above the line
191  if(Currenty > (ThrottleSlope*Currentx))
192  CurrentAboveLine = TRUE;
193  else
194  CurrentAboveLine = FALSE;
195 
196  //Find out if UAV has crossed the line
197  if(AboveLine != CurrentAboveLine && (*stateGetHorizontalSpeedNorm_f()) > Takeoff_MinSpeed)
198  {
200  kill_throttle = 0;
201  nav_init_stage();
202  }
203  break;
204  case Throttle:
205  //Follow Launch Line
206  NavVerticalAutoThrottleMode(AGR_CLIMB_PITCH);
207  NavVerticalThrottleMode(9600*(1));
209  kill_throttle = 0;
210 
212  {
214  return FALSE;
215  }
216  else
217  {
218  return TRUE;
219  }
220  break;
221  default:
222  break;
223  }
224  return TRUE;
225 }
static float * stateGetHorizontalSpeedNorm_f(void)
Get norm of horizontal ground speed (float).
Definition: state.h:854
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
bool_t kill_throttle
Definition: autopilot.c:41
#define TRUE
Definition: imu_chimu.h:144
static struct UtmCoor_f * stateGetPositionUtm_f(void)
Get position in UTM coordinates (float).
Definition: state.h:651
unsigned char uint8_t
Definition: types.h:14
API to get/set the generic vehicle states.