Paparazzi UAS v7.0_unstable
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> // https://eigen.tuxfamily.org/dox/GettingStarted.html
29
30#include "gvf_parametric.h"
31
33#ifndef GVF_PARAMETRIC_CONTROL_KROLL
34#define GVF_PARAMETRIC_CONTROL_KROLL 1
35#endif
36
38#ifndef GVF_PARAMETRIC_CONTROL_KCLIMB
39#define GVF_PARAMETRIC_CONTROL_KCLIMB 1
40#endif
41
43#ifndef GVF_PARAMETRIC_CONTROL_L
44#define GVF_PARAMETRIC_CONTROL_L 0.1
45#endif
46
48#ifndef GVF_PARAMETRIC_CONTROL_BETA
49#define GVF_PARAMETRIC_CONTROL_BETA 0.01
50#endif
51
53#ifndef GVF_PARAMETRIC_CONTROL_KPSI
54#define GVF_PARAMETRIC_CONTROL_KPSI 1
55#endif
56
57#ifdef __cplusplus
58extern "C" {
59#endif
60
61#include "autopilot.h"
62
63// Control
66
67// Time variables to check if GVF is active
69
72#if PERIODIC_TELEMETRY
92
93#if GVF_OCAML_GCS
94static void send_circle_parametric(struct transport_tx *trans, struct link_device *dev)
95{
97 uint32_t delta_T = now - gvf_parametric_t0;
98
99 if (delta_T < 200)
102 trans, dev, AC_ID,
105 }
106}
107#endif // GVF_OCAML_GCS
108
109#endif // PERIODIC TELEMETRY
110
111#ifdef __cplusplus
112}
113#endif
114
136
137void gvf_parametric_control_2D(float kx, float ky, float f1, float f2, float f1d, float f2d, float f1dd, float f2dd)
138{
139
143
144 if (gvf_parametric_control.delta_T > 300) { // We need at least two iterations for Delta_T
145 gvf_parametric_control.w = 0; // Reset w since we assume the algorithm starts
146 return;
147 }
148
149 // Carrot position
150#ifdef FIXEDWING_FIRMWARE
151 desired_x = f1;
152 desired_y = f2;
153#endif
154
155 float L = gvf_parametric_control.L;
157
158 Eigen::Vector3f X;
159 Eigen::Matrix3f J;
160
161 // Error signals phi_x and phi_y
163 float x = pos_enu->x;
164 float y = pos_enu->y;
165
166 float phi1 = L * (x - f1);
167 float phi2 = L * (y - f2);
168
169 gvf_parametric_telemetry.phi_errors[0] = phi1; // Error signals for the telemetry
172
173 // Chi
174 X(0) = L * beta * f1d - kx * phi1;
175 X(1) = L * beta * f2d - ky * phi2;
176 X(2) = L + beta * (kx * phi1 * f1d + ky * phi2 * f2d);
177 X *= L;
178
179 // Jacobian
180 J.setZero();
181 J(0, 0) = -kx * L;
182 J(1, 1) = -ky * L;
183 J(2, 0) = (beta * L) * (beta * f1dd + kx * f1d);
184 J(2, 1) = (beta * L) * (beta * f2dd + ky * f2d);
185 J(2, 2) = beta * beta * (kx * (phi1 * f1dd - L * f1d * f1d) + ky * (phi2 * f2dd - L * f2d * f2d));
186 J *= L;
187
188 // Guidance algorithm
190 float w_dot = (ground_speed * X(2)) / sqrtf(X(0) * X(0) + X(1) * X(1));
191
192 Eigen::Vector3f xi_dot;
195
196 xi_dot << vel_enu->x, vel_enu->y, w_dot;
197
198 Eigen::Matrix3f G;
199 Eigen::Matrix3f Gp;
200 Eigen::Matrix<float, 2, 3> Fp;
201 Eigen::Vector2f h;
202 Eigen::Matrix<float, 1, 2> ht;
203
204 G << 1, 0, 0,
205 0, 1, 0,
206 0, 0, 0;
207 Fp << 0, -1, 0,
208 1, 0, 0;
209 Gp << 0, -1, 0,
210 1, 0, 0,
211 0, 0, 0;
212
213 h << sinf(course), cosf(course);
214 ht = h.transpose();
215
216 Eigen::Matrix<float, 1, 3> Xt = X.transpose();
217 Eigen::Vector3f Xh = X / X.norm();
218 Eigen::Matrix<float, 1, 3> Xht = Xh.transpose();
219 Eigen::Matrix3f I;
220 I.setIdentity();
221
222 float aux = ht * Fp * X;
223 float heading_rate = -1 / (Xt * G * X) * Xt * Gp * (I - Xh * Xht) * J * xi_dot - (gvf_parametric_control.k_psi * aux /
224 sqrtf(Xt * G * X));
225
226 // From gvf_common.h TODO: implement d/dt of kppa and ori_err
227 gvf_c_ctrl.omega = heading_rate;
228 gvf_c_info.kappa = (f1d * f2dd - f1dd * f2d) / powf(f1d * f1d + f2d * f2d, 1.5);
229 gvf_c_info.ori_err = 1 - (Xh(0) * cosf(course) + Xh(1) * sinf(course));
230
231 // Virtual coordinate update, even if the vehicle is not in autonomous mode, the parameter w will get "closer" to
232 // the vehicle. So it is not only okei but advisable to update it.
234
235 gvf_low_level_control_2D(heading_rate);
236}
237
238#ifdef FIXEDWING_FIRMWARE
239void gvf_parametric_control_3D(float kx, float ky, float kz, float f1, float f2, float f3, float f1d, float f2d,
240 float f3d, float f1dd, float f2dd, float f3dd)
241{
245
246 if (gvf_parametric_control.delta_T > 300) { // We need at least two iterations for Delta_T
247 gvf_parametric_control.w = 0; // Reset w since we assume the algorithm starts
248 return;
249 }
250
251 // Carrot position
252 desired_x = f1;
253 desired_y = f2;
254
255 float L = gvf_parametric_control.L;
257
258 Eigen::Vector4f X;
259 Eigen::Matrix4f J;
260
261 // Error signals phi_x phi_y and phi_z
263 float x = pos_enu->x;
264 float y = pos_enu->y;
265 float z = pos_enu->z;
266
267 float phi1 = L * (x - f1);
268 float phi2 = L * (y - f2);
269 float phi3 = L * (z - f3);
270
271 gvf_parametric_telemetry.phi_errors[0] = phi1 / L; // Error signals in meters for the telemetry
275
276
277 // Chi
278 X(0) = -f1d * L * L * beta - kx * phi1;
279 X(1) = -f2d * L * L * beta - ky * phi2;
280 X(2) = -f3d * L * L * beta - kz * phi3;
281 X(3) = -L * L + beta * (kx * phi1 * f1d + ky * phi2 * f2d + kz * phi3 * f3d);
282 X *= L;
283
284 // Jacobian
285 J.setZero();
286 J(0, 0) = -kx * L;
287 J(1, 1) = -ky * L;
288 J(2, 2) = -kz * L;
289 J(3, 0) = kx * f1d * beta * L;
290 J(3, 1) = ky * f2d * beta * L;
291 J(3, 2) = kz * f3d * beta * L;
292 J(0, 3) = -(beta * L) * (beta * L * f1dd - kx * f1d);
293 J(1, 3) = -(beta * L) * (beta * L * f2dd - ky * f2d);
294 J(2, 3) = -(beta * L) * (beta * L * f3dd - kz * f3d);
295 J(3, 3) = beta * beta * (kx * (phi1 * f1dd - L * f1d * f1d) + ky * (phi2 * f2dd - L * f2d * f2d)
296 + kz * (phi3 * f3dd - L * f3d * f3d));
297 J *= L;
298
299 // Guidance algorithm
301 float w_dot = (ground_speed * X(3)) / sqrtf(X(0) * X(0) + X(1) * X(1));
302
303 Eigen::Vector4f xi_dot;
306
307 xi_dot << vel_enu->x, vel_enu->y, vel_enu->z, w_dot;
308
309 Eigen::Matrix2f E;
310 Eigen::Matrix<float, 2, 4> F;
311 Eigen::Matrix<float, 2, 4> Fp;
312 Eigen::Matrix4f G;
313 Eigen::Matrix4f Gp;
314 Eigen::Matrix4f I;
315 Eigen::Vector2f h;
316 Eigen::Matrix<float, 1, 2> ht;
317
318 h << sinf(course), cosf(course);
319 ht = h.transpose();
320 I.setIdentity();
321 F << 1.0, 0.0, 0.0, 0.0,
322 0.0, 1.0, 0.0, 0.0;
323 E << 0.0, -1.0,
324 1.0, 0.0;
325 G = F.transpose() * F;
326 Fp = E * F;
327 Gp = F.transpose() * E * F;
328
329 Eigen::Matrix<float, 1, 4> Xt = X.transpose();
330 Eigen::Vector4f Xh = X / X.norm();
331 Eigen::Matrix<float, 1, 4> Xht = Xh.transpose();
332
333 float aux = ht * Fp * X;
334
335 float heading_rate = -1 / (Xt * G * X) * Xt * Gp * (I - Xh * Xht) * J * xi_dot - (gvf_parametric_control.k_psi * aux /
336 sqrtf(Xt * G * X));
337 float climbing_rate = (ground_speed * X(2)) / sqrtf(X(0) * X(0) + X(1) * X(1));
338
339 // Virtual coordinate update, even if the vehicle is not in autonomous mode, the parameter w will get "closer" to
340 // the vehicle. So it is not only okei but advisable to update it.
342
344}
345#endif // FIXED_WING FIRMWARE
346
351
static void h(const real32_T x[7], const real32_T q[4], real32_T y[6])
static int16_t course[3]
Core autopilot interface common to all firmwares.
uint32_t get_sys_time_msec(void)
Get the time in milliseconds since startup.
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:848
static float stateGetHorizontalSpeedNorm_f(void)
Get norm of horizontal ground speed (float).
Definition state.h:1076
static float stateGetHorizontalSpeedDir_f(void)
Get dir of horizontal ground speed (float).
Definition state.h:1085
static struct EnuCoor_f * stateGetSpeedEnu_f(void)
Get ground speed in local ENU coordinates (float).
Definition state.h:1058
gvf_common_ctrl gvf_c_ctrl
Definition gvf_common.c:28
gvf_common_params gvf_c_params
Definition gvf_common.c:30
void gvf_low_level_control_2D(float omega)
Definition gvf_common.c:58
gvf_common_info gvf_c_info
Definition gvf_common.c:29
void gvf_low_level_control_3D(float heading_rate, float climbing_rate)
Definition gvf_common.c:79
gvf_parametric_tra gvf_parametric_trajectory
@ ELLIPSE_3D
enum trajectories_parametric type
gvf_parametric_con gvf_parametric_control
#define GVF_PARAMETRIC_CONTROL_KCLIMB
#define GVF_PARAMETRIC_CONTROL_L
#define GVF_PARAMETRIC_CONTROL_BETA
#define GVF_PARAMETRIC_CONTROL_KROLL
static void send_gvf_parametric(struct transport_tx *trans, struct link_device *dev)
TELEMETRY -----------------------------------------------------------—.
#define GVF_PARAMETRIC_CONTROL_KPSI
uint32_t gvf_parametric_t0
void gvf_parametric_init(void)
void gvf_parametric_control_2D(float kx, float ky, float f1, float f2, float f1d, float f2d, float f1dd, float f2dd)
gvf_parametric_tel gvf_parametric_telemetry
void gvf_parametric_set_direction(int8_t s)
Guiding vector field algorithm for 2D and 3D parametric trajectories.
void gvf_parametric_control_3D(float, float, float, float, float, float, float, float, float, float, float, float)
static uint32_t s
uint16_t foo
Definition main_demo5.c:58
float desired_x
Definition nav.c:308
float desired_y
Definition nav.c:308
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
static float J
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.