Paparazzi UAS  v5.17_devel-15-geeac31e
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  gvf_line_XY_heading(x1, y1, atan2f(zx, zy));
246 
248  gvf_segment.seg = 1;
249  gvf_segment.x1 = x1;
250  gvf_segment.y1 = y1;
251  gvf_segment.x2 = x2;
252  gvf_segment.y2 = y2;
253 
254  return true;
255 }
256 
258 {
259  float x1 = waypoints[wp1].x;
260  float y1 = waypoints[wp1].y;
261  float x2 = waypoints[wp2].x;
262  float y2 = waypoints[wp2].y;
263 
264  return gvf_line_XY1_XY2(x1, y1, x2, y2);
265 }
266 
267 bool gvf_segment_loop_XY1_XY2(float x1, float y1, float x2, float y2, float d1, float d2)
268 {
269  int s = out_of_segment_area(x1, y1, x2, y2, d1, d2);
270  if (s != 0) {
271  gvf_control.s = s;
272  }
273 
274  float zx = x2 - x1;
275  float zy = y2 - y1;
276  float alpha = atanf(zx / zy);
277 
278  gvf_line(x1, y1, alpha);
279 
281  gvf_segment.seg = 1;
282  gvf_segment.x1 = x1;
283  gvf_segment.y1 = y1;
284  gvf_segment.x2 = x2;
285  gvf_segment.y2 = y2;
286 
287  return true;
288 }
289 
290 bool gvf_segment_loop_wp1_wp2(uint8_t wp1, uint8_t wp2, float d1, float d2)
291 {
292  float x1 = waypoints[wp1].x;
293  float y1 = waypoints[wp1].y;
294  float x2 = waypoints[wp2].x;
295  float y2 = waypoints[wp2].y;
296 
297  return gvf_segment_loop_XY1_XY2(x1, y1, x2, y2, d1, d2);
298 }
299 
300 bool gvf_segment_XY1_XY2(float x1, float y1, float x2, float y2)
301 {
302  struct EnuCoor_f *p = stateGetPositionEnu_f();
303  float px = p->x - x1;
304  float py = p->y - y1;
305 
306  float zx = x2 - x1;
307  float zy = y2 - y1;
308 
309  float beta = atan2f(zy, zx);
310  float cosb = cosf(-beta);
311  float sinb = sinf(-beta);
312  float zxr = zx * cosb - zy * sinb;
313  float pxr = px * cosb - py * sinb;
314 
315  if ((zxr > 0 && pxr > zxr) || (zxr < 0 && pxr < zxr)) {
316  return false;
317  }
318 
319  return gvf_line_XY1_XY2(x1, y1, x2, y2);
320 }
321 
323 {
324  float x1 = waypoints[wp1].x;
325  float y1 = waypoints[wp1].y;
326  float x2 = waypoints[wp2].x;
327  float y2 = waypoints[wp2].y;
328 
329  return gvf_segment_XY1_XY2(x1, y1, x2, y2);
330 }
331 
333 {
334  heading = heading * M_PI / 180;
335 
336  float a = waypoints[wp].x;
337  float b = waypoints[wp].y;
338 
339  return gvf_line_XY_heading(a, b, heading);
340 }
341 
342 // ELLIPSE
343 
344 bool gvf_ellipse_XY(float x, float y, float a, float b, float alpha)
345 {
346  float e;
347  struct gvf_grad grad_ellipse;
348  struct gvf_Hess Hess_ellipse;
349 
350  gvf_trajectory.type = 1;
351  gvf_trajectory.p[0] = x;
352  gvf_trajectory.p[1] = y;
353  gvf_trajectory.p[2] = a;
354  gvf_trajectory.p[3] = b;
355  gvf_trajectory.p[4] = alpha;
356 
357  // SAFE MODE
358  if (a < 1 || b < 1) {
359  gvf_trajectory.p[2] = 60;
360  gvf_trajectory.p[3] = 60;
361  }
362 
363  if (gvf_trajectory.p[2] == gvf_trajectory.p[3]) {
365  } else {
367  }
368 
369  gvf_ellipse_info(&e, &grad_ellipse, &Hess_ellipse);
370  gvf_control.ke = gvf_ellipse_par.ke;
372  e, &grad_ellipse, &Hess_ellipse);
373 
374  gvf_control.error = e;
375 
376  return true;
377 }
378 
379 
380 bool gvf_ellipse_wp(uint8_t wp, float a, float b, float alpha)
381 {
382  gvf_ellipse_XY(waypoints[wp].x, waypoints[wp].y, a, b, alpha);
383  return true;
384 }
385 
386 // SINUSOIDAL (if w = 0 and off = 0, then we just have the straight line case)
387 
388 bool gvf_sin_XY_alpha(float a, float b, float alpha, float w, float off, float A)
389 {
390  float e;
391  struct gvf_grad grad_line;
392  struct gvf_Hess Hess_line;
393 
394  gvf_trajectory.type = 2;
395  gvf_trajectory.p[0] = a;
396  gvf_trajectory.p[1] = b;
397  gvf_trajectory.p[2] = alpha;
398  gvf_trajectory.p[3] = w;
399  gvf_trajectory.p[4] = off;
400  gvf_trajectory.p[5] = A;
401 
402  gvf_sin_info(&e, &grad_line, &Hess_line);
403  gvf_control.ke = gvf_sin_par.ke;
404  gvf_control_2D(1e-2 * gvf_sin_par.ke, gvf_sin_par.kn, e, &grad_line, &Hess_line);
405 
406  gvf_control.error = e;
407 
408  return true;
409 }
410 
411 bool gvf_sin_wp1_wp2(uint8_t wp1, uint8_t wp2, float w, float off, float A)
412 {
413  w = 2 * M_PI * w;
414 
415  float x1 = waypoints[wp1].x;
416  float y1 = waypoints[wp1].y;
417  float x2 = waypoints[wp2].x;
418  float y2 = waypoints[wp2].y;
419 
420  float zx = x1 - x2;
421  float zy = y1 - y2;
422 
423  float alpha = atanf(zy / zx);
424 
425  gvf_sin_XY_alpha(x1, y1, alpha, w, off, A);
426 
427  return true;
428 }
429 
430 bool gvf_sin_wp_alpha(uint8_t wp, float alpha, float w, float off, float A)
431 {
432  w = 2 * M_PI * w;
433  alpha = alpha * M_PI / 180;
434 
435  float x = waypoints[wp].x;
436  float y = waypoints[wp].y;
437 
438  gvf_sin_XY_alpha(x, y, alpha, w, off, A);
439 
440  return true;
441 }
442 
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:430
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
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:411
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:267
bool gvf_ellipse_XY(float x, float y, float a, float b, float alpha)
Definition: gvf.c:344
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:300
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:388
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: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:380
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:332
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:322
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:290
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: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:257