Paparazzi UAS  v5.15_devel-230-gc96ce27
Paparazzi is a free software Unmanned Aircraft System.
 All Data Structures Namespaces Files Functions Variables Typedefs Enumerations Enumerator Macros Modules Pages
nav_rover_base.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 
30 
34 static void nav_goto(struct EnuCoor_f *wp)
35 {
39 }
40 
41 static void nav_route(struct EnuCoor_f *wp_start, struct EnuCoor_f *wp_end)
42 {
43  struct FloatVect2 wp_diff, pos_diff;
44  VECT2_DIFF(wp_diff, *wp_end, *wp_start);
45  VECT2_DIFF(pos_diff, *stateGetPositionEnu_f(), *wp_start);
46  // leg length
47  float leg_length2 = Max((wp_diff.x * wp_diff.x + wp_diff.y * wp_diff.y), 0.1f);
48  nav_rover_base.goto_wp.leg_length = sqrtf(leg_length2);
49  // leg progress
50  nav_rover_base.goto_wp.leg_progress = (pos_diff.x * wp_diff.x + pos_diff.y * wp_diff.y) / nav_rover_base.goto_wp.leg_length;
52  // next pos on leg
53  struct FloatVect2 progress_pos;
55  VECT2_SUM(nav.target, *wp_start, progress_pos);
56 
57  nav_rover_base.goto_wp.from = *wp_start;
58  nav_rover_base.goto_wp.to = *wp_end;
61 }
62 
63 static bool nav_approaching(struct EnuCoor_f *wp, struct EnuCoor_f *from, float approaching_time)
64 {
65  float dist_to_point;
66  struct FloatVect2 diff;
67  struct EnuCoor_f *pos = stateGetPositionEnu_f();
68 
69  /* if an approaching_time is given, estimate diff after approching_time secs */
70  if (approaching_time > 0.f) {
71  struct FloatVect2 estimated_pos;
72  struct FloatVect2 estimated_progress;
73  struct EnuCoor_f *speed = stateGetSpeedEnu_f();
74  VECT2_SMUL(estimated_progress, *speed, approaching_time);
75  VECT2_SUM(estimated_pos, *pos, estimated_progress);
76  VECT2_DIFF(diff, *wp, estimated_pos);
77  }
78  /* else use current position */
79  else {
80  VECT2_DIFF(diff, *wp, *pos);
81  }
82  /* compute distance of estimated/current pos to target wp
83  */
84  //nav.dist_to_point = float_vect2_norm(&diff); FIXME
85  dist_to_point = float_vect2_norm(&diff);
86 
87  /* return TRUE if we have arrived */
88  if (dist_to_point < ARRIVED_AT_WAYPOINT) {
89  return true;
90  }
91 
92  /* if coming from a valid waypoint */
93  if (from != NULL) {
94  /* return TRUE if normal line at the end of the segment is crossed */
95  struct FloatVect2 from_diff;
96  VECT2_DIFF(from_diff, *wp, *from);
97  return (diff.x * from_diff.x + diff.y * from_diff.y < 0.f);
98  }
99 
100  return false;
101 }
102 
103 static void nav_circle(struct EnuCoor_f *wp_center, float radius)
104 {
105  struct FloatVect2 pos_diff;
106  VECT2_DIFF(pos_diff, *stateGetPositionEnu_f(), *wp_center);
107  // store last qdr
108  float last_qdr = nav_rover_base.circle.qdr;
109  // compute qdr
110  nav_rover_base.circle.qdr = atan2f(pos_diff.y, pos_diff.x);
111  // increment circle radians
112  float trigo_diff = nav_rover_base.circle.qdr - last_qdr;
113  NormRadAngle(trigo_diff);
114  nav_rover_base.circle.radians += trigo_diff;
115 
116  // direction of rotation
117  float sign_radius = radius > 0.f ? 1.f : -1.f;
118  // absolute radius
119  float abs_radius = fabsf(radius);
120  if (abs_radius > 0.1f) {
121  // carrot_angle
122  float carrot_angle = CARROT_DIST / abs_radius;
123  carrot_angle = Min(carrot_angle, M_PI / 4);
124  carrot_angle = Max(carrot_angle, M_PI / 16);
125  carrot_angle = nav_rover_base.circle.qdr - sign_radius * carrot_angle;
126  // compute setpoint
127  VECT2_ASSIGN(pos_diff, abs_radius * cosf(carrot_angle), abs_radius * sinf(carrot_angle));
128  VECT2_SUM(nav.target, *wp_center, pos_diff);
129  }
130  else {
131  // radius is too small, direct to center
132  VECT2_COPY(nav.target, *wp_center);
133  }
134 
135  nav_rover_base.circle.center = *wp_center;
136  nav_rover_base.circle.radius = radius;
138 }
139 
146 #ifndef LINE_START_FUNCTION
147 #define LINE_START_FUNCTION {}
148 #endif
149 #ifndef LINE_STOP_FUNCTION
150 #define LINE_STOP_FUNCTION {}
151 #endif
152 
153 static void _nav_oval_init(void)
154 {
157 }
158 
159 static void nav_oval(struct EnuCoor_f *wp1, struct EnuCoor_f *wp2, float radius)
160 {
161  (void) wp1;
162  (void) wp2;
163  (void) radius;
164 #if 0
165  radius = - radius; /* Historical error ? */
166  int32_t alt = waypoints[p1].enu_i.z;
167  waypoints[p2].enu_i.z = alt;
168 
169  float p2_p1_x = waypoints[p1].enu_f.x - waypoints[p2].enu_f.x;
170  float p2_p1_y = waypoints[p1].enu_f.y - waypoints[p2].enu_f.y;
171  float d = sqrtf(p2_p1_x * p2_p1_x + p2_p1_y * p2_p1_y);
172 
173  /* Unit vector from p1 to p2 */
174  int32_t u_x = POS_BFP_OF_REAL(p2_p1_x / d);
175  int32_t u_y = POS_BFP_OF_REAL(p2_p1_y / d);
176 
177  /* The half circle centers and the other leg */
178  struct EnuCoor_i p1_center = { waypoints[p1].enu_i.x + radius * -u_y,
179  waypoints[p1].enu_i.y + radius * u_x,
180  alt
181  };
182  struct EnuCoor_i p1_out = { waypoints[p1].enu_i.x + 2 * radius * -u_y,
183  waypoints[p1].enu_i.y + 2 * radius * u_x,
184  alt
185  };
186 
187  struct EnuCoor_i p2_in = { waypoints[p2].enu_i.x + 2 * radius * -u_y,
188  waypoints[p2].enu_i.y + 2 * radius * u_x,
189  alt
190  };
191  struct EnuCoor_i p2_center = { waypoints[p2].enu_i.x + radius * -u_y,
192  waypoints[p2].enu_i.y + radius * u_x,
193  alt
194  };
195 
196  int32_t qdr_out_2 = INT32_ANGLE_PI - int32_atan2_2(u_y, u_x);
197  int32_t qdr_out_1 = qdr_out_2 + INT32_ANGLE_PI;
198  if (radius < 0) {
199  qdr_out_2 += INT32_ANGLE_PI;
200  qdr_out_1 += INT32_ANGLE_PI;
201  }
202  int32_t qdr_anticipation = ANGLE_BFP_OF_REAL(radius > 0 ? -15 : 15);
203 
204  switch (oval_status) {
205  case OC1 :
206  nav_circle(&p1_center, POS_BFP_OF_REAL(-radius));
207  if (NavQdrCloseTo(INT32_DEG_OF_RAD(qdr_out_1) - qdr_anticipation)) {
208  oval_status = OR12;
209  InitStage();
211  }
212  return;
213 
214  case OR12:
215  nav_route(&p1_out, &p2_in);
216  if (nav_approaching_from(&p2_in, &p1_out, CARROT)) {
217  oval_status = OC2;
218  nav_oval_count++;
219  InitStage();
221  }
222  return;
223 
224  case OC2 :
225  nav_circle(&p2_center, POS_BFP_OF_REAL(-radius));
226  if (NavQdrCloseTo(INT32_DEG_OF_RAD(qdr_out_2) - qdr_anticipation)) {
227  oval_status = OR21;
228  InitStage();
230  }
231  return;
232 
233  case OR21:
234  nav_route(&waypoints[p2].enu_i, &waypoints[p1].enu_i);
235  if (nav_approaching_from(&waypoints[p1].enu_i, &waypoints[p2].enu_i, CARROT)) {
236  oval_status = OC1;
237  InitStage();
239  }
240  return;
241 
242  default: /* Should not occur !!! Doing nothing */
243  return;
244  }
245 #endif
246 }
247 
248 #if PERIODIC_TELEMETRY
250 
251 static void send_segment(struct transport_tx *trans, struct link_device *dev)
252 {
253  pprz_msg_send_SEGMENT(trans, dev, AC_ID,
258 }
259 
260 static void send_circle(struct transport_tx *trans, struct link_device *dev)
261 {
262  pprz_msg_send_CIRCLE(trans, dev, AC_ID,
266 }
267 
268 static void send_nav_status(struct transport_tx *trans, struct link_device *dev)
269 {
270  float dist_home = sqrtf(nav.dist2_to_home);
271  float dist_wp = sqrtf(nav_rover_base.goto_wp.dist2_to_wp);
272  pprz_msg_send_ROTORCRAFT_NAV_STATUS(trans, dev, AC_ID,
274  &dist_home, &dist_wp,
275  &nav_block, &nav_stage,
276  &nav.mode);
277  if (nav.mode == NAV_MODE_ROUTE) {
278  send_segment(trans, dev);
279  } else if (nav.mode == NAV_MODE_CIRCLE) {
280  send_circle(trans, dev);
281  }
282 }
283 #endif
284 
287 void nav_rover_init(void)
288 {
292 
293  // register nav functions
297 
298 #if PERIODIC_TELEMETRY
299  register_periodic_telemetry(DefaultPeriodic, PPRZ_MSG_ID_ROTORCRAFT_NAV_STATUS, send_nav_status);
300 #endif
301 
302 }
303 
304 
struct RoverNavigation nav
Definition: navigation.c:49
uint16_t block_time
#define Min(x, y)
Definition: esc_dshot.c:85
float x
Definition: common_nav.h:40
#define INT32_DEG_OF_RAD(_rad)
float dist2_to_home
squared distance to home waypoint
Definition: navigation.h:97
Periodic telemetry system header (includes downlink utility and generated code).
#define VECT2_SUM(_c, _a, _b)
Definition: pprz_algebra.h:86
vector in East North Up coordinates Units: meters
#define POS_BFP_OF_REAL(_af)
struct EnuCoor_f target
final target
Definition: navigation.h:88
#define VECT2_COPY(_a, _b)
Definition: pprz_algebra.h:68
#define InitStage()
#define VECT2_DIFF(_c, _a, _b)
Definition: pprz_algebra.h:92
uint8_t nav_block
#define DEFAULT_CIRCLE_RADIUS
default nav_circle_radius in meters
Definition: navigation.c:55
#define VECT2_ASSIGN(_a, _x, _y)
Definition: pprz_algebra.h:62
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
#define NAV_MODE_ROUTE
Definition: navigation.h:68
static struct EnuCoor_f * stateGetPositionEnu_f(void)
Get position in local ENU coordinates (float).
Definition: state.h:719
float y
Definition: common_nav.h:41
static float float_vect2_norm(struct FloatVect2 *v)
float x
in meters
#define NAV_MODE_WAYPOINT
Nav modes.
Definition: navigation.h:67
float get_dist2_to_point(struct EnuCoor_i *p)
Returns squared horizontal distance to given point.
Definition: navigation.c:455
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
#define Max(x, y)
Definition: main_fbw.c:53
uint16_t stage_time
In s.
#define VECT2_SMUL(_vo, _vi, _s)
Definition: pprz_algebra.h:98
signed long int32_t
Definition: types.h:19
#define ANGLE_BFP_OF_REAL(_af)
static const struct usb_device_descriptor dev
Definition: usb_ser_hw.c:73
uint8_t nav_stage
#define ARRIVED_AT_WAYPOINT
minimum horizontal distance to waypoint to mark as arrived
Definition: navigation.c:68
vector in East North Up coordinates
int32_t int32_atan2_2(int32_t y, int32_t x)
#define CARROT_DIST
Definition: navigation.c:77
static struct EnuCoor_f * stateGetSpeedEnu_f(void)
Get ground speed in local ENU coordinates (float).
Definition: state.h:917
struct point waypoints[NB_WAYPOINT]
size == nb_waypoint, waypoint 0 is a dummy waypoint
Definition: common_nav.c:38
#define INT32_ANGLE_PI
#define NAV_MODE_CIRCLE
Definition: navigation.h:69
float y
in meters
bool nav_approaching_from(struct EnuCoor_i *wp, struct EnuCoor_i *from, int16_t approaching_time)
Proximity tests on approaching a wp.
Definition: navigation.c:257
int8_t register_periodic_telemetry(struct periodic_telemetry *_pt, uint8_t _id, telemetry_cb _cb)
Register a telemetry callback function.
Definition: telemetry.c:46
#define CARROT
default approaching_time for a wp
Definition: navigation.h:40