Paparazzi UAS v7.1_unstable
Paparazzi is a free software Unmanned Aircraft System.
Loading...
Searching...
No Matches
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
26
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];
96 return nav_land_here(td_alt, radius);
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{
120 takeoff.status = NAV_TAKEOFF_INIT;
121 takeoff.climb_id = 0;
122 takeoff.timeout = false;
123 landing.status = NAV_LANDING_INIT;
124 landing.td_id = 0;
125 landing.af_id = 0;
126 landing.timeout = false;
127
129
130#if USE_MISSION
131 mission_register(nav_takeoff_mission, "TKOFF");
132 mission_register(nav_land_mission, "LAND");
133#endif
134}
135
137{
138 if (takeoff.timeout) {
139 takeoff.status = NAV_TAKEOFF_INIT;
140 }
141 if (landing.timeout) {
142 landing.status = NAV_LANDING_INIT;
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:
156 start_motor_counter = 0;
157 break;
160 start_motor_counter++;
161 if (start_motor_counter == NAVIGATION_FREQUENCY) {
162 start_motor_counter = 0;
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
175 NavGotoPoint(takeoff.climb_pos);
178 if (nav_approaching_xy(takeoff.climb_pos.x, takeoff.climb_pos.y, takeoff.start_pos.x, takeoff.start_pos.y, CARROT)
179 || (stateGetPositionEnu_f()->z - takeoff.start_pos.z > takeoff.climb_pos.z)) {
180 // end when climb point or target alt is reached
181 takeoff.status = NAV_TAKEOFF_DONE;
182 }
183 break;
184 default:
185 takeoff.status = NAV_TAKEOFF_INIT;
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{
194 if (takeoff.status == NAV_TAKEOFF_INIT) {
195 takeoff.start_pos = *stateGetPositionEnu_f();
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) {
200 takeoff.climb_pos.z = takeoff.start_pos.z + NAV_TAKEOFF_HEIGHT;
201 } else {
202 takeoff.climb_pos.z = takeoff.start_pos.z + height;
203 }
204 }
205
206 return nav_takeoff_run();
207}
208
209bool nav_takeoff_from_loc(float lat, float lon, float height)
210{
211 if (takeoff.status == NAV_TAKEOFF_INIT) {
212 takeoff.start_pos = *stateGetPositionEnu_f();
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);
222 ENU_OF_UTM_DIFF(takeoff.climb_pos, utm, *stateGetUtmOrigin_f());
223 }
224 return nav_takeoff_run();
225}
226
227bool nav_takeoff_from_here(float height)
228{
229 if (takeoff.status == NAV_TAKEOFF_INIT) {
230 takeoff.climb_id = 0;
231 takeoff.start_pos = *stateGetPositionEnu_f();
232 takeoff.climb_pos.x = takeoff.start_pos.x + NAV_TAKEOFF_DIST * sinf(RadOfDeg(nav_takeoff_direction));
233 takeoff.climb_pos.y = takeoff.start_pos.y + NAV_TAKEOFF_DIST * cosf(RadOfDeg(nav_takeoff_direction));
234 if (height < 0.f) {
235 takeoff.climb_pos.z = takeoff.start_pos.z + NAV_TAKEOFF_HEIGHT;
236 } else {
237 takeoff.climb_pos.z = takeoff.start_pos.z + height;
238 }
239 }
240
241 return nav_takeoff_run();
242}
243
244
245static bool nav_land_run(void)
246{
247 static float land_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
267 waypoints[0].x = landing.af_pos.x + y_unit * landing.radius;
268 waypoints[0].y = landing.af_pos.y - x_unit * landing.radius;
269 waypoints[0].a = landing.af_pos.z + GetAltRef();
270 land_baseleg_out_qdr = M_PI - atan2f(-y_unit, -x_unit);
271 if (landing.radius < 0.f) {
272 land_baseleg_out_qdr += M_PI;
273 }
274 } else {
275 // turn around AF point
276 waypoints[0].x = landing.af_pos.x;
277 waypoints[0].y = landing.af_pos.y;
278 waypoints[0].a = landing.af_pos.z + GetAltRef();
279 land_baseleg_out_qdr = 0.f;
280 }
282 break;
286 NavCircleWaypoint(0, landing.radius);
287 if (NavCircleCount() > 0.25 && (fabsf(GetPosAlt() - WaypointAlt(0)) < 10.f)) {
288 if (glide) {
289 // if final glide, direction should be correct
290 qdr = M_PI_2 - nav_circle_trigo_qdr;
291 NormCourseRad(qdr);
292 if (CloseRadAngles(land_baseleg_out_qdr, qdr)) {
294 }
295 } else {
296 // no final glide, start from correct altitude
298 }
299 }
300 break;
302 if (glide) {
303 nav_route_xy(landing.af_pos.x, landing.af_pos.y, landing.td_pos.x, landing.td_pos.y);
305 nav_glide_alt(landing.af_pos.z + GetAltRef(), landing.td_pos.z + GetAltRef());
306 if (nav_approaching_xy(landing.td_pos.x, landing.td_pos.y, landing.af_pos.x, landing.af_pos.y, CARROT)
307 || (GetPosAlt() < landing.td_pos.z + NAV_LANDING_FLARE_HEIGHT + GetAltRef())) {
308 landing.status = NAV_LANDING_FLARE;
309 }
310 } else {
311 NavCircleWaypoint(0, landing.radius);
314 if (GetPosAlt() < landing.td_pos.z + NAV_LANDING_FLARE_HEIGHT + GetAltRef()) {
315 landing.status = NAV_LANDING_FLARE;
316 }
317 }
318 break;
322 if (glide) {
323 nav_route_xy(landing.af_pos.x, landing.af_pos.y, landing.td_pos.x, landing.td_pos.y);
324 if (nav_approaching_xy(landing.td_pos.x, landing.td_pos.y, landing.af_pos.x, landing.af_pos.y, 0.f)) {
325 landing.status = NAV_LANDING_DONE;
326 }
327 } else {
328 NavCircleWaypoint(0, landing.radius);
329 // stay on circle, no end
330 }
331 break;
332 case NAV_LANDING_DONE:
333 default:
335 landing.status = NAV_LANDING_INIT;
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{
344 if (landing.status == NAV_LANDING_INIT) {
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);
352 landing.af_pos.z = landing.td_pos.z + NAV_LANDING_AF_HEIGHT;
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{
361 if (landing.status == NAV_LANDING_INIT) {
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);
368 landing.af_pos.x = landing.td_pos.x + dist * sinf(RadOfDeg(dir));
369 landing.af_pos.y = landing.td_pos.y + dist * cosf(RadOfDeg(dir));
370 landing.af_pos.z = landing.td_pos.z + NAV_LANDING_AF_HEIGHT;
371 landing.radius = radius;
372 }
373
374 return nav_land_run();
375}
376
377bool nav_land_here(float td_alt, float radius)
378{
379 if (landing.status == NAV_LANDING_INIT) {
380 landing.td_id = 0;
381 landing.af_id = 0;
382 landing.td_pos = *stateGetPositionEnu_f();
383 landing.td_pos.z = td_alt;
384 landing.af_pos = landing.td_pos;
385 landing.af_pos.z = landing.td_pos.z + NAV_LANDING_AF_HEIGHT;
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: kill_thro...
Definition autopilot.c:258
struct pprz_autopilot autopilot
Global autopilot structure.
Definition autopilot.c:50
bool autopilot_get_motors_on(void)
get motors status
Definition autopilot.c:303
Core autopilot interface common to all firmwares.
#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
#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:868
static struct UtmCoor_f * stateGetUtmOrigin_f(void)
Get the coordinate UTM frame origin (int).
Definition state.h:587
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
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:234
#define NavGotoPoint(_point)
Definition nav.h:101
#define GetAltRef()
Get current altitude reference for local coordinates.
Definition nav.h:243
#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)
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
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 bool nav_land_run(void)
static bool nav_takeoff_run(void)
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
bool landing
#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 lon
in radians
float lat
in radians
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:159
static const float dir[]
unsigned char uint8_t
Typedef defining 8 bit unsigned char type.