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) 2018 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
27
28#define NAV_C
29
31
32#include "pprz_debug.h"
33#include "modules/gps/gps.h" // needed by auto_nav from the flight plan
34#include "modules/ins/ins.h"
35#include "state.h"
36
37#include "autopilot.h"
38#include "generated/modules.h"
39#include "generated/flight_plan.h"
40
42
44#include "pprzlink/messages.h"
45#include "mcu_periph/uart.h"
46
47
49
51
52const float max_dist_from_home = MAX_DIST_FROM_HOME;
53const float max_dist2_from_home = MAX_DIST_FROM_HOME * MAX_DIST_FROM_HOME;
55
56
58{
59 nav.exception_flag[flag_num] = true;
60}
61
62
63#if PERIODIC_TELEMETRY
65
66static void send_wp_moved(struct transport_tx *trans, struct link_device *dev)
67{
68 static uint8_t i;
69 i++;
70 if (i >= nb_waypoint) { i = 0; }
71 pprz_msg_send_WP_MOVED_ENU(trans, dev, AC_ID,
72 &i,
73 &(waypoints[i].enu_i.x),
74 &(waypoints[i].enu_i.y),
75 &(waypoints[i].enu_i.z));
76}
77#endif
78
79void nav_init(void)
80{
83
85
86 VECT3_COPY(nav.target, waypoints[WP_HOME].enu_f);
87 VECT3_COPY(nav.carrot, waypoints[WP_HOME].enu_f);
88 nav.heading = 0.f;
90 nav.speed = 0.f;
91 nav.turn = 0.f;
92 nav.shift = 0.f;
93
94 nav.too_far_from_home = false;
95 nav.dist2_to_home = 0.f;
96
97#if PERIODIC_TELEMETRY
99#endif
100
101 // generated init function
102 auto_nav_init();
103}
104
105
106void nav_run(void)
107{
108 VECT2_COPY(nav.carrot, nav.target);
109}
110
112{
113 if (DL_BLOCK_ac_id(buf) != AC_ID) { return; }
114 nav_goto_block(DL_BLOCK_block_id(buf));
115}
116
118{
119 uint8_t ac_id = DL_MOVE_WP_ac_id(buf);
120 if (ac_id != AC_ID) { return; }
122 uint8_t wp_id = DL_MOVE_WP_wp_id(buf);
123 struct LlaCoor_i lla;
124 lla.lat = DL_MOVE_WP_lat(buf);
125 lla.lon = DL_MOVE_WP_lon(buf);
126 /* WP_alt from message is alt above MSL in mm
127 * lla.alt is above ellipsoid in mm
128 */
129 lla.alt = DL_MOVE_WP_alt(buf) - stateGetHmslOrigin_i() +
131 waypoint_move_lla(wp_id, &lla);
132 }
133}
134
135bool nav_check_wp_time(struct EnuCoor_f *wp, float stay_time)
136{
137 uint16_t time_at_wp;
138 float dist_to_point;
139 static uint16_t wp_entry_time = 0;
140 static bool wp_reached = false;
141 static struct EnuCoor_i wp_last = { 0, 0, 0 };
142 struct EnuCoor_i wp_i;
143 struct FloatVect2 diff;
144
145 ENU_BFP_OF_REAL(wp_i, *wp);
146 if ((wp_last.x != wp_i.x) || (wp_last.y != wp_i.y)) {
147 wp_reached = false;
148 wp_last = wp_i;
149 }
150
151 VECT2_DIFF(diff, *wp, *stateGetPositionEnu_f());
152 dist_to_point = float_vect2_norm(&diff);
153 if (dist_to_point < ARRIVED_AT_WAYPOINT) {
154 if (!wp_reached) {
155 wp_reached = true;
156 wp_entry_time = autopilot.flight_time;
157 time_at_wp = 0;
158 } else {
159 time_at_wp = autopilot.flight_time - wp_entry_time;
160 }
161 } else {
162 time_at_wp = 0;
163 wp_reached = false;
164 }
165 if (time_at_wp > stay_time) {
166 INT_VECT3_ZERO(wp_last);
167 return true;
168 }
169 return false;
170}
171
172
175{
176 AbiSendMsgINS_RESET(0, INS_RESET_REF);
177 /* update local ENU coordinates of global waypoints */
179}
180
182{
183 AbiSendMsgINS_RESET(0, INS_RESET_VERTICAL_REF);
185}
186
188{
190 stage_time = 0;
191 if (nav.nav_stage_init) {
192 nav.nav_stage_init();
193 }
194}
195
197{
198 RunOnceEvery(NAVIGATION_FREQUENCY, { stage_time++; block_time++; });
199
200 /* from flight_plan.h */
201 auto_nav();
202
203 /* run carrot loop */
204 nav_run();
205}
206
208{
209 return autopilot_in_flight();
210}
211
213void nav_home(void)
214{
215 nav.mode = NAV_MODE_WAYPOINT;
216 VECT3_COPY(nav.target, waypoints[WP_HOME].enu_f);
217
218 /* run carrot loop */
219 nav_run();
220}
221
224{
225 struct EnuCoor_f *pos = stateGetPositionEnu_f();
226 struct FloatVect2 pos_diff = {
227 .x = p->x - pos->x,
228 .y = p->y - pos->y
229 };
230 return pos_diff.x * pos_diff.x + pos_diff.y * pos_diff.y;
231}
232
235{
236 return get_dist2_to_point(&waypoints[wp_id].enu_f);
237}
238
243{
244 nav.dist2_to_home = get_dist2_to_waypoint(WP_HOME);
245 nav.too_far_from_home = nav.dist2_to_home > max_dist2_from_home;
246#ifdef InGeofenceSector
247 struct EnuCoor_f *pos = stateGetPositionEnu_f();
248 nav.too_far_from_home = nav.too_far_from_home || !(InGeofenceSector(pos->x, pos->y));
249#endif
250}
251
253void nav_set_heading_rad(float rad)
254{
255 nav.heading = rad;
256 NormCourseRad(nav.heading);
257}
258
260void nav_set_heading_deg(float deg)
261{
262 nav_set_heading_rad(RadOfDeg(deg));
263}
264
266void nav_set_heading_towards(float x, float y)
267{
268 struct FloatVect2 target = {x, y};
269 struct FloatVect2 pos_diff;
271 // don't change heading if closer than 0.5m to target
272 if (VECT2_NORM2(pos_diff) > 0.25f) {
273 nav.heading = atan2f(pos_diff.x, pos_diff.y);
274 }
275}
276
282
285{
286 nav_set_heading_towards(nav.target.x, nav.target.y);
287}
288
291{
292 nav.heading = stateGetNedToBodyEulers_f()->psi;
293}
294
296{
297#ifdef AP_MODE_FAILSAFE
299#endif
300}
301
302
305
310
317
322
328
329
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.
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
const uint8_t nb_waypoint
Definition common_nav.c:43
#define WaypointX(_wp)
Definition common_nav.h:45
#define WaypointY(_wp)
Definition common_nav.h:46
static struct uart_periph * dev
Device independent GPS code (interface).
float psi
in radians
static float float_vect2_norm(const struct FloatVect2 *v)
#define VECT2_DIFF(_c, _a, _b)
#define VECT2_COPY(_a, _b)
#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
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
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
void nav_oval_init(void)
Definition nav.c:756
void nav_oval(uint8_t p1, uint8_t p2, float radius)
Definition nav.c:762
static void send_wp_moved(struct transport_tx *trans, struct link_device *dev)
Definition nav.c:489
#define NAVIGATION_FREQUENCY
Default fixedwing navigation frequency.
Definition nav.h:49
static void nav_goto(struct EnuCoor_f *wp)
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
vector in East North Up coordinates Units: meters
#define AP_MODE_FAILSAFE
void nav_home(void)
Home mode navigation.
Definition navigation.c:294
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
void nav_parse_BLOCK(uint8_t *buf)
Definition navigation.c:115
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
const float max_dist2_from_home
Definition navigation.c:54
void nav_set_heading_rad(float rad)
Set nav_heading in radians.
Definition navigation.c:337
void nav_set_heading_deg(float deg)
Set nav_heading in degrees.
Definition navigation.c:344
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
void nav_set_failsafe(void)
Definition navigation.c:380
#define DEFAULT_CIRCLE_RADIUS
default nav_circle_radius in meters
Definition navigation.h:42
#define FAILSAFE_MODE_DISTANCE
Maximum distance from HOME waypoint before going into failsafe mode.
Definition navigation.h:60
#define ARRIVED_AT_WAYPOINT
minimum horizontal distance to waypoint to mark as arrived
Definition navigation.h:55
float failsafe_mode_dist2
Definition navigation.c:54
static void send_wp_moved(struct transport_tx *trans, struct link_device *dev)
Definition navigation.c:66
void set_exception_flag(uint8_t flag_num)
Definition navigation.c:57
Rover navigation functions.
void(* nav_rover_oval_init)(void)
Definition navigation.h:83
void(* nav_rover_goto)(struct EnuCoor_f *wp)
Definition navigation.h:79
void(* nav_rover_circle)(struct EnuCoor_f *wp_center, float radius)
Definition navigation.h:82
void(* nav_rover_stage_init)(void)
Definition navigation.h:78
#define NormCourseRad(x)
Normalize a rad angle between 0 and 2*PI.
Definition navigation.h:159
#define NAV_MODE_WAYPOINT
Nav modes.
Definition navigation.h:72
void(* nav_rover_route)(struct EnuCoor_f *wp_start, struct EnuCoor_f *wp_end)
Definition navigation.h:80
void(* nav_rover_oval)(struct EnuCoor_f *wp1, struct EnuCoor_f *wp2, float radius)
Definition navigation.h:84
bool(* nav_rover_approaching)(struct EnuCoor_f *wp_to, struct EnuCoor_f *wp_from, float approaching_time)
Definition navigation.h:81
General Navigation structure.
Definition navigation.h:89
API to get/set the generic vehicle states.
struct target_t target
Definition target_pos.c:65
int16_t register_periodic_telemetry(struct periodic_telemetry *_pt, uint16_t _id, telemetry_cb _cb)
Register a telemetry callback function.
Definition telemetry.c:51
Periodic telemetry system header (includes downlink utility and generated code).
#define DefaultPeriodic
Set default periodic telemetry.
Definition telemetry.h:66
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.