Paparazzi UAS  v5.0.5_stable-7-g4b8bbb7
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"
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 
47 const uint8_t nb_waypoint = NB_WAYPOINT;
48 struct EnuCoor_i waypoints[NB_WAYPOINT];
49 
52 
54 
55 uint8_t last_wp __attribute__ ((unused));
56 
58 
63 
66 
69 float nav_radius;
70 
71 #ifndef DEFAULT_CIRCLE_RADIUS
72 #define DEFAULT_CIRCLE_RADIUS 0.
73 #endif
74 
79 
80 static inline void nav_set_altitude( void );
81 
82 #define CLOSE_TO_WAYPOINT (15 << 8)
83 #define ARRIVED_AT_WAYPOINT (3 << 8)
84 #define CARROT_DIST (12 << 8)
85 
86 void nav_init(void) {
87  // convert to
88  const struct EnuCoor_f wp_tmp_float[NB_WAYPOINT] = WAYPOINTS;
89  // init int32 waypoints
90  uint8_t i = 0;
91  for (i = 0; i < nb_waypoint; i++) {
92  waypoints[i].x = POS_BFP_OF_REAL(wp_tmp_float[i].x);
93  waypoints[i].y = POS_BFP_OF_REAL(wp_tmp_float[i].y);
94  waypoints[i].z = POS_BFP_OF_REAL((wp_tmp_float[i].z - GROUND_ALT));
95  }
96  nav_block = 0;
97  nav_stage = 0;
98  ground_alt = POS_BFP_OF_REAL(GROUND_ALT);
99  nav_altitude = POS_BFP_OF_REAL(SECURITY_HEIGHT);
100  nav_flight_altitude = nav_altitude;
101  flight_altitude = SECURITY_ALT;
104 
105  horizontal_mode = HORIZONTAL_MODE_WAYPOINT;
106  vertical_mode = VERTICAL_MODE_ALT;
107 
108  nav_roll = 0;
109  nav_pitch = 0;
110  nav_heading = 0;
111  nav_course = 0;
112  nav_radius = DEFAULT_CIRCLE_RADIUS;
113  nav_throttle = 0;
114  nav_climb = 0;
115  nav_leg_progress = 0;
116  nav_leg_length = 1;
117 
118 }
119 
120 static inline void nav_advance_carrot(void) {
121  /* compute a vector to the waypoint */
122  struct Int32Vect2 path_to_waypoint;
123  VECT2_DIFF(path_to_waypoint, navigation_target, *stateGetPositionEnu_i());
124 
125  /* saturate it */
126  VECT2_STRIM(path_to_waypoint, -(1<<15), (1<<15));
127 
128  int32_t dist_to_waypoint;
129  INT32_VECT2_NORM(dist_to_waypoint, path_to_waypoint);
130 
131  if (dist_to_waypoint < CLOSE_TO_WAYPOINT) {
133  }
134  else {
135  struct Int32Vect2 path_to_carrot;
136  VECT2_SMUL(path_to_carrot, path_to_waypoint, CARROT_DIST);
137  VECT2_SDIV(path_to_carrot, path_to_carrot, dist_to_waypoint);
139  }
140 }
141 
142 void nav_run(void) {
143 
144 #if !GUIDANCE_H_USE_REF
145  PRINT_CONFIG_MSG("NOT using horizontal guidance reference :-(")
147 #else
148  PRINT_CONFIG_MSG("Using horizontal guidance reference :-)")
149  // if H_REF is used, CARROT_DIST is not used
151 #endif
152 
154 }
155 
156 void nav_circle(uint8_t wp_center, int32_t radius) {
157  if (radius == 0) {
159  }
160  else {
161  struct Int32Vect2 pos_diff;
162  VECT2_DIFF(pos_diff, *stateGetPositionEnu_i(), waypoints[wp_center]);
163  // go back to half metric precision or values are too large
164  //INT32_VECT2_RSHIFT(pos_diff,pos_diff,INT32_POS_FRAC/2);
165  // store last qdr
166  int32_t last_qdr = nav_circle_qdr;
167  // compute qdr
168  INT32_ATAN2(nav_circle_qdr, pos_diff.y, pos_diff.x);
169  // increment circle radians
170  if (nav_circle_radians != 0) {
171  int32_t angle_diff = nav_circle_qdr - last_qdr;
172  INT32_ANGLE_NORMALIZE(angle_diff);
173  nav_circle_radians += angle_diff;
174  }
175  else {
176  // Smallest angle to increment at next step
177  nav_circle_radians = 1;
178  }
179 
180  // direction of rotation
181  int8_t sign_radius = radius > 0 ? 1 : -1;
182  // absolute radius
183  int32_t abs_radius = abs(radius);
184  // carrot_angle
185  int32_t carrot_angle = ((CARROT_DIST<<INT32_ANGLE_FRAC) / abs_radius);
186  Bound(carrot_angle, (INT32_ANGLE_PI / 16), INT32_ANGLE_PI_4);
187  carrot_angle = nav_circle_qdr - sign_radius * carrot_angle;
188  int32_t s_carrot, c_carrot;
189  PPRZ_ITRIG_SIN(s_carrot, carrot_angle);
190  PPRZ_ITRIG_COS(c_carrot, carrot_angle);
191  // compute setpoint
192  VECT2_ASSIGN(pos_diff, abs_radius * c_carrot, abs_radius * s_carrot);
193  INT32_VECT2_RSHIFT(pos_diff, pos_diff, INT32_TRIG_FRAC);
194  VECT2_SUM(navigation_target, waypoints[wp_center], pos_diff);
195  }
196  nav_circle_centre = wp_center;
197  nav_circle_radius = radius;
198  horizontal_mode = HORIZONTAL_MODE_CIRCLE;
199 }
200 
201 
202 //#include "stdio.h"
203 void nav_route(uint8_t wp_start, uint8_t wp_end) {
204  struct Int32Vect2 wp_diff,pos_diff;
205  VECT2_DIFF(wp_diff, waypoints[wp_end],waypoints[wp_start]);
206  VECT2_DIFF(pos_diff, *stateGetPositionEnu_i(), waypoints[wp_start]);
207  // go back to metric precision or values are too large
208  INT32_VECT2_RSHIFT(wp_diff,wp_diff,INT32_POS_FRAC);
209  INT32_VECT2_RSHIFT(pos_diff,pos_diff,INT32_POS_FRAC);
210  int32_t leg_length2 = Max((wp_diff.x * wp_diff.x + wp_diff.y * wp_diff.y),1);
211  INT32_SQRT(nav_leg_length,leg_length2);
212  nav_leg_progress = (pos_diff.x * wp_diff.x + pos_diff.y * wp_diff.y) / nav_leg_length;
213  int32_t progress = Max((CARROT_DIST >> INT32_POS_FRAC), 0);
214  nav_leg_progress += progress;
215  int32_t prog_2 = nav_leg_length;// + progress / 2;
216  Bound(nav_leg_progress, 0, prog_2);
217  struct Int32Vect2 progress_pos;
218  VECT2_SMUL(progress_pos, wp_diff, nav_leg_progress);
219  VECT2_SDIV(progress_pos, progress_pos, nav_leg_length);
220  INT32_VECT2_LSHIFT(progress_pos,progress_pos,INT32_POS_FRAC);
221  VECT2_SUM(navigation_target,waypoints[wp_start],progress_pos);
222  //printf("target %d %d | p %d %d | s %d %d | l %d %d %d\n",
223  // navigation_target.x,
224  // navigation_target.y,
225  // progress_pos.x,
226  // progress_pos.y,
227  // waypoints[wp_start].x,
228  // waypoints[wp_start].y,
229  // leg_length, leg_length2, nav_leg_progress);
230  //fflush(stdout);
231 
232  nav_segment_start = wp_start;
233  nav_segment_end = wp_end;
234  horizontal_mode = HORIZONTAL_MODE_ROUTE;
235 }
236 
237 bool_t nav_approaching_from(uint8_t wp_idx, uint8_t from_idx) {
238  int32_t dist_to_point;
239  struct Int32Vect2 diff;
240  static uint8_t time_at_wp = 0;
241  VECT2_DIFF(diff, waypoints[wp_idx], *stateGetPositionEnu_i());
243  INT32_VECT2_NORM(dist_to_point, diff);
244  //printf("dist %d | %d %d\n", dist_to_point,diff.x,diff.y);
245  //fflush(stdout);
246  //if (dist_to_point < (ARRIVED_AT_WAYPOINT >> INT32_POS_FRAC)) return TRUE;
247  if (dist_to_point < (ARRIVED_AT_WAYPOINT >> INT32_POS_FRAC)) time_at_wp++;
248  else time_at_wp = 0;
249  if (time_at_wp > 20) return TRUE;
250  if (from_idx > 0 && from_idx < NB_WAYPOINT) {
251  struct Int32Vect2 from_diff;
252  VECT2_DIFF(from_diff, waypoints[wp_idx],waypoints[from_idx]);
253  INT32_VECT2_RSHIFT(from_diff,from_diff,INT32_POS_FRAC);
254  return (diff.x * from_diff.x + diff.y * from_diff.y < 0);
255  }
256  else return FALSE;
257 }
258 
259 static inline void nav_set_altitude( void ) {
260  static int32_t last_nav_alt = 0;
261  if (abs(nav_altitude - last_nav_alt) > (POS_BFP_OF_REAL(0.2))) {
262  nav_flight_altitude = nav_altitude;
263  last_nav_alt = nav_altitude;
264  }
265 }
266 
267 
269 unit_t nav_reset_reference( void ) {
271  ins.hf_realign = TRUE;
272  ins.vf_realign = TRUE;
273  return 0;
274 }
275 
276 unit_t nav_reset_alt( void ) {
277  ins.vf_realign = TRUE;
278 
279 #if USE_GPS
283 #endif
284 
285  return 0;
286 }
287 
288 void nav_init_stage( void ) {
290  stage_time = 0;
291  nav_circle_radians = 0;
292  horizontal_mode = HORIZONTAL_MODE_WAYPOINT;
293 }
294 
295 #include <stdio.h>
297  RunOnceEvery(16, { stage_time++; block_time++; });
298 
299  /* from flight_plan.h */
300  auto_nav();
301 
302  /* run carrot loop */
303  nav_run();
304 
305  ground_alt = POS_BFP_OF_REAL((float)ins_ltp_def.hmsl / 1000.);
306 }
307 
309 #include "messages.h"
310 #include "mcu_periph/uart.h"
311 void nav_move_waypoint(uint8_t wp_id, struct EnuCoor_i * new_pos) {
312  if (wp_id < nb_waypoint) {
313  INT32_VECT3_COPY(waypoints[wp_id],(*new_pos));
314  DOWNLINK_SEND_WP_MOVED_ENU(DefaultChannel, DefaultDevice, &wp_id, &(new_pos->x), &(new_pos->y), &(new_pos->z));
315  }
316 }
317 
318 void navigation_update_wp_from_speed(uint8_t wp, struct Int16Vect3 speed_sp, int16_t heading_rate_sp ) {
319  // MY_ASSERT(wp < nb_waypoint); FIXME
320  int32_t s_heading, c_heading;
321  PPRZ_ITRIG_SIN(s_heading, nav_heading);
322  PPRZ_ITRIG_COS(c_heading, nav_heading);
323  // FIXME : scale POS to SPEED
324  struct Int32Vect3 delta_pos;
325  VECT3_SDIV(delta_pos, speed_sp,NAV_FREQ); /* fixme :make sure the division is really a >> */
326  INT32_VECT3_RSHIFT(delta_pos, delta_pos, (INT32_SPEED_FRAC-INT32_POS_FRAC));
327  waypoints[wp].x += (s_heading * delta_pos.x + c_heading * delta_pos.y) >> INT32_TRIG_FRAC;
328  waypoints[wp].y += (c_heading * delta_pos.x - s_heading * delta_pos.y) >> INT32_TRIG_FRAC;
329  waypoints[wp].z += delta_pos.z;
330  int32_t delta_heading = heading_rate_sp / NAV_FREQ;
331  delta_heading = delta_heading >> (INT32_SPEED_FRAC-INT32_POS_FRAC);
332  nav_heading += delta_heading;
333 
334  INT32_COURSE_NORMALIZE(nav_heading);
335  RunOnceEvery(10,DOWNLINK_SEND_WP_MOVED_ENU(DefaultChannel, DefaultDevice, &wp, &(waypoints[wp].x), &(waypoints[wp].y), &(waypoints[wp].z)));
336 }
337 
338 bool_t nav_detect_ground(void) {
339  if (!autopilot_detect_ground) return FALSE;
341  return TRUE;
342 }
343 
344 void nav_home(void) {}
struct Ins ins
global INS state
Definition: ins.c:30
uint16_t block_time
struct LlaCoor_i lla_pos
position in LLA (lat,lon: rad*1e7; alt: mm over ellipsoid)
Definition: gps.h:65
#define INT32_VECT2_RSHIFT(_o, _i, _r)
arch independent UART (Universal Asynchronous Receiver/Transmitter) API
#define INT32_POS_FRAC
static float radius
Definition: chemotaxis.c:15
float z
in meters
#define INT32_SQRT(_out, _in)
uint8_t nav_block
Integrated Navigation System interface.
struct LlaCoor_i lla
Reference point in lla.
int32_t hmsl
height above mean sea level in mm
Definition: gps.h:67
Autopilot modes.
#define INT32_ANGLE_PI_4
#define INT32_TRIG_FRAC
bool_t hf_realign
realign horizontally if true
Definition: ins.h:46
float y
in meters
bool_t ins_ltp_initialised
Definition: ins_ardrone2.c:57
#define FALSE
Definition: imu_chimu.h:141
int32_t z
Up.
#define PPRZ_ITRIG_SIN(_s, _a)
Definition: pprz_trig_int.h:40
#define Max(x, y)
#define INT32_ANGLE_FRAC
#define VECT2_DIFF(_c, _a, _b)
Definition: pprz_algebra.h:67
#define INT32_ANGLE_PI
static void stateSetLocalOrigin_i(struct LtpDef_i *ltp_def)
Set the local (flat earth) coordinate frame origin (int).
Definition: state.h:440
#define INT32_VECT3_RSHIFT(_o, _i, _r)
Device independent GPS code (interface)
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
int32_t alt
in millimeters above WGS84 reference ellipsoid
bool_t autopilot_detect_ground
Definition: autopilot.c:52
#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
#define INT32_COURSE_NORMALIZE(_a)
#define VECT2_SUM(_c, _a, _b)
Definition: pprz_algebra.h:61
#define INT32_VECT2_LSHIFT(_o, _i, _l)
unsigned char uint8_t
Definition: types.h:14
API to get/set the generic vehicle states.
int32_t hmsl
Height above mean sea level in mm.
Horizontal guidance for rotorcrafts.
#define VECT2_SMUL(_vo, _vi, _s)
Definition: pprz_algebra.h:73
vector in East North Up coordinates Units: meters
struct LtpDef_i ins_ltp_def
Ins implementation state (fixed point)
Definition: ins_ardrone2.c:51
#define INT32_SPEED_FRAC
bool_t vf_realign
realign vertically if true
Definition: ins.h:47
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:167
#define VECT2_ASSIGN(_a, _x, _y)
Definition: pprz_algebra.h:37
#define VECT2_SDIV(_vo, _vi, _s)
Definition: pprz_algebra.h:79
vector in East North Up coordinates
#define VECT2_STRIM(_v, _min, _max)
Definition: pprz_algebra.h:85
#define VECT2_COPY(_a, _b)
Definition: pprz_algebra.h:43
struct GpsState gps
global GPS state
Definition: gps.c:33
int32_t x
East.
#define PPRZ_ITRIG_COS(_c, _a)
Definition: pprz_trig_int.h:50
Paparazzi fixed point algebra.
#define POS_BFP_OF_REAL(_af)