Paparazzi UAS v7.0_unstable
Paparazzi is a free software Unmanned Aircraft System.
All Data Structures Namespaces Files Functions Variables Typedefs Enumerations Enumerator Macros Modules Pages
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
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{
106 takeoff.climb_id = 0;
107 takeoff.timeout = false;
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
119#endif
120}
121
123{
124 if (takeoff.timeout) {
126 }
127 if (landing.timeout) {
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:
143 break;
145 NavResurrect();
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
162 // end when takeoff height is reached
164 }
165 break;
166 case NAV_TAKEOFF_DONE:
167 default:
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{
178 takeoff.climb_id = wp_id;
182 if (height < 0.f) {
184 } else {
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{
200 takeoff.climb_id = 0; // use dummy hidden WP
203 if (height < 0.f) {
204 takeoff.climb_pos.z += NAV_TAKEOFF_HEIGHT; // default value
205 } else {
206 takeoff.climb_pos.z += height;
207 }
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;
225 if (nav.nav_approaching(&landing.af_pos, NULL, 0.f)) {
227 }
228 break;
230 // test if glide is needed or not
233 dist2 = dx*dx + dy*dy;
234 if (dist2 > 1.f) {
237 } else {
240 }
244 }
245 break;
249 if (!nav_is_in_flight()) {
251 }
252 break;
253 case NAV_LANDING_DONE:
254 default:
257 return false;
258 }
259 landing.timeout = false;
260 return true;
261}
262
276
277bool nav_land_at_loc(float td_alt, float lat, float lon, float dir, float dist, float radius UNUSED)
278{
280 landing.td_id = 0;
281 landing.af_id = 0;
288 }
289
290 return nav_land_run();
291}
292
306
void autopilot_set_in_flight(bool in_flight)
set in_flight flag
Definition autopilot.c:330
bool autopilot_get_motors_on(void)
get motors status
Definition autopilot.c:295
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:566
struct LlaCoor_f stateGetLlaOrigin_f(void)
Get the LLA position of the frame origin (float)
Definition state.c:143
static struct EnuCoor_f * stateGetPositionEnu_f(void)
Get position in local ENU coordinates (float).
Definition state.h:848
uint16_t foo
Definition main_demo5.c:58
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:298
struct EnuCoor_f * waypoint_get_enu_f(uint8_t wp_id)
Get ENU coordinates (float)
Definition waypoints.c:387
void waypoint_set_enu(uint8_t wp_id, struct EnuCoor_f *enu)
Set local ENU waypoint coordinates.
Definition waypoints.c:177
#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 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
enum nav_takeoff_status status
current step
uint8_t td_id
touch down wp id
enum nav_landing_status status
current step
struct EnuCoor_f af_pos
start of descent point
@ NAV_LANDING_REACH_AF
@ NAV_LANDING_DESCENT
@ NAV_LANDING_FLARE
float radius
circle radius for fixedwing landing
bool timeout
true if status should be set to init
struct EnuCoor_f td_pos
touch down point
uint8_t af_id
start of descent wp id
bool timeout
true if status should be set to init
struct EnuCoor_f start_pos
start position
uint8_t climb_id
climb waypoint id
struct EnuCoor_f climb_pos
climb point
@ NAV_TAKEOFF_CLIMB
@ NAV_TAKEOFF_START_MOTOR
Structure for landing.
Structure for takeoff.
#define NAV_LANDING_DESCEND_SPEED
#define NAV_LANDING_FLARE_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 struct nav_landing landing
static bool nav_land_run(void)
static bool nav_takeoff_run(void)
float nav_takeoff_direction
Takeoff direction in range [0-360] (deg) set to flight plan QFU by default.
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.
static struct nav_takeoff takeoff
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)
#define NAV_LANDING_AF_HEIGHT
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 y
in meters
float x
in meters
float lon
in radians
float lat
in radians
float z
in meters
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.
navigation_approaching nav_approaching
Definition navigation.h:153
navigation_goto nav_goto
Definition navigation.h:151
static void NavResurrect(void)
Definition navigation.h:233
navigation_route nav_route
Definition navigation.h:152
static const float dir[]
unsigned char uint8_t
Typedef defining 8 bit unsigned char type.