Paparazzi UAS  v5.15_devel-81-gd13dafb
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"
65 #include "autopilot.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 is current hmsl (e.g. ground alt))
148  VECT3_ASSIGN(bungee_point, WaypointX(bungee_wp), WaypointY(bungee_wp), stateGetPositionUtm_f()->alt);
149 
150  // Compute other points
152 
153  // Enable Launch Status and turn kill throttle on
156 }
157 
159 {
160  float cross = 0.;
161 
162  // Get current position
163  struct FloatVect2 pos;
165 
166  switch (CTakeoffStatus) {
167  case Launch:
168  // Recalculate lines if below min speed
171  }
172 
173  // Follow Launch Line with takeoff pitch and no throttle
176  // FIXME previously using altitude mode, maybe not wise without motors
177  //NavVerticalAltitudeMode(bungee_point.z + BUNGEE_TAKEOFF_HEIGHT, 0.);
179 
181 
182  // Find out if UAV has crossed the line
183  VECT2_DIFF(pos, pos, throttle_point); // position local to throttle_point
184  cross = VECT2_DOT_PRODUCT(pos, takeoff_dir);
185 
189  nav_init_stage();
190  } else {
191  // If not crossed stay in this status
192  break;
193  }
194  // Start throttle imidiatelly
195  case Throttle:
196  //Follow Launch Line
201 
203 #if USE_AIRSPEED
204  && (stateGetAirspeed_f() > BUNGEE_TAKEOFF_AIRSPEED)
205 #endif
206  ) {
208  return false;
209  } else {
210  return true;
211  }
212  break;
213  default:
214  // Invalid status or Finished, end function
215  return false;
216  }
217  return true;
218 }
219 
static float stateGetHorizontalSpeedNorm_f(void)
Get norm of horizontal ground speed (float).
Definition: state.h:935
#define VECT2_SUM(_c, _a, _b)
Definition: pprz_algebra.h:86
#define VECT3_ASSIGN(_a, _x, _y, _z)
Definition: pprz_algebra.h:125
static void float_vect2_normalize(struct FloatVect2 *v)
normalize 2D vector in place
static float stateGetAirspeed_f(void)
Get airspeed (float).
Definition: state.h:1407
#define VECT2_DIFF(_c, _a, _b)
Definition: pprz_algebra.h:92
#define VECT2_ASSIGN(_a, _x, _y)
Definition: pprz_algebra.h:62
static struct EnuCoor_f * stateGetPositionEnu_f(void)
Get position in local ENU coordinates (float).
Definition: state.h:719
Paparazzi floating point algebra.
#define WaypointX(_wp)
Definition: common_nav.h:45
void autopilot_set_kill_throttle(bool kill)
set kill throttle
Definition: autopilot.c:219
#define WaypointY(_wp)
Definition: common_nav.h:46
#define VECT2_SMUL(_vo, _vi, _s)
Definition: pprz_algebra.h:98
Core autopilot interface common to all firmwares.
static struct UtmCoor_f * stateGetPositionUtm_f(void)
Get position in UTM coordinates (float).
Definition: state.h:692
#define USE_AIRSPEED
Definition: disco.h:112
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:116