Paparazzi UAS  v6.2_unstable
Paparazzi is a free software Unmanned Aircraft System.
nav_launcher.c
Go to the documentation of this file.
1 /*
2  *
3  * Copyright (C) 2016, Michal Podhradsky
4  *
5  * This file is part of paparazzi.
6  *
7  * paparazzi is free software; you can redistribute it and/or modify
8  * it under the terms of the GNU General Public License as published by
9  * the Free Software Foundation; either version 2, or (at your option)
10  * any later version.
11  *
12  * paparazzi is distributed in the hope that it will be useful,
13  * but WITHOUT ANY WARRANTY; without even the implied warranty of
14  * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the
15  * GNU General Public License for more details.
16  *
17  * You should have received a copy of the GNU General Public License
18  * along with paparazzi; see the file COPYING. If not, write to
19  * the Free Software Foundation, 59 Temple Place - Suite 330,
20  * Boston, MA 02111-1307, USA.
21  *
22  */
23 
47 #include "generated/airframe.h"
48 #include "state.h"
50 #include "autopilot.h"
53 
54 #ifndef LAUNCHER_TAKEOFF_PITCH //> desired takeoff pitch [rad]
55 #define LAUNCHER_TAKEOFF_PITCH 0.23
56 #endif
57 
58 #ifndef LAUNCHER_TAKEOFF_HEIGHT //> desired height AGL [m]
59 #define LAUNCHER_TAKEOFF_HEIGHT 80
60 #endif
61 
62 #ifndef LAUNCHER_TAKEOFF_DISTANCE //> minimal distance from the takeoff point [m]
63 #define LAUNCHER_TAKEOFF_DISTANCE 30
64 #endif
65 
66 #ifndef LAUNCHER_TAKEOFF_MIN_SPEED_LINE //> minimal takeoff ground speed [m/s] to switch into line following
67 #define LAUNCHER_TAKEOFF_MIN_SPEED_LINE 5
68 #endif
69 
70 #ifndef LAUNCHER_TAKEOFF_MIN_SPEED_CIRCLE //> mininmal takeoff ground speed [m/s] to switch into circle climb
71 #define LAUNCHER_TAKEOFF_MIN_SPEED_CIRCLE 8
72 #endif
73 
74 #ifndef LAUNCHER_TAKEOFF_CIRCLE_ALT //> desired circle AGL [m]
75 #define LAUNCHER_TAKEOFF_CIRCLE_ALT 200
76 #endif
77 
78 #ifndef LAUNCHER_TAKEOFF_CIRCLE_RADIUS //> desired circle radius [m]
79 #define LAUNCHER_TAKEOFF_CIRCLE_RADIUS 200
80 #endif
81 
82 #ifndef LAUNCHER_TAKEOFF_MAX_CIRCLE_DISTANCE //> max distance from the takeoff point [m] before the plane circles up
83 #define LAUNCHER_TAKEOFF_MAX_CIRCLE_DISTANCE 800
84 #endif
85 
86 #ifndef LAUNCHER_TAKEOFF_HEIGHT_THRESHOLD //> height threshold [m] before switching modes
87 #define LAUNCHER_TAKEOFF_HEIGHT_THRESHOLD 10
88 #endif
89 
90 struct Point2D
91 {
92  float x;
93  float y;
94 };
96 {
98 };
100 static float launch_x;
101 static float launch_y;
102 static float launch_alt;
103 static float launch_pitch;
104 static float launch_time;
105 static struct Point2D launch_circle;
106 static float launch_circle_alt;
107 
108 static float launch_line_x;
109 static float launch_line_y;
110 
112 {
117  launch_time = 0;
118 
121 
123 }
124 
126 {
127  //Find distance from laucher
128  float dist_x = stateGetPositionEnu_f()->x - launch_x;
129  float dist_y = stateGetPositionEnu_f()->y - launch_y;
130  float launch_dist = sqrtf(dist_x * dist_x + dist_y * dist_y);
131  if (launch_dist <= 0.01) {
132  launch_dist = 0.01;
133  }
134 
135  switch (CLaunch_Status) {
136  case L_Pitch_Nav:
137  //Follow Launch Line
141  NavAttitude(0);
142 
143 
144  //If the plane has been launched and has traveled for more than a specified distance, switch to line nav
146  if (launch_dist > LAUNCHER_TAKEOFF_DISTANCE) {
150  }
151  }
152 
153  break;
154  case L_Line_Nav:
155  //Follow Launch Line
160 
161  //If the aircraft is above a specific alt, greater than a specific speed or too far away, circle up
162  if (((stateGetPositionUtm_f()->alt
166  || (launch_dist > LAUNCHER_TAKEOFF_MAX_CIRCLE_DISTANCE)) {
168 
169  //Find position of circle
170  float x_1 = dist_x / launch_dist;
171  float y_1 = dist_y / launch_dist;
172 
177  }
178  break;
179  case L_CircleUp:
184 
185  if (stateGetPositionUtm_f()->alt
188  return FALSE;
189  }
190  break;
191  default:
192  break;
193  }
194  return TRUE;
195 }
196 
MAX_PPRZ
#define MAX_PPRZ
Definition: paparazzi.h:8
LAUNCHER_TAKEOFF_MIN_SPEED_LINE
#define LAUNCHER_TAKEOFF_MIN_SPEED_LINE
Definition: nav_launcher.c:67
LAUNCHER_TAKEOFF_HEIGHT_THRESHOLD
#define LAUNCHER_TAKEOFF_HEIGHT_THRESHOLD
Definition: nav_launcher.c:87
stateGetPositionEnu_f
static struct EnuCoor_f * stateGetPositionEnu_f(void)
Get position in local ENU coordinates (float).
Definition: state.h:719
LAUNCHER_TAKEOFF_HEIGHT
#define LAUNCHER_TAKEOFF_HEIGHT
Definition: nav_launcher.c:59
L_Line_Nav
@ L_Line_Nav
Definition: nav_launcher.c:97
stateGetNedToBodyEulers_f
static struct FloatEulers * stateGetNedToBodyEulers_f(void)
Get vehicle body attitude euler angles (float).
Definition: state.h:1143
nav_circle_XY
void nav_circle_XY(float x, float y, float radius)
Navigates around (x, y).
Definition: nav.c:111
stabilization_attitude.h
NavVerticalThrottleMode
#define NavVerticalThrottleMode(_throttle)
Set the vertical mode to fixed throttle with the specified setpoint.
Definition: nav.h:207
LAUNCHER_TAKEOFF_PITCH
#define LAUNCHER_TAKEOFF_PITCH
Definition: nav_launcher.c:55
Point2D::x
float x
Definition: nav_launcher.c:92
stateGetPositionUtm_f
static struct UtmCoor_f * stateGetPositionUtm_f(void)
Get position in UTM coordinates (float).
Definition: state.h:692
launch_pitch
static float launch_pitch
Definition: nav_launcher.c:103
nav_launcher_setup
void nav_launcher_setup(void)
Definition: nav_launcher.c:111
LAUNCHER_TAKEOFF_DISTANCE
#define LAUNCHER_TAKEOFF_DISTANCE
Definition: nav_launcher.c:63
EnuCoor_f::y
float y
in meters
Definition: pprz_geodetic_float.h:74
NavVerticalAutoThrottleMode
#define NavVerticalAutoThrottleMode(_pitch)
Set the climb control to auto-throttle with the specified pitch pre-command.
Definition: nav.h:180
CLaunch_Status
static enum Launch_Status CLaunch_Status
Definition: nav_launcher.c:99
FloatEulers::theta
float theta
in radians
Definition: pprz_algebra_float.h:86
launch_line_x
static float launch_line_x
Definition: nav_launcher.c:108
Launch_Status
Launch_Status
Definition: nav_launcher.c:95
nav_route_xy
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:383
L_CircleUp
@ L_CircleUp
Definition: nav_launcher.c:97
nav_launcher_run
bool nav_launcher_run(void)
Definition: nav_launcher.c:125
UtmCoor_f::alt
float alt
in meters (above WGS84 reference ellipsoid or above MSL)
Definition: pprz_geodetic_float.h:84
launch_line_y
static float launch_line_y
Definition: nav_launcher.c:109
launch_circle_alt
static float launch_circle_alt
Definition: nav_launcher.c:106
launch_y
static float launch_y
Definition: nav_launcher.c:101
L_Finished
@ L_Finished
Definition: nav_launcher.c:97
autopilot.h
LAUNCHER_TAKEOFF_MIN_SPEED_CIRCLE
#define LAUNCHER_TAKEOFF_MIN_SPEED_CIRCLE
Definition: nav_launcher.c:71
LAUNCHER_TAKEOFF_CIRCLE_ALT
#define LAUNCHER_TAKEOFF_CIRCLE_ALT
Definition: nav_launcher.c:75
nav.h
Point2D
Definition: nav_launcher.c:90
stateGetHorizontalSpeedNorm_f
static float stateGetHorizontalSpeedNorm_f(void)
Get norm of horizontal ground speed (float).
Definition: state.h:935
LAUNCHER_TAKEOFF_MAX_CIRCLE_DISTANCE
#define LAUNCHER_TAKEOFF_MAX_CIRCLE_DISTANCE
Definition: nav_launcher.c:83
launch_time
static float launch_time
Definition: nav_launcher.c:104
launch_alt
static float launch_alt
Definition: nav_launcher.c:102
launch_circle
static struct Point2D launch_circle
Definition: nav_launcher.c:105
Point2D::y
float y
Definition: nav_launcher.c:93
launch_x
static float launch_x
Definition: nav_launcher.c:100
L_Pitch_Nav
@ L_Pitch_Nav
Definition: nav_launcher.c:97
EnuCoor_f::x
float x
in meters
Definition: pprz_geodetic_float.h:73
state.h
FALSE
#define FALSE
Definition: std.h:5
NavAttitude
#define NavAttitude(_roll)
Definition: nav.h:217
TRUE
#define TRUE
Definition: std.h:4
LAUNCHER_TAKEOFF_CIRCLE_RADIUS
#define LAUNCHER_TAKEOFF_CIRCLE_RADIUS
Definition: nav_launcher.c:79
NavVerticalAltitudeMode
#define NavVerticalAltitudeMode(_alt, _pre_climb)
Set the vertical mode to altitude control with the specified altitude setpoint and climb pre-command.
Definition: nav.h:194
nav_launcher.h
Pneumatic launcher system See video of the system: https://www.youtube.com/watch?v=qc1uwH-8Dbw Launch...