Paparazzi UAS  v5.2.2_stable-0-gd6b9f29
Paparazzi is a free software Unmanned Aircraft System.
 All Data Structures Files Functions Variables Typedefs Enumerations Enumerator Macros Modules Pages
navigation.c
Go to the documentation of this file.
1 /*
2  * Copyright (C) 2008-2009 Antoine Drouin <poinix@gmail.com>
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, write to
18  * the Free Software Foundation, 59 Temple Place - Suite 330,
19  * Boston, MA 02111-1307, USA.
20  */
21 
29 #define NAV_C
30 
32 
33 #include "pprz_debug.h"
34 #include "subsystems/gps.h" // needed by auto_nav from the flight plan
35 #include "subsystems/ins.h"
36 #include "state.h"
37 
39 #include "generated/modules.h"
40 #include "generated/flight_plan.h"
41 
42 /* for default GUIDANCE_H_USE_REF */
44 
45 #include "math/pprz_algebra_int.h"
46 
48 #include "messages.h"
49 #include "mcu_periph/uart.h"
50 
51 const uint8_t nb_waypoint = NB_WAYPOINT;
52 struct EnuCoor_i waypoints[NB_WAYPOINT];
53 
56 
58 
60 
62 #ifndef FAILSAFE_MODE_DISTANCE
63 #define FAILSAFE_MODE_DISTANCE (1.5*MAX_DIST_FROM_HOME)
64 #endif
65 
66 const float max_dist_from_home = MAX_DIST_FROM_HOME;
67 const float max_dist2_from_home = MAX_DIST_FROM_HOME * MAX_DIST_FROM_HOME;
71 
73 
75 struct EnuCoor_i nav_segment_start, nav_segment_end;
78 
81 
84 float nav_radius;
85 
87 #ifndef DEFAULT_CIRCLE_RADIUS
88 #define DEFAULT_CIRCLE_RADIUS 5.
89 #endif
90 
95 
96 static inline void nav_set_altitude( void );
97 
98 #define CLOSE_TO_WAYPOINT (15 << 8)
99 #define CARROT_DIST (12 << 8)
100 
102 #ifndef ARRIVED_AT_WAYPOINT
103 #define ARRIVED_AT_WAYPOINT 3.0
104 #endif
105 
106 #if PERIODIC_TELEMETRY
108 
109 static void send_nav_status(void) {
110  float dist_home = sqrtf(dist2_to_home);
111  float dist_wp = sqrtf(dist2_to_wp);
112  DOWNLINK_SEND_ROTORCRAFT_NAV_STATUS(DefaultChannel, DefaultDevice,
114  &dist_home, &dist_wp,
115  &nav_block, &nav_stage,
116  &horizontal_mode);
118  float sx = POS_FLOAT_OF_BFP(nav_segment_start.x);
119  float sy = POS_FLOAT_OF_BFP(nav_segment_start.y);
120  float ex = POS_FLOAT_OF_BFP(nav_segment_end.x);
121  float ey = POS_FLOAT_OF_BFP(nav_segment_end.y);
122  DOWNLINK_SEND_SEGMENT(DefaultChannel, DefaultDevice, &sx, &sy, &ex, &ey);
123  }
128  DOWNLINK_SEND_CIRCLE(DefaultChannel, DefaultDevice, &cx, &cy, &r);
129  }
130 }
131 
132 static void send_wp_moved(void) {
133  static uint8_t i;
134  i++; if (i >= nb_waypoint) i = 0;
135  DOWNLINK_SEND_WP_MOVED_ENU(DefaultChannel, DefaultDevice,
136  &i,
137  &(waypoints[i].x),
138  &(waypoints[i].y),
139  &(waypoints[i].z));
140 }
141 #endif
142 
143 void nav_init(void) {
144  // convert to
145  const struct EnuCoor_f wp_tmp_float[NB_WAYPOINT] = WAYPOINTS_ENU;
146  // init int32 waypoints
147  uint8_t i = 0;
148  for (i = 0; i < nb_waypoint; i++) {
149  waypoints[i].x = POS_BFP_OF_REAL(wp_tmp_float[i].x);
150  waypoints[i].y = POS_BFP_OF_REAL(wp_tmp_float[i].y);
151  waypoints[i].z = POS_BFP_OF_REAL(wp_tmp_float[i].z);
152  }
153  nav_block = 0;
154  nav_stage = 0;
155  nav_altitude = POS_BFP_OF_REAL(SECURITY_HEIGHT);
157  flight_altitude = SECURITY_ALT;
160 
163 
164  nav_roll = 0;
165  nav_pitch = 0;
166  nav_heading = 0;
168  nav_throttle = 0;
169  nav_climb = 0;
170  nav_leg_progress = 0;
171  nav_leg_length = 1;
172 
174  dist2_to_home = 0;
175  dist2_to_wp = 0;
176 
177 #if PERIODIC_TELEMETRY
178  register_periodic_telemetry(DefaultPeriodic, "ROTORCRAFT_NAV_STATUS", send_nav_status);
179  register_periodic_telemetry(DefaultPeriodic, "WP_MOVED", send_wp_moved);
180 #endif
181 }
182 
183 static inline void UNUSED nav_advance_carrot(void) {
184  struct EnuCoor_i *pos = stateGetPositionEnu_i();
185  /* compute a vector to the waypoint */
186  struct Int32Vect2 path_to_waypoint;
187  VECT2_DIFF(path_to_waypoint, navigation_target, *pos);
188 
189  /* saturate it */
190  VECT2_STRIM(path_to_waypoint, -(1<<15), (1<<15));
191 
192  int32_t dist_to_waypoint;
193  INT32_VECT2_NORM(dist_to_waypoint, path_to_waypoint);
194 
195  if (dist_to_waypoint < CLOSE_TO_WAYPOINT) {
197  }
198  else {
199  struct Int32Vect2 path_to_carrot;
200  VECT2_SMUL(path_to_carrot, path_to_waypoint, CARROT_DIST);
201  VECT2_SDIV(path_to_carrot, path_to_carrot, dist_to_waypoint);
202  VECT2_SUM(navigation_carrot, path_to_carrot, *pos);
203  }
204 }
205 
206 void nav_run(void) {
207 
208 #if GUIDANCE_H_USE_REF
209  // if GUIDANCE_H_USE_REF, CARROT_DIST is not used
211 #else
213 #endif
214 
216 }
217 
218 void nav_circle(struct EnuCoor_i * wp_center, int32_t radius) {
219  if (radius == 0) {
220  VECT2_COPY(navigation_target, *wp_center);
221  dist2_to_wp = get_dist2_to_point(wp_center);
222  }
223  else {
224  struct Int32Vect2 pos_diff;
225  VECT2_DIFF(pos_diff, *stateGetPositionEnu_i(), *wp_center);
226  // go back to half metric precision or values are too large
227  //INT32_VECT2_RSHIFT(pos_diff,pos_diff,INT32_POS_FRAC/2);
228  // store last qdr
229  int32_t last_qdr = nav_circle_qdr;
230  // compute qdr
231  INT32_ATAN2(nav_circle_qdr, pos_diff.y, pos_diff.x);
232  // increment circle radians
233  if (nav_circle_radians != 0) {
234  int32_t angle_diff = nav_circle_qdr - last_qdr;
235  INT32_ANGLE_NORMALIZE(angle_diff);
236  nav_circle_radians += angle_diff;
237  }
238  else {
239  // Smallest angle to increment at next step
240  nav_circle_radians = 1;
241  }
242 
243  // direction of rotation
244  int8_t sign_radius = radius > 0 ? 1 : -1;
245  // absolute radius
246  int32_t abs_radius = abs(radius);
247  // carrot_angle
248  int32_t carrot_angle = ((CARROT_DIST<<INT32_ANGLE_FRAC) / abs_radius);
249  Bound(carrot_angle, (INT32_ANGLE_PI / 16), INT32_ANGLE_PI_4);
250  carrot_angle = nav_circle_qdr - sign_radius * carrot_angle;
251  int32_t s_carrot, c_carrot;
252  PPRZ_ITRIG_SIN(s_carrot, carrot_angle);
253  PPRZ_ITRIG_COS(c_carrot, carrot_angle);
254  // compute setpoint
255  VECT2_ASSIGN(pos_diff, abs_radius * c_carrot, abs_radius * s_carrot);
256  INT32_VECT2_RSHIFT(pos_diff, pos_diff, INT32_TRIG_FRAC);
257  VECT2_SUM(navigation_target, *wp_center, pos_diff);
258  }
259  nav_circle_center = *wp_center;
262 }
263 
264 
265 void nav_route(struct EnuCoor_i * wp_start, struct EnuCoor_i * wp_end) {
266  struct Int32Vect2 wp_diff,pos_diff,wp_diff_prec;
267  VECT2_DIFF(wp_diff, *wp_end, *wp_start);
268  VECT2_DIFF(pos_diff, *stateGetPositionEnu_i(), *wp_start);
269  // go back to metric precision or values are too large
270  VECT2_COPY(wp_diff_prec, wp_diff);
271  INT32_VECT2_RSHIFT(wp_diff,wp_diff,INT32_POS_FRAC);
272  INT32_VECT2_RSHIFT(pos_diff,pos_diff,INT32_POS_FRAC);
273  int32_t leg_length2 = Max((wp_diff.x * wp_diff.x + wp_diff.y * wp_diff.y),1);
274  INT32_SQRT(nav_leg_length,leg_length2);
275  nav_leg_progress = (pos_diff.x * wp_diff.x + pos_diff.y * wp_diff.y) / nav_leg_length;
276  int32_t progress = Max((CARROT_DIST >> INT32_POS_FRAC), 0);
277  nav_leg_progress += progress;
278  int32_t prog_2 = nav_leg_length;
279  Bound(nav_leg_progress, 0, prog_2);
280  struct Int32Vect2 progress_pos;
281  VECT2_SMUL(progress_pos, wp_diff_prec, ((float)nav_leg_progress)/nav_leg_length);
282  VECT2_SUM(navigation_target, *wp_start, progress_pos);
283 
284  nav_segment_start = *wp_start;
285  nav_segment_end = *wp_end;
287 
289 }
290 
291 bool_t nav_approaching_from(struct EnuCoor_i * wp, struct EnuCoor_i * from, int16_t approaching_time) {
292  int32_t dist_to_point;
293  struct Int32Vect2 diff;
294  struct EnuCoor_i* pos = stateGetPositionEnu_i();
295 
296  /* if an approaching_time is given, estimate diff after approching_time secs */
297  if (approaching_time > 0) {
298  struct Int32Vect2 estimated_pos;
299  struct Int32Vect2 estimated_progress;
300  struct EnuCoor_i* speed = stateGetSpeedEnu_i();
301  VECT2_SMUL(estimated_progress, *speed, approaching_time);
302  INT32_VECT2_RSHIFT(estimated_progress, estimated_progress, (INT32_SPEED_FRAC - INT32_POS_FRAC));
303  VECT2_SUM(estimated_pos, *pos, estimated_progress);
304  VECT2_DIFF(diff, *wp, estimated_pos);
305  }
306  /* else use current position */
307  else {
308  VECT2_DIFF(diff, *wp, *pos);
309  }
310  /* compute distance of estimated/current pos to target wp
311  * distance with half metric precision (6.25 cm)
312  */
313  INT32_VECT2_RSHIFT(diff, diff, INT32_POS_FRAC/2);
314  INT32_VECT2_NORM(dist_to_point, diff);
315 
316  /* return TRUE if we have arrived */
317  if (dist_to_point < BFP_OF_REAL(ARRIVED_AT_WAYPOINT, INT32_POS_FRAC/2))
318  return TRUE;
319 
320  /* if coming from a valid waypoint */
321  if (from != NULL) {
322  /* return TRUE if normal line at the end of the segment is crossed */
323  struct Int32Vect2 from_diff;
324  VECT2_DIFF(from_diff, *wp, *from);
325  INT32_VECT2_RSHIFT(from_diff, from_diff, INT32_POS_FRAC/2);
326  return (diff.x * from_diff.x + diff.y * from_diff.y < 0);
327  }
328 
329  return FALSE;
330 }
331 
332 bool_t nav_check_wp_time(struct EnuCoor_i * wp, uint16_t stay_time) {
333  uint16_t time_at_wp;
334  int32_t dist_to_point;
335  static uint16_t wp_entry_time = 0;
336  static bool_t wp_reached = FALSE;
337  static struct EnuCoor_i wp_last = { 0, 0, 0 };
338  struct Int32Vect2 diff;
339 
340  if ((wp_last.x != wp->x) || (wp_last.y != wp->y)) {
341  wp_reached = FALSE;
342  wp_last = *wp;
343  }
344  VECT2_DIFF(diff, *wp, *stateGetPositionEnu_i());
345  INT32_VECT2_RSHIFT(diff, diff, INT32_POS_FRAC/2);
346  INT32_VECT2_NORM(dist_to_point, diff);
347  if (dist_to_point < BFP_OF_REAL(ARRIVED_AT_WAYPOINT, INT32_POS_FRAC/2)){
348  if (!wp_reached) {
349  wp_reached = TRUE;
350  wp_entry_time = autopilot_flight_time;
351  time_at_wp = 0;
352  }
353  else {
354  time_at_wp = autopilot_flight_time - wp_entry_time;
355  }
356  }
357  else {
358  time_at_wp = 0;
359  wp_reached = FALSE;
360  }
361  if (time_at_wp > stay_time) {
362  INT_VECT3_ZERO(wp_last);
363  return TRUE;
364  }
365  return FALSE;
366 }
367 
368 static inline void nav_set_altitude( void ) {
369  static int32_t last_nav_alt = 0;
370  if (abs(nav_altitude - last_nav_alt) > (POS_BFP_OF_REAL(0.2))) {
372  last_nav_alt = nav_altitude;
373  }
374 }
375 
376 
378 unit_t nav_reset_reference( void ) {
380  return 0;
381 }
382 
383 unit_t nav_reset_alt( void ) {
385  return 0;
386 }
387 
388 void nav_init_stage( void ) {
390  stage_time = 0;
391  nav_circle_radians = 0;
393 }
394 
395 #include <stdio.h>
396 void nav_periodic_task(void) {
397  RunOnceEvery(16, { stage_time++; block_time++; });
398 
399  dist2_to_wp = 0;
400 
401  /* from flight_plan.h */
402  auto_nav();
403 
404  /* run carrot loop */
405  nav_run();
406 }
407 
408 void nav_move_waypoint_lla(uint8_t wp_id, struct LlaCoor_i* new_lla_pos) {
410  struct EnuCoor_i enu;
411  enu_of_lla_point_i(&enu, &state.ned_origin_i, new_lla_pos);
412  // convert ENU pos from cm to BFP with INT32_POS_FRAC
413  enu.x = POS_BFP_OF_REAL(enu.x)/100;
414  enu.y = POS_BFP_OF_REAL(enu.y)/100;
415  enu.z = POS_BFP_OF_REAL(enu.z)/100;
416  nav_move_waypoint(wp_id, &enu);
417  }
418 }
419 
420 void nav_move_waypoint(uint8_t wp_id, struct EnuCoor_i * new_pos) {
421  if (wp_id < nb_waypoint) {
422  INT32_VECT3_COPY(waypoints[wp_id],(*new_pos));
423  DOWNLINK_SEND_WP_MOVED_ENU(DefaultChannel, DefaultDevice, &wp_id, &(new_pos->x),
424  &(new_pos->y), &(new_pos->z));
425  }
426 }
427 
428 void navigation_update_wp_from_speed(uint8_t wp, struct Int16Vect3 speed_sp, int16_t heading_rate_sp ) {
429  // MY_ASSERT(wp < nb_waypoint); FIXME
430  int32_t s_heading, c_heading;
431  PPRZ_ITRIG_SIN(s_heading, nav_heading);
432  PPRZ_ITRIG_COS(c_heading, nav_heading);
433  // FIXME : scale POS to SPEED
434  struct Int32Vect3 delta_pos;
435  VECT3_SDIV(delta_pos, speed_sp, NAV_FREQ); /* fixme :make sure the division is really a >> */
436  INT32_VECT3_RSHIFT(delta_pos, delta_pos, (INT32_SPEED_FRAC-INT32_POS_FRAC));
437  waypoints[wp].x += (s_heading * delta_pos.x + c_heading * delta_pos.y) >> INT32_TRIG_FRAC;
438  waypoints[wp].y += (c_heading * delta_pos.x - s_heading * delta_pos.y) >> INT32_TRIG_FRAC;
439  waypoints[wp].z += delta_pos.z;
440  int32_t delta_heading = heading_rate_sp / NAV_FREQ;
441  delta_heading = delta_heading >> (INT32_SPEED_FRAC-INT32_POS_FRAC);
442  nav_heading += delta_heading;
443 
444  INT32_COURSE_NORMALIZE(nav_heading);
445  RunOnceEvery(10,DOWNLINK_SEND_WP_MOVED_ENU(DefaultChannel, DefaultDevice, &wp, &(waypoints[wp].x), &(waypoints[wp].y), &(waypoints[wp].z)));
446 }
447 
448 bool_t nav_detect_ground(void) {
449  if (!autopilot_ground_detected) return FALSE;
451  return TRUE;
452 }
453 
454 bool_t nav_is_in_flight(void) {
455  return autopilot_in_flight;
456 }
457 
459 void nav_home(void) {
462 
464  nav_altitude = waypoints[WP_HOME].z;
466 
468 
469  /* run carrot loop */
470  nav_run();
471 }
472 
475  struct EnuCoor_f* pos = stateGetPositionEnu_f();
476  struct FloatVect2 pos_diff;
477  pos_diff.x = POS_FLOAT_OF_BFP(p->x) - pos->x;
478  pos_diff.y = POS_FLOAT_OF_BFP(p->y) - pos->y;
479  return pos_diff.x * pos_diff.x + pos_diff.y * pos_diff.y;
480 }
481 
484  return get_dist2_to_point(&waypoints[wp_id]);
485 }
486 
493 }
494 
496 bool_t nav_set_heading_rad(float rad) {
499  return FALSE;
500 }
501 
503 bool_t nav_set_heading_deg(float deg) {
504  return nav_set_heading_rad(RadOfDeg(deg));
505 }
506 
508 bool_t nav_set_heading_towards(float x, float y) {
509  struct FloatVect2 target = {x, y};
510  struct FloatVect2 pos_diff;
511  VECT2_DIFF(pos_diff, target, *stateGetPositionEnu_f());
512  // don't change heading if closer than 0.5m to target
513  if (FLOAT_VECT2_NORM2(pos_diff) > 0.25) {
514  float heading_f = atan2f(pos_diff.x, pos_diff.y);
515  nav_heading = ANGLE_BFP_OF_REAL(heading_f);
516  }
517  // return false so it can be called from the flightplan
518  // meaning it will continue to the next stage
519  return FALSE;
520 }
521 
525 }
526 
530  return FALSE;
531 }
uint16_t block_time
unsigned short uint16_t
Definition: types.h:16
#define INT32_VECT2_RSHIFT(_o, _i, _r)
void ins_reset_local_origin(void)
Reset the geographic reference to the current GPS fix.
arch independent UART (Universal Asynchronous Receiver/Transmitter) API
#define ANGLE_BFP_OF_REAL(_af)
bool_t autopilot_ground_detected
Definition: autopilot.c:60
Periodic telemetry system header (includes downlink utility and generated code).
#define INT32_POS_FRAC
uint16_t autopilot_flight_time
flight time in seconds.
Definition: autopilot.c:47
static float radius
Definition: chemotaxis.c:15
float z
in meters
#define INT32_SQRT(_out, _in)
uint8_t nav_block
Integrated Navigation System interface.
Autopilot modes.
#define INT32_ANGLE_PI_4
vector in Latitude, Longitude and Altitude
#define INT32_TRIG_FRAC
bool_t register_periodic_telemetry(struct pprz_telemetry *_pt, const char *_msg, telemetry_cb _cb)
Register a telemetry callback function.
Definition: telemetry.c:38
static struct EnuCoor_f * stateGetPositionEnu_f(void)
Get position in local ENU coordinates (float).
Definition: state.h:672
float y
in meters
#define FALSE
Definition: imu_chimu.h:141
int32_t z
Up.
#define PPRZ_ITRIG_SIN(_s, _a)
Definition: pprz_trig_int.h:40
void enu_of_lla_point_i(struct EnuCoor_i *enu, struct LtpDef_i *def, struct LlaCoor_i *lla)
#define Max(x, y)
#define INT32_ANGLE_FRAC
#define VECT2_DIFF(_c, _a, _b)
Definition: pprz_algebra.h:74
#define POS_FLOAT_OF_BFP(_ai)
#define INT32_ANGLE_PI
#define INT32_VECT3_RSHIFT(_o, _i, _r)
Device independent GPS code (interface)
bool_t autopilot_in_flight
Definition: autopilot.c:50
static bool_t stateIsLocalCoordinateValid(void)
Test if local coordinates are valid.
Definition: state.h:489
unsigned long uint32_t
Definition: types.h:18
#define INT32_ATAN2(_a, _y, _x)
signed short int16_t
Definition: types.h:17
float x
in meters
#define BFP_OF_REAL(_vr, _frac)
#define INT_VECT3_ZERO(_v)
#define FLOAT_VECT2_NORM2(_v)
#define INT32_ANGLE_NORMALIZE(_a)
uint16_t stage_time
In s.
#define INT32_VECT3_COPY(_o, _i)
signed long int32_t
Definition: types.h:19
#define INT32_VECT2_NORM(n, v)
#define TRUE
Definition: imu_chimu.h:144
uint8_t nav_stage
struct LtpDef_i ned_origin_i
Definition of the local (flat earth) coordinate system.
Definition: state.h:162
#define INT32_COURSE_NORMALIZE(_a)
#define VECT2_SUM(_c, _a, _b)
Definition: pprz_algebra.h:68
unsigned char uint8_t
Definition: types.h:14
API to get/set the generic vehicle states.
static struct EnuCoor_i * stateGetSpeedEnu_i(void)
Get ground speed in local ENU coordinates (int).
Definition: state.h:805
int32_t psi
in rad with INT32_ANGLE_FRAC
Horizontal guidance for rotorcrafts.
#define VECT2_SMUL(_vo, _vi, _s)
Definition: pprz_algebra.h:80
static float p[2][2]
vector in East North Up coordinates Units: meters
#define INT32_SPEED_FRAC
int32_t y
North.
static struct EnuCoor_i * stateGetPositionEnu_i(void)
Get position in local ENU coordinates (int).
Definition: state.h:637
signed char int8_t
Definition: types.h:15
#define VECT3_SDIV(_vo, _vi, _s)
Definition: pprz_algebra.h:185
static struct Int32Eulers * stateGetNedToBodyEulers_i(void)
Get vehicle body attitude euler angles (int).
Definition: state.h:1012
#define VECT2_ASSIGN(_a, _x, _y)
Definition: pprz_algebra.h:44
#define VECT2_SDIV(_vo, _vi, _s)
Definition: pprz_algebra.h:86
vector in East North Up coordinates
struct sockaddr_in pc_addr drone_at drone_nav from
Definition: at_com.c:43
#define VECT2_STRIM(_v, _min, _max)
Definition: pprz_algebra.h:92
#define VECT2_COPY(_a, _b)
Definition: pprz_algebra.h:50
void ins_reset_altitude_ref(void)
INS altitude reference reset.
int32_t x
East.
struct State state
Definition: state.c:36
#define PPRZ_ITRIG_COS(_c, _a)
Definition: pprz_trig_int.h:50
Paparazzi fixed point algebra.
#define POS_BFP_OF_REAL(_af)