Paparazzi UAS  v7.0_unstable
Paparazzi is a free software Unmanned Aircraft System.
gvf.c
Go to the documentation of this file.
1 /*
2  * Copyright (C) 2016 Hector Garcia de Marina
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  */
22 
23 #include <math.h>
24 #include "std.h"
25 
26 #include "gvf.h"
27 #include "gvf_low_level_control.h"
29 #include "trajectories/gvf_line.h"
30 #include "trajectories/gvf_sin.h"
31 #include "autopilot.h"
32 #include "../gvf_common.h"
33 
34 
35 // Control
37 
38 // State
40 
41 // Trajectory
44 
45 // Time variables to check if GVF is active
47 
48 // Param array lenght
49 int gvf_plen = 1;
50 int gvf_plen_wps = 0;
51 
52 #if PERIODIC_TELEMETRY
54 static void send_gvf(struct transport_tx *trans, struct link_device *dev)
55 {
56  uint8_t traj_type = (uint8_t)gvf_trajectory.type;
57 
59  uint32_t delta_T = now - gvf_t0;
60 
61  if (delta_T < 200) {
62  pprz_msg_send_GVF(trans, dev, AC_ID, &gvf_control.error, &traj_type,
64 
65 #if GVF_OCAML_GCS
66  if (gvf_trajectory.type == ELLIPSE &&
67  ((int)gvf_trajectory.p[2] == (int)gvf_trajectory.p[3])) {
68  pprz_msg_send_CIRCLE(trans, dev, AC_ID,
70  &gvf_trajectory.p[2]);
71  }
72 
73  if (gvf_trajectory.type == LINE && gvf_segment.seg == 1) {
74  pprz_msg_send_SEGMENT(trans, dev, AC_ID,
77  }
78 #endif // GVF_OCAML_GCS
79 
80  }
81 }
82 
83 #endif // PERIODIC_TELEMETRY
84 
85 static int out_of_segment_area(float x1, float y1, float x2, float y2, float d1, float d2)
86 {
87  struct EnuCoor_f *p = stateGetPositionEnu_f();
88  float px = p->x - x1;
89  float py = p->y - y1;
90 
91  float zx = x2 - x1;
92  float zy = y2 - y1;
93  float alpha = atan2f(zy, zx);
94 
95  float cosa = cosf(-alpha);
96  float sina = sinf(-alpha);
97 
98  float pxr = px * cosa - py * sina;
99  float zxr = zx * cosa - zy * sina;
100 
101  int s = 0;
102 
103  if (pxr < -d1) {
104  s = 1;
105  } else if (pxr > (zxr + d2)) {
106  s = -1;
107  }
108 
109  if (zy < 0) {
110  s *= -1;
111  }
112 
113  return s;
114 }
115 
116 void gvf_init(void)
117 {
118  gvf_control.ke = 1;
119  gvf_control.kn = 1;
120  gvf_control.s = 1;
122 
123 #if PERIODIC_TELEMETRY
125 #endif
126 }
127 
128 // GENERIC TRAJECTORY CONTROLLER
129 void gvf_control_2D(float ke, float kn, float e,
130  struct gvf_grad *grad, struct gvf_Hess *hess)
131 {
133 
135  float course = gvf_state.course;
136  float px_dot = gvf_state.px_dot;
137  float py_dot = gvf_state.py_dot;
138 
139  int s = gvf_control.s;
140 
141  // gradient Phi
142  float nx = grad->nx;
143  float ny = grad->ny;
144 
145  // tangent to Phi
146  float tx = s * grad->ny;
147  float ty = -s * grad->nx;
148 
149  // Hessian
150  float H11 = hess->H11;
151  float H12 = hess->H12;
152  float H21 = hess->H21;
153  float H22 = hess->H22;
154 
155  // Calculation of the desired angular velocity in the vector field
156  float pdx_dot = tx - ke * e * nx;
157  float pdy_dot = ty - ke * e * ny;
158 
159  float Apd_dot_dot_x = -ke * (nx * px_dot + ny * py_dot) * nx;
160  float Apd_dot_dot_y = -ke * (nx * px_dot + ny * py_dot) * ny;
161 
162  float Bpd_dot_dot_x = ((-ke * e * H11) + s * H21) * px_dot
163  + ((-ke * e * H12) + s * H22) * py_dot;
164  float Bpd_dot_dot_y = -(s * H11 + (ke * e * H21)) * px_dot
165  - (s * H12 + (ke * e * H22)) * py_dot;
166 
167  float pd_dot_dot_x = Apd_dot_dot_x + Bpd_dot_dot_x;
168  float pd_dot_dot_y = Apd_dot_dot_y + Bpd_dot_dot_y;
169 
170  float norm_pd_dot = sqrtf(pdx_dot * pdx_dot + pdy_dot * pdy_dot);
171  float md_x = pdx_dot / norm_pd_dot;
172  float md_y = pdy_dot / norm_pd_dot;
173 
174  float md_dot_const = -(md_x * pd_dot_dot_y - md_y * pd_dot_dot_x)
175  / norm_pd_dot;
176 
177  float md_dot_x = md_y * md_dot_const;
178  float md_dot_y = -md_x * md_dot_const;
179 
180 
181  #ifdef ROTORCRAFT_FIRMWARE
182 
183  // Set nav for command
184 
185  // Use parameter kn as the speed command
186  nav.speed.x = md_x * kn;
187  nav.speed.y = md_y * kn;
188 
189 
190  // Acceleration induced by the field with speed set to kn (!WIP!)
191 #warning "Using GVF for rotorcraft is still experimental, proceed with caution"
192  float n_norm = sqrtf(nx*nx+ny*ny);
193  float hess_px_dot = px_dot * H11 + py_dot * H12;
194  float hess_py_dot = px_dot * H21 + py_dot * H22;
195 
196  float hess_pdx_dot = pdx_dot * H11 + pdy_dot * H12;
197  float hess_pdy_dot = pdx_dot * H21 + pdy_dot * H22;
198 
199  float curvature_correction = tx * hess_px_dot + ty * hess_py_dot / (n_norm * n_norm);
200  float accel_correction_x = kn * hess_py_dot / n_norm;
201  float accel_correction_y = - kn * hess_px_dot / n_norm;
202  float accel_cmd_x = accel_correction_x + px_dot * curvature_correction;
203  float accel_cmd_y = accel_correction_y + py_dot * curvature_correction;
204 
205  float speed_cmd_x = kn*tx / n_norm - ke * e * nx / (n_norm);
206  float speed_cmd_y = kn*ty / n_norm - ke * e * ny / (n_norm);
207 
208  // TODO don't change nav struct directly
209  nav.accel.x = accel_cmd_x + (speed_cmd_x - px_dot);
210  nav.accel.y = accel_cmd_y + (speed_cmd_y - py_dot);
211  nav.heading = atan2f(md_x,md_y);
212 
213  #else
214 
215  float omega_d = -(md_dot_x * md_y - md_dot_y * md_x);
216 
217  float mr_x = sinf(course);
218  float mr_y = cosf(course);
219 
220  float omega = omega_d + kn * (mr_x * md_y - mr_y * md_x);
221 
222  gvf_control.omega = omega;
223 
224  // From gvf_common.h TODO: derivative of curvature and ori_err
225  gvf_c_omega.omega = omega;
226  gvf_c_info.kappa = (nx*(H12*ny - nx*H22) + ny*(H21*nx - H11*ny))/powf(nx*nx + ny*ny,1.5);
227  gvf_c_info.ori_err = 1 - (md_x*cosf(course) + md_y*sinf(course));
229 
230  #endif
231 }
232 
234 {
235  gvf_control.s = s;
236 }
237 
238 // STRAIGHT LINE
239 
240 static void gvf_line(float a, float b, float heading)
241 {
242  float e;
243  struct gvf_grad grad_line;
244  struct gvf_Hess Hess_line;
245 
246  gvf_trajectory.type = 0;
247  gvf_trajectory.p[0] = a;
248  gvf_trajectory.p[1] = b;
249  gvf_trajectory.p[2] = heading;
250  gvf_plen = 3 + gvf_plen_wps;
251  gvf_plen_wps = 0;
252 
253  gvf_line_info(&e, &grad_line, &Hess_line);
255  gvf_control_2D(1e-2 * gvf_line_par.ke, gvf_line_par.kn, e, &grad_line, &Hess_line);
256 
257  gvf_control.error = e;
258 
259  gvf_setNavMode(GVF_MODE_WAYPOINT);
260 
261  gvf_segment.seg = 0;
262 }
263 
264 bool gvf_line_XY_heading(float a, float b, float heading)
265 {
267  gvf_line(a, b, heading);
268  return true;
269 }
270 
271 bool gvf_line_XY1_XY2(float x1, float y1, float x2, float y2)
272 {
273  if (gvf_plen_wps != 2) {
274  gvf_trajectory.p[3] = x2;
275  gvf_trajectory.p[4] = y2;
276  gvf_trajectory.p[5] = 0;
277  gvf_plen_wps = 3;
278  }
279 
280  float zx = x2 - x1;
281  float zy = y2 - y1;
282 
283  gvf_line_XY_heading(x1, y1, atan2f(zx, zy));
284 
285  gvf_setNavMode(GVF_MODE_ROUTE);
286  gvf_segment.seg = 1;
287  gvf_segment.x1 = x1;
288  gvf_segment.y1 = y1;
289  gvf_segment.x2 = x2;
290  gvf_segment.y2 = y2;
291 
292  return true;
293 }
294 
296 {
297  gvf_trajectory.p[3] = wp1;
298  gvf_trajectory.p[4] = wp2;
299  gvf_plen_wps = 2;
300 
301  float x1 = WaypointX(wp1);
302  float y1 = WaypointY(wp1);
303  float x2 = WaypointX(wp2);
304  float y2 = WaypointY(wp2);
305 
306  return gvf_line_XY1_XY2(x1, y1, x2, y2);
307 }
308 
309 bool gvf_segment_loop_XY1_XY2(float x1, float y1, float x2, float y2, float d1, float d2)
310 {
311  int s = out_of_segment_area(x1, y1, x2, y2, d1, d2);
312  if (s != 0) {
314  }
315 
316  float zx = x2 - x1;
317  float zy = y2 - y1;
318  float alpha = atanf(zx / zy);
319 
320  gvf_line(x1, y1, alpha);
321 
322  gvf_setNavMode(GVF_MODE_ROUTE);
323 
324  gvf_segment.seg = 1;
325  gvf_segment.x1 = x1;
326  gvf_segment.y1 = y1;
327  gvf_segment.x2 = x2;
328  gvf_segment.y2 = y2;
329 
330  return true;
331 }
332 
333 bool gvf_segment_loop_wp1_wp2(uint8_t wp1, uint8_t wp2, float d1, float d2)
334 {
335  gvf_trajectory.p[3] = wp1;
336  gvf_trajectory.p[4] = wp2;
337  gvf_trajectory.p[5] = d1;
338  gvf_trajectory.p[6] = d2;
339  gvf_plen_wps = 4;
340 
341  float x1 = WaypointX(wp1);
342  float y1 = WaypointY(wp1);
343  float x2 = WaypointX(wp2);
344  float y2 = WaypointY(wp2);
345 
346  return gvf_segment_loop_XY1_XY2(x1, y1, x2, y2, d1, d2);
347 }
348 
349 bool gvf_segment_XY1_XY2(float x1, float y1, float x2, float y2)
350 {
351  struct EnuCoor_f *p = stateGetPositionEnu_f();
352  float px = p->x - x1;
353  float py = p->y - y1;
354 
355  float zx = x2 - x1;
356  float zy = y2 - y1;
357 
358  float beta = atan2f(zy, zx);
359  float cosb = cosf(-beta);
360  float sinb = sinf(-beta);
361  float zxr = zx * cosb - zy * sinb;
362  float pxr = px * cosb - py * sinb;
363 
364  if ((zxr > 0 && pxr > zxr) || (zxr < 0 && pxr < zxr)) {
365  return false;
366  }
367 
368  return gvf_line_XY1_XY2(x1, y1, x2, y2);
369 }
370 
372 {
373  gvf_trajectory.p[3] = wp1;
374  gvf_trajectory.p[4] = wp2;
375  gvf_plen_wps = 2;
376 
377  float x1 = WaypointX(wp1);
378  float y1 = WaypointY(wp1);
379  float x2 = WaypointX(wp2);
380  float y2 = WaypointY(wp2);
381 
382  return gvf_segment_XY1_XY2(x1, y1, x2, y2);
383 }
384 
386 {
387  gvf_trajectory.p[3] = wp;
388  gvf_plen_wps = 1;
389 
390  heading = RadOfDeg(heading);
391 
392  float a = WaypointX(wp);
393  float b = WaypointY(wp);
394 
395  return gvf_line_XY_heading(a, b, heading);
396 }
397 
398 // ELLIPSE
399 
400 bool gvf_ellipse_XY(float x, float y, float a, float b, float alpha)
401 {
402  float e;
403  struct gvf_grad grad_ellipse;
404  struct gvf_Hess Hess_ellipse;
405 
406  gvf_trajectory.type = 1;
407  gvf_trajectory.p[0] = x;
408  gvf_trajectory.p[1] = y;
409  gvf_trajectory.p[2] = a;
410  gvf_trajectory.p[3] = b;
411  gvf_trajectory.p[4] = alpha;
412  gvf_plen = 5 + gvf_plen_wps;
413  gvf_plen_wps = 0;
414 
415  // SAFE MODE
416  if (a < 1 || b < 1) {
417  gvf_trajectory.p[2] = 60;
418  gvf_trajectory.p[3] = 60;
419  }
420 
421  if ((int)gvf_trajectory.p[2] == (int)gvf_trajectory.p[3]) {
422  gvf_setNavMode(GVF_MODE_CIRCLE);
423 
424  } else {
425  gvf_setNavMode(GVF_MODE_WAYPOINT);
426  }
427 
428  gvf_ellipse_info(&e, &grad_ellipse, &Hess_ellipse);
431  e, &grad_ellipse, &Hess_ellipse);
432 
433  gvf_control.error = e;
434 
435  return true;
436 }
437 
438 
439 bool gvf_ellipse_wp(uint8_t wp, float a, float b, float alpha)
440 {
441  gvf_trajectory.p[5] = wp;
442  gvf_plen_wps = 1;
443 
444  gvf_ellipse_XY(WaypointX(wp), WaypointY(wp), a, b, alpha);
445  return true;
446 }
447 
448 // SINUSOIDAL (if w = 0 and off = 0, then we just have the straight line case)
449 
450 bool gvf_sin_XY_alpha(float a, float b, float alpha, float w, float off, float A)
451 {
452  float e;
453  struct gvf_grad grad_line;
454  struct gvf_Hess Hess_line;
455 
456  gvf_trajectory.type = 2;
457  gvf_trajectory.p[0] = a;
458  gvf_trajectory.p[1] = b;
459  gvf_trajectory.p[2] = alpha;
460  gvf_trajectory.p[3] = w;
461  gvf_trajectory.p[4] = off;
462  gvf_trajectory.p[5] = A;
463  gvf_plen = 6 + gvf_plen_wps;
464  gvf_plen_wps = 0;
465 
466  gvf_sin_info(&e, &grad_line, &Hess_line);
468  gvf_control_2D(1e-2 * gvf_sin_par.ke, gvf_sin_par.kn, e, &grad_line, &Hess_line);
469 
470  gvf_control.error = e;
471 
472  return true;
473 }
474 
475 bool gvf_sin_wp1_wp2(uint8_t wp1, uint8_t wp2, float w, float off, float A)
476 {
477  w = 2 * M_PI * w;
478 
479  gvf_trajectory.p[6] = wp1;
480  gvf_trajectory.p[7] = wp2;
481  gvf_plen_wps = 2;
482 
483  float x1 = WaypointX(wp1);
484  float y1 = WaypointY(wp1);
485  float x2 = WaypointX(wp2);
486  float y2 = WaypointY(wp2);
487 
488  float zx = x1 - x2;
489  float zy = y1 - y2;
490 
491  float alpha = atanf(zy / zx);
492 
493  gvf_sin_XY_alpha(x1, y1, alpha, w, off, A);
494 
495  return true;
496 }
497 
498 bool gvf_sin_wp_alpha(uint8_t wp, float alpha, float w, float off, float A)
499 {
500  w = 2 * M_PI * w;
501  alpha = RadOfDeg(alpha);
502 
503  gvf_trajectory.p[6] = wp;
504  gvf_plen_wps = 1;
505 
506  float x = WaypointX(wp);
507  float y = WaypointY(wp);
508 
509  gvf_sin_XY_alpha(x, y, alpha, w, off, A);
510 
511  return true;
512 }
513 
static int16_t course[3]
Definition: airspeed_uADC.c:58
Core autopilot interface common to all firmwares.
static uint16_t d1
Definition: baro_MS5534A.c:202
static uint16_t d2
Definition: baro_MS5534A.c:202
uint32_t get_sys_time_msec(void)
Get the time in milliseconds since startup.
Definition: sys_time_arch.c:98
#define WaypointX(_wp)
Definition: common_nav.h:45
#define WaypointY(_wp)
Definition: common_nav.h:46
#define A
static struct EnuCoor_f * stateGetPositionEnu_f(void)
Get position in local ENU coordinates (float).
Definition: state.h:719
bool gvf_segment_XY1_XY2(float x1, float y1, float x2, float y2)
Definition: gvf.c:349
bool gvf_sin_XY_alpha(float a, float b, float alpha, float w, float off, float A)
Definition: gvf.c:450
bool gvf_segment_loop_XY1_XY2(float x1, float y1, float x2, float y2, float d1, float d2)
Definition: gvf.c:309
int gvf_plen
Definition: gvf.c:49
bool gvf_ellipse_wp(uint8_t wp, float a, float b, float alpha)
Definition: gvf.c:439
bool gvf_line_wp1_wp2(uint8_t wp1, uint8_t wp2)
Definition: gvf.c:295
int gvf_plen_wps
Definition: gvf.c:50
gvf_con gvf_control
Definition: gvf.c:36
bool gvf_sin_wp1_wp2(uint8_t wp1, uint8_t wp2, float w, float off, float A)
Definition: gvf.c:475
bool gvf_segment_wp1_wp2(uint8_t wp1, uint8_t wp2)
Definition: gvf.c:371
static int out_of_segment_area(float x1, float y1, float x2, float y2, float d1, float d2)
Definition: gvf.c:85
gvf_seg gvf_segment
Definition: gvf.c:43
bool gvf_sin_wp_alpha(uint8_t wp, float alpha, float w, float off, float A)
Definition: gvf.c:498
void gvf_init(void)
Definition: gvf.c:116
void gvf_control_2D(float ke, float kn, float e, struct gvf_grad *grad, struct gvf_Hess *hess)
Definition: gvf.c:129
static void send_gvf(struct transport_tx *trans, struct link_device *dev)
Definition: gvf.c:54
gvf_st gvf_state
Definition: gvf.c:39
void gvf_set_direction(int8_t s)
Definition: gvf.c:233
gvf_tra gvf_trajectory
Definition: gvf.c:42
bool gvf_segment_loop_wp1_wp2(uint8_t wp1, uint8_t wp2, float d1, float d2)
Definition: gvf.c:333
static void gvf_line(float a, float b, float heading)
Definition: gvf.c:240
bool gvf_ellipse_XY(float x, float y, float a, float b, float alpha)
Definition: gvf.c:400
bool gvf_line_XY_heading(float a, float b, float heading)
Definition: gvf.c:264
bool gvf_line_wp_heading(uint8_t wp, float heading)
Definition: gvf.c:385
bool gvf_line_XY1_XY2(float x1, float y1, float x2, float y2)
Definition: gvf.c:271
uint32_t gvf_t0
Definition: gvf.c:46
Guidance algorithm based on vector fields.
float ke
Definition: gvf.h:48
float x1
Definition: gvf.h:87
float H12
Definition: gvf.h:103
float error
Definition: gvf.h:50
float py_dot
Definition: gvf.h:60
enum trajectories type
Definition: gvf.h:73
float p[16]
Definition: gvf.h:74
float H21
Definition: gvf.h:105
int8_t s
Definition: gvf.h:52
float H11
Definition: gvf.h:102
@ ELLIPSE
Definition: gvf.h:67
@ LINE
Definition: gvf.h:66
float ny
Definition: gvf.h:97
float nx
Definition: gvf.h:96
int seg
Definition: gvf.h:86
float y1
Definition: gvf.h:88
float y2
Definition: gvf.h:90
float course
Definition: gvf.h:58
float px_dot
Definition: gvf.h:59
float x2
Definition: gvf.h:89
float kn
Definition: gvf.h:49
float H22
Definition: gvf.h:106
float omega
Definition: gvf.h:51
Definition: gvf.h:101
Definition: gvf.h:47
Definition: gvf.h:95
Definition: gvf.h:85
Definition: gvf.h:57
Definition: gvf.h:72
gvf_common_omega gvf_c_omega
Definition: gvf_common.c:23
gvf_common_params gvf_c_info
Definition: gvf_common.c:24
void gvf_ellipse_info(float *phi, struct gvf_grad *grad, struct gvf_Hess *hess)
Definition: gvf_ellipse.c:63
gvf_ell_par gvf_ellipse_par
Definition: gvf_ellipse.c:59
Guidance algorithm based on vector fields 2D Ellipse trajectory.
void gvf_line_info(float *phi, struct gvf_grad *grad, struct gvf_Hess *hess)
Definition: gvf_line.c:62
gvf_li_par gvf_line_par
Definition: gvf_line.c:59
Guidance algorithm based on vector fields 2D straight line trajectory.
float ke
Definition: gvf_line.h:41
float kn
Definition: gvf_line.h:42
void gvf_low_level_control_2D(float omega)
void gvf_low_level_getState(void)
Firmware dependent file for the guiding vector field algorithm for 2D trajectories.
gvf_s_par gvf_sin_par
Definition: gvf_sin.c:64
void gvf_sin_info(float *phi, struct gvf_grad *grad, struct gvf_Hess *hess)
Definition: gvf_sin.c:69
Guidance algorithm based on vector fields 2D sinusoidal trajectory.
float kn
Definition: gvf_sin.h:45
float ke
Definition: gvf_sin.h:44
static float p[2][2]
static uint32_t s
float y
in meters
float x
in meters
vector in East North Up coordinates Units: meters
struct RotorcraftNavigation nav
Definition: navigation.c:51
struct EnuCoor_f speed
speed setpoint (in m/s)
Definition: navigation.h:128
struct EnuCoor_f accel
accel setpoint (in m/s)
Definition: navigation.h:129
float heading
heading setpoint (in radians)
Definition: navigation.h:133
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
float alpha
Definition: textons.c:133
unsigned int uint32_t
Typedef defining 32 bit unsigned int type.
Definition: vl53l1_types.h:78
unsigned char uint8_t
Typedef defining 8 bit unsigned char type.
Definition: vl53l1_types.h:98
signed char int8_t
Typedef defining 8 bit char type.
Definition: vl53l1_types.h:103
float b
Definition: wedgebug.c:202
float heading
Definition: wedgebug.c:258