Paparazzi UAS  v6.3_unstable
Paparazzi is a free software Unmanned Aircraft System.
gvf_parametric.cpp
Go to the documentation of this file.
1 /*
2  * Copyright (C) 2020 Hector Garcia de Marina <hgarciad@ucm.es>
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, see
18  * <http://www.gnu.org/licenses/>.
19  */
20 
27 #include <iostream>
28 #include <Eigen/Dense> // https://eigen.tuxfamily.org/dox/GettingStarted.html
29 
30 #include "gvf_parametric.h"
35 
36 #ifdef __cplusplus
37 extern "C" {
38 #endif
39 
40 #include "autopilot.h"
41 
42 // Control
43 uint32_t gvf_parametric_t0 = 0; // We need it for calculting the time lapse delta_T
45 
46 // Trajectory
48 
49 // Parameters array lenght
52 
53 // Error signals array lenght
55 
56 #if PERIODIC_TELEMETRY
58 static void send_gvf_parametric(struct transport_tx *trans, struct link_device *dev)
59 {
61 
63  uint32_t delta_T = now - gvf_parametric_t0;
64 
66 
67  if (delta_T < 200) {
68  pprz_msg_send_GVF_PARAMETRIC(trans, dev, AC_ID, &traj_type, &gvf_parametric_control.s, &wb, gvf_parametric_plen,
70  }
71 }
72 
73 #if GVF_OCAML_GCS
74 static void send_circle_parametric(struct transport_tx *trans, struct link_device *dev)
75 {
77  uint32_t delta_T = now - gvf_parametric_t0;
78 
79  if (delta_T < 200)
81  pprz_msg_send_CIRCLE(trans, dev, AC_ID, &gvf_parametric_trajectory.p_parametric[0],
83  }
84 }
85 #endif // GVF_OCAML_GCS
86 
87 #endif // PERIODIC TELEMETRY
88 
89 #ifdef __cplusplus
90 }
91 #endif
92 
94 {
103 
104 #if PERIODIC_TELEMETRY
106 #if GVF_OCAML_GCS
107  register_periodic_telemetry(DefaultPeriodic, PPRZ_MSG_ID_CIRCLE, send_circle_parametric);
108 #endif // GVF_OCAML_GCS
109 #endif // PERIODIC_TELEMETRY
110 
111 }
112 
114 {
116 }
117 
118 void gvf_parametric_control_2D(float kx, float ky, float f1, float f2, float f1d, float f2d, float f1dd, float f2dd)
119 {
120 
121  uint32_t now = get_sys_time_msec();
123  gvf_parametric_t0 = now;
124 
125  if (gvf_parametric_control.delta_T > 300) { // We need at least two iterations for Delta_T
126  gvf_parametric_control.w = 0; // Reset w since we assume the algorithm starts
127  return;
128  }
129 
130  // Carrot position
131  desired_x = f1;
132  desired_y = f2;
133 
134  float L = gvf_parametric_control.L;
136 
137  Eigen::Vector3f X;
138  Eigen::Matrix3f J;
139 
140  // Error signals phi_x and phi_y
141  struct EnuCoor_f *pos_enu = stateGetPositionEnu_f();
142  float x = pos_enu->x;
143  float y = pos_enu->y;
144 
145  float phi1 = L * (x - f1);
146  float phi2 = L * (y - f2);
147 
148  gvf_parametric_trajectory.phi_errors[0] = phi1; // Error signals for the telemetry
151 
152  // Chi
153  X(0) = L * beta * f1d - kx * phi1;
154  X(1) = L * beta * f2d - ky * phi2;
155  X(2) = L + beta * (kx * phi1 * f1d + ky * phi2 * f2d);
156  X *= L;
157 
158  // Jacobian
159  J.setZero();
160  J(0, 0) = -kx * L;
161  J(1, 1) = -ky * L;
162  J(2, 0) = (beta * L) * (beta * f1dd + kx * f1d);
163  J(2, 1) = (beta * L) * (beta * f2dd + ky * f2d);
164  J(2, 2) = beta * beta * (kx * (phi1 * f1dd - L * f1d * f1d) + ky * (phi2 * f2dd - L * f2d * f2d));
165  J *= L;
166 
167  // Guidance algorithm
169  float w_dot = (ground_speed * X(2)) / sqrtf(X(0) * X(0) + X(1) * X(1));
170 
171  Eigen::Vector3f xi_dot;
172  struct EnuCoor_f *vel_enu = stateGetSpeedEnu_f();
174 
175  xi_dot << vel_enu->x, vel_enu->y, w_dot;
176 
177  Eigen::Matrix3f G;
178  Eigen::Matrix3f Gp;
179  Eigen::Matrix<float, 2, 3> Fp;
180  Eigen::Vector2f h;
181  Eigen::Matrix<float, 1, 2> ht;
182 
183  G << 1, 0, 0,
184  0, 1, 0,
185  0, 0, 0;
186  Fp << 0, -1, 0,
187  1, 0, 0;
188  Gp << 0, -1, 0,
189  1, 0, 0,
190  0, 0, 0;
191 
192  h << sinf(course), cosf(course);
193  ht = h.transpose();
194 
195  Eigen::Matrix<float, 1, 3> Xt = X.transpose();
196  Eigen::Vector3f Xh = X / X.norm();
197  Eigen::Matrix<float, 1, 3> Xht = Xh.transpose();
198  Eigen::Matrix3f I;
199  I.setIdentity();
200 
201  float aux = ht * Fp * X;
202  float heading_rate = -1 / (Xt * G * X) * Xt * Gp * (I - Xh * Xht) * J * xi_dot - (gvf_parametric_control.k_psi * aux /
203  sqrtf(Xt * G * X));
204 
205  // Virtual coordinate update, even if the vehicle is not in autonomous mode, the parameter w will get "closer" to
206  // the vehicle. So it is not only okei but advisable to update it.
208 
210 }
211 
212 void gvf_parametric_control_3D(float kx, float ky, float kz, float f1, float f2, float f3, float f1d, float f2d,
213  float f3d, float f1dd, float f2dd, float f3dd)
214 {
215  uint32_t now = get_sys_time_msec();
217  gvf_parametric_t0 = now;
218 
219  if (gvf_parametric_control.delta_T > 300) { // We need at least two iterations for Delta_T
220  gvf_parametric_control.w = 0; // Reset w since we assume the algorithm starts
221  return;
222  }
223 
224  // Carrot position
225  desired_x = f1;
226  desired_y = f2;
227 
228  float L = gvf_parametric_control.L;
230 
231  Eigen::Vector4f X;
232  Eigen::Matrix4f J;
233 
234  // Error signals phi_x phi_y and phi_z
235  struct EnuCoor_f *pos_enu = stateGetPositionEnu_f();
236  float x = pos_enu->x;
237  float y = pos_enu->y;
238  float z = pos_enu->z;
239 
240  float phi1 = L * (x - f1);
241  float phi2 = L * (y - f2);
242  float phi3 = L * (z - f3);
243 
244  gvf_parametric_trajectory.phi_errors[0] = phi1 / L; // Error signals in meters for the telemetry
248 
249 
250  // Chi
251  X(0) = -f1d * L * L * beta - kx * phi1;
252  X(1) = -f2d * L * L * beta - ky * phi2;
253  X(2) = -f3d * L * L * beta - kz * phi3;
254  X(3) = -L * L + beta * (kx * phi1 * f1d + ky * phi2 * f2d + kz * phi3 * f3d);
255  X *= L;
256 
257  // Jacobian
258  J.setZero();
259  J(0, 0) = -kx * L;
260  J(1, 1) = -ky * L;
261  J(2, 2) = -kz * L;
262  J(3, 0) = kx * f1d * beta * L;
263  J(3, 1) = ky * f2d * beta * L;
264  J(3, 2) = kz * f3d * beta * L;
265  J(0, 3) = -(beta * L) * (beta * L * f1dd - kx * f1d);
266  J(1, 3) = -(beta * L) * (beta * L * f2dd - ky * f2d);
267  J(2, 3) = -(beta * L) * (beta * L * f3dd - kz * f3d);
268  J(3, 3) = beta * beta * (kx * (phi1 * f1dd - L * f1d * f1d) + ky * (phi2 * f2dd - L * f2d * f2d)
269  + kz * (phi3 * f3dd - L * f3d * f3d));
270  J *= L;
271 
272  // Guidance algorithm
274  float w_dot = (ground_speed * X(3)) / sqrtf(X(0) * X(0) + X(1) * X(1));
275 
276  Eigen::Vector4f xi_dot;
277  struct EnuCoor_f *vel_enu = stateGetSpeedEnu_f();
279 
280  xi_dot << vel_enu->x, vel_enu->y, vel_enu->z, w_dot;
281 
282  Eigen::Matrix2f E;
283  Eigen::Matrix<float, 2, 4> F;
284  Eigen::Matrix<float, 2, 4> Fp;
285  Eigen::Matrix4f G;
286  Eigen::Matrix4f Gp;
287  Eigen::Matrix4f I;
288  Eigen::Vector2f h;
289  Eigen::Matrix<float, 1, 2> ht;
290 
291  h << sinf(course), cosf(course);
292  ht = h.transpose();
293  I.setIdentity();
294  F << 1.0, 0.0, 0.0, 0.0,
295  0.0, 1.0, 0.0, 0.0;
296  E << 0.0, -1.0,
297  1.0, 0.0;
298  G = F.transpose() * F;
299  Fp = E * F;
300  Gp = F.transpose() * E * F;
301 
302  Eigen::Matrix<float, 1, 4> Xt = X.transpose();
303  Eigen::Vector4f Xh = X / X.norm();
304  Eigen::Matrix<float, 1, 4> Xht = Xh.transpose();
305 
306  float aux = ht * Fp * X;
307 
308  float heading_rate = -1 / (Xt * G * X) * Xt * Gp * (I - Xh * Xht) * J * xi_dot - (gvf_parametric_control.k_psi * aux /
309  sqrtf(Xt * G * X));
310  float climbing_rate = (ground_speed * X(2)) / sqrtf(X(0) * X(0) + X(1) * X(1));
311 
312  // Virtual coordinate update, even if the vehicle is not in autonomous mode, the parameter w will get "closer" to
313  // the vehicle. So it is not only okei but advisable to update it.
315 
316  gvf_parametric_low_level_control_3D(heading_rate, climbing_rate);
317 }
318 
320 // 2D TREFOIL KNOT
321 
322 bool gvf_parametric_2D_trefoil_XY(float xo, float yo, float w1, float w2, float ratio, float r, float alpha)
323 {
334 
335  float f1, f2, f1d, f2d, f1dd, f2dd;
336 
337  gvf_parametric_2d_trefoil_info(&f1, &f2, &f1d, &f2d, &f1dd, &f2dd);
339  f2dd);
340 
341  return true;
342 }
343 
344 bool gvf_parametric_2D_trefoil_wp(uint8_t wp, float w1, float w2, float ratio, float r, float alpha)
345 {
348 
349  gvf_parametric_2D_trefoil_XY(waypoints[wp].x, waypoints[wp].y, w1, w2, ratio, r, alpha);
350  return true;
351 }
352 
354 // 3D ELLIPSE
355 
356 bool gvf_parametric_3D_ellipse_XYZ(float xo, float yo, float r, float zl, float zh, float alpha)
357 {
358  horizontal_mode = HORIZONTAL_MODE_CIRCLE; // Circle for the 2D GCS
359 
360  // Safety first! If the asked altitude is low
361  if (zl > zh) {
362  zl = zh;
363  }
364  if (zl < 1 || zh < 1) {
365  zl = 10;
366  zh = 10;
367  }
368  if (r < 1) {
369  r = 60;
370  }
371 
381 
382  float f1, f2, f3, f1d, f2d, f3d, f1dd, f2dd, f3dd;
383 
384  gvf_parametric_3d_ellipse_info(&f1, &f2, &f3, &f1d, &f2d, &f3d, &f1dd, &f2dd, &f3dd);
386  gvf_parametric_3d_ellipse_par.kz, f1, f2, f3, f1d, f2d, f3d, f1dd, f2dd, f3dd);
387 
388  return true;
389 }
390 
391 bool gvf_parametric_3D_ellipse_wp(uint8_t wp, float r, float zl, float zh, float alpha)
392 {
395 
397  return true;
398 }
399 
400 bool gvf_parametric_3D_ellipse_wp_delta(uint8_t wp, float r, float alt_center, float delta, float alpha)
401 {
402  float zl = alt_center - delta;
403  float zh = alt_center + delta;
404 
406  return true;
407 }
408 
409 // 3D Lissajous
410 
411 bool gvf_parametric_3D_lissajous_XYZ(float xo, float yo, float zo, float cx, float cy, float cz, float wx, float wy,
412  float wz, float dx, float dy, float dz, float alpha)
413 {
414  // Safety first! If the asked altitude is low
415  if ((zo - cz) < 1) {
416  zo = 10;
417  cz = 0;
418  }
419 
436 
437  float f1, f2, f3, f1d, f2d, f3d, f1dd, f2dd, f3dd;
438 
439  gvf_parametric_3d_lissajous_info(&f1, &f2, &f3, &f1d, &f2d, &f3d, &f1dd, &f2dd, &f3dd);
441  gvf_parametric_3d_lissajous_par.kz, f1, f2, f3, f1d, f2d, f3d, f1dd, f2dd, f3dd);
442 
443  return true;
444 }
445 
446 bool gvf_parametric_3D_lissajous_wp_center(uint8_t wp, float zo, float cx, float cy, float cz, float wx, float wy,
447  float wz, float dx, float dy, float dz, float alpha)
448 {
451 
452  gvf_parametric_3D_lissajous_XYZ(waypoints[wp].x, waypoints[wp].y, zo, cx, cy, cz, wx, wy, wz, dx, dy, dz, alpha);
453  return true;
454 }
static void h(const real32_T x[7], const real32_T q[4], real32_T y[6])
static int16_t course[3]
Definition: airspeed_uADC.c:58
Core autopilot interface common to all firmwares.
uint32_t get_sys_time_msec(void)
Get the time in milliseconds since startup.
Definition: sys_time_arch.c:82
struct point waypoints[NB_WAYPOINT]
size == nb_waypoint, waypoint 0 is a dummy waypoint
Definition: common_nav.c:39
static float ground_speed
Definition: follow_me.c:76
#define E
static struct EnuCoor_f * stateGetPositionEnu_f(void)
Get position in local ENU coordinates (float).
Definition: state.h:719
static float stateGetHorizontalSpeedNorm_f(void)
Get norm of horizontal ground speed (float).
Definition: state.h:935
static float stateGetHorizontalSpeedDir_f(void)
Get dir of horizontal ground speed (float).
Definition: state.h:944
static struct EnuCoor_f * stateGetSpeedEnu_f(void)
Get ground speed in local ENU coordinates (float).
Definition: state.h:917
gvf_parametric_con gvf_parametric_control
int gvf_parametric_elen
bool gvf_parametric_3D_lissajous_XYZ(float xo, float yo, float zo, float cx, float cy, float cz, float wx, float wy, float wz, float dx, float dy, float dz, float alpha)
bool gvf_parametric_3D_lissajous_wp_center(uint8_t wp, float zo, float cx, float cy, float cz, float wx, float wy, float wz, float dx, float dy, float dz, float alpha)
void gvf_parametric_control_3D(float kx, float ky, float kz, float f1, float f2, float f3, float f1d, float f2d, float f3d, float f1dd, float f2dd, float f3dd)
bool gvf_parametric_3D_ellipse_wp_delta(uint8_t wp, float r, float alt_center, float delta, float alpha)
bool gvf_parametric_2D_trefoil_XY(float xo, float yo, float w1, float w2, float ratio, float r, float alpha)
2D TRAJECTORIES
bool gvf_parametric_3D_ellipse_wp(uint8_t wp, float r, float zl, float zh, float alpha)
int gvf_parametric_plen_wps
static void send_gvf_parametric(struct transport_tx *trans, struct link_device *dev)
uint32_t gvf_parametric_t0
bool gvf_parametric_3D_ellipse_XYZ(float xo, float yo, float r, float zl, float zh, float alpha)
3D TRAJECTORIES
void gvf_parametric_init(void)
gvf_parametric_tra gvf_parametric_trajectory
void gvf_parametric_control_2D(float kx, float ky, float f1, float f2, float f1d, float f2d, float f1dd, float f2dd)
bool gvf_parametric_2D_trefoil_wp(uint8_t wp, float w1, float w2, float ratio, float r, float alpha)
int gvf_parametric_plen
void gvf_parametric_set_direction(int8_t s)
Guiding vector field algorithm for 2D and 3D parametric trajectories.
@ LISSAJOUS_3D
@ TREFOIL_2D
@ ELLIPSE_3D
#define GVF_PARAMETRIC_CONTROL_KCLIMB
#define GVF_PARAMETRIC_CONTROL_L
#define GVF_PARAMETRIC_CONTROL_BETA
#define GVF_PARAMETRIC_CONTROL_KROLL
#define GVF_PARAMETRIC_CONTROL_KPSI
enum trajectories_parametric type
void gvf_parametric_2d_trefoil_info(float *f1, float *f2, float *f1d, float *f2d, float *f1dd, float *f2dd)
gvf_par_2d_tre_par gvf_parametric_2d_trefoil_par
Guiding vector field algorithm for 2D and 3D complex trajectories.
void gvf_parametric_3d_ellipse_info(float *f1, float *f2, float *f3, float *f1d, float *f2d, float *f3d, float *f1dd, float *f2dd, float *f3dd)
gvf_par_3d_ell_par gvf_parametric_3d_ellipse_par
Guiding vector field algorithm for 2D and 3D complex trajectories.
gvf_par_3d_lis_par gvf_parametric_3d_lissajous_par
void gvf_parametric_3d_lissajous_info(float *f1, float *f2, float *f3, float *f1d, float *f2d, float *f3d, float *f1dd, float *f2dd, float *f3dd)
Guiding vector field algorithm for 2D and 3D complex trajectories.
void gvf_parametric_low_level_control_2D(float heading_rate)
void gvf_parametric_low_level_control_3D(float heading_rate, float climbing_rate)
Firmware dependent file for the guiding vector field algorithm for 2D and 3D parametric trajectories.
static uint32_t s
float desired_x
Definition: nav.c:308
float desired_y
Definition: nav.c:308
uint8_t horizontal_mode
Definition: nav.c:70
#define HORIZONTAL_MODE_CIRCLE
Definition: nav.h:92
float y
in meters
float x
in meters
float z
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:107
static float I
Definition: trilateration.c:35
static float J
Definition: trilateration.c:35
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