Paparazzi UAS  v5.17_devel-114-gb0d9eaf
Paparazzi is a free software Unmanned Aircraft System.
 All Data Structures Namespaces Files Functions Variables Typedefs Enumerations Enumerator Macros Modules Pages
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 
30 
34 #include "autopilot.h"
35 
36 // Control
38 
39 // Trajectory
42 
43 // Time variables to check if GVF is active
45 
46 #if PERIODIC_TELEMETRY
48 static void send_gvf(struct transport_tx *trans, struct link_device *dev)
49 {
50  // Do not know whether is a good idea to do this check here or to include
51  // this plen in gvf_trajectory
52  int plen;
53 
54  switch (gvf_trajectory.type) {
55  case LINE:
56  plen = 3;
57  break;
58  case ELLIPSE:
59  plen = 5;
60  break;
61  case SIN:
62  plen = 6;
63  break;
64  default:
65  plen = 1;
66  break;
67  }
68 
69  uint8_t traj_type = (uint8_t)gvf_trajectory.type;
70 
72  uint32_t delta_T = now - gvf_t0;
73 
74  if (delta_T < 200)
75  pprz_msg_send_GVF(trans, dev, AC_ID, &gvf_control.error, &traj_type,
76  &gvf_control.s, &gvf_control.ke, plen, gvf_trajectory.p);
77 }
78 
79 static void send_circle(struct transport_tx *trans, struct link_device *dev)
80 {
82  uint32_t delta_T = now - gvf_t0;
83 
84  if (delta_T < 200)
85  if (gvf_trajectory.type == ELLIPSE &&
86  (gvf_trajectory.p[2] == gvf_trajectory.p[3])) {
87  pprz_msg_send_CIRCLE(trans, dev, AC_ID,
88  &gvf_trajectory.p[0], &gvf_trajectory.p[1],
89  &gvf_trajectory.p[2]);
90  }
91 }
92 
93 static void send_segment(struct transport_tx *trans, struct link_device *dev)
94 {
96  uint32_t delta_T = now - gvf_t0;
97 
98  if (delta_T < 200)
99  if (gvf_trajectory.type == LINE && gvf_segment.seg == 1) {
100  pprz_msg_send_SEGMENT(trans, dev, AC_ID,
101  &gvf_segment.x1, &gvf_segment.y1,
102  &gvf_segment.x2, &gvf_segment.y2);
103  }
104 }
105 
106 #endif
107 
108 static int out_of_segment_area(float x1, float y1, float x2, float y2, float d1, float d2)
109 {
110  struct EnuCoor_f *p = stateGetPositionEnu_f();
111  float px = p->x - x1;
112  float py = p->y - y1;
113 
114  float zx = x2 - x1;
115  float zy = y2 - y1;
116  float alpha = atan2f(zy, zx);
117 
118  float cosa = cosf(-alpha);
119  float sina = sinf(-alpha);
120 
121  float pxr = px * cosa - py * sina;
122  float zxr = zx * cosa - zy * sina;
123 
124  int s = 0;
125 
126  if (pxr < -d1) {
127  s = 1;
128  } else if (pxr > (zxr + d2)) {
129  s = -1;
130  }
131 
132  if (zy < 0) {
133  s *= -1;
134  }
135 
136  return s;
137 }
138 
139 void gvf_init(void)
140 {
141  gvf_control.ke = 1;
142  gvf_control.kn = 1;
143  gvf_control.s = 1;
144  gvf_trajectory.type = NONE;
145 
146 #if PERIODIC_TELEMETRY
150 #endif
151 }
152 
153 // GENERIC TRAJECTORY CONTROLLER
154 void gvf_control_2D(float ke, float kn, float e,
155  struct gvf_grad *grad, struct gvf_Hess *hess)
156 {
158 
159  struct FloatEulers *att = stateGetNedToBodyEulers_f();
160  float ground_speed = stateGetHorizontalSpeedNorm_f();
162  float px_dot = ground_speed * sinf(course);
163  float py_dot = ground_speed * cosf(course);
164  int s = gvf_control.s;
165 
166  // gradient Phi
167  float nx = grad->nx;
168  float ny = grad->ny;
169 
170  // tangent to Phi
171  float tx = s * grad->ny;
172  float ty = -s * grad->nx;
173 
174  // Hessian
175  float H11 = hess->H11;
176  float H12 = hess->H12;
177  float H21 = hess->H21;
178  float H22 = hess->H22;
179 
180  // Calculation of the desired angular velocity in the vector field
181  float pdx_dot = tx - ke * e * nx;
182  float pdy_dot = ty - ke * e * ny;
183 
184  float norm_pd_dot = sqrtf(pdx_dot * pdx_dot + pdy_dot * pdy_dot);
185  float md_x = pdx_dot / norm_pd_dot;
186  float md_y = pdy_dot / norm_pd_dot;
187 
188  float Apd_dot_dot_x = -ke * (nx * px_dot + ny * py_dot) * nx;
189  float Apd_dot_dot_y = -ke * (nx * px_dot + ny * py_dot) * ny;
190 
191  float Bpd_dot_dot_x = ((-ke * e * H11) + s * H21) * px_dot
192  + ((-ke * e * H12) + s * H22) * py_dot;
193  float Bpd_dot_dot_y = -(s * H11 + (ke * e * H21)) * px_dot
194  - (s * H12 + (ke * e * H22)) * py_dot;
195 
196  float pd_dot_dot_x = Apd_dot_dot_x + Bpd_dot_dot_x;
197  float pd_dot_dot_y = Apd_dot_dot_y + Bpd_dot_dot_y;
198 
199  float md_dot_const = -(md_x * pd_dot_dot_y - md_y * pd_dot_dot_x)
200  / norm_pd_dot;
201 
202  float md_dot_x = md_y * md_dot_const;
203  float md_dot_y = -md_x * md_dot_const;
204 
205  float omega_d = -(md_dot_x * md_y - md_dot_y * md_x);
206 
207  float mr_x = sinf(course);
208  float mr_y = cosf(course);
209 
210  float omega = omega_d + kn * (mr_x * md_y - mr_y * md_x);
211 
212  // Coordinated turn
215  -atanf(omega * ground_speed / GVF_GRAVITY / cosf(att->theta));
217 
219  }
220 }
221 
223 {
224  gvf_control.s = s;
225 }
226 
227 // STRAIGHT LINE
228 
229 static void gvf_line(float a, float b, float heading)
230 {
231  float e;
232  struct gvf_grad grad_line;
233  struct gvf_Hess Hess_line;
234 
235  gvf_trajectory.type = 0;
236  gvf_trajectory.p[0] = a;
237  gvf_trajectory.p[1] = b;
238  gvf_trajectory.p[2] = heading;
239 
240  gvf_line_info(&e, &grad_line, &Hess_line);
241  gvf_control.ke = gvf_line_par.ke;
242  gvf_control_2D(1e-2 * gvf_line_par.ke, gvf_line_par.kn, e, &grad_line, &Hess_line);
243 
244  gvf_control.error = e;
245 
247  gvf_segment.seg = 0;
248 }
249 
250 bool gvf_line_XY_heading(float a, float b, float heading)
251 {
253  gvf_line(a, b, heading);
254  return true;
255 }
256 
257 bool gvf_line_XY1_XY2(float x1, float y1, float x2, float y2)
258 {
259  float zx = x2 - x1;
260  float zy = y2 - y1;
261 
262  gvf_line_XY_heading(x1, y1, atan2f(zx, zy));
263 
265  gvf_segment.seg = 1;
266  gvf_segment.x1 = x1;
267  gvf_segment.y1 = y1;
268  gvf_segment.x2 = x2;
269  gvf_segment.y2 = y2;
270 
271  return true;
272 }
273 
275 {
276  float x1 = waypoints[wp1].x;
277  float y1 = waypoints[wp1].y;
278  float x2 = waypoints[wp2].x;
279  float y2 = waypoints[wp2].y;
280 
281  return gvf_line_XY1_XY2(x1, y1, x2, y2);
282 }
283 
284 bool gvf_segment_loop_XY1_XY2(float x1, float y1, float x2, float y2, float d1, float d2)
285 {
286  int s = out_of_segment_area(x1, y1, x2, y2, d1, d2);
287  if (s != 0) {
288  gvf_control.s = s;
289  }
290 
291  float zx = x2 - x1;
292  float zy = y2 - y1;
293  float alpha = atanf(zx / zy);
294 
295  gvf_line(x1, y1, alpha);
296 
298  gvf_segment.seg = 1;
299  gvf_segment.x1 = x1;
300  gvf_segment.y1 = y1;
301  gvf_segment.x2 = x2;
302  gvf_segment.y2 = y2;
303 
304  return true;
305 }
306 
307 bool gvf_segment_loop_wp1_wp2(uint8_t wp1, uint8_t wp2, float d1, float d2)
308 {
309  float x1 = waypoints[wp1].x;
310  float y1 = waypoints[wp1].y;
311  float x2 = waypoints[wp2].x;
312  float y2 = waypoints[wp2].y;
313 
314  return gvf_segment_loop_XY1_XY2(x1, y1, x2, y2, d1, d2);
315 }
316 
317 bool gvf_segment_XY1_XY2(float x1, float y1, float x2, float y2)
318 {
319  struct EnuCoor_f *p = stateGetPositionEnu_f();
320  float px = p->x - x1;
321  float py = p->y - y1;
322 
323  float zx = x2 - x1;
324  float zy = y2 - y1;
325 
326  float beta = atan2f(zy, zx);
327  float cosb = cosf(-beta);
328  float sinb = sinf(-beta);
329  float zxr = zx * cosb - zy * sinb;
330  float pxr = px * cosb - py * sinb;
331 
332  if ((zxr > 0 && pxr > zxr) || (zxr < 0 && pxr < zxr)) {
333  return false;
334  }
335 
336  return gvf_line_XY1_XY2(x1, y1, x2, y2);
337 }
338 
340 {
341  float x1 = waypoints[wp1].x;
342  float y1 = waypoints[wp1].y;
343  float x2 = waypoints[wp2].x;
344  float y2 = waypoints[wp2].y;
345 
346  return gvf_segment_XY1_XY2(x1, y1, x2, y2);
347 }
348 
350 {
351  heading = heading * M_PI / 180;
352 
353  float a = waypoints[wp].x;
354  float b = waypoints[wp].y;
355 
356  return gvf_line_XY_heading(a, b, heading);
357 }
358 
359 // ELLIPSE
360 
361 bool gvf_ellipse_XY(float x, float y, float a, float b, float alpha)
362 {
363  float e;
364  struct gvf_grad grad_ellipse;
365  struct gvf_Hess Hess_ellipse;
366 
367  gvf_trajectory.type = 1;
368  gvf_trajectory.p[0] = x;
369  gvf_trajectory.p[1] = y;
370  gvf_trajectory.p[2] = a;
371  gvf_trajectory.p[3] = b;
372  gvf_trajectory.p[4] = alpha;
373 
374  // SAFE MODE
375  if (a < 1 || b < 1) {
376  gvf_trajectory.p[2] = 60;
377  gvf_trajectory.p[3] = 60;
378  }
379 
380  if (gvf_trajectory.p[2] == gvf_trajectory.p[3]) {
382  } else {
384  }
385 
386  gvf_ellipse_info(&e, &grad_ellipse, &Hess_ellipse);
387  gvf_control.ke = gvf_ellipse_par.ke;
389  e, &grad_ellipse, &Hess_ellipse);
390 
391  gvf_control.error = e;
392 
393  return true;
394 }
395 
396 
397 bool gvf_ellipse_wp(uint8_t wp, float a, float b, float alpha)
398 {
399  gvf_ellipse_XY(waypoints[wp].x, waypoints[wp].y, a, b, alpha);
400  return true;
401 }
402 
403 // SINUSOIDAL (if w = 0 and off = 0, then we just have the straight line case)
404 
405 bool gvf_sin_XY_alpha(float a, float b, float alpha, float w, float off, float A)
406 {
407  float e;
408  struct gvf_grad grad_line;
409  struct gvf_Hess Hess_line;
410 
411  gvf_trajectory.type = 2;
412  gvf_trajectory.p[0] = a;
413  gvf_trajectory.p[1] = b;
414  gvf_trajectory.p[2] = alpha;
415  gvf_trajectory.p[3] = w;
416  gvf_trajectory.p[4] = off;
417  gvf_trajectory.p[5] = A;
418 
419  gvf_sin_info(&e, &grad_line, &Hess_line);
420  gvf_control.ke = gvf_sin_par.ke;
421  gvf_control_2D(1e-2 * gvf_sin_par.ke, gvf_sin_par.kn, e, &grad_line, &Hess_line);
422 
423  gvf_control.error = e;
424 
425  return true;
426 }
427 
428 bool gvf_sin_wp1_wp2(uint8_t wp1, uint8_t wp2, float w, float off, float A)
429 {
430  w = 2 * M_PI * w;
431 
432  float x1 = waypoints[wp1].x;
433  float y1 = waypoints[wp1].y;
434  float x2 = waypoints[wp2].x;
435  float y2 = waypoints[wp2].y;
436 
437  float zx = x1 - x2;
438  float zy = y1 - y2;
439 
440  float alpha = atanf(zy / zx);
441 
442  gvf_sin_XY_alpha(x1, y1, alpha, w, off, A);
443 
444  return true;
445 }
446 
447 bool gvf_sin_wp_alpha(uint8_t wp, float alpha, float w, float off, float A)
448 {
449  w = 2 * M_PI * w;
450  alpha = alpha * M_PI / 180;
451 
452  float x = waypoints[wp].x;
453  float y = waypoints[wp].y;
454 
455  gvf_sin_XY_alpha(x, y, alpha, w, off, A);
456 
457  return true;
458 }
459 
static uint16_t d2
Definition: baro_MS5534A.c:202
float H22
Definition: gvf.h:92
float y2
Definition: gvf.h:76
static void send_segment(struct transport_tx *trans, struct link_device *dev)
Definition: gvf.c:93
bool gvf_sin_wp_alpha(uint8_t wp, float alpha, float w, float off, float A)
Definition: gvf.c:447
static uint32_t s
static float stateGetHorizontalSpeedNorm_f(void)
Get norm of horizontal ground speed (float).
Definition: state.h:935
float x
Definition: common_nav.h:40
uint8_t lateral_mode
float p[16]
Definition: gvf.h:60
gvf_li_par gvf_line_par
Definition: gvf_line.c:59
float heading
Definition: wedgebug.c:258
static struct FloatEulers * stateGetNedToBodyEulers_f(void)
Get vehicle body attitude euler angles (float).
Definition: state.h:1143
bool gvf_line_XY_heading(float a, float b, float heading)
Definition: gvf.c:250
Periodic telemetry system header (includes downlink utility and generated code).
float H12
Definition: gvf.h:89
uint32_t get_sys_time_msec(void)
Get the time in milliseconds since startup.
Definition: sys_time_arch.c:78
Guidance algorithm based on vector fields 2D sinusoidal trajectory.
float ke
Definition: gvf.h:43
vector in East North Up coordinates Units: meters
Definition: gvf.h:71
float alpha
Definition: textons.c:107
gvf_s_par gvf_sin_par
Definition: gvf_sin.c:64
float ke
Definition: gvf_line.h:41
static uint16_t d1
Definition: baro_MS5534A.c:202
float H21
Definition: gvf.h:91
Definition: gvf.h:58
bool gvf_sin_wp1_wp2(uint8_t wp1, uint8_t wp2, float w, float off, float A)
Definition: gvf.c:428
Definition: gvf.h:87
bool gvf_segment_loop_XY1_XY2(float x1, float y1, float x2, float y2, float d1, float d2)
Definition: gvf.c:284
bool gvf_ellipse_XY(float x, float y, float a, float b, float alpha)
Definition: gvf.c:361
Guidance algorithm based on vector fields 2D straight line trajectory.
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:317
void gvf_line_info(float *phi, struct gvf_grad *grad, struct gvf_Hess *hess)
Definition: gvf_line.c:62
gvf_seg gvf_segment
Definition: gvf.c:41
euler angles
bool gvf_sin_XY_alpha(float a, float b, float alpha, float w, float off, float A)
Definition: gvf.c:405
float ke
Definition: gvf_sin.h:44
Fixed wing horizontal control.
float theta
in radians
Definition: gvf.h:53
static int out_of_segment_area(float x1, float y1, float x2, float y2, float d1, float d2)
Definition: gvf.c:108
void gvf_sin_info(float *phi, struct gvf_grad *grad, struct gvf_Hess *hess)
Definition: gvf_sin.c:69
float x1
Definition: gvf.h:73
float y
Definition: common_nav.h:41
#define AP_MODE_AUTO2
Definition: gvf.h:42
static void send_gvf(struct transport_tx *trans, struct link_device *dev)
Definition: gvf.c:48
float x
in meters
float ny
Definition: gvf.h:83
float nx
Definition: gvf.h:82
void gvf_init(void)
Definition: gvf.c:139
unsigned long uint32_t
Definition: types.h:18
Guidance algorithm based on vector fields.
float x2
Definition: gvf.h:75
Definition: gvf.h:55
#define DefaultPeriodic
Set default periodic telemetry.
Definition: telemetry.h:66
float y1
Definition: gvf.h:74
static void gvf_line(float a, float b, float heading)
Definition: gvf.c:229
float error
Definition: gvf.h:45
void gvf_set_direction(int8_t s)
Definition: gvf.c:222
float kn
Definition: gvf.h:44
float b
Definition: wedgebug.c:202
enum trajectories type
Definition: gvf.h:59
Guidance algorithm based on vector fields 2D Ellipse trajectory.
Definition: gvf.h:81
float h_ctl_roll_max_setpoint
void gvf_control_2D(float ke, float kn, float e, struct gvf_grad *grad, struct gvf_Hess *hess)
Definition: gvf.c:154
static const struct usb_device_descriptor dev
Definition: usb_ser_hw.c:74
gvf_ell_par gvf_ellipse_par
Definition: gvf_ellipse.c:59
bool gvf_ellipse_wp(uint8_t wp, float a, float b, float alpha)
Definition: gvf.c:397
Core autopilot interface common to all firmwares.
float kn
Definition: gvf_line.h:42
Definition: gvf.h:54
static float stateGetHorizontalSpeedDir_f(void)
Get dir of horizontal ground speed (float).
Definition: state.h:944
bool gvf_line_wp_heading(uint8_t wp, float heading)
Definition: gvf.c:349
Definition: gvf.h:52
unsigned char uint8_t
Definition: types.h:14
#define LATERAL_MODE_ROLL
float h_ctl_roll_setpoint
uint32_t gvf_t0
Definition: gvf.c:44
struct point waypoints[NB_WAYPOINT]
size == nb_waypoint, waypoint 0 is a dummy waypoint
Definition: common_nav.c:38
float kn
Definition: gvf_sin.h:45
static int16_t course[3]
Definition: airspeed_uADC.c:57
gvf_con gvf_control
Definition: gvf.c:37
gvf_tra gvf_trajectory
Definition: gvf.c:40
static float p[2][2]
#define A
#define GVF_GRAVITY
Definition: gvf.h:31
int seg
Definition: gvf.h:72
bool gvf_segment_wp1_wp2(uint8_t wp1, uint8_t wp2)
Definition: gvf.c:339
bool gvf_line_XY1_XY2(float x1, float y1, float x2, float y2)
Definition: gvf.c:257
signed char int8_t
Definition: types.h:15
float y
in meters
int8_t s
Definition: gvf.h:46
bool gvf_segment_loop_wp1_wp2(uint8_t wp1, uint8_t wp2, float d1, float d2)
Definition: gvf.c:307
int8_t register_periodic_telemetry(struct periodic_telemetry *_pt, uint8_t _id, telemetry_cb _cb)
Register a telemetry callback function.
Definition: telemetry.c:46
uint8_t autopilot_get_mode(void)
get autopilot mode
Definition: autopilot.c:184
float H11
Definition: gvf.h:88
static void send_circle(struct transport_tx *trans, struct link_device *dev)
Definition: gvf.c:79
void gvf_ellipse_info(float *phi, struct gvf_grad *grad, struct gvf_Hess *hess)
Definition: gvf_ellipse.c:63
bool gvf_line_wp1_wp2(uint8_t wp1, uint8_t wp2)
Definition: gvf.c:274