Paparazzi UAS  v5.14.0_stable-0-g3f680d1
Paparazzi is a free software Unmanned Aircraft System.
 All Data Structures Namespaces Files Functions Variables Typedefs Enumerations Enumerator Macros Modules Pages
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 "subsystems/gps.h" // needed by auto_nav from the flight plan
34 #include "subsystems/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 
50 
51 uint8_t last_wp __attribute__((unused));
52 
53 const float max_dist_from_home = MAX_DIST_FROM_HOME;
54 const float max_dist2_from_home = MAX_DIST_FROM_HOME * MAX_DIST_FROM_HOME;
56 
57 
59 {
60  nav.exception_flag[flag_num] = true;
61 }
62 
63 
64 #if PERIODIC_TELEMETRY
66 
67 static void send_wp_moved(struct transport_tx *trans, struct link_device *dev)
68 {
69  static uint8_t i;
70  i++;
71  if (i >= nb_waypoint) { i = 0; }
72  pprz_msg_send_WP_MOVED_ENU(trans, dev, AC_ID,
73  &i,
74  &(waypoints[i].enu_i.x),
75  &(waypoints[i].enu_i.y),
76  &(waypoints[i].enu_i.z));
77 }
78 #endif
79 
80 void nav_init(void)
81 {
83 
84  nav_block = 0;
85  nav_stage = 0;
86 
88 
89  VECT3_COPY(nav.target, waypoints[WP_HOME].enu_f);
90  VECT3_COPY(nav.carrot, waypoints[WP_HOME].enu_f);
91  nav.heading = 0.f;
93  nav.speed = 0.f;
94  nav.turn = 0.f;
95  nav.shift = 0.f;
96 
97  nav.too_far_from_home = false;
98  nav.dist2_to_home = 0.f;
99 
100 #if PERIODIC_TELEMETRY
102 #endif
103 
104  // generated init function
105  auto_nav_init();
106 }
107 
108 
109 void nav_run(void)
110 {
112 }
113 
114 
115 bool nav_check_wp_time(struct EnuCoor_f *wp, float stay_time)
116 {
117  (void) wp;
118  (void) stay_time;
119  return true;
120 // uint16_t time_at_wp;
121 // float dist_to_point;
122 // static uint16_t wp_entry_time = 0;
123 // static bool wp_reached = false;
124 // static struct EnuCoor_i wp_last = { 0, 0, 0 };
125 // struct Int32Vect2 diff;
126 //
127 // if ((wp_last.x != wp->x) || (wp_last.y != wp->y)) {
128 // wp_reached = false;
129 // wp_last = *wp;
130 // }
131 //
132 // VECT2_DIFF(diff, *wp, *stateGetPositionEnu_i());
133 // struct FloatVect2 diff_f = {POS_FLOAT_OF_BFP(diff.x), POS_FLOAT_OF_BFP(diff.y)};
134 // dist_to_point = float_vect2_norm(&diff_f);
135 // if (dist_to_point < ARRIVED_AT_WAYPOINT) {
136 // if (!wp_reached) {
137 // wp_reached = true;
138 // wp_entry_time = autopilot.flight_time;
139 // time_at_wp = 0;
140 // } else {
141 // time_at_wp = autopilot.flight_time - wp_entry_time;
142 // }
143 // } else {
144 // time_at_wp = 0;
145 // wp_reached = false;
146 // }
147 // if (time_at_wp > stay_time) {
148 // INT_VECT3_ZERO(wp_last);
149 // return true;
150 // }
151 // return false;
152 }
153 
154 
157 {
159  /* update local ENU coordinates of global waypoints */
161 }
162 
163 void nav_reset_alt(void)
164 {
167 }
168 
169 void nav_init_stage(void)
170 {
172  stage_time = 0;
173  //nav_circle_radians = 0; FIXME
174 }
175 
177 {
178  RunOnceEvery(NAV_FREQ, { stage_time++; block_time++; });
179 
180  //nav.dist2_to_wp = 0; FIXME
181 
182  /* from flight_plan.h */
183  auto_nav();
184 
185  /* run carrot loop */
186  nav_run();
187 }
188 
190 {
191  return autopilot_in_flight();
192 }
193 
195 void nav_home(void)
196 {
198  VECT3_COPY(nav.target, waypoints[WP_HOME].enu_f);
199 
200  //nav.dist2_to_wp = nav.dist2_to_home; FIXME
201 
202  /* run carrot loop */
203  nav_run();
204 }
205 
208 {
209  struct EnuCoor_f *pos = stateGetPositionEnu_f();
210  struct FloatVect2 pos_diff = {
211  .x = p->x - pos->x,
212  .y = p->y - pos->y
213  };
214  return pos_diff.x * pos_diff.x + pos_diff.y * pos_diff.y;
215 }
216 
219 {
220  return get_dist2_to_point(&waypoints[wp_id].enu_f);
221 }
222 
227 {
230 #ifdef InGeofenceSector
231  struct EnuCoor_f *pos = stateGetPositionEnu_f();
232  nav.too_far_from_home = nav.too_far_from_home || !(InGeofenceSector(pos->x, pos->y));
233 #endif
234 }
235 
237 void nav_set_heading_rad(float rad)
238 {
239  nav.heading = rad;
241 }
242 
244 void nav_set_heading_deg(float deg)
245 {
246  nav_set_heading_rad(RadOfDeg(deg));
247 }
248 
250 void nav_set_heading_towards(float x, float y)
251 {
252  struct FloatVect2 target = {x, y};
253  struct FloatVect2 pos_diff;
254  VECT2_DIFF(pos_diff, target, *stateGetPositionEnu_f());
255  // don't change heading if closer than 0.5m to target
256  if (VECT2_NORM2(pos_diff) > 0.25f) {
257  nav.heading = atan2f(pos_diff.x, pos_diff.y);
258  }
259 }
260 
263 {
265 }
266 
269 {
271 }
272 
275 {
277 }
278 
280 {
281 #ifdef AP_MODE_FAILSAFE
283 #endif
284 }
285 
286 
291 {
295 }
296 
298 {
300 }
301 
303 {
306 }
307 
308 
const float max_dist2_from_home
Definition: navigation.c:80
struct RoverNavigation nav
Definition: navigation.c:49
uint16_t block_time
arch independent UART (Universal Asynchronous Receiver/Transmitter) API
float x
Definition: common_nav.h:40
void nav_reset_reference(void)
Reset the geographic reference to the current GPS fix.
Definition: navigation.c:348
#define FAILSAFE_MODE_DISTANCE
Maximum distance from HOME waypoint before going into failsafe mode.
Definition: navigation.c:73
void nav_oval_init(void)
Definition: navigation.c:625
static struct FloatEulers * stateGetNedToBodyEulers_f(void)
Get vehicle body attitude euler angles (float).
Definition: state.h:1143
float dist2_to_home
squared distance to home waypoint
Definition: navigation.h:97
Periodic telemetry system header (includes downlink utility and generated code).
void ins_reset_local_origin(void)
local implemetation of the ins_reset functions
void set_exception_flag(uint8_t flag_num)
Definition: navigation.c:128
static void send_wp_moved(struct transport_tx *trans, struct link_device *dev)
Definition: navigation.c:67
vector in East North Up coordinates Units: meters
#define VECT3_COPY(_a, _b)
Definition: pprz_algebra.h:140
#define NormCourseRad(x)
Normalize a rad angle between 0 and 2*PI.
Definition: navigation.h:149
struct EnuCoor_f target
final target
Definition: navigation.h:88
#define VECT2_COPY(_a, _b)
Definition: pprz_algebra.h:68
void nav_set_heading_towards_target(void)
Set heading in the direction of the target.
Definition: navigation.c:516
float psi
in radians
#define VECT2_DIFF(_c, _a, _b)
Definition: pprz_algebra.h:92
uint8_t nav_block
Integrated Navigation System interface.
void nav_home(void)
Home mode navigation.
Definition: navigation.c:421
void(* nav_rover_circle)(struct EnuCoor_f *wp_center, float radius)
Definition: navigation.h:76
#define DEFAULT_CIRCLE_RADIUS
default nav_circle_radius in meters
Definition: navigation.c:55
void nav_set_heading_current(void)
Set heading to the current yaw angle.
Definition: navigation.c:523
void(* nav_rover_goto)(struct EnuCoor_f *wp)
Definition: navigation.h:73
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:290
bool autopilot_set_mode(uint8_t new_autopilot_mode)
set autopilot mode
Definition: autopilot.c:160
void(* nav_rover_route)(struct EnuCoor_f *wp_start, struct EnuCoor_f *wp_end)
Definition: navigation.h:74
static struct EnuCoor_f * stateGetPositionEnu_f(void)
Get position in local ENU coordinates (float).
Definition: state.h:719
const float max_dist_from_home
Definition: navigation.c:79
General Navigation structure.
Definition: navigation.h:83
float speed
speed setpoint
Definition: navigation.h:92
void nav_periodic_task(void)
Navigation main: call to the code generated from the XML flight plan.
Definition: navigation.c:369
float y
Definition: common_nav.h:41
bool(* nav_rover_approaching)(struct EnuCoor_f *wp_to, struct EnuCoor_f *wp_from, float approaching_time)
Definition: navigation.h:75
nav_rover_goto nav_goto
Definition: navigation.h:104
void nav_run(void)
Definition: navigation.c:243
#define WaypointX(_wp)
Definition: common_nav.h:45
float x
in meters
float radius
radius setpoint
Definition: navigation.h:91
bool autopilot_in_flight(void)
get in_flight flag
Definition: autopilot.c:256
uint8_t last_wp
Index of last waypoint.
Definition: navigation.c:51
Device independent GPS code (interface)
#define NAV_MODE_WAYPOINT
Nav modes.
Definition: navigation.h:67
bool exception_flag[10]
array of flags that might be used in flight plans
Definition: navigation.h:100
float get_dist2_to_point(struct EnuCoor_i *p)
Returns squared horizontal distance to given point.
Definition: navigation.c:455
bool nav_is_in_flight(void)
Definition: navigation.c:415
uint8_t mode
Definition: navigation.h:85
#define DefaultPeriodic
Set default periodic telemetry.
Definition: telemetry.h:66
void nav_register_oval(nav_rover_oval_init nav_oval_init, nav_rover_oval nav_oval)
Definition: navigation.c:302
void nav_register_circle(nav_rover_circle nav_circle)
Definition: navigation.c:297
void nav_set_heading_rad(float rad)
Set nav_heading in radians.
Definition: navigation.c:484
#define VECT2_NORM2(_v)
Definition: pprz_algebra.h:118
float get_dist2_to_waypoint(uint8_t wp_id)
Returns squared horizontal distance to given waypoint.
Definition: navigation.c:465
float shift
lateral shift (in meters)
Definition: navigation.h:94
void compute_dist2_to_home(void)
Computes squared distance to the HOME waypoint potentially sets too_far_from_home.
Definition: navigation.c:473
float heading
heading setpoint (in radians)
Definition: navigation.h:90
uint16_t stage_time
In s.
#define WaypointY(_wp)
Definition: common_nav.h:46
const uint8_t nb_waypoint
Definition: common_nav.c:37
static const struct usb_device_descriptor dev
Definition: usb_ser_hw.c:73
uint8_t nav_stage
float turn
turn rate setpoint
Definition: navigation.h:93
Core autopilot interface common to all firmwares.
struct EnuCoor_f last_pos
last stage position
Definition: navigation.h:101
void(* nav_rover_oval_init)(void)
Definition: navigation.h:77
unsigned char uint8_t
Definition: types.h:14
void nav_oval(uint8_t p1, uint8_t p2, float radius)
Navigation along a figure O.
Definition: navigation.c:637
API to get/set the generic vehicle states.
nav_rover_oval nav_oval
Definition: navigation.h:109
struct point waypoints[NB_WAYPOINT]
size == nb_waypoint, waypoint 0 is a dummy waypoint
Definition: common_nav.c:38
void nav_circle(struct EnuCoor_i *wp_center, int32_t radius)
Definition: navigation.c:538
void nav_set_heading_deg(float deg)
Set nav_heading in degrees.
Definition: navigation.c:491
nav_rover_circle nav_circle
Definition: navigation.h:107
nav_rover_approaching nav_approaching
Definition: navigation.h:106
static float p[2][2]
void nav_set_heading_towards_waypoint(uint8_t wp)
Set heading in the direction of a waypoint.
Definition: navigation.c:510
bool too_far_from_home
too_far flag
Definition: navigation.h:98
void nav_route(struct EnuCoor_i *wp_start, struct EnuCoor_i *wp_end)
Definition: navigation.c:584
nav_rover_oval_init nav_oval_init
Definition: navigation.h:108
float failsafe_mode_dist2
maximum squared distance to home wp before going to failsafe mode
Definition: navigation.c:81
void waypoints_localize_all(void)
update local ENU coordinates of global waypoints
Definition: waypoints.c:265
struct EnuCoor_f carrot
carrot position
Definition: navigation.h:89
#define AP_MODE_FAILSAFE
void nav_init(void)
Navigation Initialisation.
Definition: navigation.c:179
nav_rover_route nav_route
Definition: navigation.h:105
void nav_set_failsafe(void)
Definition: navigation.c:528
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:301
void ins_reset_altitude_ref(void)
INS altitude reference reset.
void nav_set_heading_towards(float x, float y)
Set heading to point towards x,y position in local coordinates.
Definition: navigation.c:497
void nav_reset_alt(void)
Definition: navigation.c:355
float y
in meters
#define NAV_FREQ
Definition: navigation.h:43
Rover navigation functions.
void waypoints_init(void)
initialize global and local waypoints
Definition: waypoints.c:35
int8_t register_periodic_telemetry(struct periodic_telemetry *_pt, uint8_t _id, telemetry_cb _cb)
Register a telemetry callback function.
Definition: telemetry.c:46
Paparazzi fixed point algebra.
void(* nav_rover_oval)(struct EnuCoor_f *wp1, struct EnuCoor_f *wp2, float radius)
Definition: navigation.h:78
void nav_init_stage(void)
needs to be implemented by fixedwing and rotorcraft seperately
Definition: navigation.c:361