Paparazzi UAS  v6.2_unstable
Paparazzi is a free software Unmanned Aircraft System.
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 
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 
41 #include "math/pprz_algebra_int.h"
42 
44 #include "pprzlink/messages.h"
45 #include "mcu_periph/uart.h"
46 
47 
48 PRINT_CONFIG_VAR(NAVIGATION_FREQUENCY)
49 
51 
52 uint8_t last_wp __attribute__((unused));
53 
54 const float max_dist_from_home = MAX_DIST_FROM_HOME;
55 const float max_dist2_from_home = MAX_DIST_FROM_HOME * MAX_DIST_FROM_HOME;
57 
58 
60 {
61  nav.exception_flag[flag_num] = true;
62 }
63 
64 
65 #if PERIODIC_TELEMETRY
67 
68 static void send_wp_moved(struct transport_tx *trans, struct link_device *dev)
69 {
70  static uint8_t i;
71  i++;
72  if (i >= nb_waypoint) { i = 0; }
73  pprz_msg_send_WP_MOVED_ENU(trans, dev, AC_ID,
74  &i,
75  &(waypoints[i].enu_i.x),
76  &(waypoints[i].enu_i.y),
77  &(waypoints[i].enu_i.z));
78 }
79 #endif
80 
81 void nav_init(void)
82 {
84 
85  nav_block = 0;
86  nav_stage = 0;
87 
89 
90  VECT3_COPY(nav.target, waypoints[WP_HOME].enu_f);
91  VECT3_COPY(nav.carrot, waypoints[WP_HOME].enu_f);
92  nav.heading = 0.f;
94  nav.speed = 0.f;
95  nav.turn = 0.f;
96  nav.shift = 0.f;
97 
98  nav.too_far_from_home = false;
99  nav.dist2_to_home = 0.f;
100 
101 #if PERIODIC_TELEMETRY
103 #endif
104 
105  // generated init function
106  auto_nav_init();
107 }
108 
109 
110 void nav_run(void)
111 {
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) - state.ned_origin_i.hmsl +
135  waypoint_move_lla(wp_id, &lla);
136  }
137 }
138 
139 bool nav_check_wp_time(struct EnuCoor_f *wp, float stay_time)
140 {
141  (void) wp;
142  (void) stay_time;
143  return true;
144 // uint16_t time_at_wp;
145 // float dist_to_point;
146 // static uint16_t wp_entry_time = 0;
147 // static bool wp_reached = false;
148 // static struct EnuCoor_i wp_last = { 0, 0, 0 };
149 // struct Int32Vect2 diff;
150 //
151 // if ((wp_last.x != wp->x) || (wp_last.y != wp->y)) {
152 // wp_reached = false;
153 // wp_last = *wp;
154 // }
155 //
156 // VECT2_DIFF(diff, *wp, *stateGetPositionEnu_i());
157 // struct FloatVect2 diff_f = {POS_FLOAT_OF_BFP(diff.x), POS_FLOAT_OF_BFP(diff.y)};
158 // dist_to_point = float_vect2_norm(&diff_f);
159 // if (dist_to_point < ARRIVED_AT_WAYPOINT) {
160 // if (!wp_reached) {
161 // wp_reached = true;
162 // wp_entry_time = autopilot.flight_time;
163 // time_at_wp = 0;
164 // } else {
165 // time_at_wp = autopilot.flight_time - wp_entry_time;
166 // }
167 // } else {
168 // time_at_wp = 0;
169 // wp_reached = false;
170 // }
171 // if (time_at_wp > stay_time) {
172 // INT_VECT3_ZERO(wp_last);
173 // return true;
174 // }
175 // return false;
176 }
177 
178 
181 {
183  /* update local ENU coordinates of global waypoints */
185 }
186 
187 void nav_reset_alt(void)
188 {
191 }
192 
193 void nav_init_stage(void)
194 {
196  stage_time = 0;
197  //nav_circle_radians = 0; FIXME
198 }
199 
201 {
202  RunOnceEvery(NAVIGATION_FREQUENCY, { stage_time++; block_time++; });
203 
204  //nav.dist2_to_wp = 0; FIXME
205 
206  /* from flight_plan.h */
207  auto_nav();
208 
209  /* run carrot loop */
210  nav_run();
211 }
212 
214 {
215  return autopilot_in_flight();
216 }
217 
219 void nav_home(void)
220 {
222  VECT3_COPY(nav.target, waypoints[WP_HOME].enu_f);
223 
224  //nav.dist2_to_wp = nav.dist2_to_home; FIXME
225 
226  /* run carrot loop */
227  nav_run();
228 }
229 
232 {
233  struct EnuCoor_f *pos = stateGetPositionEnu_f();
234  struct FloatVect2 pos_diff = {
235  .x = p->x - pos->x,
236  .y = p->y - pos->y
237  };
238  return pos_diff.x * pos_diff.x + pos_diff.y * pos_diff.y;
239 }
240 
243 {
244  return get_dist2_to_point(&waypoints[wp_id].enu_f);
245 }
246 
251 {
254 #ifdef InGeofenceSector
255  struct EnuCoor_f *pos = stateGetPositionEnu_f();
256  nav.too_far_from_home = nav.too_far_from_home || !(InGeofenceSector(pos->x, pos->y));
257 #endif
258 }
259 
261 void nav_set_heading_rad(float rad)
262 {
263  nav.heading = rad;
265 }
266 
268 void nav_set_heading_deg(float deg)
269 {
270  nav_set_heading_rad(RadOfDeg(deg));
271 }
272 
274 void nav_set_heading_towards(float x, float y)
275 {
276  struct FloatVect2 target = {x, y};
277  struct FloatVect2 pos_diff;
279  // don't change heading if closer than 0.5m to target
280  if (VECT2_NORM2(pos_diff) > 0.25f) {
281  nav.heading = atan2f(pos_diff.x, pos_diff.y);
282  }
283 }
284 
287 {
289 }
290 
293 {
295 }
296 
299 {
301 }
302 
304 {
305 #ifdef AP_MODE_FAILSAFE
307 #endif
308 }
309 
310 
315 {
319 }
320 
322 {
324 }
325 
327 {
330 }
331 
332 
nav_rover_oval_init
void(* nav_rover_oval_init)(void)
Definition: navigation.h:82
LlaCoor_i::lon
int32_t lon
in degrees*1e7
Definition: pprz_geodetic_int.h:61
nav_parse_BLOCK
void nav_parse_BLOCK(uint8_t *buf)
Definition: navigation.c:231
RoverNavigation::dist2_to_home
float dist2_to_home
squared distance to home waypoint
Definition: navigation.h:102
ins.h
pprz_debug.h
uint8_t
unsigned char uint8_t
Typedef defining 8 bit unsigned char type.
Definition: vl53l1_types.h:98
block_time
uint16_t block_time
Definition: common_flight_plan.c:33
LlaCoor_i::alt
int32_t alt
in millimeters above WGS84 reference ellipsoid
Definition: pprz_geodetic_int.h:62
RoverNavigation::too_far_from_home
bool too_far_from_home
too_far flag
Definition: navigation.h:103
RoverNavigation::nav_goto
nav_rover_goto nav_goto
Definition: navigation.h:109
stateIsLocalCoordinateValid
static bool stateIsLocalCoordinateValid(void)
Test if local coordinates are valid.
Definition: state.h:508
waypoints_localize_all
void waypoints_localize_all(void)
update local ENU coordinates of global waypoints
Definition: waypoints.c:293
State::ned_origin_i
struct LtpDef_i ned_origin_i
Definition of the local (flat earth) coordinate system.
Definition: state.h:166
compute_dist2_to_home
void compute_dist2_to_home(void)
Computes squared distance to the HOME waypoint potentially sets too_far_from_home.
Definition: navigation.c:483
waypoints_init
void waypoints_init(void)
initialize global and local waypoints
Definition: waypoints.c:35
WaypointY
#define WaypointY(_wp)
Definition: common_nav.h:46
RoverNavigation::carrot
struct EnuCoor_f carrot
carrot position
Definition: navigation.h:94
nav_check_wp_time
bool nav_check_wp_time(struct EnuCoor_i *wp, uint16_t stay_time)
Check the time spent in a radius of 'ARRIVED_AT_WAYPOINT' around a wp
Definition: navigation.c:335
AP_MODE_FAILSAFE
#define AP_MODE_FAILSAFE
Definition: autopilot_static.h:36
stateGetPositionEnu_f
static struct EnuCoor_f * stateGetPositionEnu_f(void)
Get position in local ENU coordinates (float).
Definition: state.h:719
nav_rover_goto
void(* nav_rover_goto)(struct EnuCoor_f *wp)
Definition: navigation.h:78
RoverNavigation::target
struct EnuCoor_f target
final target
Definition: navigation.h:93
nav_circle
void nav_circle(struct EnuCoor_i *wp_center, int32_t radius)
Definition: navigation.c:548
WaypointX
#define WaypointX(_wp)
Definition: common_nav.h:45
stateGetNedToBodyEulers_f
static struct FloatEulers * stateGetNedToBodyEulers_f(void)
Get vehicle body attitude euler angles (float).
Definition: state.h:1143
nav_set_heading_towards
void nav_set_heading_towards(float x, float y)
Set heading to point towards x,y position in local coordinates.
Definition: navigation.c:507
NAV_MODE_WAYPOINT
#define NAV_MODE_WAYPOINT
Nav modes.
Definition: navigation.h:72
nav_oval_init
void nav_oval_init(void)
Definition: navigation.c:635
nav_reset_reference
void nav_reset_reference(void)
Reset the geographic reference to the current GPS fix.
Definition: navigation.c:382
nav_stage
uint8_t nav_stage
Definition: common_flight_plan.c:35
nav
struct RoverNavigation nav
Definition: navigation.c:50
NormCourseRad
#define NormCourseRad(x)
Normalize a rad angle between 0 and 2*PI.
Definition: navigation.h:156
get_dist2_to_waypoint
float get_dist2_to_waypoint(uint8_t wp_id)
Returns squared horizontal distance to given waypoint.
Definition: navigation.c:475
waypoints
struct point waypoints[NB_WAYPOINT]
size == nb_waypoint, waypoint 0 is a dummy waypoint
Definition: common_nav.c:39
nav_set_heading_current
void nav_set_heading_current(void)
Set heading to the current yaw angle.
Definition: navigation.c:533
EnuCoor_f::y
float y
in meters
Definition: pprz_geodetic_float.h:74
pprz_algebra_int.h
Paparazzi fixed point algebra.
nav_parse_MOVE_WP
void nav_parse_MOVE_WP(uint8_t *buf)
Definition: navigation.c:237
VECT2_DIFF
#define VECT2_DIFF(_c, _a, _b)
Definition: pprz_algebra.h:92
nav_reset_alt
void nav_reset_alt(void)
Definition: navigation.c:389
VECT2_COPY
#define VECT2_COPY(_a, _b)
Definition: pprz_algebra.h:68
nav_register_goto_wp
void nav_register_goto_wp(nav_rover_goto nav_goto, nav_rover_route nav_route, nav_rover_approaching nav_approaching)
Register functions.
Definition: navigation.c:314
nav_init
void nav_init(void)
Navigation Initialisation.
Definition: navigation.c:186
autopilot_in_flight
bool autopilot_in_flight(void)
get in_flight flag
Definition: autopilot.c:289
pos_diff
struct FloatVect2 pos_diff
Definition: obstacle_avoidance.c:75
FloatVect2
Definition: pprz_algebra_float.h:49
LlaCoor_i::lat
int32_t lat
in degrees*1e7
Definition: pprz_geodetic_int.h:60
telemetry.h
last_wp
uint8_t last_wp
Index of last waypoint.
Definition: navigation.c:52
uart.h
arch independent UART (Universal Asynchronous Receiver/Transmitter) API
nav_rover_route
void(* nav_rover_route)(struct EnuCoor_f *wp_start, struct EnuCoor_f *wp_end)
Definition: navigation.h:79
nav_rover_oval
void(* nav_rover_oval)(struct EnuCoor_f *wp1, struct EnuCoor_f *wp2, float radius)
Definition: navigation.h:83
gps.h
Device independent GPS code (interface)
RoverNavigation::mode
uint8_t mode
Definition: navigation.h:90
nav_run
void nav_run(void)
Definition: navigation.c:277
waypoint_move_lla
void waypoint_move_lla(uint8_t wp_id, struct LlaCoor_i *lla)
Definition: waypoints.c:189
RoverNavigation::nav_route
nav_rover_route nav_route
Definition: navigation.h:110
DEFAULT_CIRCLE_RADIUS
#define DEFAULT_CIRCLE_RADIUS
default nav_circle_radius in meters
Definition: navigation.c:56
nb_waypoint
const uint8_t nb_waypoint
Definition: common_nav.c:38
nav_rover_circle
void(* nav_rover_circle)(struct EnuCoor_f *wp_center, float radius)
Definition: navigation.h:81
dev
static const struct usb_device_descriptor dev
Definition: usb_ser_hw.c:74
get_dist2_to_point
float get_dist2_to_point(struct EnuCoor_i *p)
Returns squared horizontal distance to given point.
Definition: navigation.c:465
nav_block
uint8_t nav_block
Definition: common_flight_plan.c:35
nav_route
void nav_route(struct EnuCoor_i *wp_start, struct EnuCoor_i *wp_end)
Definition: navigation.c:594
NAVIGATION_FREQUENCY
#define NAVIGATION_FREQUENCY
Default fixedwing navigation frequency.
Definition: nav.h:49
register_periodic_telemetry
int8_t register_periodic_telemetry(struct periodic_telemetry *_pt, uint8_t _id, telemetry_cb _cb)
Register a telemetry callback function.
Definition: telemetry.c:51
point::x
float x
Definition: common_nav.h:40
nav_oval
void nav_oval(uint8_t p1, uint8_t p2, float radius)
Navigation along a figure O.
Definition: navigation.c:647
EnuCoor_f
vector in East North Up coordinates Units: meters
Definition: pprz_geodetic_float.h:72
RoverNavigation::nav_circle
nav_rover_circle nav_circle
Definition: navigation.h:112
failsafe_mode_dist2
float failsafe_mode_dist2
maximum squared distance to home wp before going to failsafe mode
Definition: navigation.c:90
VECT2_NORM2
#define VECT2_NORM2(_v)
Definition: pprz_algebra.h:118
autopilot.h
nav_home
void nav_home(void)
Home mode navigation.
Definition: navigation.c:431
stage_time
uint16_t stage_time
In s.
Definition: common_flight_plan.c:33
RoverNavigation
General Navigation structure.
Definition: navigation.h:88
nav_register_circle
void nav_register_circle(nav_rover_circle nav_circle)
Definition: navigation.c:321
f
uint16_t f
Camera baseline, in meters (i.e. horizontal distance between the two cameras of the stereo setup)
Definition: wedgebug.c:204
RoverNavigation::nav_oval
nav_rover_oval nav_oval
Definition: navigation.h:114
FloatVect2::y
float y
Definition: pprz_algebra_float.h:51
set_exception_flag
void set_exception_flag(uint8_t flag_num)
Definition: navigation.c:135
send_wp_moved
static void send_wp_moved(struct transport_tx *trans, struct link_device *dev)
Definition: navigation.c:68
navigation.h
FAILSAFE_MODE_DISTANCE
#define FAILSAFE_MODE_DISTANCE
Maximum distance from HOME waypoint before going into failsafe mode.
Definition: navigation.c:74
nav_periodic_task
void nav_periodic_task(void)
Navigation main: call to the code generated from the XML flight plan.
Definition: navigation.c:403
ins_reset_altitude_ref
void WEAK ins_reset_altitude_ref(void)
INS altitude reference reset.
Definition: ins.c:65
LlaCoor_i
vector in Latitude, Longitude and Altitude
Definition: pprz_geodetic_int.h:59
LtpDef_i::hmsl
int32_t hmsl
Height above mean sea level in mm.
Definition: pprz_geodetic_int.h:102
nav_register_oval
void nav_register_oval(nav_rover_oval_init nav_oval_init, nav_rover_oval nav_oval)
Definition: navigation.c:326
autopilot_set_mode
bool autopilot_set_mode(uint8_t new_autopilot_mode)
set autopilot mode
Definition: autopilot.c:192
point::y
float y
Definition: common_nav.h:41
nav_rover_approaching
bool(* nav_rover_approaching)(struct EnuCoor_f *wp_to, struct EnuCoor_f *wp_from, float approaching_time)
Definition: navigation.h:80
nav_set_heading_deg
void nav_set_heading_deg(float deg)
Set nav_heading in degrees.
Definition: navigation.c:501
nav_init_stage
void nav_init_stage(void)
needs to be implemented by fixedwing and rotorcraft seperately
Definition: navigation.c:395
RoverNavigation::speed
float speed
speed setpoint
Definition: navigation.h:97
nav_goto
static void nav_goto(struct EnuCoor_f *wp)
Implement basic nav function.
Definition: nav_rover_base.c:34
FloatVect2::x
float x
Definition: pprz_algebra_float.h:50
RoverNavigation::exception_flag
bool exception_flag[10]
array of flags that might be used in flight plans
Definition: navigation.h:105
EnuCoor_f::x
float x
in meters
Definition: pprz_geodetic_float.h:73
ins_reset_local_origin
void WEAK ins_reset_local_origin(void)
INS local origin reset.
Definition: ins.c:55
state.h
RoverNavigation::shift
float shift
lateral shift (in meters)
Definition: navigation.h:99
RoverNavigation::last_pos
struct EnuCoor_f last_pos
last stage position
Definition: navigation.h:106
nav_set_heading_towards_waypoint
void nav_set_heading_towards_waypoint(uint8_t wp)
Set heading in the direction of a waypoint.
Definition: navigation.c:520
FloatEulers::psi
float psi
in radians
Definition: pprz_algebra_float.h:87
nav_is_in_flight
bool nav_is_in_flight(void)
Definition: navigation.c:425
max_dist2_from_home
const float max_dist2_from_home
Definition: navigation.c:89
ac_id
uint8_t ac_id
Definition: sim_ap.c:47
nav_set_heading_rad
void nav_set_heading_rad(float rad)
Set nav_heading in radians.
Definition: navigation.c:494
RoverNavigation::heading
float heading
heading setpoint (in radians)
Definition: navigation.h:95
state
struct State state
Definition: state.c:36
LtpDef_i::lla
struct LlaCoor_i lla
Reference point in lla.
Definition: pprz_geodetic_int.h:100
target
struct FloatVect2 target
Definition: obstacle_avoidance.c:78
DefaultPeriodic
#define DefaultPeriodic
Set default periodic telemetry.
Definition: telemetry.h:66
RoverNavigation::nav_oval_init
nav_rover_oval_init nav_oval_init
Definition: navigation.h:113
nav_set_heading_towards_target
void nav_set_heading_towards_target(void)
Set heading in the direction of the target.
Definition: navigation.c:526
RoverNavigation::radius
float radius
radius setpoint
Definition: navigation.h:96
nav_goto_block
void nav_goto_block(uint8_t b)
Definition: common_flight_plan.c:51
p
static float p[2][2]
Definition: ins_alt_float.c:268
VECT3_COPY
#define VECT3_COPY(_a, _b)
Definition: pprz_algebra.h:140
RoverNavigation::nav_approaching
nav_rover_approaching nav_approaching
Definition: navigation.h:111
nav_set_failsafe
void nav_set_failsafe(void)
Definition: navigation.c:538
max_dist_from_home
const float max_dist_from_home
Definition: navigation.c:88
nav_approaching
static bool nav_approaching(struct EnuCoor_f *wp, struct EnuCoor_f *from, float approaching_time)
Definition: nav_rover_base.c:63
RoverNavigation::turn
float turn
turn rate setpoint
Definition: navigation.h:98