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_fw.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_PITCH
35#define NAV_TAKEOFF_PITCH 15.f
36#endif
37
38#ifndef NAV_TAKEOFF_THROTTLE
39#define NAV_TAKEOFF_THROTTLE 1.f
40#endif
41
42#ifndef NAV_TAKEOFF_HEIGHT
43#define NAV_TAKEOFF_HEIGHT 20.f
44#endif
45
46#ifndef NAV_TAKEOFF_DIST
47#define NAV_TAKEOFF_DIST 200.f
48#endif
49
50#ifndef NAV_TAKEOFF_AUTO_LAUNCH
51#define NAV_TAKEOFF_AUTO_LAUNCH true
52#endif
53
54#ifndef NAV_LANDING_DESCEND_SPEED
55#define NAV_LANDING_DESCEND_SPEED -1.f
56#endif
57
58#ifndef NAV_LANDING_AF_HEIGHT
59#define NAV_LANDING_AF_HEIGHT 30.f
60#endif
61
62#ifndef NAV_LANDING_FLARE_HEIGHT
63#define NAV_LANDING_FLARE_HEIGHT 10.f
64#endif
65
66static struct nav_takeoff takeoff;
67static struct nav_landing landing;
68
69static bool nav_takeoff_run(void);
70static bool nav_land_run(void);
71
73
74#if USE_MISSION
76
77static bool nav_takeoff_mission(uint8_t nb UNUSED, float *params UNUSED, enum MissionRunFlag flag)
78{
79 if (flag == MissionInit && nb == 1) {
80 float height = params[0];
82 return nav_takeoff_from_here(height);
83 }
84 else if (flag == MissionRun) {
85 return nav_takeoff_run();
86 }
87 return false; // not a valid case
88}
89
90static bool nav_land_mission(uint8_t nb, float *params, enum MissionRunFlag flag)
91{
92 if (flag == MissionInit && nb == 2) {
93 float td_alt = params[0];
94 float radius = params[1];
97 } else if (flag == MissionInit && nb == 3) {
98 uint8_t td_id = (uint8_t)(params[0]);
99 uint8_t af_id = (uint8_t)(params[1]);
100 float radius = params[2];
102 } else if (flag == MissionInit && nb == 6) {
103 float td_alt = params[0];
104 float lat = params[1];
105 float lon = params[2];
106 float dir = params[3];
107 float dist = params[4];
108 float radius = params[5];
109 return nav_land_at_loc(td_alt, lat, lon, dir, dist, radius);
110 } else if (flag == MissionRun) {
111 return nav_land_run();
112 }
113 return false; // not a valid case
114}
115
116#endif
117
119{
121 takeoff.climb_id = 0;
122 takeoff.timeout = false;
124 landing.td_id = 0;
125 landing.af_id = 0;
126 landing.timeout = false;
127
129
130#if USE_MISSION
133#endif
134}
135
137{
138 if (takeoff.timeout) {
140 }
141 if (landing.timeout) {
143 }
144
145 // reset flag to true, set to false if run function is called during period
146 takeoff.timeout = true;
147 landing.timeout = true;
148}
149
150static bool nav_takeoff_run(void) {
151 static int start_motor_counter = 0;
152
153 switch (takeoff.status) {
154 case NAV_TAKEOFF_INIT:
157 break;
164#if NAV_TAKEOFF_AUTO_LAUNCH
165 autopilot.launch = true;
166#endif
167 takeoff.status = NAV_TAKEOFF_CLIMB; // next step after 1s
168 } else {
169 takeoff.status = NAV_TAKEOFF_DONE; // failed to start motors, end now
170 }
171 }
172 break;
174 // climb towards point at specified pitch and throttle
180 // end when climb point or target alt is reached
182 }
183 break;
184 default:
186 return false;
187 }
188 takeoff.timeout = false;
189 return true;
190}
191
192bool nav_takeoff_from_wp(uint8_t wp_id, float height)
193{
196 takeoff.climb_id = wp_id;
197 takeoff.climb_pos.x = WaypointX(wp_id);
198 takeoff.climb_pos.y = WaypointY(wp_id);
199 if (height < 0.f) {
201 } else {
203 }
204 }
205
206 return nav_takeoff_run();
207}
208
209bool nav_takeoff_from_loc(float lat, float lon, float height)
210{
213 float alt = stateGetPositionLla_f()->alt;
214 if (height < 0.f) {
215 alt += NAV_TAKEOFF_HEIGHT;
216 } else {
217 alt += height;
218 }
219 struct LlaCoor_f lla = { RadOfDeg(lat), RadOfDeg(lon), alt };
220 struct UtmCoor_f utm;
221 utm_of_lla_f(&utm, &lla);
223 }
224 return nav_takeoff_run();
225}
226
243
244
245static bool nav_land_run(void)
246{
247 static float baseleg_out_qdr = 0.f;
248 float qdr = 0.f;
249
250 // test if glide is needed or not
251 float dx = landing.td_pos.x - landing.af_pos.x;
252 float dy = landing.td_pos.y - landing.af_pos.y;
253 float dist = sqrtf(dx*dx + dy*dy);
254 bool glide = false;
255 float x_unit = 0.f;
256 float y_unit = 0.f;
257 if (dist > 1.f) {
258 glide = true;
259 x_unit = dx / dist;
260 y_unit = dy / dist;
261 }
262
263 switch (landing.status) {
264 case NAV_LANDING_INIT:
265 if (glide) {
266 // compute baseleg circle center, store in dummy WP 0
271 if (landing.radius < 0.f) {
273 }
274 } else {
275 // turn around AF point
279 baseleg_out_qdr = 0.f;
280 }
282 break;
287 if (NavCircleCount() > 0.25 && (fabsf(GetPosAlt() - WaypointAlt(0)) < 10.f)) {
288 if (glide) {
289 // if final glide, direction should be correct
291 NormCourseRad(qdr);
294 }
295 } else {
296 // no final glide, start from correct altitude
298 }
299 }
300 break;
302 if (glide) {
309 }
310 } else {
316 }
317 }
318 break;
322 if (glide) {
326 }
327 } else {
329 // stay on circle, no end
330 }
331 break;
332 case NAV_LANDING_DONE:
333 default:
336 return false;
337 }
338 landing.timeout = false;
339 return true;
340}
341
342bool nav_land_at_wp(uint8_t td_id, uint8_t af_id, float radius)
343{
345 landing.td_id = td_id;
346 landing.af_id = af_id;
347 landing.td_pos.x = WaypointX(td_id);
348 landing.td_pos.y = WaypointY(td_id);
349 landing.td_pos.z = WaypointAlt(td_id) - GetAltRef();
350 landing.af_pos.x = WaypointX(af_id);
351 landing.af_pos.y = WaypointY(af_id);
353 landing.radius = radius;
354 }
355
356 return nav_land_run();
357}
358
359bool nav_land_at_loc(float td_alt, float lat, float lon, float dir, float dist, float radius)
360{
362 landing.td_id = 0;
363 landing.af_id = 0;
364 struct LlaCoor_f lla = { RadOfDeg(lat), RadOfDeg(lon), GetAltRef() + td_alt };
365 struct UtmCoor_f utm = {0};
366 utm_of_lla_f(&utm, &lla);
371 landing.radius = radius;
372 }
373
374 return nav_land_run();
375}
376
377bool nav_land_here(float td_alt, float radius)
378{
380 landing.td_id = 0;
381 landing.af_id = 0;
386 landing.radius = radius;
387 }
388
389 return nav_land_run();
390}
391
392
bool autopilot_set_motors_on(bool motors_on)
turn motors on/off, eventually depending of the current mode set kill_throttle accordingly FIXME is i...
Definition autopilot.c:250
struct pprz_autopilot autopilot
Global autopilot structure.
Definition autopilot.c:49
bool autopilot_get_motors_on(void)
get motors status
Definition autopilot.c:295
Core autopilot interface common to all firmwares.
bool launch
request launch
Definition autopilot.h:71
#define UNUSED(x)
struct point waypoints[NB_WAYPOINT]
size == nb_waypoint, waypoint 0 is a dummy waypoint
Definition common_nav.c:44
#define WaypointX(_wp)
Definition common_nav.h:45
#define WaypointAlt(_wp)
waypoint altitude in m above MSL
Definition common_nav.h:48
float y
Definition common_nav.h:41
float a
Definition common_nav.h:42
float x
Definition common_nav.h:40
#define WaypointY(_wp)
Definition common_nav.h:46
Vertical control for fixed wing vehicles.
#define ENU_OF_UTM_DIFF(_pos, _utm1, _utm2)
static struct LlaCoor_f * stateGetPositionLla_f(void)
Get position in LLA coordinates (float).
Definition state.h:857
static struct UtmCoor_f * stateGetUtmOrigin_f(void)
Get the coordinate UTM frame origin (int)
Definition state.h:576
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
float baseleg_out_qdr
Definition nav.c:219
bool nav_approaching_xy(float x, float y, float from_x, float from_y, float approaching_time)
Decide if the UAV is approaching the current waypoint.
Definition nav.c:325
float nav_circle_trigo_qdr
Definition nav.c:55
void nav_glide_alt(float start_alt, float final_alt)
Definition nav.c:163
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 GetPosAlt()
Get current altitude above MSL.
Definition nav.h:233
#define NavGotoPoint(_point)
Definition nav.h:101
#define GetAltRef()
Get current altitude reference for local coordinates.
Definition nav.h:242
#define NavVerticalClimbMode(_climb)
Set the vertical mode to climb control with the specified climb setpoint.
Definition nav.h:198
#define NavCircleWaypoint(wp, radius)
Definition nav.h:151
#define NavVerticalThrottleMode(_throttle)
Set the vertical mode to fixed throttle with the specified setpoint.
Definition nav.h:204
#define NavCircleCount()
Definition nav.h:155
#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
#define NavVerticalAutoThrottleMode(_pitch)
Set the climb control to auto-throttle with the specified pitch pre-command.
Definition nav.h:177
#define CloseRadAngles(_c1, _c2)
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)
Land at lat long location.
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_at_wp(uint8_t td_id, uint8_t af_id, float radius)
Land at waypoint location.
#define NAV_TAKEOFF_DIST
void nav_takeoff_and_landing_init(void)
Init function.
#define NAV_TAKEOFF_PITCH
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_here(float td_alt, float radius)
Land at current location.
#define NAV_TAKEOFF_HEIGHT
bool nav_takeoff_from_loc(float lat, float lon, float height)
Takeoff from lat long location.
#define NAV_LANDING_AF_HEIGHT
#define NAV_TAKEOFF_THROTTLE
#define TRIM_UPPRZ(pprz)
Definition paparazzi.h:14
#define MAX_PPRZ
Definition paparazzi.h:8
void utm_of_lla_f(struct UtmCoor_f *utm, 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
position in UTM coordinates Units: meters
#define CARROT
default approaching_time for a wp
Definition navigation.h:70
#define NormCourseRad(x)
Normalize a rad angle between 0 and 2*PI.
Definition navigation.h:156
static const float dir[]
unsigned char uint8_t
Typedef defining 8 bit unsigned char type.