Paparazzi UAS v7.1_unstable
Paparazzi is a free software Unmanned Aircraft System.
Loading...
Searching...
No Matches
nav_takeoff_and_landing_rotorcraft.c
Go to the documentation of this file.
1/*
2 * Copyright (C) 2023 Gautier Hattenberger <gautier.hattenberger@enac.fr>
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
26
29#include "generated/flight_plan.h"
32#include "autopilot.h"
33
34#ifndef NAV_TAKEOFF_CLIMB_SPEED
35#define NAV_TAKEOFF_CLIMB_SPEED NAV_CLIMB_VSPEED
36#endif
37
38#ifndef NAV_TAKEOFF_HEIGHT
39#define NAV_TAKEOFF_HEIGHT 2.f
40#endif
41
42#ifndef NAV_LANDING_DESCEND_SPEED
43#define NAV_LANDING_DESCEND_SPEED NAV_DESCEND_VSPEED
44#endif
45
46#ifndef NAV_LANDING_AF_HEIGHT
47#define NAV_LANDING_AF_HEIGHT 5.f
48#endif
49
50#ifndef NAV_LANDING_FLARE_HEIGHT
51#define NAV_LANDING_FLARE_HEIGHT 2.f
52#endif
53
54static struct nav_takeoff takeoff;
55static struct nav_landing landing;
56
57static bool nav_takeoff_run(void);
58static bool nav_land_run(void);
59
61
62#if USE_MISSION
64
65static bool nav_takeoff_mission(uint8_t nb UNUSED, float *params UNUSED, enum MissionRunFlag flag)
66{
67 if (flag == MissionInit && nb == 1) {
68 float height = params[0];
70 return nav_takeoff_from_here(height);
71 }
72 else if (flag == MissionRun) {
73 return nav_takeoff_run();
74 }
75 return false; // not a valid case
76}
77
78static bool nav_land_mission(uint8_t nb, float *params, enum MissionRunFlag flag)
79{
80 if (flag == MissionInit && nb == 1) {
81 float td_alt = params[0];
83 return nav_land_here(td_alt, 0.f);
84 } else if (flag == MissionInit && nb == 2) {
85 uint8_t td_id = (uint8_t)(params[0]);
86 uint8_t af_id = (uint8_t)(params[1]);
87 return nav_land_at_wp(td_id, af_id, 0.f);
88 } else if (flag == MissionInit && nb == 5) {
89 float td_alt = params[0];
90 float lat = params[1];
91 float lon = params[2];
92 float dir = params[3];
93 float dist = params[4];
94 return nav_land_at_loc(td_alt, lat, lon, dir, dist, 0.f);
95 } else if (flag == MissionRun) {
96 return nav_land_run();
97 }
98 return false; // not a valid case
99}
100
101#endif
102
104{
105 takeoff.status = NAV_TAKEOFF_INIT;
106 takeoff.climb_id = 0;
107 takeoff.timeout = false;
108 landing.status = NAV_LANDING_INIT;
109 landing.radius = 0.f; // no effect
110 landing.td_id = 0;
111 landing.af_id = 0;
112 landing.timeout = false;
113
115
116#if USE_MISSION
117 mission_register(nav_takeoff_mission, "TKOFF");
118 mission_register(nav_land_mission, "LAND");
119#endif
120}
121
123{
124 if (takeoff.timeout) {
125 takeoff.status = NAV_TAKEOFF_INIT;
126 }
127 if (landing.timeout) {
128 landing.status = NAV_LANDING_INIT;
129 }
130
131 // reset flag to true, set to false if run function is called during period
132 takeoff.timeout = true;
133 landing.timeout = true;
134}
135
136static bool nav_takeoff_run(void) {
137 static int start_motor_counter = 0;
138
139 switch (takeoff.status) {
140 case NAV_TAKEOFF_INIT:
142 start_motor_counter = 0;
143 break;
145 NavResurrect();
146 start_motor_counter++;
147 if (start_motor_counter == NAVIGATION_FREQUENCY) {
148 start_motor_counter = 0;
150 takeoff.status = NAV_TAKEOFF_CLIMB; // next step after 1s
151 } else {
152 takeoff.status = NAV_TAKEOFF_DONE; // failed to start motors, end now
153 }
154 }
155 break;
157 // call vertical climb from nav/guidance
159 NavGotoWaypoint(takeoff.climb_id);
161 if (stateGetPositionEnu_f()->z - takeoff.start_pos.z > takeoff.climb_pos.z) {
162 // end when takeoff height is reached
163 takeoff.status = NAV_TAKEOFF_DONE;
164 }
165 break;
166 case NAV_TAKEOFF_DONE:
167 default:
168 takeoff.status = NAV_TAKEOFF_INIT;
169 return false;
170 }
171 takeoff.timeout = false;
172 return true;
173}
174
175bool nav_takeoff_from_wp(uint8_t wp_id, float height)
176{
177 if (takeoff.status == NAV_TAKEOFF_INIT) {
178 takeoff.climb_id = wp_id;
180 takeoff.climb_pos = *waypoint_get_enu_f(wp_id);
181 takeoff.start_pos = *stateGetPositionEnu_f();
182 if (height < 0.f) {
183 takeoff.climb_pos.z = takeoff.start_pos.z + NAV_TAKEOFF_HEIGHT; // default value
184 } else {
185 takeoff.climb_pos.z = takeoff.start_pos.z + height;
186 }
187 }
188
189 return nav_takeoff_run();
190}
191
192bool nav_takeoff_from_loc(float lat UNUSED, float lon UNUSED, float height)
193{
194 return nav_takeoff_from_here(height);
195}
196
197bool nav_takeoff_from_here(float height)
198{
199 if (takeoff.status == NAV_TAKEOFF_INIT) {
200 takeoff.climb_id = 0; // use dummy hidden WP
201 takeoff.start_pos = *stateGetPositionEnu_f();
202 takeoff.climb_pos = takeoff.start_pos;
203 if (height < 0.f) {
204 takeoff.climb_pos.z += NAV_TAKEOFF_HEIGHT; // default value
205 } else {
206 takeoff.climb_pos.z += height;
207 }
208 waypoint_set_enu(0, &takeoff.climb_pos);
209 }
210
211 return nav_takeoff_run();
212}
213
214
215static bool nav_land_run(void)
216{
217 float dx, dy, dist2;
218 switch (landing.status) {
219 case NAV_LANDING_INIT:
221 break;
223 nav.nav_goto(&landing.af_pos);
224 NavVerticalAltitudeMode(landing.af_pos.z, 0.f);
225 if (nav.nav_approaching(&landing.af_pos, NULL, 0.f)) {
227 }
228 break;
230 // test if glide is needed or not
231 dx = landing.af_pos.x - landing.td_pos.x;
232 dy = landing.af_pos.y - landing.td_pos.y;
233 dist2 = dx*dx + dy*dy;
234 if (dist2 > 1.f) {
235 nav.nav_route(&landing.af_pos, &landing.td_pos);
236 nav_glide_points(&landing.af_pos, &landing.td_pos);
237 } else {
238 nav.nav_goto(&landing.td_pos);
240 }
241 if (nav.nav_approaching(&landing.td_pos, &landing.af_pos, 0.f)
242 || (stateGetPositionEnu_f()->z < landing.td_pos.z)) {
243 landing.status = NAV_LANDING_FLARE;
244 }
245 break;
247 nav.nav_goto(&landing.td_pos);
249 if (!nav_is_in_flight()) {
250 landing.status = NAV_LANDING_DONE;
251 }
252 break;
253 case NAV_LANDING_DONE:
254 default:
256 landing.status = NAV_LANDING_INIT;
257 return false;
258 }
259 landing.timeout = false;
260 return true;
261}
262
264{
265 if (landing.status == NAV_LANDING_INIT) {
266 landing.td_id = td_id;
267 landing.af_id = af_id;
269 landing.td_pos.z = landing.td_pos.z + NAV_LANDING_FLARE_HEIGHT;
271 landing.af_pos.z = landing.td_pos.z + NAV_LANDING_AF_HEIGHT;
272 }
273
274 return nav_land_run();
275}
276
277bool nav_land_at_loc(float td_alt, float lat, float lon, float dir, float dist, float radius UNUSED)
278{
279 if (landing.status == NAV_LANDING_INIT) {
280 landing.td_id = 0;
281 landing.af_id = 0;
282 struct LlaCoor_f lla = { RadOfDeg(lat), RadOfDeg(lon), stateGetLlaOrigin_f().alt + td_alt };
284 landing.td_pos.z = landing.td_pos.z + NAV_LANDING_FLARE_HEIGHT;
285 landing.af_pos.x = landing.td_pos.x + dist * sinf(RadOfDeg(dir));
286 landing.af_pos.y = landing.td_pos.y + dist * cosf(RadOfDeg(dir));
287 landing.af_pos.z = landing.td_pos.z + NAV_LANDING_AF_HEIGHT;
288 }
289
290 return nav_land_run();
291}
292
293bool nav_land_here(float td_alt, float radius UNUSED)
294{
295 if (landing.status == NAV_LANDING_INIT) {
296 landing.td_id = 0;
297 landing.af_id = 0;
298 landing.td_pos = *stateGetPositionEnu_f();
299 landing.td_pos.z = td_alt + NAV_LANDING_FLARE_HEIGHT;
300 landing.af_pos = landing.td_pos;
301 landing.af_pos.z = landing.td_pos.z + NAV_LANDING_AF_HEIGHT;
302 }
303
304 return nav_land_run();
305}
306
void autopilot_set_in_flight(bool in_flight)
set in_flight flag
Definition autopilot.c:341
bool autopilot_get_motors_on(void)
get motors status
Definition autopilot.c:303
Core autopilot interface common to all firmwares.
#define UNUSED(x)
static struct LtpDef_f * stateGetNedOrigin_f(void)
Get the coordinate NED frame origin (float).
Definition state.h:577
struct LlaCoor_f stateGetLlaOrigin_f(void)
Get the LLA position of the frame origin (float).
Definition state.c:149
static struct EnuCoor_f * stateGetPositionEnu_f(void)
Get position in local ENU coordinates (float).
Definition state.h:859
bool mission_register(mission_custom_cb cb, char *type)
Register a new navigation or action callback function.
mission planner library
MissionRunFlag
@ MissionInit
first exec
@ MissionRun
normal run
void waypoint_set_here_2d(uint8_t wp_id)
set waypoint to current horizontal location without modifying altitude
Definition waypoints.c:320
struct EnuCoor_f * waypoint_get_enu_f(uint8_t wp_id)
Get ENU coordinates (float).
Definition waypoints.c:406
void waypoint_set_enu(uint8_t wp_id, struct EnuCoor_f *enu)
Set local ENU waypoint coordinates.
Definition waypoints.c:204
#define NavVerticalClimbMode(_climb)
Set the vertical mode to climb control with the specified climb setpoint.
Definition nav.h:198
#define NavGotoWaypoint(_wp)
Definition nav.h:96
#define NavResurrect()
Definition nav.h:227
#define NavKillThrottle()
Definition nav.h:226
#define NAVIGATION_FREQUENCY
Default fixedwing navigation frequency.
Definition nav.h:49
#define NavVerticalAltitudeMode(_alt, _pre_climb)
Set the vertical mode to altitude control with the specified altitude setpoint and climb pre-command.
Definition nav.h:191
uint8_t td_id
touch down wp id
float nav_takeoff_direction
Takeoff direction in range [0-360] (deg) set to flight plan QFU by default.
@ NAV_LANDING_REACH_AF
@ NAV_LANDING_DESCENT
@ NAV_LANDING_FLARE
float radius
circle radius for fixedwing landing
uint8_t af_id
start of descent wp id
@ NAV_TAKEOFF_CLIMB
@ NAV_TAKEOFF_START_MOTOR
Structure for landing.
Structure for takeoff.
#define NAV_LANDING_DESCEND_SPEED
#define NAV_LANDING_FLARE_HEIGHT
static struct nav_takeoff takeoff
#define NAV_TAKEOFF_HEIGHT
#define NAV_LANDING_AF_HEIGHT
bool nav_takeoff_from_wp(uint8_t wp_id, float height)
Takeoff from a waypoint.
bool nav_land_at_loc(float td_alt, float lat, float lon, float dir, float dist, float radius UNUSED)
static bool nav_land_run(void)
static bool nav_takeoff_run(void)
bool nav_land_here(float td_alt, float radius UNUSED)
void nav_takeoff_and_landing_init(void)
Init function.
#define NAV_TAKEOFF_CLIMB_SPEED
bool nav_takeoff_from_loc(float lat UNUSED, float lon UNUSED, float height)
bool nav_takeoff_from_here(float height)
Takeoff from current location.
void nav_takeoff_and_landing_periodic(void)
Periodic timeout check function.
bool nav_land_at_wp(uint8_t td_id, uint8_t af_id, float radius UNUSED)
bool landing
void enu_of_lla_point_f(struct EnuCoor_f *enu, struct LtpDef_f *def, struct LlaCoor_f *lla)
Paparazzi floating point math for geodetic calculations.
float alt
in meters (normally above WGS84 reference ellipsoid)
float lon
in radians
float lat
in radians
vector in Latitude, Longitude and Altitude
bool nav_is_in_flight(void)
Definition navigation.c:277
struct RotorcraftNavigation nav
Definition navigation.c:51
void nav_glide_points(struct EnuCoor_f *start_point, struct EnuCoor_f *end_point)
Definition navigation.c:282
Rotorcraft navigation functions.
static const float dir[]
unsigned char uint8_t
Typedef defining 8 bit unsigned char type.