Paparazzi UAS  v5.8.2_stable-0-g6260b7c
Paparazzi is a free software Unmanned Aircraft System.
 All Data Structures Namespaces 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-2015 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, see
18  * <http://www.gnu.org/licenses/>.
19  */
20 
61 
62 #include "state.h"
63 #include "paparazzi.h"
66 #include "generated/flight_plan.h"
67 #include "generated/airframe.h"
69 
70 
71 // Backward compatibility
72 #ifdef Takeoff_Distance
73 #warning "Takeoff_Distance depreciated, please use BUNGEE_TAKEOFF_DISTANCE instead"
74 #define BUNGEE_TAKEOFF_DISTANCE Takeoff_Distance
75 #endif
76 #ifdef Takeoff_Height
77 #warning "Takeoff_Height depreciated, please use BUNGEE_TAKEOFF_HEIGHT instead"
78 #define BUNGEE_TAKEOFF_HEIGHT Takeoff_Height
79 #endif
80 #ifdef Takeoff_Speed
81 #warning "Takeoff_Speed depreciated, please use BUNGEE_TAKEOFF_AIRSPEED instead (beware that USE_AIRSPEED flag is needed)"
82 #define BUNGEE_TAKEOFF_AIRSPEED Takeoff_Speed
83 #endif
84 #ifdef Takeoff_MinSpeed
85 #warning "Takeoff_MinSpeed depreciated, please use BUNGEE_TAKEOFF_MIN_SPEED instead"
86 #define BUNGEE_TAKEOFF_MIN_SPEED Takeoff_MinSpeed
87 #endif
88 
89 
90 #ifndef BUNGEE_TAKEOFF_DISTANCE
91 #define BUNGEE_TAKEOFF_DISTANCE 10.0
92 #endif
93 #ifndef BUNGEE_TAKEOFF_HEIGHT
94 #define BUNGEE_TAKEOFF_HEIGHT 30.0
95 #endif
96 #if USE_AIRSPEED
97 #ifndef BUNGEE_TAKEOFF_AIRSPEED
98 #define BUNGEE_TAKEOFF_AIRSPEED 15.0
99 #endif
100 #else
101 #ifdef BUNGEE_TAKEOFF_AIRSPEED
102 #warning "BUNGEE_TAKEOFF_AIRSPEED is defined but not USE_AIRSPEED. Airspeed limit will not be used"
103 #endif
104 #endif
105 #ifndef BUNGEE_TAKEOFF_MIN_SPEED
106 #define BUNGEE_TAKEOFF_MIN_SPEED 5.0
107 #endif
108 #ifndef BUNGEE_TAKEOFF_THROTTLE
109 #define BUNGEE_TAKEOFF_THROTTLE 1.0
110 #endif
111 #ifndef BUNGEE_TAKEOFF_PITCH
112 #ifdef AGR_CLIMB_PITCH
113 #define BUNGEE_TAKEOFF_PITCH AGR_CLIMB_PITCH
114 #else
115 #define BUNGEE_TAKEOFF_PITCH RadOfDeg(15.)
116 #endif
117 #endif
118 
123 };
124 
126 
127 static struct FloatVect2 init_point;
129 static struct FloatVect2 takeoff_dir;
130 static struct FloatVect3 bungee_point;
131 
132 static void compute_points_from_bungee(void)
133 {
134  // Store init point (current position, where the plane will be released)
136  // Compute unitary 2D vector (bungee_point - init_point) = takeoff direction
139  // Find throttle point (the point where the throttle line and launch line intersect)
140  // If TakeOff_Distance is positive, throttle point is after bungee point, before otherwise
143 }
144 
146 {
147  // Store bungee point (from WP id, altitude should be ground alt)
148  // FIXME use current alt instead ?
149  VECT3_ASSIGN(bungee_point, WaypointX(bungee_wp), WaypointY(bungee_wp), WaypointAlt(bungee_wp));
150 
151  // Compute other points
153 
154  // Enable Launch Status and turn kill throttle on
156  kill_throttle = 1;
157 
158  return FALSE;
159 }
160 
162 {
163  float cross = 0.;
164 
165  // Get current position
166  struct FloatVect2 pos;
168 
169  switch (CTakeoffStatus) {
170  case Launch:
171  // Recalculate lines if below min speed
174  }
175 
176  // Follow Launch Line with takeoff pitch and no throttle
179  // FIXME previously using altitude mode, maybe not wise without motors
180  //NavVerticalAltitudeMode(bungee_point.z + BUNGEE_TAKEOFF_HEIGHT, 0.);
182 
183  kill_throttle = 1;
184 
185  // Find out if UAV has crossed the line
186  VECT2_DIFF(pos, pos, throttle_point); // position local to throttle_point
187  cross = VECT2_DOT_PRODUCT(pos, takeoff_dir);
188 
191  kill_throttle = 0;
192  nav_init_stage();
193  } else {
194  // If not crossed stay in this status
195  break;
196  }
197  // Start throttle imidiatelly
198  case Throttle:
199  //Follow Launch Line
203  kill_throttle = 0;
204 
206 #if USE_AIRSPEED
207  && (stateGetAirspeed_f() > BUNGEE_TAKEOFF_AIRSPEED)
208 #endif
209  ) {
211  return FALSE;
212  } else {
213  return TRUE;
214  }
215  break;
216  default:
217  // Invalid status or Finished, end function
218  return FALSE;
219  }
220  return TRUE;
221 }
222 
static float stateGetHorizontalSpeedNorm_f(void)
Get norm of horizontal ground speed (float).
Definition: state.h:912
#define WaypointAlt(_wp)
Definition: common_nav.h:47
#define VECT2_SUM(_c, _a, _b)
Definition: pprz_algebra.h:85
#define VECT3_ASSIGN(_a, _x, _y, _z)
Definition: pprz_algebra.h:124
static void float_vect2_normalize(struct FloatVect2 *v)
normalize 2D vector in place
static float stateGetAirspeed_f(void)
Get airspeed (float).
Definition: state.h:1310
#define VECT2_DIFF(_c, _a, _b)
Definition: pprz_algebra.h:91
#define VECT2_ASSIGN(_a, _x, _y)
Definition: pprz_algebra.h:61
static struct EnuCoor_f * stateGetPositionEnu_f(void)
Get position in local ENU coordinates (float).
Definition: state.h:702
#define FALSE
Definition: std.h:5
#define TRUE
Definition: std.h:4
Paparazzi floating point algebra.
#define WaypointX(_wp)
Definition: common_nav.h:45
bool_t kill_throttle
Definition: autopilot.c:42
#define WaypointY(_wp)
Definition: common_nav.h:46
#define VECT2_SMUL(_vo, _vi, _s)
Definition: pprz_algebra.h:97
static struct UtmCoor_f * stateGetPositionUtm_f(void)
Get position in UTM coordinates (float).
Definition: state.h:675
unsigned char uint8_t
Definition: types.h:14
API to get/set the generic vehicle states.
#define MAX_PPRZ
Definition: paparazzi.h:8
#define VECT2_DOT_PRODUCT(_v1, _v2)
Definition: pprz_algebra.h:115
Fixedwing autopilot modes.