Paparazzi UAS  v7.0_unstable
Paparazzi is a free software Unmanned Aircraft System.
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 {
38  nav.mode = NAV_MODE_WAYPOINT;
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);
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
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;
60  nav.mode = NAV_MODE_ROUTE;
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
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;
137  nav.mode = NAV_MODE_CIRCLE;
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 #if ROVER_BASE_SEND_TRAJECTORY
252 static void send_segment(struct transport_tx *trans, struct link_device *dev)
253 {
254  pprz_msg_send_SEGMENT(trans, dev, AC_ID,
259 }
260 
261 static void send_circle(struct transport_tx *trans, struct link_device *dev)
262 {
263  pprz_msg_send_CIRCLE(trans, dev, AC_ID,
267 }
268 #endif // ROVER_BASE_SEND_TRAJECTORY
269 
270 static void send_nav_status(struct transport_tx *trans, struct link_device *dev)
271 {
272  float dist_home = sqrtf(nav.dist2_to_home);
273  float dist_wp = sqrtf(nav_rover_base.goto_wp.dist2_to_wp);
274  pprz_msg_send_ROTORCRAFT_NAV_STATUS(trans, dev, AC_ID,
276  &dist_home, &dist_wp,
277  &nav_block, &nav_stage,
278  &nav.mode);
279 #if ROVER_BASE_SEND_TRAJECTORY
280  if (nav.mode == NAV_MODE_ROUTE) {
281  send_segment(trans, dev);
282  } else if (nav.mode == NAV_MODE_CIRCLE) {
283  send_circle(trans, dev);
284  }
285 #endif // ROVER_BASE_SEND_TRAJECTORY
286 }
287 #endif
288 
291 void nav_rover_init(void)
292 {
296 
297  // register nav functions
301 
302 #if PERIODIC_TELEMETRY
303  register_periodic_telemetry(DefaultPeriodic, PPRZ_MSG_ID_ROTORCRAFT_NAV_STATUS, send_nav_status);
304 #endif
305 
306 }
307 
308 
uint8_t nav_stage
uint8_t nav_block
uint16_t stage_time
In s.
uint16_t block_time
#define InitStage()
struct point waypoints[NB_WAYPOINT]
size == nb_waypoint, waypoint 0 is a dummy waypoint
Definition: common_nav.c:39
float y
Definition: common_nav.h:41
float x
Definition: common_nav.h:40
#define Min(x, y)
Definition: esc_dshot.c:101
static float float_vect2_norm(struct FloatVect2 *v)
#define VECT2_SMUL(_vo, _vi, _s)
Definition: pprz_algebra.h:98
#define VECT2_DIFF(_c, _a, _b)
Definition: pprz_algebra.h:92
#define VECT2_COPY(_a, _b)
Definition: pprz_algebra.h:68
#define VECT2_SUM(_c, _a, _b)
Definition: pprz_algebra.h:86
#define VECT2_ASSIGN(_a, _x, _y)
Definition: pprz_algebra.h:62
#define INT32_DEG_OF_RAD(_rad)
#define ANGLE_BFP_OF_REAL(_af)
#define INT32_ANGLE_PI
#define POS_BFP_OF_REAL(_af)
vector in East North Up coordinates
static struct EnuCoor_f * stateGetPositionEnu_f(void)
Get position in local ENU coordinates (float).
Definition: state.h:719
static struct EnuCoor_f * stateGetSpeedEnu_f(void)
Get ground speed in local ENU coordinates (float).
Definition: state.h:917
uint8_t nav_oval_count
Navigation along a figure O.
Definition: nav.c:755
static void send_circle(struct transport_tx *trans, struct link_device *dev)
Definition: nav.c:504
static void send_segment(struct transport_tx *trans, struct link_device *dev)
Definition: nav.c:512
oval_status
Definition: nav.h:60
@ OC2
Definition: nav.h:60
@ OC1
Definition: nav.h:60
@ OR12
Definition: nav.h:60
@ OR21
Definition: nav.h:60
#define NavQdrCloseTo(x)
True if x (in degrees) is close to the current QDR (less than 10 degrees)
Definition: nav.h:161
static void send_nav_status(struct transport_tx *trans, struct link_device *dev)
#define LINE_STOP_FUNCTION
static void nav_goto(struct EnuCoor_f *wp)
Implement basic nav function.
void nav_rover_init(void)
Init and register nav functions.
#define LINE_START_FUNCTION
Navigation along a figure O.
static void _nav_oval_init(void)
static void nav_route(struct EnuCoor_f *wp_start, struct EnuCoor_f *wp_end)
static void nav_circle(struct EnuCoor_f *wp_center, float radius)
struct RoverNavBase nav_rover_base
static bool nav_approaching(struct EnuCoor_f *wp, struct EnuCoor_f *from, float approaching_time)
static void nav_oval(struct EnuCoor_f *wp1, struct EnuCoor_f *wp2, float radius)
struct RoverNavCircle circle
uint8_t count
number of laps
float leg_progress
progress over leg
struct RoverNavOval oval
enum oval_status status
oval status
struct RoverNavGoto goto_wp
float radians
incremental angular distance
struct EnuCoor_f from
start WP position
float leg_length
leg length
struct EnuCoor_f center
center WP position
float qdr
qdr in radians
float dist2_to_wp
squared distance to next waypoint
struct EnuCoor_f to
end WP position
float radius
radius in meters
Basic Nav struct.
struct FloatVect2 pos_diff
float y
in meters
float x
in meters
vector in East North Up coordinates Units: meters
int32_t int32_atan2_2(int32_t y, int32_t x)
void nav_register_oval(navigation_oval_init _nav_oval_init, navigation_oval nav_oval)
Definition: navigation.c:402
struct RotorcraftNavigation nav
Definition: navigation.c:51
void nav_register_circle(navigation_circle nav_circle)
Definition: navigation.c:397
void nav_register_goto_wp(navigation_goto nav_goto, navigation_route nav_route, navigation_approaching nav_approaching)
Registering functions.
Definition: navigation.c:390
float get_dist2_to_point(struct EnuCoor_f *p)
Returns squared horizontal distance to given point.
Definition: navigation.c:306
float dist2_to_home
squared distance to home waypoint
Definition: navigation.h:142
#define DEFAULT_CIRCLE_RADIUS
default nav_circle_radius in meters
Definition: navigation.h:42
#define CARROT
default approaching_time for a wp
Definition: navigation.h:70
struct EnuCoor_f target
final target position (in meters)
Definition: navigation.h:126
#define ARRIVED_AT_WAYPOINT
minimum horizontal distance to waypoint to mark as arrived
Definition: navigation.h:55
#define NAV_MODE_CIRCLE
Definition: navigation.h:74
#define CARROT_DIST
Definition: navigation.h:43
#define NAV_MODE_ROUTE
Definition: navigation.h:73
#define NAV_MODE_WAYPOINT
Nav modes.
Definition: navigation.h:72
static const struct usb_device_descriptor dev
Definition: usb_ser_hw.c:74
int8_t register_periodic_telemetry(struct periodic_telemetry *_pt, uint8_t _id, telemetry_cb _cb)
Register a telemetry callback function.
Definition: telemetry.c:51
Periodic telemetry system header (includes downlink utility and generated code).
#define DefaultPeriodic
Set default periodic telemetry.
Definition: telemetry.h:66
int int32_t
Typedef defining 32 bit int type.
Definition: vl53l1_types.h:83