Paparazzi UAS  v7.0_unstable
Paparazzi is a free software Unmanned Aircraft System.
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 
122  Finished
123 };
124 
125 static enum TakeoffStatus CTakeoffStatus;
126 
127 static struct FloatVect2 init_point;
128 static struct FloatVect2 throttle_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
199  autopilot.launch = true; // turn on motor
202 
204 #if USE_AIRSPEED
205  && (stateGetAirspeed_f() > BUNGEE_TAKEOFF_AIRSPEED)
206 #endif
207  ) {
209  return false;
210  } else {
211  return true;
212  }
213  break;
214  default:
215  // Invalid status or Finished, end function
216  return false;
217  }
218  return true;
219 }
220 
struct pprz_autopilot autopilot
Global autopilot structure.
Definition: autopilot.c:49
void autopilot_set_kill_throttle(bool kill)
set kill throttle
Definition: autopilot.c:297
Core autopilot interface common to all firmwares.
bool launch
request launch
Definition: autopilot.h:71
#define WaypointX(_wp)
Definition: common_nav.h:45
#define WaypointY(_wp)
Definition: common_nav.h:46
#define USE_AIRSPEED
Definition: disco.h:112
static void float_vect2_normalize(struct FloatVect2 *v)
normalize 2D vector in place
#define VECT2_SMUL(_vo, _vi, _s)
Definition: pprz_algebra.h:98
#define VECT2_DIFF(_c, _a, _b)
Definition: pprz_algebra.h:92
#define VECT2_SUM(_c, _a, _b)
Definition: pprz_algebra.h:86
#define VECT2_DOT_PRODUCT(_v1, _v2)
Definition: pprz_algebra.h:116
#define VECT3_ASSIGN(_a, _x, _y, _z)
Definition: pprz_algebra.h:125
#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
static struct UtmCoor_f * stateGetPositionUtm_f(void)
Get position in UTM coordinates (float).
Definition: state.h:692
static float stateGetHorizontalSpeedNorm_f(void)
Get norm of horizontal ground speed (float).
Definition: state.h:935
static float stateGetAirspeed_f(void)
Get airspeed (float).
Definition: state.h:1407
void nav_init_stage(void)
needs to be implemented by fixedwing and rotorcraft seperately
Definition: nav.c:92
void nav_route_xy(float last_wp_x, float last_wp_y, float wp_x, float wp_y)
Computes the carrot position along the desired segment.
Definition: nav.c:382
Fixedwing Navigation library.
#define NavVerticalThrottleMode(_throttle)
Set the vertical mode to fixed throttle with the specified setpoint.
Definition: nav.h:204
#define NavVerticalAutoThrottleMode(_pitch)
Set the climb control to auto-throttle with the specified pitch pre-command.
Definition: nav.h:177
static struct FloatVect2 throttle_point
bool nav_bungee_takeoff_run(void)
Bungee takeoff run function.
static void compute_points_from_bungee(void)
TakeoffStatus
@ Throttle
@ Finished
@ Launch
#define BUNGEE_TAKEOFF_THROTTLE
#define BUNGEE_TAKEOFF_PITCH
#define BUNGEE_TAKEOFF_DISTANCE
void nav_bungee_takeoff_setup(uint8_t bungee_wp)
Initialization function.
#define BUNGEE_TAKEOFF_MIN_SPEED
static struct FloatVect2 init_point
static struct FloatVect2 takeoff_dir
static enum TakeoffStatus CTakeoffStatus
static struct FloatVect3 bungee_point
#define BUNGEE_TAKEOFF_HEIGHT
Takeoff functions for bungee takeoff.
#define MAX_PPRZ
Definition: paparazzi.h:8
Paparazzi floating point algebra.
API to get/set the generic vehicle states.
unsigned char uint8_t
Typedef defining 8 bit unsigned char type.
Definition: vl53l1_types.h:98