Paparazzi UAS  v5.12_stable-4-g9b43e9b
Paparazzi is a free software Unmanned Aircraft System.
 All Data Structures Namespaces Files Functions Variables Typedefs Enumerations Enumerator Macros Modules Pages
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 
static float stateGetHorizontalSpeedNorm_f(void)
Get norm of horizontal ground speed (float).
Definition: state.h:935
static struct FloatEulers * stateGetNedToBodyEulers_f(void)
Get vehicle body attitude euler angles (float).
Definition: state.h:1143
float alt
in meters (above WGS84 reference ellipsoid or above MSL)
static struct EnuCoor_f * stateGetPositionEnu_f(void)
Get position in local ENU coordinates (float).
Definition: state.h:719
#define FALSE
Definition: std.h:5
Fixed wing horizontal control.
float theta
in radians
#define TRUE
Definition: std.h:4
float x
in meters
Core autopilot interface common to all firmwares.
static struct UtmCoor_f * stateGetPositionUtm_f(void)
Get position in UTM coordinates (float).
Definition: state.h:692
API to get/set the generic vehicle states.
#define MAX_PPRZ
Definition: paparazzi.h:8
float y
in meters