Paparazzi UAS  v6.3_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 
31 #include "autopilot.h"
32 
33 // Control
35 
36 // State
38 
39 // Trajectory
42 
43 // Time variables to check if GVF is active
45 
46 // Param array lenght
47 int gvf_plen = 1;
48 int gvf_plen_wps = 0;
49 
50 #if PERIODIC_TELEMETRY
52 static void send_gvf(struct transport_tx *trans, struct link_device *dev)
53 {
54  uint8_t traj_type = (uint8_t)gvf_trajectory.type;
55 
57  uint32_t delta_T = now - gvf_t0;
58 
59  if (delta_T < 200) {
60  pprz_msg_send_GVF(trans, dev, AC_ID, &gvf_control.error, &traj_type,
62 
63 #if GVF_OCAML_GCS
64  if (gvf_trajectory.type == ELLIPSE &&
65  ((int)gvf_trajectory.p[2] == (int)gvf_trajectory.p[3])) {
66  pprz_msg_send_CIRCLE(trans, dev, AC_ID,
68  &gvf_trajectory.p[2]);
69  }
70 
71  if (gvf_trajectory.type == LINE && gvf_segment.seg == 1) {
72  pprz_msg_send_SEGMENT(trans, dev, AC_ID,
75  }
76 #endif // GVF_OCAML_GCS
77 
78  }
79 }
80 
81 #endif // PERIODIC_TELEMETRY
82 
83 static int out_of_segment_area(float x1, float y1, float x2, float y2, float d1, float d2)
84 {
85  struct EnuCoor_f *p = stateGetPositionEnu_f();
86  float px = p->x - x1;
87  float py = p->y - y1;
88 
89  float zx = x2 - x1;
90  float zy = y2 - y1;
91  float alpha = atan2f(zy, zx);
92 
93  float cosa = cosf(-alpha);
94  float sina = sinf(-alpha);
95 
96  float pxr = px * cosa - py * sina;
97  float zxr = zx * cosa - zy * sina;
98 
99  int s = 0;
100 
101  if (pxr < -d1) {
102  s = 1;
103  } else if (pxr > (zxr + d2)) {
104  s = -1;
105  }
106 
107  if (zy < 0) {
108  s *= -1;
109  }
110 
111  return s;
112 }
113 
114 void gvf_init(void)
115 {
116  gvf_control.ke = 1;
117  gvf_control.kn = 1;
118  gvf_control.s = 1;
120 
121 #if PERIODIC_TELEMETRY
123 #endif
124 }
125 
126 // GENERIC TRAJECTORY CONTROLLER
127 void gvf_control_2D(float ke, float kn, float e,
128  struct gvf_grad *grad, struct gvf_Hess *hess)
129 {
131 
133  float course = gvf_state.course;
134  float px_dot = gvf_state.px_dot;
135  float py_dot = gvf_state.py_dot;
136 
137  int s = gvf_control.s;
138 
139  // gradient Phi
140  float nx = grad->nx;
141  float ny = grad->ny;
142 
143  // tangent to Phi
144  float tx = s * grad->ny;
145  float ty = -s * grad->nx;
146 
147  // Hessian
148  float H11 = hess->H11;
149  float H12 = hess->H12;
150  float H21 = hess->H21;
151  float H22 = hess->H22;
152 
153  // Calculation of the desired angular velocity in the vector field
154  float pdx_dot = tx - ke * e * nx;
155  float pdy_dot = ty - ke * e * ny;
156 
157  float norm_pd_dot = sqrtf(pdx_dot * pdx_dot + pdy_dot * pdy_dot);
158  float md_x = pdx_dot / norm_pd_dot;
159  float md_y = pdy_dot / norm_pd_dot;
160 
161  float Apd_dot_dot_x = -ke * (nx * px_dot + ny * py_dot) * nx;
162  float Apd_dot_dot_y = -ke * (nx * px_dot + ny * py_dot) * ny;
163 
164  float Bpd_dot_dot_x = ((-ke * e * H11) + s * H21) * px_dot
165  + ((-ke * e * H12) + s * H22) * py_dot;
166  float Bpd_dot_dot_y = -(s * H11 + (ke * e * H21)) * px_dot
167  - (s * H12 + (ke * e * H22)) * py_dot;
168 
169  float pd_dot_dot_x = Apd_dot_dot_x + Bpd_dot_dot_x;
170  float pd_dot_dot_y = Apd_dot_dot_y + Bpd_dot_dot_y;
171 
172  float md_dot_const = -(md_x * pd_dot_dot_y - md_y * pd_dot_dot_x)
173  / norm_pd_dot;
174 
175  float md_dot_x = md_y * md_dot_const;
176  float md_dot_y = -md_x * md_dot_const;
177 
178  float omega_d = -(md_dot_x * md_y - md_dot_y * md_x);
179 
180  float mr_x = sinf(course);
181  float mr_y = cosf(course);
182 
183  float omega = omega_d + kn * (mr_x * md_y - mr_y * md_x);
184 
185  gvf_control.omega = omega;
187 }
188 
190 {
191  gvf_control.s = s;
192 }
193 
194 // STRAIGHT LINE
195 
196 static void gvf_line(float a, float b, float heading)
197 {
198  float e;
199  struct gvf_grad grad_line;
200  struct gvf_Hess Hess_line;
201 
202  gvf_trajectory.type = 0;
203  gvf_trajectory.p[0] = a;
204  gvf_trajectory.p[1] = b;
205  gvf_trajectory.p[2] = heading;
206  gvf_plen = 3 + gvf_plen_wps;
207  gvf_plen_wps = 0;
208 
209  gvf_line_info(&e, &grad_line, &Hess_line);
211  gvf_control_2D(1e-2 * gvf_line_par.ke, gvf_line_par.kn, e, &grad_line, &Hess_line);
212 
213  gvf_control.error = e;
214 
215  gvf_setNavMode(GVF_MODE_WAYPOINT);
216 
217  gvf_segment.seg = 0;
218 }
219 
220 bool gvf_line_XY_heading(float a, float b, float heading)
221 {
223  gvf_line(a, b, heading);
224  return true;
225 }
226 
227 bool gvf_line_XY1_XY2(float x1, float y1, float x2, float y2)
228 {
229  if (gvf_plen_wps != 2) {
230  gvf_trajectory.p[3] = x2;
231  gvf_trajectory.p[4] = y2;
232  gvf_trajectory.p[5] = 0;
233  gvf_plen_wps = 3;
234  }
235 
236  float zx = x2 - x1;
237  float zy = y2 - y1;
238 
239  gvf_line_XY_heading(x1, y1, atan2f(zx, zy));
240 
241  gvf_setNavMode(GVF_MODE_ROUTE);
242  gvf_segment.seg = 1;
243  gvf_segment.x1 = x1;
244  gvf_segment.y1 = y1;
245  gvf_segment.x2 = x2;
246  gvf_segment.y2 = y2;
247 
248  return true;
249 }
250 
252 {
253  gvf_trajectory.p[3] = wp1;
254  gvf_trajectory.p[4] = wp2;
255  gvf_plen_wps = 2;
256 
257  float x1 = WaypointX(wp1);
258  float y1 = WaypointY(wp1);
259  float x2 = WaypointX(wp2);
260  float y2 = WaypointY(wp2);
261 
262  return gvf_line_XY1_XY2(x1, y1, x2, y2);
263 }
264 
265 bool gvf_segment_loop_XY1_XY2(float x1, float y1, float x2, float y2, float d1, float d2)
266 {
267  int s = out_of_segment_area(x1, y1, x2, y2, d1, d2);
268  if (s != 0) {
270  }
271 
272  float zx = x2 - x1;
273  float zy = y2 - y1;
274  float alpha = atanf(zx / zy);
275 
276  gvf_line(x1, y1, alpha);
277 
278  gvf_setNavMode(GVF_MODE_ROUTE);
279 
280  gvf_segment.seg = 1;
281  gvf_segment.x1 = x1;
282  gvf_segment.y1 = y1;
283  gvf_segment.x2 = x2;
284  gvf_segment.y2 = y2;
285 
286  return true;
287 }
288 
289 bool gvf_segment_loop_wp1_wp2(uint8_t wp1, uint8_t wp2, float d1, float d2)
290 {
291  gvf_trajectory.p[3] = wp1;
292  gvf_trajectory.p[4] = wp2;
293  gvf_trajectory.p[5] = d1;
294  gvf_trajectory.p[6] = d2;
295  gvf_plen_wps = 4;
296 
297  float x1 = WaypointX(wp1);
298  float y1 = WaypointY(wp1);
299  float x2 = WaypointX(wp2);
300  float y2 = WaypointY(wp2);
301 
302  return gvf_segment_loop_XY1_XY2(x1, y1, x2, y2, d1, d2);
303 }
304 
305 bool gvf_segment_XY1_XY2(float x1, float y1, float x2, float y2)
306 {
307  struct EnuCoor_f *p = stateGetPositionEnu_f();
308  float px = p->x - x1;
309  float py = p->y - y1;
310 
311  float zx = x2 - x1;
312  float zy = y2 - y1;
313 
314  float beta = atan2f(zy, zx);
315  float cosb = cosf(-beta);
316  float sinb = sinf(-beta);
317  float zxr = zx * cosb - zy * sinb;
318  float pxr = px * cosb - py * sinb;
319 
320  if ((zxr > 0 && pxr > zxr) || (zxr < 0 && pxr < zxr)) {
321  return false;
322  }
323 
324  return gvf_line_XY1_XY2(x1, y1, x2, y2);
325 }
326 
328 {
329  gvf_trajectory.p[3] = wp1;
330  gvf_trajectory.p[4] = wp2;
331  gvf_plen_wps = 2;
332 
333  float x1 = WaypointX(wp1);
334  float y1 = WaypointY(wp1);
335  float x2 = WaypointX(wp2);
336  float y2 = WaypointY(wp2);
337 
338  return gvf_segment_XY1_XY2(x1, y1, x2, y2);
339 }
340 
342 {
343  gvf_trajectory.p[3] = wp;
344  gvf_plen_wps = 1;
345 
346  heading = RadOfDeg(heading);
347 
348  float a = WaypointX(wp);
349  float b = WaypointY(wp);
350 
351  return gvf_line_XY_heading(a, b, heading);
352 }
353 
354 // ELLIPSE
355 
356 bool gvf_ellipse_XY(float x, float y, float a, float b, float alpha)
357 {
358  float e;
359  struct gvf_grad grad_ellipse;
360  struct gvf_Hess Hess_ellipse;
361 
362  gvf_trajectory.type = 1;
363  gvf_trajectory.p[0] = x;
364  gvf_trajectory.p[1] = y;
365  gvf_trajectory.p[2] = a;
366  gvf_trajectory.p[3] = b;
367  gvf_trajectory.p[4] = alpha;
368  gvf_plen = 5 + gvf_plen_wps;
369  gvf_plen_wps = 0;
370 
371  // SAFE MODE
372  if (a < 1 || b < 1) {
373  gvf_trajectory.p[2] = 60;
374  gvf_trajectory.p[3] = 60;
375  }
376 
377  if ((int)gvf_trajectory.p[2] == (int)gvf_trajectory.p[3]) {
378  gvf_setNavMode(GVF_MODE_CIRCLE);
379 
380  } else {
381  gvf_setNavMode(GVF_MODE_WAYPOINT);
382  }
383 
384  gvf_ellipse_info(&e, &grad_ellipse, &Hess_ellipse);
387  e, &grad_ellipse, &Hess_ellipse);
388 
389  gvf_control.error = e;
390 
391  return true;
392 }
393 
394 
395 bool gvf_ellipse_wp(uint8_t wp, float a, float b, float alpha)
396 {
397  gvf_trajectory.p[5] = wp;
398  gvf_plen_wps = 1;
399 
400  gvf_ellipse_XY(WaypointX(wp), WaypointY(wp), a, b, alpha);
401  return true;
402 }
403 
404 // SINUSOIDAL (if w = 0 and off = 0, then we just have the straight line case)
405 
406 bool gvf_sin_XY_alpha(float a, float b, float alpha, float w, float off, float A)
407 {
408  float e;
409  struct gvf_grad grad_line;
410  struct gvf_Hess Hess_line;
411 
412  gvf_trajectory.type = 2;
413  gvf_trajectory.p[0] = a;
414  gvf_trajectory.p[1] = b;
415  gvf_trajectory.p[2] = alpha;
416  gvf_trajectory.p[3] = w;
417  gvf_trajectory.p[4] = off;
418  gvf_trajectory.p[5] = A;
419  gvf_plen = 6 + gvf_plen_wps;
420  gvf_plen_wps = 0;
421 
422  gvf_sin_info(&e, &grad_line, &Hess_line);
424  gvf_control_2D(1e-2 * gvf_sin_par.ke, gvf_sin_par.kn, e, &grad_line, &Hess_line);
425 
426  gvf_control.error = e;
427 
428  return true;
429 }
430 
431 bool gvf_sin_wp1_wp2(uint8_t wp1, uint8_t wp2, float w, float off, float A)
432 {
433  w = 2 * M_PI * w;
434 
435  gvf_trajectory.p[6] = wp1;
436  gvf_trajectory.p[7] = wp2;
437  gvf_plen_wps = 2;
438 
439  float x1 = WaypointX(wp1);
440  float y1 = WaypointY(wp1);
441  float x2 = WaypointX(wp2);
442  float y2 = WaypointY(wp2);
443 
444  float zx = x1 - x2;
445  float zy = y1 - y2;
446 
447  float alpha = atanf(zy / zx);
448 
449  gvf_sin_XY_alpha(x1, y1, alpha, w, off, A);
450 
451  return true;
452 }
453 
454 bool gvf_sin_wp_alpha(uint8_t wp, float alpha, float w, float off, float A)
455 {
456  w = 2 * M_PI * w;
457  alpha = RadOfDeg(alpha);
458 
459  gvf_trajectory.p[6] = wp;
460  gvf_plen_wps = 1;
461 
462  float x = WaypointX(wp);
463  float y = WaypointY(wp);
464 
465  gvf_sin_XY_alpha(x, y, alpha, w, off, A);
466 
467  return true;
468 }
469 
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:305
bool gvf_sin_XY_alpha(float a, float b, float alpha, float w, float off, float A)
Definition: gvf.c:406
bool gvf_segment_loop_XY1_XY2(float x1, float y1, float x2, float y2, float d1, float d2)
Definition: gvf.c:265
int gvf_plen
Definition: gvf.c:47
bool gvf_ellipse_wp(uint8_t wp, float a, float b, float alpha)
Definition: gvf.c:395
bool gvf_line_wp1_wp2(uint8_t wp1, uint8_t wp2)
Definition: gvf.c:251
int gvf_plen_wps
Definition: gvf.c:48
gvf_con gvf_control
Definition: gvf.c:34
bool gvf_sin_wp1_wp2(uint8_t wp1, uint8_t wp2, float w, float off, float A)
Definition: gvf.c:431
bool gvf_segment_wp1_wp2(uint8_t wp1, uint8_t wp2)
Definition: gvf.c:327
static int out_of_segment_area(float x1, float y1, float x2, float y2, float d1, float d2)
Definition: gvf.c:83
gvf_seg gvf_segment
Definition: gvf.c:41
bool gvf_sin_wp_alpha(uint8_t wp, float alpha, float w, float off, float A)
Definition: gvf.c:454
void gvf_init(void)
Definition: gvf.c:114
void gvf_control_2D(float ke, float kn, float e, struct gvf_grad *grad, struct gvf_Hess *hess)
Definition: gvf.c:127
static void send_gvf(struct transport_tx *trans, struct link_device *dev)
Definition: gvf.c:52
gvf_st gvf_state
Definition: gvf.c:37
void gvf_set_direction(int8_t s)
Definition: gvf.c:189
gvf_tra gvf_trajectory
Definition: gvf.c:40
bool gvf_segment_loop_wp1_wp2(uint8_t wp1, uint8_t wp2, float d1, float d2)
Definition: gvf.c:289
static void gvf_line(float a, float b, float heading)
Definition: gvf.c:196
bool gvf_ellipse_XY(float x, float y, float a, float b, float alpha)
Definition: gvf.c:356
bool gvf_line_XY_heading(float a, float b, float heading)
Definition: gvf.c:220
bool gvf_line_wp_heading(uint8_t wp, float heading)
Definition: gvf.c:341
bool gvf_line_XY1_XY2(float x1, float y1, float x2, float y2)
Definition: gvf.c:227
uint32_t gvf_t0
Definition: gvf.c:44
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
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
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:132
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