Paparazzi UAS  v4.2.2_stable-4-gcc32f65
Paparazzi is a free software Unmanned Aircraft System.
 All Data Structures Files Functions Variables Typedefs Enumerations Enumerator Macros Pages
navigation.c
Go to the documentation of this file.
1 /*
2  * $Id$
3  *
4  * Copyright (C) 2008-2009 Antoine Drouin <poinix@gmail.com>
5  *
6  * This file is part of paparazzi.
7  *
8  * paparazzi is free software; you can redistribute it and/or modify
9  * it under the terms of the GNU General Public License as published by
10  * the Free Software Foundation; either version 2, or (at your option)
11  * any later version.
12  *
13  * paparazzi is distributed in the hope that it will be useful,
14  * but WITHOUT ANY WARRANTY; without even the implied warranty of
15  * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the
16  * GNU General Public License for more details.
17  *
18  * You should have received a copy of the GNU General Public License
19  * along with paparazzi; see the file COPYING. If not, write to
20  * the Free Software Foundation, 59 Temple Place - Suite 330,
21  * Boston, MA 02111-1307, USA.
22  */
23 
24 #define NAV_C
25 
27 
28 #include "pprz_debug.h"
29 #include "subsystems/gps.h"
30 #include "subsystems/ins.h"
31 
33 #include "generated/modules.h"
34 #include "generated/flight_plan.h"
35 
36 #include "math/pprz_algebra_int.h"
37 
38 const uint8_t nb_waypoint = NB_WAYPOINT;
39 struct EnuCoor_f waypoints_float[NB_WAYPOINT] = WAYPOINTS;
40 struct EnuCoor_i waypoints[NB_WAYPOINT];
41 
44 
46 
47 uint8_t last_wp __attribute__ ((unused));
48 
50 
55 
58 
61 float nav_radius;
62 
63 #ifndef DEFAULT_CIRCLE_RADIUS
64 #define DEFAULT_CIRCLE_RADIUS 0.
65 #endif
66 
71 
72 static inline void nav_set_altitude( void );
73 
74 #define CLOSE_TO_WAYPOINT (15 << 8)
75 #define ARRIVED_AT_WAYPOINT (3 << 8)
76 #define CARROT_DIST (12 << 8)
77 
78 void nav_init(void) {
79  // init int32 waypoints
80  uint8_t i = 0;
81  for (i = 0; i < nb_waypoint; i++) {
82  waypoints[i].x = POS_BFP_OF_REAL(waypoints_float[i].x);
83  waypoints[i].y = POS_BFP_OF_REAL(waypoints_float[i].y);
84  waypoints[i].z = POS_BFP_OF_REAL((waypoints_float[i].z - GROUND_ALT));
85  }
86  nav_block = 0;
87  nav_stage = 0;
88  ground_alt = POS_BFP_OF_REAL(GROUND_ALT);
89  nav_altitude = POS_BFP_OF_REAL(SECURITY_HEIGHT);
90  nav_flight_altitude = nav_altitude;
91  flight_altitude = SECURITY_ALT;
94 
95  horizontal_mode = HORIZONTAL_MODE_WAYPOINT;
96  vertical_mode = VERTICAL_MODE_ALT;
97 
98  nav_roll = 0;
99  nav_pitch = 0;
100  nav_heading = 0;
101  nav_course = 0;
102  nav_radius = DEFAULT_CIRCLE_RADIUS;
103  nav_throttle = 0;
104  nav_climb = 0;
105  nav_leg_progress = 0;
106  nav_leg_length = 1;
107 
108 }
109 
110 void nav_run(void) {
111 
112  /* compute a vector to the waypoint */
113  struct Int32Vect2 path_to_waypoint;
114  VECT2_DIFF(path_to_waypoint, navigation_target, ins_enu_pos);
115 
116  /* saturate it */
117  VECT2_STRIM(path_to_waypoint, -(1<<15), (1<<15));
118 
119 #if !GUIDANCE_H_USE_REF
120  int32_t dist_to_waypoint;
121  INT32_VECT2_NORM(dist_to_waypoint, path_to_waypoint);
122 
123  if (dist_to_waypoint < CLOSE_TO_WAYPOINT) {
125  }
126  else {
127  struct Int32Vect2 path_to_carrot;
128  VECT2_SMUL(path_to_carrot, path_to_waypoint, CARROT_DIST);
129  VECT2_SDIV(path_to_carrot, path_to_carrot, dist_to_waypoint);
130  VECT2_SUM(navigation_carrot, path_to_carrot, ins_enu_pos);
131  }
132 #else
133  // if H_REF is used, CARROT_DIST is not used
135 #endif
136 
138 }
139 
140 void nav_circle(uint8_t wp_center, int32_t radius) {
141  if (radius == 0) {
143  }
144  else {
145  struct Int32Vect2 pos_diff;
146  VECT2_DIFF(pos_diff, ins_enu_pos,waypoints[wp_center]);
147  // go back to half metric precision or values are too large
148  //INT32_VECT2_RSHIFT(pos_diff,pos_diff,INT32_POS_FRAC/2);
149  // store last qdr
150  int32_t last_qdr = nav_circle_qdr;
151  // compute qdr
152  INT32_ATAN2(nav_circle_qdr, pos_diff.y, pos_diff.x);
153  // increment circle radians
154  if (nav_circle_radians != 0) {
155  int32_t angle_diff = nav_circle_qdr - last_qdr;
156  INT32_ANGLE_NORMALIZE(angle_diff);
157  nav_circle_radians += angle_diff;
158  }
159  else {
160  // Smallest angle to increment at next step
161  nav_circle_radians = 1;
162  }
163 
164  // direction of rotation
165  int8_t sign_radius = radius > 0 ? 1 : -1;
166  // absolute radius
167  int32_t abs_radius = abs(radius);
168  // carrot_angle
169  int32_t carrot_angle = ((CARROT_DIST<<INT32_ANGLE_FRAC) / abs_radius);
170  Bound(carrot_angle, (INT32_ANGLE_PI / 16), INT32_ANGLE_PI_4);
171  carrot_angle = nav_circle_qdr - sign_radius * carrot_angle;
172  int32_t s_carrot, c_carrot;
173  PPRZ_ITRIG_SIN(s_carrot, carrot_angle);
174  PPRZ_ITRIG_COS(c_carrot, carrot_angle);
175  // compute setpoint
176  VECT2_ASSIGN(pos_diff, abs_radius * c_carrot, abs_radius * s_carrot);
177  INT32_VECT2_RSHIFT(pos_diff, pos_diff, INT32_TRIG_FRAC);
178  VECT2_SUM(navigation_target, waypoints[wp_center], pos_diff);
179  }
180  nav_circle_centre = wp_center;
181  nav_circle_radius = radius;
182  horizontal_mode = HORIZONTAL_MODE_CIRCLE;
183 }
184 
185 
186 //#include "stdio.h"
187 void nav_route(uint8_t wp_start, uint8_t wp_end) {
188  struct Int32Vect2 wp_diff,pos_diff;
189  VECT2_DIFF(wp_diff, waypoints[wp_end],waypoints[wp_start]);
190  VECT2_DIFF(pos_diff, ins_enu_pos,waypoints[wp_start]);
191  // go back to metric precision or values are too large
192  INT32_VECT2_RSHIFT(wp_diff,wp_diff,INT32_POS_FRAC);
193  INT32_VECT2_RSHIFT(pos_diff,pos_diff,INT32_POS_FRAC);
194  int32_t leg_length2 = Max((wp_diff.x * wp_diff.x + wp_diff.y * wp_diff.y),1);
195  INT32_SQRT(nav_leg_length,leg_length2);
196  nav_leg_progress = (pos_diff.x * wp_diff.x + pos_diff.y * wp_diff.y) / nav_leg_length;
197  int32_t progress = Max((CARROT_DIST >> INT32_POS_FRAC), 0);
198  nav_leg_progress += progress;
199  int32_t prog_2 = nav_leg_length;// + progress / 2;
200  Bound(nav_leg_progress, 0, prog_2);
201  struct Int32Vect2 progress_pos;
202  VECT2_SMUL(progress_pos, wp_diff, nav_leg_progress);
203  VECT2_SDIV(progress_pos, progress_pos, nav_leg_length);
204  INT32_VECT2_LSHIFT(progress_pos,progress_pos,INT32_POS_FRAC);
205  VECT2_SUM(navigation_target,waypoints[wp_start],progress_pos);
206  //printf("target %d %d | p %d %d | s %d %d | l %d %d %d\n",
207  // navigation_target.x,
208  // navigation_target.y,
209  // progress_pos.x,
210  // progress_pos.y,
211  // waypoints[wp_start].x,
212  // waypoints[wp_start].y,
213  // leg_length, leg_length2, nav_leg_progress);
214  //fflush(stdout);
215 
216  nav_segment_start = wp_start;
217  nav_segment_end = wp_end;
218  horizontal_mode = HORIZONTAL_MODE_ROUTE;
219 }
220 
221 bool_t nav_approaching_from(uint8_t wp_idx, uint8_t from_idx) {
222  int32_t dist_to_point;
223  struct Int32Vect2 diff;
224  static uint8_t time_at_wp = 0;
225  VECT2_DIFF(diff, waypoints[wp_idx], ins_enu_pos);
227  INT32_VECT2_NORM(dist_to_point, diff);
228  //printf("dist %d | %d %d\n", dist_to_point,diff.x,diff.y);
229  //fflush(stdout);
230  //if (dist_to_point < (ARRIVED_AT_WAYPOINT >> INT32_POS_FRAC)) return TRUE;
231  if (dist_to_point < (ARRIVED_AT_WAYPOINT >> INT32_POS_FRAC)) time_at_wp++;
232  else time_at_wp = 0;
233  if (time_at_wp > 20) return TRUE;
234  if (from_idx > 0 && from_idx < NB_WAYPOINT) {
235  struct Int32Vect2 from_diff;
236  VECT2_DIFF(from_diff, waypoints[wp_idx],waypoints[from_idx]);
237  INT32_VECT2_RSHIFT(from_diff,from_diff,INT32_POS_FRAC);
238  return (diff.x * from_diff.x + diff.y * from_diff.y < 0);
239  }
240  else return FALSE;
241 }
242 
243 static inline void nav_set_altitude( void ) {
244  static int32_t last_nav_alt = 0;
245  if (abs(nav_altitude - last_nav_alt) > (POS_BFP_OF_REAL(0.2))) {
246  nav_flight_altitude = nav_altitude;
247  last_nav_alt = nav_altitude;
248  }
249 }
250 
251 
253 unit_t nav_reset_reference( void ) {
257  return 0;
258 }
259 
260 unit_t nav_reset_alt( void ) {
262 
263 #if USE_GPS
266 #endif
267 
268  return 0;
269 }
270 
271 void nav_init_stage( void ) {
273  stage_time = 0;
274  nav_circle_radians = 0;
275  horizontal_mode = HORIZONTAL_MODE_WAYPOINT;
276 }
277 
278 #include <stdio.h>
280  RunOnceEvery(16, { stage_time++; block_time++; });
281 
282  /* from flight_plan.h */
283  auto_nav();
284 
285  /* run carrot loop */
286  nav_run();
287 
288  ground_alt = POS_BFP_OF_REAL((float)ins_ltp_def.hmsl / 1000.);
289 }
290 
292 #include "messages.h"
293 #include "mcu_periph/uart.h"
294 void nav_move_waypoint(uint8_t wp_id, struct EnuCoor_i * new_pos) {
295  if (wp_id < nb_waypoint) {
296  INT32_VECT3_COPY(waypoints[wp_id],(*new_pos));
297  DOWNLINK_SEND_WP_MOVED_ENU(DefaultChannel, DefaultDevice, &wp_id, &(new_pos->x), &(new_pos->y), &(new_pos->z));
298  }
299 }
300 
301 void navigation_update_wp_from_speed(uint8_t wp, struct Int16Vect3 speed_sp, int16_t heading_rate_sp ) {
302  // MY_ASSERT(wp < nb_waypoint); FIXME
303  int32_t s_heading, c_heading;
304  PPRZ_ITRIG_SIN(s_heading, nav_heading);
305  PPRZ_ITRIG_COS(c_heading, nav_heading);
306  // FIXME : scale POS to SPEED
307  struct Int32Vect3 delta_pos;
308  VECT3_SDIV(delta_pos, speed_sp,NAV_FREQ); /* fixme :make sure the division is really a >> */
309  INT32_VECT3_RSHIFT(delta_pos, delta_pos, (INT32_SPEED_FRAC-INT32_POS_FRAC));
310  waypoints[wp].x += (s_heading * delta_pos.x + c_heading * delta_pos.y) >> INT32_TRIG_FRAC;
311  waypoints[wp].y += (c_heading * delta_pos.x - s_heading * delta_pos.y) >> INT32_TRIG_FRAC;
312  waypoints[wp].z += delta_pos.z;
313  int32_t delta_heading = heading_rate_sp / NAV_FREQ;
314  delta_heading = delta_heading >> (INT32_SPEED_FRAC-INT32_POS_FRAC);
315  nav_heading += delta_heading;
316 
317  INT32_COURSE_NORMALIZE(nav_heading);
318  RunOnceEvery(10,DOWNLINK_SEND_WP_MOVED_ENU(DefaultChannel, DefaultDevice, &wp, &(waypoints[wp].x), &(waypoints[wp].y), &(waypoints[wp].z)));
319 }
320 
321 bool_t nav_detect_ground(void) {
322  if (!autopilot_detect_ground) return FALSE;
324  return TRUE;
325 }
326 
327 void nav_home(void) {}
uint16_t block_time
struct LlaCoor_i lla_pos
position in LLA (lat,lon: rad*1e7; alt: mm over ellipsoid)
Definition: gps.h:64
#define INT32_VECT2_RSHIFT(_o, _i, _r)
arch independent UART (Universal Asynchronous Receiver/Transmitter) API
#define INT32_POS_FRAC
struct LtpDef_i ins_ltp_def
Definition: ins.c:53
struct EnuCoor_i ins_enu_pos
Definition: ins.c:80
bool_t autopilot_detect_ground
Definition: autopilot.c:44
static float radius
Definition: chemotaxis.c:15
bool_t ins_vf_realign
Definition: ins.c:74
#define INT32_SQRT(_out, _in)
uint8_t nav_block
Device independent INS code.
struct LlaCoor_i lla
Reference point in lla.
int32_t hmsl
height above mean sea level in mm
Definition: gps.h:66
#define INT32_ANGLE_PI_4
#define INT32_TRIG_FRAC
#define FALSE
Definition: imu_chimu.h:141
#define PPRZ_ITRIG_SIN(_s, _a)
Definition: pprz_trig_int.h:42
#define Max(x, y)
#define INT32_ANGLE_FRAC
#define VECT2_DIFF(_c, _a, _b)
Definition: pprz_algebra.h:69
#define INT32_ANGLE_PI
#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
bool_t ins_hf_realign
Definition: ins.c:62
int32_t alt
in millimeters above WGS84 reference ellipsoid
#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
bool_t ins_ltp_initialised
Definition: ins.c:54
uint8_t nav_stage
#define INT32_COURSE_NORMALIZE(_a)
#define VECT2_SUM(_c, _a, _b)
Definition: pprz_algebra.h:63
#define INT32_VECT2_LSHIFT(_o, _i, _l)
unsigned char uint8_t
Definition: types.h:14
int32_t hmsl
Height above mean sea level in mm.
#define VECT2_SMUL(_vo, _vi, _s)
Definition: pprz_algebra.h:75
vector in East North Up coordinates Units: meters
#define INT32_SPEED_FRAC
signed char int8_t
Definition: types.h:15
#define VECT3_SDIV(_vo, _vi, _s)
Definition: pprz_algebra.h:169
#define VECT2_ASSIGN(_a, _x, _y)
Definition: pprz_algebra.h:39
#define VECT2_SDIV(_vo, _vi, _s)
Definition: pprz_algebra.h:81
vector in East North Up coordinates
#define VECT2_STRIM(_v, _min, _max)
Definition: pprz_algebra.h:87
#define VECT2_COPY(_a, _b)
Definition: pprz_algebra.h:45
struct GpsState gps
global GPS state
Definition: gps.c:31
#define PPRZ_ITRIG_COS(_c, _a)
Definition: pprz_trig_int.h:52
Paparazzi fixed point algebra.
#define POS_BFP_OF_REAL(_af)