33#ifndef GVF_PARAMETRIC_CONTROL_KROLL
34 #define GVF_PARAMETRIC_CONTROL_KROLL 1
38#ifndef GVF_PARAMETRIC_CONTROL_KCLIMB
39 #define GVF_PARAMETRIC_CONTROL_KCLIMB 1
43#ifndef GVF_PARAMETRIC_CONTROL_L
44 #if GVF_PARAMETRIC_CONTROL_STEP_ADAPTATION > 0
45 #define GVF_PARAMETRIC_CONTROL_L 1
47 #define GVF_PARAMETRIC_CONTROL_L 0.1
52#ifndef GVF_PARAMETRIC_CONTROL_BETA
53 #if GVF_PARAMETRIC_CONTROL_STEP_ADAPTATION > 0
54 #define GVF_PARAMETRIC_CONTROL_BETA 1
56 #define GVF_PARAMETRIC_CONTROL_BETA 0.01
62#ifndef GVF_PARAMETRIC_CONTROL_KPSI
63 #define GVF_PARAMETRIC_CONTROL_KPSI 1
96 pprz_msg_send_GVF_PARAMETRIC(
105static void send_circle_parametric(
struct transport_tx *trans,
struct link_device *
dev)
112 pprz_msg_send_CIRCLE(
141#if PERIODIC_TELEMETRY
162#ifdef FIXEDWING_FIRMWARE
167 float f_v2 = f1d * f1d + f2d * f2d;
168 float f_v = sqrtf(f_v2);
170 float f_d_dot_f_dd = f1d * f1dd + f2d * f2dd;
175 float og_f1dd = f1dd;
176 float og_f2dd = f2dd;
187 f1dd = (f1dd - f_d_dot_f_dd * f1d / f_v2) / f_v2;
188 f2dd = (f2dd - f_d_dot_f_dd * f2d / f_v2) / f_v2;
203 float x = pos_enu->
x;
204 float y = pos_enu->
y;
206 float phi1 = L * (
x - f1);
207 float phi2 = L * (
y - f2);
214 X(0) = L * beta * f1d - kx * phi1;
215 X(1) = L * beta * f2d - ky * phi2;
216 X(2) = L + beta * (kx * phi1 * f1d + ky * phi2 * f2d);
223 J(2, 0) = (beta * L) * (beta * f1dd + kx * f1d);
224 J(2, 1) = (beta * L) * (beta * f2dd + ky * f2d);
225 J(2, 2) = beta * beta * (kx * (phi1 * f1dd - L * f1d * f1d) + ky * (phi2 * f2dd - L * f2d * f2d));
236 w_dot = (
ground_speed * X(2)) / sqrtf(X(0) * X(0) + X(1) * X(1));
239 Eigen::Vector3f xi_dot;
243 xi_dot << vel_enu->
x, vel_enu->
y, w_dot;
247 Eigen::Matrix<float, 2, 3> Fp;
249 Eigen::Matrix<float, 1, 2> ht;
263 Eigen::Matrix<float, 1, 3> Xt = X.transpose();
264 Eigen::Vector3f Xh = X / X.norm();
265 Eigen::Matrix<float, 1, 3> Xht = Xh.transpose();
269 float aux = ht * Fp * X;
270 float heading_rate = -1 / (Xt * G * X) * Xt * Gp * (
I - Xh * Xht) *
J * xi_dot - (
gvf_parametric_control.k_psi * aux /
275 gvf_c_info.kappa = (f1d * f2dd - f1dd * f2d) / powf(f1d * f1d + f2d * f2d, 1.5);
285#ifdef FIXEDWING_FIRMWARE
287 float f3d,
float f1dd,
float f2dd,
float f3dd)
310 float f_v2 = f1d * f1d + f2d * f2d + f3d * f3d;
311 float f_v = sqrtf(f_v2);
313 float f_d_dot_f_dd = f1d * f1dd + f2d * f2dd + f3d * f3dd;
319 float og_f1dd = f1dd;
320 float og_f2dd = f2dd;
321 float og_f3dd = f3dd;
334 f1dd = (f1dd - f_d_dot_f_dd * f1d / f_v2) / f_v2;
335 f2dd = (f2dd - f_d_dot_f_dd * f2d / f_v2) / f_v2;
336 f3dd = (f3dd - f_d_dot_f_dd * f3d / f_v2) / f_v2;
346 float x = pos_enu->
x;
347 float y = pos_enu->
y;
348 float z = pos_enu->
z;
350 float phi1 = L * (
x - f1);
351 float phi2 = L * (
y - f2);
352 float phi3 = L * (
z - f3);
361 X(0) = -f1d * L * L * beta - kx * phi1;
362 X(1) = -f2d * L * L * beta - ky * phi2;
363 X(2) = -f3d * L * L * beta - kz * phi3;
364 X(3) = -L * L + beta * (kx * phi1 * f1d + ky * phi2 * f2d + kz * phi3 * f3d);
372 J(3, 0) = kx * f1d * beta * L;
373 J(3, 1) = ky * f2d * beta * L;
374 J(3, 2) = kz * f3d * beta * L;
375 J(0, 3) = -(beta * L) * (beta * L * f1dd - kx * f1d);
376 J(1, 3) = -(beta * L) * (beta * L * f2dd - ky * f2d);
377 J(2, 3) = -(beta * L) * (beta * L * f3dd - kz * f3d);
378 J(3, 3) = beta * beta * (kx * (phi1 * f1dd - L * f1d * f1d) + ky * (phi2 * f2dd - L * f2d * f2d)
379 + kz * (phi3 * f3dd - L * f3d * f3d));
393 Eigen::Vector4f xi_dot;
397 xi_dot << vel_enu->
x, vel_enu->
y, vel_enu->
z, w_dot;
400 Eigen::Matrix<float, 2, 4> F;
401 Eigen::Matrix<float, 2, 4> Fp;
406 Eigen::Matrix<float, 1, 2> ht;
411 F << 1.0, 0.0, 0.0, 0.0,
415 G = F.transpose() * F;
417 Gp = F.transpose() *
E * F;
419 Eigen::Matrix<float, 1, 4> Xt =
X.transpose();
420 Eigen::Vector4f Xh =
X /
X.norm();
421 Eigen::Matrix<float, 1, 4> Xht = Xh.transpose();
423 float aux = ht * Fp *
X;
static void h(const real32_T x[7], const real32_T q[4], real32_T y[6])
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
static struct uart_periph * dev
static struct EnuCoor_f * stateGetPositionEnu_f(void)
Get position in local ENU coordinates (float).
static float stateGetHorizontalSpeedNorm_f(void)
Get norm of horizontal ground speed (float).
static float stateGetHorizontalSpeedDir_f(void)
Get dir of horizontal ground speed (float).
static struct EnuCoor_f * stateGetSpeedEnu_f(void)
Get ground speed in local ENU coordinates (float).
float step_adaptation(float ds, float f1d, float f2d, float f3d, float f1dd, float f2dd, float f3dd)
Compute the adapted parametric step given the wanted geometric distance.
Dynamic parametric step adaptation for the GVF algorithm.
gvf_common_ctrl gvf_c_ctrl
gvf_common_params gvf_c_params
void gvf_low_level_control_2D(float omega)
gvf_common_info gvf_c_info
void gvf_low_level_control_3D(float heading_rate, float climbing_rate)
gvf_parametric_tra gvf_parametric_trajectory
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)
#define GVF_PARAMETRIC_CONTROL_STEP_ADAPTATION
vector in East North Up coordinates Units: meters
int16_t register_periodic_telemetry(struct periodic_telemetry *_pt, uint16_t _id, telemetry_cb _cb)
Register a telemetry callback function.
Periodic telemetry system header (includes downlink utility and generated code).
#define DefaultPeriodic
Set default periodic telemetry.
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.