Paparazzi UAS v7.1_unstable
Paparazzi is a free software Unmanned Aircraft System.
Loading...
Searching...
No Matches
navigation.c
Go to the documentation of this file.
1/*
2 * Copyright (C) 2008-2009 Antoine Drouin <poinix@gmail.com>
3 * Copyright (C) 2022 Gautier Hattenberger <gautier.hattenberger@enac.fr>
4 *
5 * This file is part of paparazzi.
6 *
7 * paparazzi is free software; you can redistribute it and/or modify
8 * it under the terms of the GNU General Public License as published by
9 * the Free Software Foundation; either version 2, or (at your option)
10 * any later version.
11 *
12 * paparazzi is distributed in the hope that it will be useful,
13 * but WITHOUT ANY WARRANTY; without even the implied warranty of
14 * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the
15 * GNU General Public License for more details.
16 *
17 * You should have received a copy of the GNU General Public License
18 * along with paparazzi; see the file COPYING. If not, see
19 * <http://www.gnu.org/licenses/>.
20 */
21
27
28
29#define NAV_C
30
32
33#include "pprz_debug.h"
34#include "state.h"
35#include "autopilot.h"
36#include "generated/modules.h"
37#include "generated/flight_plan.h"
38#include "modules/ins/ins.h"
39
40/* for default GUIDANCE_H_USE_REF */
42
44
46#include "pprzlink/messages.h"
47#include "mcu_periph/uart.h"
48
50
52
53const float max_dist_from_home = MAX_DIST_FROM_HOME;
54const float max_dist2_from_home = MAX_DIST_FROM_HOME * MAX_DIST_FROM_HOME;
55
57
60static void empty_stage_init(void) {}
61static void empty_goto(struct EnuCoor_f *wp UNUSED) {}
62static void empty_route(struct EnuCoor_f *wp_start UNUSED, struct EnuCoor_f *wp_end UNUSED) {}
63static bool empty_approaching(struct EnuCoor_f *wp_to UNUSED, struct EnuCoor_f *wp_from UNUSED, float approaching_time UNUSED) { return true; }
64static void empty_circle(struct EnuCoor_f *wp_center UNUSED, float radius UNUSED) {}
65static void empty_oval_init(void) {}
66static void empty_oval(struct EnuCoor_f *wp1 UNUSED, struct EnuCoor_f *wp2 UNUSED, float radius UNUSED) {};
67
68static inline void nav_set_altitude(void);
69
70void nav_init(void)
71{
74
75 nav.horizontal_mode = NAV_HORIZONTAL_MODE_WAYPOINT;
76 nav.vertical_mode = NAV_VERTICAL_MODE_ALT;
77 nav.setpoint_mode = NAV_SETPOINT_MODE_POS;
78
79 VECT3_COPY(nav.target, waypoints[WP_HOME].enu_f);
80 VECT3_COPY(nav.carrot, waypoints[WP_HOME].enu_f);
81 FLOAT_VECT3_ZERO(nav.speed);
82 FLOAT_VECT3_ZERO(nav.accel);
84 FLOAT_RATES_ZERO(nav.rates);
85
86 nav.throttle = 0;
87 nav.roll = 0.f;
88 nav.pitch = 0.f;
89 nav.heading = 0.f;
91 nav.climb = 0.f;
92 nav.fp_altitude = SECURITY_HEIGHT;
93 nav.nav_altitude = SECURITY_HEIGHT;
94 nav.fp_max_speed = -1.f;
95 flight_altitude = SECURITY_ALT;
96
97 nav.too_far_from_home = false;
99 nav.dist2_to_home = 0.f;
100 nav.climb_vspeed = NAV_CLIMB_VSPEED;
101 nav.descend_vspeed = NAV_DESCEND_VSPEED;
102
103 nav.nav_stage_init = empty_stage_init;
104 nav.nav_goto = empty_goto;
105 nav.nav_route = empty_route;
106 nav.nav_approaching = empty_approaching;
107 nav.nav_circle = empty_circle;
108 nav.nav_oval_init = empty_oval_init;
109 nav.nav_oval = empty_oval;
110
111 // generated init function
112 auto_nav_init();
113}
114
116{
117 if (DL_BLOCK_ac_id(buf) != AC_ID) { return; }
118 nav_goto_block(DL_BLOCK_block_id(buf));
119}
120
122{
123 uint8_t ac_id = DL_MOVE_WP_ac_id(buf);
124 if (ac_id != AC_ID) { return; }
126 uint8_t wp_id = DL_MOVE_WP_wp_id(buf);
127 struct LlaCoor_i lla;
128 lla.lat = DL_MOVE_WP_lat(buf);
129 lla.lon = DL_MOVE_WP_lon(buf);
130 /* WP_alt from message is alt above MSL in mm
131 * lla.alt is above ellipsoid in mm
132 */
133 lla.alt = DL_MOVE_WP_alt(buf) - stateGetHmslOrigin_i() +
135 waypoint_move_lla(wp_id, &lla);
136 }
137}
138
139#ifndef CLOSE_TO_WAYPOINT
140#define CLOSE_TO_WAYPOINT 15.f
141#endif
142
143static inline void UNUSED nav_advance_carrot(void)
144{
145 struct EnuCoor_f *pos = stateGetPositionEnu_f();
146 /* compute a vector to the waypoint */
147 struct FloatVect2 path_to_waypoint;
148 VECT2_DIFF(path_to_waypoint, nav.target, *pos);
149
150 /* saturate it */
151 VECT2_STRIM(path_to_waypoint, -150.f, 150.f);
152
153 float dist_to_waypoint = float_vect2_norm(&path_to_waypoint);
154
155 if (dist_to_waypoint < CLOSE_TO_WAYPOINT) {
156 VECT2_COPY(nav.carrot, nav.target);
157 } else {
158 struct Int32Vect2 path_to_carrot;
159 VECT2_SMUL(path_to_carrot, path_to_waypoint, NAV_CARROT_DIST);
160 VECT2_SDIV(path_to_carrot, path_to_carrot, dist_to_waypoint);
161 VECT2_SUM(nav.carrot, path_to_carrot, *pos);
162 }
163}
164
165void nav_run(void)
166{
167
168#if GUIDANCE_H_USE_REF
169 // if GUIDANCE_H_USE_REF, CARROT_DIST is not used
170 VECT2_COPY(nav.carrot, nav.target);
171#else
173#endif
174
175 // update altitude setpoint if needed
177}
178
179
180bool nav_check_wp_time(struct EnuCoor_f *wp, uint16_t stay_time)
181{
182 uint16_t time_at_wp;
183 float dist_to_point;
184 static uint16_t wp_entry_time = 0;
185 static bool wp_reached = false;
186 static struct EnuCoor_i wp_last = { 0, 0, 0 };
187 struct EnuCoor_i wp_i;
188 struct FloatVect2 diff;
189
190 ENU_BFP_OF_REAL(wp_i, *wp);
191 if ((wp_last.x != wp_i.x) || (wp_last.y != wp_i.y)) {
192 wp_reached = false;
193 wp_last = wp_i;
194 }
195
196 VECT2_DIFF(diff, *wp, *stateGetPositionEnu_f());
197 dist_to_point = float_vect2_norm(&diff);
198 if (dist_to_point < ARRIVED_AT_WAYPOINT) {
199 if (!wp_reached) {
200 wp_reached = true;
201 wp_entry_time = autopilot.flight_time;
202 time_at_wp = 0;
203 } else {
204 time_at_wp = autopilot.flight_time - wp_entry_time;
205 }
206 } else {
207 time_at_wp = 0;
208 wp_reached = false;
209 }
210 if (time_at_wp > stay_time) {
211 INT_VECT3_ZERO(wp_last);
212 return true;
213 }
214 return false;
215}
216
217static inline void nav_set_altitude(void)
218{
219 static float last_alt = 0.f;
220 // if the fp_altitude setpoint change is large enough, set this alt as the new reference
221 // otherwise, don't change nav_altitude (whose value can be changed by the operator
222 // through the flight_altitude setting)
223 // nav_altitude is the value that is used by guidance as a setpoint when flying in
224 // altitude mode
225 if (fabsf(nav.fp_altitude - last_alt) > 0.2f) {
226 nav.nav_altitude = nav.fp_altitude;
227 flight_altitude = nav.nav_altitude + stateGetHmslOrigin_f();
228 last_alt = nav.fp_altitude;
229 }
230}
231
232
235{
236 AbiSendMsgINS_RESET(0, INS_RESET_REF);
237 /* update local ENU coordinates of global waypoints */
239}
240
242{
243 AbiSendMsgINS_RESET(0, INS_RESET_VERTICAL_REF);
245}
246
248{
249 stage_time = 0;
250 if (nav.nav_stage_init) {
251 nav.nav_stage_init();
252 }
253}
254
255#include <stdio.h>
257{
258 RunOnceEvery(NAVIGATION_FREQUENCY, { stage_time++; block_time++; });
259
260 /* from flight_plan.h */
261 auto_nav();
262
263 /* run carrot loop */
264 nav_run();
265}
266
268{
269 if (autopilot.ground_detected) {
270 autopilot.ground_detected = false;
271 return true;
272 } else {
273 return false;
274 }
275}
276
278{
279 return autopilot_in_flight();
280}
281
282void nav_glide_points(struct EnuCoor_f *start_point, struct EnuCoor_f *end_point)
283{
284 struct FloatVect2 wp_diff, pos_diff;
285 VECT2_DIFF(wp_diff, *end_point, *start_point);
286 VECT2_DIFF(pos_diff, *stateGetPositionEnu_f(), *start_point);
287 float length2 = Max(float_vect2_norm2(&wp_diff), 0.1f);
288 float progress = (pos_diff.x * wp_diff.x + pos_diff.y * wp_diff.y) / length2;
289 float alt = start_point->z + (end_point->z - start_point->z) * progress;
291}
292
294void nav_home(void)
295{
296 nav.horizontal_mode = NAV_HORIZONTAL_MODE_WAYPOINT;
297 nav.setpoint_mode = NAV_SETPOINT_MODE_POS;
298 VECT3_COPY(nav.target, waypoints[WP_HOME].enu_f);
299
300 nav.vertical_mode = NAV_VERTICAL_MODE_ALT;
301 nav.nav_altitude = waypoint_get_alt(WP_HOME);
302
303 /* run carrot loop */
304 nav_run();
305}
306
309{
310 struct EnuCoor_f *pos = stateGetPositionEnu_f();
311 struct FloatVect2 pos_diff;
312 pos_diff.x = p->x - pos->x;
313 pos_diff.y = p->y - pos->y;
314 return pos_diff.x * pos_diff.x + pos_diff.y * pos_diff.y;
315}
316
319{
320 return get_dist2_to_point(&waypoints[wp_id].enu_f);
321}
322
327{
328 nav.dist2_to_home = get_dist2_to_waypoint(WP_HOME);
329 nav.too_far_from_home = nav.dist2_to_home > max_dist2_from_home;
330#ifdef InGeofenceSector
331 struct EnuCoor_f *pos = stateGetPositionEnu_f();
332 nav.too_far_from_home = nav.too_far_from_home || !(InGeofenceSector(pos->x, pos->y));
333#endif
334}
335
337void nav_set_heading_rad(float rad)
338{
339 nav.heading = rad;
340 NormCourseRad(nav.heading);
341}
342
344void nav_set_heading_deg(float deg)
345{
346 nav_set_heading_rad(RadOfDeg(deg));
347}
348
350void nav_set_heading_towards(float x, float y)
351{
352 struct FloatVect2 target = {x, y};
353 struct FloatVect2 pos_diff;
355 // don't change heading if closer than 0.5m to target
356 if (VECT2_NORM2(pos_diff) > 0.25f) {
357 float heading_f = atan2f(pos_diff.x, pos_diff.y);
358 nav.heading = heading_f;
359 }
360}
361
367
370{
371 nav_set_heading_towards(nav.target.x, nav.target.y);
372}
373
376{
377 nav.heading = stateGetNedToBodyEulers_f()->psi;
378}
379
384
391
398
403
409
bool autopilot_set_mode(uint8_t new_autopilot_mode)
set autopilot mode
Definition autopilot.c:200
struct pprz_autopilot autopilot
Global autopilot structure.
Definition autopilot.c:50
bool autopilot_in_flight(void)
get in_flight flag
Definition autopilot.c:351
Core autopilot interface common to all firmwares.
#define UNUSED(x)
void common_flight_plan_init(void)
uint16_t stage_time
In s.
void nav_goto_block(uint8_t b)
uint16_t block_time
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 WaypointY(_wp)
Definition common_nav.h:46
float psi
in radians
static void float_quat_identity(struct FloatQuat *q)
initialises a quaternion to identity
#define FLOAT_VECT3_ZERO(_v)
static float float_vect2_norm(const struct FloatVect2 *v)
static float float_vect2_norm2(const struct FloatVect2 *v)
#define FLOAT_RATES_ZERO(_r)
#define VECT2_SMUL(_vo, _vi, _s)
#define VECT2_DIFF(_c, _a, _b)
#define VECT2_STRIM(_v, _min, _max)
#define VECT2_COPY(_a, _b)
#define VECT2_SUM(_c, _a, _b)
#define VECT2_SDIV(_vo, _vi, _s)
#define VECT3_COPY(_a, _b)
#define VECT2_NORM2(_v)
#define INT_VECT3_ZERO(_v)
int32_t lat
in degrees*1e7
int32_t y
North.
int32_t alt
in millimeters above WGS84 reference ellipsoid
int32_t x
East.
int32_t lon
in degrees*1e7
#define ENU_BFP_OF_REAL(_o, _i)
vector in East North Up coordinates
vector in Latitude, Longitude and Altitude
static struct FloatEulers * stateGetNedToBodyEulers_f(void)
Get vehicle body attitude euler angles (float).
Definition state.h:1325
int32_t stateGetHmslOrigin_i(void)
Get the HMSL of the frame origin (int).
Definition state.c:196
float stateGetHmslOrigin_f(void)
Get the HMSL of the frame origin (float).
Definition state.c:210
struct LlaCoor_i stateGetLlaOrigin_i(void)
Get the LLA position of the frame origin (int).
Definition state.c:130
static bool stateIsLocalCoordinateValid(void)
Test if local coordinates are valid.
Definition state.h:624
static struct EnuCoor_f * stateGetPositionEnu_f(void)
Get position in local ENU coordinates (float).
Definition state.h:859
Integrated Navigation System interface.
#define INS_RESET_VERTICAL_REF
Definition ins.h:37
#define INS_RESET_REF
flags for INS reset
Definition ins.h:36
static float p[2][2]
void waypoints_localize_all(void)
update local ENU coordinates of global waypoints
Definition waypoints.c:376
float waypoint_get_alt(uint8_t wp_id)
Get altitude of waypoint in meters (above reference).
Definition waypoints.c:140
void waypoints_init(void)
initialize global and local waypoints
Definition waypoints.c:78
void waypoint_move_lla(uint8_t wp_id, struct LlaCoor_i *lla)
Definition waypoints.c:278
float flight_altitude
Dynamically adjustable, reset to nav_altitude when it is changing.
Definition nav.c:74
void nav_oval(uint8_t p1, uint8_t p2, float radius)
Definition nav.c:762
#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
static void nav_goto(struct EnuCoor_f *wp)
static void _nav_oval_init(void)
static void nav_route(struct EnuCoor_f *wp_start, struct EnuCoor_f *wp_end)
static void nav_circle(struct EnuCoor_f *wp_center, float radius)
static bool nav_approaching(struct EnuCoor_f *wp, struct EnuCoor_f *from, float approaching_time)
static void nav_stage_init(void)
Implement basic nav function.
struct FloatVect2 pos_diff
PRINT_CONFIG_VAR(ONELOOP_ANDI_FILT_CUTOFF)
Paparazzi fixed point algebra.
float y
in meters
float x
in meters
float z
in meters
vector in East North Up coordinates Units: meters
#define AP_MODE_FAILSAFE
Horizontal guidance for rotorcrafts.
void nav_home(void)
Home mode navigation.
Definition navigation.c:294
static void empty_oval_init(void)
Definition navigation.c:65
static bool empty_approaching(struct EnuCoor_f *wp_to UNUSED, struct EnuCoor_f *wp_from UNUSED, float approaching_time UNUSED)
Definition navigation.c:63
void nav_reset_reference(void)
Reset the geographic reference to the current GPS fix.
Definition navigation.c:234
void nav_register_oval(navigation_oval_init _nav_oval_init, navigation_oval nav_oval)
Definition navigation.c:404
void nav_set_heading_current(void)
Set heading to the current yaw angle.
Definition navigation.c:375
void nav_init_stage(void)
needs to be implemented by fixedwing and rotorcraft seperately
Definition navigation.c:247
void nav_reset_alt(void)
Definition navigation.c:241
const float max_dist_from_home
Definition navigation.c:53
void nav_set_heading_towards_waypoint(uint8_t wp)
Set heading in the direction of a waypoint.
Definition navigation.c:363
void compute_dist2_to_home(void)
Computes squared distance to the HOME waypoint potentially sets too_far_from_home.
Definition navigation.c:326
bool nav_is_in_flight(void)
Definition navigation.c:277
void nav_run(void)
Definition navigation.c:165
void nav_set_heading_towards(float x, float y)
Set heading to point towards x,y position in local coordinates.
Definition navigation.c:350
struct RotorcraftNavigation nav
Definition navigation.c:51
void nav_init(void)
Navigation Initialisation.
Definition navigation.c:70
void nav_parse_MOVE_WP(uint8_t *buf)
Definition navigation.c:121
void nav_register_circle(navigation_circle nav_circle)
Definition navigation.c:399
void nav_register_goto_wp(navigation_goto nav_goto, navigation_route nav_route, navigation_approaching nav_approaching)
Definition navigation.c:392
static void empty_stage_init(void)
Empty navigation functions set at init.
Definition navigation.c:60
static void empty_oval(struct EnuCoor_f *wp1 UNUSED, struct EnuCoor_f *wp2 UNUSED, float radius UNUSED)
Definition navigation.c:66
void nav_parse_BLOCK(uint8_t *buf)
Definition navigation.c:115
#define CLOSE_TO_WAYPOINT
Definition navigation.c:140
static void empty_goto(struct EnuCoor_f *wp UNUSED)
Definition navigation.c:61
void nav_set_heading_towards_target(void)
Set heading in the direction of the target.
Definition navigation.c:369
float get_dist2_to_point(struct EnuCoor_f *p)
Returns squared horizontal distance to given point.
Definition navigation.c:308
void nav_register_stage_init(navigation_stage_init nav_stage_init)
Register functions.
Definition navigation.c:387
static void empty_route(struct EnuCoor_f *wp_start UNUSED, struct EnuCoor_f *wp_end UNUSED)
Definition navigation.c:62
const float max_dist2_from_home
Definition navigation.c:54
void nav_glide_points(struct EnuCoor_f *start_point, struct EnuCoor_f *end_point)
Definition navigation.c:282
void nav_set_heading_rad(float rad)
Set nav_heading in radians.
Definition navigation.c:337
bool nav_detect_ground(void)
Definition navigation.c:267
void nav_set_heading_deg(float deg)
Set nav_heading in degrees.
Definition navigation.c:344
static void empty_circle(struct EnuCoor_f *wp_center UNUSED, float radius UNUSED)
Definition navigation.c:64
bool nav_check_wp_time(struct EnuCoor_f *wp, uint16_t stay_time)
Check the time spent in a radius of 'ARRIVED_AT_WAYPOINT' around a wp.
Definition navigation.c:180
void nav_periodic_task(void)
Navigation main: call to the code generated from the XML flight plan.
Definition navigation.c:256
float get_dist2_to_waypoint(uint8_t wp_id)
Returns squared horizontal distance to given waypoint.
Definition navigation.c:318
static void UNUSED nav_advance_carrot(void)
Definition navigation.c:143
static void nav_set_altitude(void)
Definition navigation.c:217
void nav_set_failsafe(void)
Definition navigation.c:380
Rotorcraft navigation functions.
#define NAV_HORIZONTAL_MODE_WAYPOINT
Nav modes these modes correspond to the flight plan instructions used to set the high level navigatio...
Definition navigation.h:85
void(* navigation_goto)(struct EnuCoor_f *wp)
Definition navigation.h:109
#define NAV_DESCEND_VSPEED
Definition navigation.h:50
bool(* navigation_approaching)(struct EnuCoor_f *wp_to, struct EnuCoor_f *wp_from, float approaching_time)
Definition navigation.h:111
void(* navigation_oval)(struct EnuCoor_f *wp1, struct EnuCoor_f *wp2, float radius)
Definition navigation.h:114
#define DEFAULT_CIRCLE_RADIUS
default nav_circle_radius in meters
Definition navigation.h:42
#define NAV_CARROT_DIST
Carrot distance during navigation.
Definition navigation.h:65
void(* navigation_oval_init)(void)
Definition navigation.h:113
#define NAV_VERTICAL_MODE_ALT
Definition navigation.h:94
void(* navigation_route)(struct EnuCoor_f *wp_start, struct EnuCoor_f *wp_end)
Definition navigation.h:110
void(* navigation_circle)(struct EnuCoor_f *wp_center, float radius)
Definition navigation.h:112
#define NAV_SETPOINT_MODE_POS
Nav setpoint modes these modes correspond to submodes defined by navigation routines to tell which se...
Definition navigation.h:100
void(* navigation_stage_init)(void)
Definition navigation.h:108
#define FAILSAFE_MODE_DISTANCE
Maximum distance from HOME waypoint before going into failsafe mode.
Definition navigation.h:60
#define NAV_CLIMB_VSPEED
Definition navigation.h:46
#define ARRIVED_AT_WAYPOINT
minimum horizontal distance to waypoint to mark as arrived
Definition navigation.h:55
General Navigation structure.
Definition navigation.h:119
#define NormCourseRad(x)
Normalize a rad angle between 0 and 2*PI.
Definition navigation.h:159
API to get/set the generic vehicle states.
struct target_t target
Definition target_pos.c:65
arch independent UART (Universal Asynchronous Receiver/Transmitter) API
unsigned short uint16_t
Typedef defining 16 bit unsigned short type.
unsigned char uint8_t
Typedef defining 8 bit unsigned char type.