Paparazzi UAS  v6.3_unstable
Paparazzi is a free software Unmanned Aircraft System.
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 
66 static struct nav_takeoff takeoff;
67 static struct nav_landing landing;
68 
69 static bool nav_takeoff_run(void);
70 static bool nav_land_run(void);
71 
73 
74 #if USE_MISSION
76 
77 static bool nav_takeoff_mission(uint8_t nb UNUSED, float *params UNUSED, enum MissionRunFlag flag)
78 {
79  if (flag == MissionInit) {
81  return nav_takeoff_from_here();
82  }
83  else if (flag == MissionRun) {
84  return nav_takeoff_run();
85  }
86  return false; // not a valid case
87 }
88 
89 static bool nav_land_mission(uint8_t nb, float *params, enum MissionRunFlag flag)
90 {
91  if (flag == MissionInit && nb == 2) {
92  float td_alt = params[0];
93  float radius = params[1];
95  return nav_land_here(td_alt, radius);
96  } else if (flag == MissionInit && nb == 3) {
97  uint8_t td_id = (uint8_t)(params[0]);
98  uint8_t af_id = (uint8_t)(params[1]);
99  float radius = params[2];
100  return nav_land_at_wp(td_id, af_id, radius);
101  } else if (flag == MissionInit && nb == 6) {
102  float td_alt = params[0];
103  float lat = params[1];
104  float lon = params[2];
105  float dir = params[3];
106  float dist = params[4];
107  float radius = params[5];
108  return nav_land_at_loc(td_alt, lat, lon, dir, dist, radius);
109  } else if (flag == MissionRun) {
110  return nav_land_run();
111  }
112  return false; // not a valid case
113 }
114 
115 #endif
116 
118 {
120  takeoff.climb_id = 0;
121  takeoff.timeout = false;
123  landing.td_id = 0;
124  landing.af_id = 0;
125  landing.timeout = false;
126 
127  nav_takeoff_direction = QFU;
128 
129 #if USE_MISSION
130  mission_register(nav_takeoff_mission, "TKOFF");
131  mission_register(nav_land_mission, "LAND");
132 #endif
133 }
134 
136 {
137  if (takeoff.timeout) {
139  }
140  if (landing.timeout) {
142  }
143 
144  // reset flag to true, set to false if run function is called during period
145  takeoff.timeout = true;
146  landing.timeout = true;
147 }
148 
149 static bool nav_takeoff_run(void) {
150  static int start_motor_counter = 0;
151 
152  switch (takeoff.status) {
153  case NAV_TAKEOFF_INIT:
155  start_motor_counter = 0;
156  break;
159  start_motor_counter++;
160  if (start_motor_counter == NAVIGATION_FREQUENCY) {
161  start_motor_counter = 0;
162  if (autopilot_get_motors_on()) {
163 #if NAV_TAKEOFF_AUTO_LAUNCH
164  autopilot.launch = true;
165 #endif
166  takeoff.status = NAV_TAKEOFF_CLIMB; // next step after 1s
167  } else {
168  takeoff.status = NAV_TAKEOFF_DONE; // failed to start motors, end now
169  }
170  }
171  break;
172  case NAV_TAKEOFF_CLIMB:
173  // climb towards point at specified pitch and throttle
179  // end when climb point or target alt is reached
181  }
182  break;
183  default:
185  return false;
186  }
187  takeoff.timeout = false;
188  return true;
189 }
190 
192 {
194  takeoff.climb_id = wp_id;
195  takeoff.climb_pos.x = WaypointX(wp_id);
196  takeoff.climb_pos.y = WaypointY(wp_id);
197  takeoff.climb_pos.z = WaypointAlt(wp_id) - GetAltRef();
199  }
200 
201  return nav_takeoff_run();
202 }
203 
204 bool nav_takeoff_from_loc(float lat, float lon)
205 {
207  struct LlaCoor_f lla = { RadOfDeg(lat), RadOfDeg(lon), stateGetPositionLla_f()->alt + NAV_TAKEOFF_HEIGHT };
208  struct UtmCoor_f utm;
209  utm_of_lla_f(&utm, &lla);
211  }
212  return nav_takeoff_from_here();
213 }
214 
216 {
218  takeoff.climb_id = 0;
224  }
225 
226  return nav_takeoff_run();
227 }
228 
229 
230 static bool nav_land_run(void)
231 {
232  static float baseleg_out_qdr = 0.f;
233  float qdr = 0.f;
234 
235  // test if glide is needed or not
236  float dx = landing.td_pos.x - landing.af_pos.x;
237  float dy = landing.td_pos.y - landing.af_pos.y;
238  float dist = sqrtf(dx*dx + dy*dy);
239  bool glide = false;
240  float x_unit = 0.f;
241  float y_unit = 0.f;
242  if (dist > 1.f) {
243  glide = true;
244  x_unit = dx / dist;
245  y_unit = dy / dist;
246  }
247 
248  switch (landing.status) {
249  case NAV_LANDING_INIT:
250  if (glide) {
251  // compute baseleg circle center, store in dummy WP 0
252  waypoints[0].x = landing.af_pos.x + y_unit * landing.radius;
253  waypoints[0].y = landing.af_pos.y - x_unit * landing.radius;
254  waypoints[0].a = landing.af_pos.z + GetAltRef();
255  baseleg_out_qdr = M_PI - atan2f(-y_unit, -x_unit);
256  if (landing.radius < 0.f) {
257  baseleg_out_qdr += M_PI;
258  }
259  } else {
260  // turn around AF point
261  waypoints[0].x = landing.af_pos.x;
262  waypoints[0].y = landing.af_pos.y;
263  waypoints[0].a = landing.af_pos.z + GetAltRef();
264  baseleg_out_qdr = 0.f;
265  }
267  break;
272  if (NavCircleCount() > 0.25 && (fabsf(GetPosAlt() - WaypointAlt(0)) < 10.f)) {
273  if (glide) {
274  // if final glide, direction should be correct
275  qdr = M_PI_2 - nav_circle_trigo_qdr;
276  NormCourseRad(qdr);
277  if (CloseRadAngles(baseleg_out_qdr, qdr)) {
279  }
280  } else {
281  // no final glide, start from correct altitude
283  }
284  }
285  break;
286  case NAV_LANDING_DESCENT:
287  if (glide) {
294  }
295  } else {
301  }
302  }
303  break;
304  case NAV_LANDING_FLARE:
307  if (glide) {
311  }
312  } else {
314  // stay on circle, no end
315  }
316  break;
317  case NAV_LANDING_DONE:
318  default:
320  return false;
321  }
322  landing.timeout = false;
323  return true;
324 }
325 
326 bool nav_land_at_wp(uint8_t td_id, uint8_t af_id, float radius)
327 {
329  landing.td_id = td_id;
330  landing.af_id = af_id;
331  landing.td_pos.x = WaypointX(td_id);
332  landing.td_pos.y = WaypointY(td_id);
333  landing.td_pos.z = WaypointAlt(td_id) - GetAltRef();
334  landing.af_pos.x = WaypointX(af_id);
335  landing.af_pos.y = WaypointY(af_id);
337  landing.radius = radius;
338  }
339 
340  return nav_land_run();
341 }
342 
343 bool nav_land_at_loc(float td_alt, float lat, float lon, float dir, float dist, float radius)
344 {
346  landing.td_id = 0;
347  landing.af_id = 0;
348  struct LlaCoor_f lla = { RadOfDeg(lat), RadOfDeg(lon), GetAltRef() + td_alt };
349  struct UtmCoor_f utm;
350  utm_of_lla_f(&utm, &lla);
352  landing.af_pos.x = landing.td_pos.x + dist * sinf(RadOfDeg(dir));
353  landing.af_pos.y = landing.td_pos.y + dist * cosf(RadOfDeg(dir));
355  landing.radius = radius;
356  }
357 
358  return nav_land_run();
359 }
360 
361 bool nav_land_here(float td_alt, float radius)
362 {
364  landing.td_id = 0;
365  landing.af_id = 0;
367  landing.td_pos.z = td_alt;
370  landing.radius = radius;
371  }
372 
373  return nav_land_run();
374 }
375 
376 
struct pprz_autopilot autopilot
Global autopilot structure.
Definition: autopilot.c:48
bool autopilot_get_motors_on(void)
get motors status
Definition: autopilot.c:244
void 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:232
Core autopilot interface common to all firmwares.
bool launch
request launch
Definition: autopilot.h:71
uint8_t last_wp UNUSED
struct point waypoints[NB_WAYPOINT]
size == nb_waypoint, waypoint 0 is a dummy waypoint
Definition: common_nav.c:39
#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)
Definition: pprz_geodetic.h:78
struct State state
Definition: state.c:36
static struct EnuCoor_f * stateGetPositionEnu_f(void)
Get position in local ENU coordinates (float).
Definition: state.h:719
struct UtmCoor_f utm_origin_f
Definition of the origin of Utm coordinate system.
Definition: state.h:233
static struct LlaCoor_f * stateGetPositionLla_f(void)
Get position in LLA coordinates (float).
Definition: state.h:728
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: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 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
@ NAV_LANDING_INIT
@ NAV_LANDING_DONE
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_DONE
@ NAV_TAKEOFF_INIT
@ NAV_TAKEOFF_CLIMB
@ NAV_TAKEOFF_START_MOTOR
Structure for landing.
Structure for takeoff.
bool nav_takeoff_from_wp(uint8_t wp_id)
Takeoff from a waypoint.
#define NAV_LANDING_DESCEND_SPEED
#define NAV_LANDING_FLARE_HEIGHT
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.
bool nav_takeoff_from_loc(float lat, float lon)
Takeoff from lat long location.
#define NAV_TAKEOFF_PITCH
bool nav_takeoff_from_here(void)
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
#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.
Definition: vl53l1_types.h:98