Paparazzi UAS  v7.0_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 
50 struct RoverNavigation nav;
51 
52 const float max_dist_from_home = MAX_DIST_FROM_HOME;
53 const 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 
66 static 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 
79 void nav_init(void)
80 {
83 
84  nav.mode = NAV_MODE_WAYPOINT;
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 
106 void nav_run(void)
107 {
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) - state.ned_origin_i.hmsl +
131  waypoint_move_lla(wp_id, &lla);
132  }
133 }
134 
135 bool nav_check_wp_time(struct EnuCoor_f *wp, float stay_time)
136 {
137  (void) wp;
138  (void) stay_time;
139  return true;
140 // uint16_t time_at_wp;
141 // float dist_to_point;
142 // static uint16_t wp_entry_time = 0;
143 // static bool wp_reached = false;
144 // static struct EnuCoor_i wp_last = { 0, 0, 0 };
145 // struct Int32Vect2 diff;
146 //
147 // if ((wp_last.x != wp->x) || (wp_last.y != wp->y)) {
148 // wp_reached = false;
149 // wp_last = *wp;
150 // }
151 //
152 // VECT2_DIFF(diff, *wp, *stateGetPositionEnu_i());
153 // struct FloatVect2 diff_f = {POS_FLOAT_OF_BFP(diff.x), POS_FLOAT_OF_BFP(diff.y)};
154 // dist_to_point = float_vect2_norm(&diff_f);
155 // if (dist_to_point < ARRIVED_AT_WAYPOINT) {
156 // if (!wp_reached) {
157 // wp_reached = true;
158 // wp_entry_time = autopilot.flight_time;
159 // time_at_wp = 0;
160 // } else {
161 // time_at_wp = autopilot.flight_time - wp_entry_time;
162 // }
163 // } else {
164 // time_at_wp = 0;
165 // wp_reached = false;
166 // }
167 // if (time_at_wp > stay_time) {
168 // INT_VECT3_ZERO(wp_last);
169 // return true;
170 // }
171 // return false;
172 }
173 
174 
177 {
179  /* update local ENU coordinates of global waypoints */
181 }
182 
183 void nav_reset_alt(void)
184 {
187 }
188 
189 void nav_init_stage(void)
190 {
191  VECT3_COPY(nav.last_pos, *stateGetPositionEnu_f());
192  stage_time = 0;
193  //nav_circle_radians = 0; FIXME
194 }
195 
197 {
198  RunOnceEvery(NAVIGATION_FREQUENCY, { stage_time++; block_time++; });
199 
200  //nav.dist2_to_wp = 0; FIXME
201 
202  /* from flight_plan.h */
203  auto_nav();
204 
205  /* run carrot loop */
206  nav_run();
207 }
208 
210 {
211  return autopilot_in_flight();
212 }
213 
215 void nav_home(void)
216 {
217  nav.mode = NAV_MODE_WAYPOINT;
218  VECT3_COPY(nav.target, waypoints[WP_HOME].enu_f);
219 
220  //nav.dist2_to_wp = nav.dist2_to_home; FIXME
221 
222  /* run carrot loop */
223  nav_run();
224 }
225 
228 {
229  struct EnuCoor_f *pos = stateGetPositionEnu_f();
230  struct FloatVect2 pos_diff = {
231  .x = p->x - pos->x,
232  .y = p->y - pos->y
233  };
234  return pos_diff.x * pos_diff.x + pos_diff.y * pos_diff.y;
235 }
236 
239 {
240  return get_dist2_to_point(&waypoints[wp_id].enu_f);
241 }
242 
247 {
250 #ifdef InGeofenceSector
251  struct EnuCoor_f *pos = stateGetPositionEnu_f();
252  nav.too_far_from_home = nav.too_far_from_home || !(InGeofenceSector(pos->x, pos->y));
253 #endif
254 }
255 
257 void nav_set_heading_rad(float rad)
258 {
259  nav.heading = rad;
261 }
262 
264 void nav_set_heading_deg(float deg)
265 {
266  nav_set_heading_rad(RadOfDeg(deg));
267 }
268 
270 void nav_set_heading_towards(float x, float y)
271 {
272  struct FloatVect2 target = {x, y};
273  struct FloatVect2 pos_diff;
275  // don't change heading if closer than 0.5m to target
276  if (VECT2_NORM2(pos_diff) > 0.25f) {
277  nav.heading = atan2f(pos_diff.x, pos_diff.y);
278  }
279 }
280 
283 {
285 }
286 
289 {
291 }
292 
295 {
297 }
298 
300 {
301 #ifdef AP_MODE_FAILSAFE
303 #endif
304 }
305 
306 
311 {
315 }
316 
318 {
320 }
321 
323 {
326 }
327 
328 
bool autopilot_set_mode(uint8_t new_autopilot_mode)
set autopilot mode
Definition: autopilot.c:193
bool autopilot_in_flight(void)
get in_flight flag
Definition: autopilot.c:340
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:39
const uint8_t nb_waypoint
Definition: common_nav.c:38
#define WaypointX(_wp)
Definition: common_nav.h:45
float y
Definition: common_nav.h:41
float x
Definition: common_nav.h:40
#define WaypointY(_wp)
Definition: common_nav.h:46
Device independent GPS code (interface)
float psi
in radians
#define VECT2_DIFF(_c, _a, _b)
Definition: pprz_algebra.h:92
#define VECT2_COPY(_a, _b)
Definition: pprz_algebra.h:68
#define VECT3_COPY(_a, _b)
Definition: pprz_algebra.h:140
#define VECT2_NORM2(_v)
Definition: pprz_algebra.h:118
int32_t lat
in degrees*1e7
int32_t hmsl
Height above mean sea level in mm.
int32_t alt
in millimeters above WGS84 reference ellipsoid
struct LlaCoor_i lla
Reference point in lla.
int32_t lon
in degrees*1e7
vector in Latitude, Longitude and Altitude
static struct FloatEulers * stateGetNedToBodyEulers_f(void)
Get vehicle body attitude euler angles (float).
Definition: state.h:1143
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 LtpDef_i ned_origin_i
Definition of the local (flat earth) coordinate system.
Definition: state.h:166
static bool stateIsLocalCoordinateValid(void)
Test if local coordinates are valid.
Definition: state.h:508
void WEAK ins_reset_local_origin(void)
INS local origin reset.
Definition: ins.c:55
void WEAK ins_reset_altitude_ref(void)
INS altitude reference reset.
Definition: ins.c:65
Integrated Navigation System interface.
static float p[2][2]
void waypoints_localize_all(void)
update local ENU coordinates of global waypoints
Definition: waypoints.c:357
void waypoints_init(void)
initialize global and local waypoints
Definition: waypoints.c:51
void waypoint_move_lla(uint8_t wp_id, struct LlaCoor_i *lla)
Definition: waypoints.c:253
void nav_oval_init(void)
Definition: nav.c:756
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
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)
struct FloatVect2 pos_diff
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)
Registering functions.
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
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
struct EnuCoor_f speed
speed setpoint (in m/s)
Definition: navigation.h:128
float dist2_to_home
squared distance to home waypoint
Definition: navigation.h:143
navigation_oval nav_oval
Definition: navigation.h:156
navigation_approaching nav_approaching
Definition: navigation.h:153
#define DEFAULT_CIRCLE_RADIUS
default nav_circle_radius in meters
Definition: navigation.h:42
navigation_circle nav_circle
Definition: navigation.h:154
float radius
radius setpoint (in meters)
Definition: navigation.h:136
navigation_oval_init nav_oval_init
Definition: navigation.h:155
float heading
heading setpoint (in radians)
Definition: navigation.h:133
struct EnuCoor_f carrot
carrot position (also used for GCS display)
Definition: navigation.h:127
navigation_goto nav_goto
Definition: navigation.h:151
navigation_route nav_route
Definition: navigation.h:152
#define FAILSAFE_MODE_DISTANCE
Maximum distance from HOME waypoint before going into failsafe mode.
Definition: navigation.h:60
struct EnuCoor_f target
final target position (in meters)
Definition: navigation.h:126
bool too_far_from_home
too_far flag
Definition: navigation.h:144
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:82
void(* nav_rover_goto)(struct EnuCoor_f *wp)
Definition: navigation.h:78
void(* nav_rover_circle)(struct EnuCoor_f *wp_center, float radius)
Definition: navigation.h:81
#define NormCourseRad(x)
Normalize a rad angle between 0 and 2*PI.
Definition: navigation.h:156
#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:79
void(* nav_rover_oval)(struct EnuCoor_f *wp1, struct EnuCoor_f *wp2, float radius)
Definition: navigation.h:83
bool(* nav_rover_approaching)(struct EnuCoor_f *wp_to, struct EnuCoor_f *wp_from, float approaching_time)
Definition: navigation.h:80
General Navigation structure.
Definition: navigation.h:88
API to get/set the generic vehicle states.
static const struct usb_device_descriptor dev
Definition: usb_ser_hw.c:74
struct target_t target
Definition: target_pos.c:64
int8_t register_periodic_telemetry(struct periodic_telemetry *_pt, uint8_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 char uint8_t
Typedef defining 8 bit unsigned char type.
Definition: vl53l1_types.h:98