Paparazzi UAS  v5.14.0_stable-0-g3f680d1
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 #if PERIODIC_TELEMETRY
45 static void send_gvf(struct transport_tx *trans, struct link_device *dev)
46 {
47  // Do not know whether is a good idea to do this check here or to include
48  // this plen in gvf_trajectory
49  int plen;
50 
51  switch (gvf_trajectory.type) {
52  case LINE:
53  plen = 3;
54  break;
55  case ELLIPSE:
56  plen = 5;
57  break;
58  case SIN:
59  plen = 6;
60  break;
61  default:
62  plen = 1;
63  break;
64  }
65 
66  uint8_t traj_type = (uint8_t)gvf_trajectory.type;
67 
68  pprz_msg_send_GVF(trans, dev, AC_ID, &gvf_control.error, &traj_type,
69  &gvf_control.s, &gvf_control.ke, plen, gvf_trajectory.p);
70 }
71 
72 static void send_circle(struct transport_tx *trans, struct link_device *dev)
73 {
74  if (gvf_trajectory.type == ELLIPSE &&
75  (gvf_trajectory.p[2] == gvf_trajectory.p[3])) {
76  pprz_msg_send_CIRCLE(trans, dev, AC_ID,
77  &gvf_trajectory.p[0], &gvf_trajectory.p[1],
78  &gvf_trajectory.p[2]);
79  }
80 }
81 
82 static void send_segment(struct transport_tx *trans, struct link_device *dev)
83 {
84  if (gvf_trajectory.type == LINE && gvf_segment.seg == 1) {
85  pprz_msg_send_SEGMENT(trans, dev, AC_ID,
86  &gvf_segment.x1, &gvf_segment.y1,
87  &gvf_segment.x2, &gvf_segment.y2);
88  }
89 }
90 
91 #endif
92 
93 static int out_of_segment_area(float x1, float y1, float x2, float y2, float d1, float d2)
94 {
95  struct EnuCoor_f *p = stateGetPositionEnu_f();
96  float px = p->x - x1;
97  float py = p->y - y1;
98 
99  float zx = x2 - x1;
100  float zy = y2 - y1;
101  float alpha = atan2f(zy, zx);
102 
103  float cosa = cosf(-alpha);
104  float sina = sinf(-alpha);
105 
106  float pxr = px * cosa - py * sina;
107  float zxr = zx * cosa - zy * sina;
108 
109  int s = 0;
110 
111  if (pxr < -d1) {
112  s = 1;
113  } else if (pxr > (zxr + d2)) {
114  s = -1;
115  }
116 
117  if (zy < 0) {
118  s *= -1;
119  }
120 
121  return s;
122 }
123 
124 void gvf_init(void)
125 {
126  gvf_control.ke = 1;
127  gvf_control.kn = 1;
128  gvf_control.s = 1;
129  gvf_trajectory.type = NONE;
130 
131 #if PERIODIC_TELEMETRY
135 #endif
136 }
137 
138 // GENERIC TRAJECTORY CONTROLLER
139 void gvf_control_2D(float ke, float kn, float e,
140  struct gvf_grad *grad, struct gvf_Hess *hess)
141 {
142  struct FloatEulers *att = stateGetNedToBodyEulers_f();
143  float ground_speed = stateGetHorizontalSpeedNorm_f();
145  float px_dot = ground_speed * sinf(course);
146  float py_dot = ground_speed * cosf(course);
147  int s = gvf_control.s;
148 
149  // gradient Phi
150  float nx = grad->nx;
151  float ny = grad->ny;
152 
153  // tangent to Phi
154  float tx = s * grad->ny;
155  float ty = -s * grad->nx;
156 
157  // Hessian
158  float H11 = hess->H11;
159  float H12 = hess->H12;
160  float H21 = hess->H21;
161  float H22 = hess->H22;
162 
163  // Calculation of the desired angular velocity in the vector field
164  float pdx_dot = tx - ke * e * nx;
165  float pdy_dot = ty - ke * e * ny;
166 
167  float norm_pd_dot = sqrtf(pdx_dot * pdx_dot + pdy_dot * pdy_dot);
168  float md_x = pdx_dot / norm_pd_dot;
169  float md_y = pdy_dot / norm_pd_dot;
170 
171  float Apd_dot_dot_x = -ke * (nx * px_dot + ny * py_dot) * nx;
172  float Apd_dot_dot_y = -ke * (nx * px_dot + ny * py_dot) * ny;
173 
174  float Bpd_dot_dot_x = ((-ke * e * H11) + s * H21) * px_dot
175  + ((-ke * e * H12) + s * H22) * py_dot;
176  float Bpd_dot_dot_y = -(s * H11 + (ke * e * H21)) * px_dot
177  - (s * H12 + (ke * e * H22)) * py_dot;
178 
179  float pd_dot_dot_x = Apd_dot_dot_x + Bpd_dot_dot_x;
180  float pd_dot_dot_y = Apd_dot_dot_y + Bpd_dot_dot_y;
181 
182  float md_dot_const = -(md_x * pd_dot_dot_y - md_y * pd_dot_dot_x)
183  / norm_pd_dot;
184 
185  float md_dot_x = md_y * md_dot_const;
186  float md_dot_y = -md_x * md_dot_const;
187 
188  float omega_d = -(md_dot_x * md_y - md_dot_y * md_x);
189 
190  float mr_x = sinf(course);
191  float mr_y = cosf(course);
192 
193  float omega = omega_d + kn * (mr_x * md_y - mr_y * md_x);
194 
195  // Coordinated turn
198  -atanf(omega * ground_speed / GVF_GRAVITY / cosf(att->theta));
200 
202  }
203 }
204 
206 {
207  gvf_control.s = s;
208 }
209 
210 // STRAIGHT LINE
211 
212 static void gvf_line(float a, float b, float heading)
213 {
214  float e;
215  struct gvf_grad grad_line;
216  struct gvf_Hess Hess_line;
217 
218  gvf_trajectory.type = 0;
219  gvf_trajectory.p[0] = a;
220  gvf_trajectory.p[1] = b;
221  gvf_trajectory.p[2] = heading;
222 
223  gvf_line_info(&e, &grad_line, &Hess_line);
224  gvf_control.ke = gvf_line_par.ke;
225  gvf_control_2D(1e-2 * gvf_line_par.ke, gvf_line_par.kn, e, &grad_line, &Hess_line);
226 
227  gvf_control.error = e;
228 
230  gvf_segment.seg = 0;
231 }
232 
233 bool gvf_line_XY_heading(float a, float b, float heading)
234 {
236  gvf_line(a, b, heading);
237  return true;
238 }
239 
240 bool gvf_line_XY1_XY2(float x1, float y1, float x2, float y2)
241 {
242  float zx = x2 - x1;
243  float zy = y2 - y1;
244 
245  float alpha = atanf(zx / zy);
246 
247  float beta = atan2f(zy, zx);
248  float cosb = cosf(-beta);
249  float sinb = sinf(-beta);
250  float zxr = zx * cosb - zy * sinb;
251  if((zxr > 0 && zy > 0) || (zxr < 0 && zy < 0)) {
253  } else {
254  gvf_set_direction(-1);
255  }
256 
257  gvf_line(x1, y1, alpha);
258 
260  gvf_segment.seg = 1;
261  gvf_segment.x1 = x1;
262  gvf_segment.y1 = y1;
263  gvf_segment.x2 = x2;
264  gvf_segment.y2 = y2;
265 
266  return true;
267 }
268 
270 {
271  float x1 = waypoints[wp1].x;
272  float y1 = waypoints[wp1].y;
273  float x2 = waypoints[wp2].x;
274  float y2 = waypoints[wp2].y;
275 
276  return gvf_line_XY1_XY2(x1, y1, x2, y2);
277 }
278 
279 bool gvf_segment_loop_XY1_XY2(float x1, float y1, float x2, float y2, float d1, float d2)
280 {
281  int s = out_of_segment_area(x1, y1, x2, y2, d1, d2);
282  if (s != 0) {
283  gvf_control.s = s;
284  }
285 
286  float zx = x2 - x1;
287  float zy = y2 - y1;
288  float alpha = atanf(zx / zy);
289 
290  gvf_line(x1, y1, alpha);
291 
293  gvf_segment.seg = 1;
294  gvf_segment.x1 = x1;
295  gvf_segment.y1 = y1;
296  gvf_segment.x2 = x2;
297  gvf_segment.y2 = y2;
298 
299  return true;
300 }
301 
302 bool gvf_segment_loop_wp1_wp2(uint8_t wp1, uint8_t wp2, float d1, float d2)
303 {
304  float x1 = waypoints[wp1].x;
305  float y1 = waypoints[wp1].y;
306  float x2 = waypoints[wp2].x;
307  float y2 = waypoints[wp2].y;
308 
309  return gvf_segment_loop_XY1_XY2(x1, y1, x2, y2, d1, d2);
310 }
311 
312 bool gvf_segment_XY1_XY2(float x1, float y1, float x2, float y2)
313 {
314  struct EnuCoor_f *p = stateGetPositionEnu_f();
315  float px = p->x - x1;
316  float py = p->y - y1;
317 
318  float zx = x2 - x1;
319  float zy = y2 - y1;
320 
321  float beta = atan2f(zy, zx);
322  float cosb = cosf(-beta);
323  float sinb = sinf(-beta);
324  float zxr = zx * cosb - zy * sinb;
325  float pxr = px * cosb - py * sinb;
326 
327  if((zxr > 0 && pxr > zxr) || (zxr < 0 && pxr < zxr)) {
328  return false;
329  }
330 
331  return gvf_line_XY1_XY2(x1, y1, x2, y2);
332 }
333 
335 {
336  float x1 = waypoints[wp1].x;
337  float y1 = waypoints[wp1].y;
338  float x2 = waypoints[wp2].x;
339  float y2 = waypoints[wp2].y;
340 
341  return gvf_segment_XY1_XY2(x1, y1, x2, y2);
342 }
343 
345 {
346  heading = heading * M_PI / 180;
347 
348  float a = waypoints[wp].x;
349  float b = waypoints[wp].y;
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 
369  // SAFE MODE
370  if (a < 1 || b < 1) {
371  gvf_trajectory.p[2] = 60;
372  gvf_trajectory.p[3] = 60;
373  }
374 
375  if (gvf_trajectory.p[2] == gvf_trajectory.p[3]) {
377  } else {
379  }
380 
381  gvf_ellipse_info(&e, &grad_ellipse, &Hess_ellipse);
382  gvf_control.ke = gvf_ellipse_par.ke;
384  e, &grad_ellipse, &Hess_ellipse);
385 
386  gvf_control.error = e;
387 
388  return true;
389 }
390 
391 
392 bool gvf_ellipse_wp(uint8_t wp, float a, float b, float alpha)
393 {
394  gvf_ellipse_XY(waypoints[wp].x, waypoints[wp].y, a, b, alpha);
395  return true;
396 }
397 
398 // SINUSOIDAL (if w = 0 and off = 0, then we just have the straight line case)
399 
400 bool gvf_sin_XY_alpha(float a, float b, float alpha, float w, float off, float A)
401 {
402  float e;
403  struct gvf_grad grad_line;
404  struct gvf_Hess Hess_line;
405 
406  gvf_trajectory.type = 2;
407  gvf_trajectory.p[0] = a;
408  gvf_trajectory.p[1] = b;
409  gvf_trajectory.p[2] = alpha;
410  gvf_trajectory.p[3] = w;
411  gvf_trajectory.p[4] = off;
412  gvf_trajectory.p[5] = A;
413 
414  gvf_sin_info(&e, &grad_line, &Hess_line);
415  gvf_control.ke = gvf_sin_par.ke;
416  gvf_control_2D(1e-2 * gvf_sin_par.ke, gvf_sin_par.kn, e, &grad_line, &Hess_line);
417 
418  gvf_control.error = e;
419 
420  return true;
421 }
422 
423 bool gvf_sin_wp1_wp2(uint8_t wp1, uint8_t wp2, float w, float off, float A)
424 {
425  w = 2 * M_PI * w;
426 
427  float x1 = waypoints[wp1].x;
428  float y1 = waypoints[wp1].y;
429  float x2 = waypoints[wp2].x;
430  float y2 = waypoints[wp2].y;
431 
432  float zx = x1 - x2;
433  float zy = y1 - y2;
434 
435  float alpha = atanf(zy / zx);
436 
437  gvf_sin_XY_alpha(x1, y1, alpha, w, off, A);
438 
439  return true;
440 }
441 
442 bool gvf_sin_wp_alpha(uint8_t wp, float alpha, float w, float off, float A)
443 {
444  w = 2 * M_PI * w;
445  alpha = alpha * M_PI / 180;
446 
447  float x = waypoints[wp].x;
448  float y = waypoints[wp].y;
449 
450  gvf_sin_XY_alpha(x, y, alpha, w, off, A);
451 
452  return true;
453 }
454 
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:82
bool gvf_sin_wp_alpha(uint8_t wp, float alpha, float w, float off, float A)
Definition: gvf.c:442
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
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:233
Periodic telemetry system header (includes downlink utility and generated code).
float H12
Definition: gvf.h:89
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:423
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:279
bool gvf_ellipse_XY(float x, float y, float a, float b, float alpha)
Definition: gvf.c:356
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:312
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:400
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:93
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
static float heading
Definition: ahrs_infrared.c:45
#define AP_MODE_AUTO2
Definition: gvf.h:42
static void send_gvf(struct transport_tx *trans, struct link_device *dev)
Definition: gvf.c:45
float x
in meters
float ny
Definition: gvf.h:83
float nx
Definition: gvf.h:82
void gvf_init(void)
Definition: gvf.c:124
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:212
float error
Definition: gvf.h:45
void gvf_set_direction(int8_t s)
Definition: gvf.c:205
float kn
Definition: gvf.h:44
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:139
static const struct usb_device_descriptor dev
Definition: usb_ser_hw.c:73
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:392
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:344
Definition: gvf.h:52
unsigned char uint8_t
Definition: types.h:14
#define LATERAL_MODE_ROLL
float h_ctl_roll_setpoint
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:334
bool gvf_line_XY1_XY2(float x1, float y1, float x2, float y2)
Definition: gvf.c:240
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:302
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:183
float H11
Definition: gvf.h:88
static void send_circle(struct transport_tx *trans, struct link_device *dev)
Definition: gvf.c:72
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:269