Paparazzi UAS  v6.0_unstable-92-g17422e4-dirty
Paparazzi is a free software Unmanned Aircraft System.
 All Data Structures Namespaces Files Functions Variables Typedefs Enumerations Enumerator Macros Modules Pages
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>
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 #if PERIODIC_TELEMETRY
51 static void send_gvf_parametric(struct transport_tx *trans, struct link_device *dev)
52 {
53  // Do not know whether is a good idea to do this check here or to include
54  // this plen in gvf_trajectory
55  int plen;
56  int elen;
57 
58  switch (gvf_parametric_trajectory.type) {
59  case TREFOIL_2D:
60  plen = 7;
61  elen = 2;
62  break;
63  case ELLIPSE_3D:
64  plen = 6;
65  elen = 3;
66  break;
67  case LISSAJOUS_3D:
68  plen = 13;
69  elen = 3;
70  break;
71  default:
72  plen = 1;
73  elen = 3;
74  }
75 
76  uint8_t traj_type = (uint8_t)gvf_parametric_trajectory.type;
77 
79  uint32_t delta_T = now - gvf_parametric_t0;
80 
81  float wb = gvf_parametric_control.w * gvf_parametric_control.beta;
82 
83  if (delta_T < 200) {
84  pprz_msg_send_GVF_PARAMETRIC(trans, dev, AC_ID, &traj_type, &gvf_parametric_control.s, &wb, plen,
85  gvf_parametric_trajectory.p_parametric, elen, gvf_parametric_trajectory.phi_errors);
86  }
87 }
88 
89 static void send_circle_parametric(struct transport_tx *trans, struct link_device *dev)
90 {
92  uint32_t delta_T = now - gvf_parametric_t0;
93 
94  if (delta_T < 200)
95  if (gvf_parametric_trajectory.type == ELLIPSE_3D) {
96  pprz_msg_send_CIRCLE(trans, dev, AC_ID, &gvf_parametric_trajectory.p_parametric[0],
97  &gvf_parametric_trajectory.p_parametric[1], &gvf_parametric_trajectory.p_parametric[2]);
98  }
99 }
100 
101 #endif // PERIODIC TELEMETRY
102 
103 #ifdef __cplusplus
104 }
105 #endif
106 
108 {
117 
118 #if PERIODIC_TELEMETRY
121 #endif
122 
123 }
124 
126 {
128 }
129 
130 void gvf_parametric_control_2D(float kx, float ky, float f1, float f2, float f1d, float f2d, float f1dd, float f2dd)
131 {
132 
133  uint32_t now = get_sys_time_msec();
135  gvf_parametric_t0 = now;
136 
137  if (gvf_parametric_control.delta_T > 300) { // We need at least two iterations for Delta_T
138  gvf_parametric_control.w = 0; // Reset w since we assume the algorithm starts
139  return;
140  }
141 
142  float L = gvf_parametric_control.L;
144 
145  Eigen::Vector3f X;
146  Eigen::Matrix3f J;
147 
148  // Error signals phi_x and phi_y
149  struct EnuCoor_f *pos_enu = stateGetPositionEnu_f();
150  float x = pos_enu->x;
151  float y = pos_enu->y;
152 
153  float phi1 = L * (x - f1);
154  float phi2 = L * (y - f2);
155 
156  gvf_parametric_trajectory.phi_errors[0] = phi1; // Error signals for the telemetry
158 
159  // Chi
160  X(0) = L * beta * f1d - kx * phi1;
161  X(1) = L * beta * f2d - ky * phi2;
162  X(2) = L + beta * (kx * phi1 * f1d + ky * phi2 * f2d);
163  X *= L;
164 
165  // Jacobian
166  J.setZero();
167  J(0, 0) = -kx * L;
168  J(1, 1) = -ky * L;
169  J(2, 0) = (beta * L) * (beta * f1dd + kx * f1d);
170  J(2, 1) = (beta * L) * (beta * f2dd + ky * f2d);
171  J(2, 2) = beta * beta * (kx * (phi1 * f1dd - L * f1d * f1d) + ky * (phi2 * f2dd - L * f2d * f2d));
172  J *= L;
173 
174  // Guidance algorithm
176  float w_dot = (ground_speed * X(2)) / sqrtf(X(0) * X(0) + X(1) * X(1));
177 
178  Eigen::Vector3f xi_dot;
179  struct EnuCoor_f *vel_enu = stateGetSpeedEnu_f();
181 
182  xi_dot << vel_enu->x, vel_enu->y, w_dot;
183 
184  Eigen::Matrix3f G;
185  Eigen::Matrix3f Gp;
186  Eigen::Matrix<float, 2, 3> Fp;
187  Eigen::Vector2f h;
188  Eigen::Matrix<float, 1, 2> ht;
189 
190  G << 1, 0, 0,
191  0, 1, 0,
192  0, 0, 0;
193  Fp << 0, -1, 0,
194  1, 0, 0;
195  Gp << 0, -1, 0,
196  1, 0, 0,
197  0, 0, 0;
198 
199  h << sinf(course), cosf(course);
200  ht = h.transpose();
201 
202  Eigen::Matrix<float, 1, 3> Xt = X.transpose();
203  Eigen::Vector3f Xh = X / X.norm();
204  Eigen::Matrix<float, 1, 3> Xht = Xh.transpose();
205  Eigen::Matrix3f I;
206  I.setIdentity();
207 
208  float aux = ht * Fp * X;
209  float heading_rate = -1 / (Xt * G * X) * Xt * Gp * (I - Xh * Xht) * J * xi_dot - (gvf_parametric_control.k_psi * aux /
210  sqrtf(Xt * G * X));
211 
212  // Virtual coordinate update, even if the vehicle is not in autonomous mode, the parameter w will get "closer" to
213  // the vehicle. So it is not only okei but advisable to update it.
215 
217 }
218 
219 void gvf_parametric_control_3D(float kx, float ky, float kz, float f1, float f2, float f3, float f1d, float f2d,
220  float f3d, float f1dd, float f2dd, float f3dd)
221 {
222  uint32_t now = get_sys_time_msec();
224  gvf_parametric_t0 = now;
225 
226  if (gvf_parametric_control.delta_T > 300) { // We need at least two iterations for Delta_T
227  gvf_parametric_control.w = 0; // Reset w since we assume the algorithm starts
228  return;
229  }
230 
231  float L = gvf_parametric_control.L;
233 
234  Eigen::Vector4f X;
235  Eigen::Matrix4f J;
236 
237  // Error signals phi_x phi_y and phi_z
238  struct EnuCoor_f *pos_enu = stateGetPositionEnu_f();
239  float x = pos_enu->x;
240  float y = pos_enu->y;
241  float z = pos_enu->z;
242 
243  float phi1 = L * (x - f1);
244  float phi2 = L * (y - f2);
245  float phi3 = L * (z - f3);
246 
247  gvf_parametric_trajectory.phi_errors[0] = phi1 / L; // Error signals in meters for the telemetry
250 
251  // Chi
252  X(0) = -f1d * L * L * beta - kx * phi1;
253  X(1) = -f2d * L * L * beta - ky * phi2;
254  X(2) = -f3d * L * L * beta - kz * phi3;
255  X(3) = -L * L + beta * (kx * phi1 * f1d + ky * phi2 * f2d + kz * phi3 * f3d);
256  X *= L;
257 
258  // Jacobian
259  J.setZero();
260  J(0, 0) = -kx * L;
261  J(1, 1) = -ky * L;
262  J(2, 2) = -kz * L;
263  J(3, 0) = kx * f1d * beta * L;
264  J(3, 1) = ky * f2d * beta * L;
265  J(3, 2) = kz * f3d * beta * L;
266  J(0, 3) = -(beta * L) * (beta * L * f1dd - kx * f1d);
267  J(1, 3) = -(beta * L) * (beta * L * f2dd - ky * f2d);
268  J(2, 3) = -(beta * L) * (beta * L * f3dd - kz * f3d);
269  J(3, 3) = beta * beta * (kx * (phi1 * f1dd - L * f1d * f1d) + ky * (phi2 * f2dd - L * f2d * f2d)
270  + kz * (phi3 * f3dd - L * f3d * f3d));
271  J *= L;
272 
273  // Guidance algorithm
275  float w_dot = (ground_speed * X(3)) / sqrtf(X(0) * X(0) + X(1) * X(1));
276 
277  Eigen::Vector4f xi_dot;
278  struct EnuCoor_f *vel_enu = stateGetSpeedEnu_f();
280 
281  xi_dot << vel_enu->x, vel_enu->y, vel_enu->z, w_dot;
282 
283  Eigen::Matrix2f E;
284  Eigen::Matrix<float, 2, 4> F;
285  Eigen::Matrix<float, 2, 4> Fp;
286  Eigen::Matrix4f G;
287  Eigen::Matrix4f Gp;
288  Eigen::Matrix4f I;
289  Eigen::Vector2f h;
290  Eigen::Matrix<float, 1, 2> ht;
291 
292  h << sinf(course), cosf(course);
293  ht = h.transpose();
294  I.setIdentity();
295  F << 1.0, 0.0, 0.0, 0.0,
296  0.0, 1.0, 0.0, 0.0;
297  E << 0.0, -1.0,
298  1.0, 0.0;
299  G = F.transpose() * F;
300  Fp = E * F;
301  Gp = F.transpose() * E * F;
302 
303  Eigen::Matrix<float, 1, 4> Xt = X.transpose();
304  Eigen::Vector4f Xh = X / X.norm();
305  Eigen::Matrix<float, 1, 4> Xht = Xh.transpose();
306 
307  float aux = ht * Fp * X;
308 
309  float heading_rate = -1 / (Xt * G * X) * Xt * Gp * (I - Xh * Xht) * J * xi_dot - (gvf_parametric_control.k_psi * aux /
310  sqrtf(Xt * G * X));
311  float climbing_rate = (ground_speed * X(2)) / sqrtf(X(0) * X(0) + X(1) * X(1));
312 
313  // Virtual coordinate update, even if the vehicle is not in autonomous mode, the parameter w will get "closer" to
314  // the vehicle. So it is not only okei but advisable to update it.
316 
317  gvf_parametric_low_level_control_3D(heading_rate, climbing_rate);
318 }
319 
321 // 2D TREFOIL KNOT
322 
323 bool gvf_parametric_2D_trefoil_XY(float xo, float yo, float w1, float w2, float ratio, float r, float alpha)
324 {
333 
334  float f1, f2, f1d, f2d, f1dd, f2dd;
335 
336  gvf_parametric_2d_trefoil_info(&f1, &f2, &f1d, &f2d, &f1dd, &f2dd);
338  f2dd);
339 
340  return true;
341 }
342 
343 bool gvf_parametric_2D_trefoil_wp(uint8_t wp, float w1, float w2, float ratio, float r, float alpha)
344 {
345  gvf_parametric_2D_trefoil_XY(waypoints[wp].x, waypoints[wp].y, w1, w2, ratio, r, alpha);
346  return true;
347 }
348 
350 // 3D ELLIPSE
351 
352 bool gvf_parametric_3D_ellipse_XYZ(float xo, float yo, float r, float zl, float zh, float alpha)
353 {
354  horizontal_mode = HORIZONTAL_MODE_CIRCLE; // Circle for the 2D GCS
355 
356  // Safety first! If the asked altitude is low
357  if (zl > zh) {
358  zl = zh;
359  }
360  if (zl < 1 || zh < 1) {
361  zl = 10;
362  zh = 10;
363  }
364  if (r < 1) {
365  r = 60;
366  }
367 
375 
376  float f1, f2, f3, f1d, f2d, f3d, f1dd, f2dd, f3dd;
377 
378  gvf_parametric_3d_ellipse_info(&f1, &f2, &f3, &f1d, &f2d, &f3d, &f1dd, &f2dd, &f3dd);
380  gvf_parametric_3d_ellipse_par.kz, f1, f2, f3, f1d, f2d, f3d, f1dd, f2dd, f3dd);
381 
382  return true;
383 }
384 
385 bool gvf_parametric_3D_ellipse_wp(uint8_t wp, float r, float zl, float zh, float alpha)
386 {
387  gvf_parametric_3D_ellipse_XYZ(waypoints[wp].x, waypoints[wp].y, r, zl, zh, alpha);
388  return true;
389 }
390 
391 bool gvf_parametric_3D_ellipse_wp_delta(uint8_t wp, float r, float alt_center, float delta, float alpha)
392 {
393  float zl = alt_center - delta;
394  float zh = alt_center + delta;
395 
396  gvf_parametric_3D_ellipse_XYZ(waypoints[wp].x, waypoints[wp].y, r, zl, zh, alpha);
397  return true;
398 }
399 
400 // 3D Lissajous
401 
402 bool gvf_parametric_3D_lissajous_XYZ(float xo, float yo, float zo, float cx, float cy, float cz, float wx, float wy,
403  float wz, float dx, float dy, float dz, float alpha)
404 {
405  // Safety first! If the asked altitude is low
406  if ((zo - cz) < 1) {
407  zo = 10;
408  cz = 0;
409  }
410 
425 
426  float f1, f2, f3, f1d, f2d, f3d, f1dd, f2dd, f3dd;
427 
428  gvf_parametric_3d_lissajous_info(&f1, &f2, &f3, &f1d, &f2d, &f3d, &f1dd, &f2dd, &f3dd);
430  gvf_parametric_3d_lissajous_par.kz, f1, f2, f3, f1d, f2d, f3d, f1dd, f2dd, f3dd);
431 
432  return true;
433 }
434 
435 bool gvf_parametric_3D_lissajous_wp_center(uint8_t wp, float zo, float cx, float cy, float cz, float wx, float wy,
436  float wz, float dx, float dy, float dz, float alpha)
437 {
438  gvf_parametric_3D_lissajous_XYZ(waypoints[wp].x, waypoints[wp].y, zo, cx, cy, cz, wx, wy, wz, dx, dy, dz, alpha);
439  return true;
440 }
void gvf_parametric_3d_lissajous_info(float *f1, float *f2, float *f3, float *f1d, float *f2d, float *f3d, float *f1dd, float *f2dd, float *f3dd)
static const struct usb_device_descriptor dev
Definition: usb_ser_hw.c:74
static uint32_t s
static float stateGetHorizontalSpeedNorm_f(void)
Get norm of horizontal ground speed (float).
Definition: state.h:935
gvf_par_3d_lis_par gvf_parametric_3d_lissajous_par
Guiding vector field algorithm for 2D and 3D complex trajectories.
gvf_par_3d_ell_par gvf_parametric_3d_ellipse_par
Periodic telemetry system header (includes downlink utility and generated code).
static float ground_speed
Definition: follow_me.c:76
uint32_t get_sys_time_msec(void)
Get the time in milliseconds since startup.
Definition: sys_time_arch.c:82
vector in East North Up coordinates Units: meters
void gvf_parametric_low_level_control_2D(float heading_rate)
float alpha
Definition: textons.c:107
signed char int8_t
Typedef defining 8 bit char type.
Definition: vl53l1_types.h:103
Guiding vector field algorithm for 2D and 3D complex trajectories.
void gvf_parametric_control_2D(float kx, float ky, float f1, float f2, float f1d, float f2d, float f1dd, float f2dd)
#define GVF_PARAMETRIC_CONTROL_BETA
bool gvf_parametric_2D_trefoil_XY(float xo, float yo, float w1, float w2, float ratio, float r, float alpha)
2D TRAJECTORIES
uint32_t gvf_parametric_t0
static float I
Definition: trilateration.c:35
void gvf_parametric_set_direction(int8_t s)
static struct EnuCoor_f * stateGetPositionEnu_f(void)
Get position in local ENU coordinates (float).
Definition: state.h:719
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_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)
static float J
Definition: trilateration.c:35
bool gvf_parametric_3D_ellipse_wp_delta(uint8_t wp, float r, float alt_center, float delta, float alpha)
float x
in meters
Guiding vector field algorithm for 2D and 3D complex trajectories.
#define DefaultPeriodic
Set default periodic telemetry.
Definition: telemetry.h:66
float p_parametric[16]
#define GVF_PARAMETRIC_CONTROL_L
void gvf_parametric_3d_ellipse_info(float *f1, float *f2, float *f3, float *f1d, float *f2d, float *f3d, float *f1dd, float *f2dd, float *f3dd)
bool gvf_parametric_3D_ellipse_XYZ(float xo, float yo, float r, float zl, float zh, float alpha)
3D TRAJECTORIES
gvf_par_2d_tre_par gvf_parametric_2d_trefoil_par
Core autopilot interface common to all firmwares.
gvf_parametric_con gvf_parametric_control
static float stateGetHorizontalSpeedDir_f(void)
Get dir of horizontal ground speed (float).
Definition: state.h:944
static void h(const real32_T x[7], const real32_T q[4], real32_T y[6])
void gvf_parametric_init(void)
static struct EnuCoor_f * stateGetSpeedEnu_f(void)
Get ground speed in local ENU coordinates (float).
Definition: state.h:917
#define GVF_PARAMETRIC_CONTROL_KROLL
struct point waypoints[NB_WAYPOINT]
size == nb_waypoint, waypoint 0 is a dummy waypoint
Definition: common_nav.c:38
float z
in meters
static int16_t course[3]
Definition: airspeed_uADC.c:58
gvf_parametric_tra gvf_parametric_trajectory
#define GVF_PARAMETRIC_CONTROL_KPSI
Guiding vector field algorithm for 2D and 3D parametric trajectories.
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)
void gvf_parametric_low_level_control_3D(float heading_rate, float climbing_rate)
#define E
bool gvf_parametric_2D_trefoil_wp(uint8_t wp, float w1, float w2, float ratio, float r, float alpha)
#define GVF_PARAMETRIC_CONTROL_KCLIMB
enum trajectories_parametric type
static void send_circle_parametric(struct transport_tx *trans, struct link_device *dev)
static void send_gvf_parametric(struct transport_tx *trans, struct link_device *dev)
void gvf_parametric_2d_trefoil_info(float *f1, float *f2, float *f1d, float *f2d, float *f1dd, float *f2dd)
bool gvf_parametric_3D_ellipse_wp(uint8_t wp, float r, float zl, float zh, float alpha)
float y
in meters
Firmware dependent file for the guiding vector field algorithm for 2D and 3D parametric trajectories...
int8_t register_periodic_telemetry(struct periodic_telemetry *_pt, uint8_t _id, telemetry_cb _cb)
Register a telemetry callback function.
Definition: telemetry.c:46
unsigned char uint8_t
Typedef defining 8 bit unsigned char type.
Definition: vl53l1_types.h:98
unsigned int uint32_t
Typedef defining 32 bit unsigned int type.
Definition: vl53l1_types.h:78