Paparazzi UAS v7.0_unstable
Paparazzi is a free software Unmanned Aircraft System.
Loading...
Searching...
No Matches
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"
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
49int gvf_plen = 1;
51
52#if PERIODIC_TELEMETRY
54static void send_gvf(struct transport_tx *trans, struct link_device *dev)
55{
57
59 uint32_t delta_T = now - gvf_t0;
60
61 if (delta_T < 200) {
64
65#if GVF_OCAML_GCS
67 ((int)gvf_trajectory.p[2] == (int)gvf_trajectory.p[3])) {
70 &gvf_trajectory.p[2]);
71 }
72
73 if (gvf_trajectory.type == LINE && gvf_segment.seg == 1) {
77 }
78#endif // GVF_OCAML_GCS
79
80 }
81}
82
83#endif // PERIODIC_TELEMETRY
84
85static int out_of_segment_area(float x1, float y1, float x2, float y2, float d1, float d2)
86{
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
116void gvf_init(void)
117{
118 gvf_control.ke = 1;
119 gvf_control.kn = 1;
120 gvf_control.s = 1;
121 gvf_control.speed = 1.0; // Rotorcraft only (for now)
122 gvf_control.align = false; // Rotorcraft only
124
125#if PERIODIC_TELEMETRY
127#endif
128}
129
130// GENERIC TRAJECTORY CONTROLLER
131void gvf_control_2D(float ke, float kn __attribute__((unused)), float e,
132 struct gvf_grad *grad, struct gvf_Hess *hess)
133{
135
137 float course __attribute__((unused)) = gvf_state.course;
138 float px_dot = gvf_state.px_dot;
139 float py_dot = gvf_state.py_dot;
140
141 int s = gvf_control.s;
142
143 // gradient Phi
144 float nx = grad->nx;
145 float ny = grad->ny;
146
147 // tangent to Phi
148 float tx = s * grad->ny;
149 float ty = -s * grad->nx;
150
151 // Hessian
152 float H11 = hess->H11;
153 float H12 = hess->H12;
154 float H21 = hess->H21;
155 float H22 = hess->H22;
156
157 // Calculation of the desired angular velocity in the vector field
158 float pdx_dot = tx - ke * e * nx;
159 float pdy_dot = ty - ke * e * ny;
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
171
173 float md_x = pdx_dot / norm_pd_dot;
174 float md_y = pdy_dot / norm_pd_dot;
175
177 / norm_pd_dot;
178
179 float md_dot_x __attribute__((unused)) = md_y * md_dot_const;
180 float md_dot_y __attribute__((unused))= -md_x * md_dot_const;
181
182 #if defined(ROTORCRAFT_FIRMWARE)
183
184 // Use accel based control. Not recommended as of current implementation
185 #if defined(GVF_ROTORCRAFT_USE_ACCEL)
186
187 // Set nav for command
188 // Use parameter kn as the speed command
189 nav.speed.x = md_x * kn;
190 nav.speed.y = md_y * kn;
191
192 // Acceleration induced by the field with speed set to kn (!WIP!)
193 #warning "Using GVF for rotorcraft is still experimental, proceed with caution"
194 float n_norm = sqrtf(nx*nx+ny*ny);
195 float hess_px_dot = px_dot * H11 + py_dot * H12;
196 float hess_py_dot = px_dot * H21 + py_dot * H22;
197
198 float hess_pdx_dot = pdx_dot * H11 + pdy_dot * H12;
199 float hess_pdy_dot = pdx_dot * H21 + pdy_dot * H22;
200
203 float accel_correction_y = - kn * hess_px_dot / n_norm;
206
207 float speed_cmd_x = kn*tx / n_norm - ke * e * nx / (n_norm);
208 float speed_cmd_y = kn*ty / n_norm - ke * e * ny / (n_norm);
209
210 // TODO: don't change nav struct directly
211 nav.accel.x = accel_cmd_x + (speed_cmd_x - px_dot);
212 nav.accel.y = accel_cmd_y + (speed_cmd_y - py_dot);
214
215 #else // SPEED_BASED_GVF
216
218
219 // Speed-based control, acceleration based control not implemented yet
222
223 // Optionally align heading with trajectory
224 if (gvf_control.align)
225 {
227 }
228
229 #endif
230
231 #else // FIXEDWING / ROVER FIRMWARE
232
233 float omega_d = -(md_dot_x * md_y - md_dot_y * md_x);
234
235 float mr_x = sinf(course);
236 float mr_y = cosf(course);
237
238 float omega = omega_d + kn * (mr_x * md_y - mr_y * md_x);
239
240 gvf_control.omega = omega;
241
242 // From gvf_common.h TODO: derivative of curvature and ori_err
243 gvf_c_omega.omega = omega;
244 gvf_c_info.kappa = (nx*(H12*ny - nx*H22) + ny*(H21*nx - H11*ny))/powf(nx*nx + ny*ny,1.5);
247
248 #endif
249}
250
251// BEGIN ROTORCRAFT
252
253void gvf_set_speed(float speed)
254{
255 if (speed < 0.0) speed = 0.0;
256 gvf_control.speed = speed;
257}
258
259void gvf_set_align(bool align)
260{
261 gvf_control.align = align;
262}
263
264// END ROTORCRAFT
265
267{
268 gvf_control.s = s;
269}
270
271// STRAIGHT LINE
272
273static void gvf_line(float a, float b, float heading)
274{
275 float e;
276 struct gvf_grad grad_line;
277 struct gvf_Hess Hess_line;
278
280 gvf_trajectory.p[0] = a;
281 gvf_trajectory.p[1] = b;
284 gvf_plen_wps = 0;
285
289
290 gvf_control.error = e;
291
293
294 gvf_segment.seg = 0;
295}
296
297bool gvf_line_XY_heading(float a, float b, float heading)
298{
300 gvf_line(a, b, heading);
301 return true;
302}
303
304bool gvf_line_XY1_XY2(float x1, float y1, float x2, float y2)
305{
306 if (gvf_plen_wps != 2) {
307 gvf_trajectory.p[3] = x2;
308 gvf_trajectory.p[4] = y2;
309 gvf_trajectory.p[5] = 0;
310 gvf_plen_wps = 3;
311 }
312
313 float zx = x2 - x1;
314 float zy = y2 - y1;
315
316 gvf_line_XY_heading(x1, y1, atan2f(zx, zy));
317
319 gvf_segment.seg = 1;
320 gvf_segment.x1 = x1;
321 gvf_segment.y1 = y1;
322 gvf_segment.x2 = x2;
323 gvf_segment.y2 = y2;
324
325 return true;
326}
327
329{
330 gvf_trajectory.p[3] = wp1;
331 gvf_trajectory.p[4] = wp2;
332 gvf_plen_wps = 2;
333
334 float x1 = WaypointX(wp1);
335 float y1 = WaypointY(wp1);
336 float x2 = WaypointX(wp2);
337 float y2 = WaypointY(wp2);
338
339 return gvf_line_XY1_XY2(x1, y1, x2, y2);
340}
341
342bool gvf_segment_loop_XY1_XY2(float x1, float y1, float x2, float y2, float d1, float d2)
343{
344 int s = out_of_segment_area(x1, y1, x2, y2, d1, d2);
345 if (s != 0) {
347 }
348
349 float zx = x2 - x1;
350 float zy = y2 - y1;
351 float alpha = atanf(zx / zy);
352
353 gvf_line(x1, y1, alpha);
354
356
357 gvf_segment.seg = 1;
358 gvf_segment.x1 = x1;
359 gvf_segment.y1 = y1;
360 gvf_segment.x2 = x2;
361 gvf_segment.y2 = y2;
362
363 return true;
364}
365
367{
368 gvf_trajectory.p[3] = wp1;
369 gvf_trajectory.p[4] = wp2;
370 gvf_trajectory.p[5] = d1;
371 gvf_trajectory.p[6] = d2;
372 gvf_plen_wps = 4;
373
374 float x1 = WaypointX(wp1);
375 float y1 = WaypointY(wp1);
376 float x2 = WaypointX(wp2);
377 float y2 = WaypointY(wp2);
378
379 return gvf_segment_loop_XY1_XY2(x1, y1, x2, y2, d1, d2);
380}
381
382bool gvf_segment_XY1_XY2(float x1, float y1, float x2, float y2)
383{
385 float px = p->x - x1;
386 float py = p->y - y1;
387
388 float zx = x2 - x1;
389 float zy = y2 - y1;
390
391 float beta = atan2f(zy, zx);
392 float cosb = cosf(-beta);
393 float sinb = sinf(-beta);
394 float zxr = zx * cosb - zy * sinb;
395 float pxr = px * cosb - py * sinb;
396
397 if ((zxr > 0 && pxr > zxr) || (zxr < 0 && pxr < zxr)) {
398 return false;
399 }
400
401 return gvf_line_XY1_XY2(x1, y1, x2, y2);
402}
403
405{
406 gvf_trajectory.p[3] = wp1;
407 gvf_trajectory.p[4] = wp2;
408 gvf_plen_wps = 2;
409
410 float x1 = WaypointX(wp1);
411 float y1 = WaypointY(wp1);
412 float x2 = WaypointX(wp2);
413 float y2 = WaypointY(wp2);
414
415 return gvf_segment_XY1_XY2(x1, y1, x2, y2);
416}
417
419{
420 gvf_trajectory.p[3] = wp;
421 gvf_plen_wps = 1;
422
424
425 float a = WaypointX(wp);
426 float b = WaypointY(wp);
427
428 return gvf_line_XY_heading(a, b, heading);
429}
430
431// ELLIPSE
432
433bool gvf_ellipse_XY(float x, float y, float a, float b, float alpha)
434{
435 float e;
436 struct gvf_grad grad_ellipse;
437 struct gvf_Hess Hess_ellipse;
438
440 gvf_trajectory.p[0] = x;
441 gvf_trajectory.p[1] = y;
442 gvf_trajectory.p[2] = a;
443 gvf_trajectory.p[3] = b;
446 gvf_plen_wps = 0;
447
448 // SAFE MODE
449 if (a < 1 || b < 1) {
450 gvf_trajectory.p[2] = 60;
451 gvf_trajectory.p[3] = 60;
452 }
453
454 if ((int)gvf_trajectory.p[2] == (int)gvf_trajectory.p[3]) {
456
457 } else {
459 }
460
465
466 gvf_control.error = e;
467
468 return true;
469}
470
471
472bool gvf_ellipse_wp(uint8_t wp, float a, float b, float alpha)
473{
474 gvf_trajectory.p[5] = wp;
475 gvf_plen_wps = 1;
476
478 return true;
479}
480
481// SINUSOIDAL (if w = 0 and off = 0, then we just have the straight line case)
482
483bool gvf_sin_XY_alpha(float a, float b, float alpha, float w, float off, float A)
484{
485 float e;
486 struct gvf_grad grad_line;
487 struct gvf_Hess Hess_line;
488
490 gvf_trajectory.p[0] = a;
491 gvf_trajectory.p[1] = b;
493 gvf_trajectory.p[3] = w;
494 gvf_trajectory.p[4] = off;
495 gvf_trajectory.p[5] = A;
497 gvf_plen_wps = 0;
498
502
503 gvf_control.error = e;
504
505 return true;
506}
507
508bool gvf_sin_wp1_wp2(uint8_t wp1, uint8_t wp2, float w, float off, float A)
509{
510 w = 2 * M_PI * w;
511
512 gvf_trajectory.p[6] = wp1;
513 gvf_trajectory.p[7] = wp2;
514 gvf_plen_wps = 2;
515
516 float x1 = WaypointX(wp1);
517 float y1 = WaypointY(wp1);
518 float x2 = WaypointX(wp2);
519 float y2 = WaypointY(wp2);
520
521 float zx = x1 - x2;
522 float zy = y1 - y2;
523
524 float alpha = atanf(zy / zx);
525
526 gvf_sin_XY_alpha(x1, y1, alpha, w, off, A);
527
528 return true;
529}
530
531bool gvf_sin_wp_alpha(uint8_t wp, float alpha, float w, float off, float A)
532{
533 w = 2 * M_PI * w;
535
536 gvf_trajectory.p[6] = wp;
537 gvf_plen_wps = 1;
538
539 float x = WaypointX(wp);
540 float y = WaypointY(wp);
541
542 gvf_sin_XY_alpha(x, y, alpha, w, off, A);
543
544 return true;
545}
546
static int16_t course[3]
Core autopilot interface common to all firmwares.
static uint16_t d1
static uint16_t d2
uint32_t get_sys_time_msec(void)
Get the time in milliseconds since startup.
#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:848
bool gvf_segment_XY1_XY2(float x1, float y1, float x2, float y2)
Definition gvf.c:382
bool gvf_sin_XY_alpha(float a, float b, float alpha, float w, float off, float A)
Definition gvf.c:483
bool gvf_segment_loop_XY1_XY2(float x1, float y1, float x2, float y2, float d1, float d2)
Definition gvf.c:342
int gvf_plen
Definition gvf.c:49
bool gvf_ellipse_wp(uint8_t wp, float a, float b, float alpha)
Definition gvf.c:472
void gvf_set_align(bool align)
Definition gvf.c:259
bool gvf_line_wp1_wp2(uint8_t wp1, uint8_t wp2)
Definition gvf.c:328
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:508
bool gvf_segment_wp1_wp2(uint8_t wp1, uint8_t wp2)
Definition gvf.c:404
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:531
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:131
static void send_gvf(struct transport_tx *trans, struct link_device *dev)
Definition gvf.c:54
void gvf_set_speed(float speed)
Definition gvf.c:253
gvf_st gvf_state
Definition gvf.c:39
void gvf_set_direction(int8_t s)
Definition gvf.c:266
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:366
static void gvf_line(float a, float b, float heading)
Definition gvf.c:273
bool gvf_ellipse_XY(float x, float y, float a, float b, float alpha)
Definition gvf.c:433
bool gvf_line_XY_heading(float a, float b, float heading)
Definition gvf.c:297
bool gvf_line_wp_heading(uint8_t wp, float heading)
Definition gvf.c:418
bool gvf_line_XY1_XY2(float x1, float y1, float x2, float y2)
Definition gvf.c:304
uint32_t gvf_t0
Definition gvf.c:46
Guidance algorithm based on vector fields.
float ke
Definition gvf.h:51
float x1
Definition gvf.h:92
float error
Definition gvf.h:53
float py_dot
Definition gvf.h:65
enum trajectories type
Definition gvf.h:78
float speed
Definition gvf.h:55
float p[16]
Definition gvf.h:79
int8_t s
Definition gvf.h:56
@ ELLIPSE
Definition gvf.h:72
@ LINE
Definition gvf.h:71
bool align
Definition gvf.h:57
int seg
Definition gvf.h:91
float y1
Definition gvf.h:93
float y2
Definition gvf.h:95
float course
Definition gvf.h:63
float px_dot
Definition gvf.h:64
float x2
Definition gvf.h:94
float kn
Definition gvf.h:52
float omega
Definition gvf.h:54
Definition gvf.h:50
Definition gvf.h:90
Definition gvf.h:62
Definition gvf.h:77
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
uint16_t foo
Definition main_demo5.c:58
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
#define NAV_SETPOINT_MODE_SPEED
Definition navigation.h:101
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.
unsigned char uint8_t
Typedef defining 8 bit unsigned char type.
signed char int8_t
Typedef defining 8 bit char type.
float b
Definition wedgebug.c:202
float heading
Definition wedgebug.c:258