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#define GVF_PARAMETRIC_CONTROL_L 0.1
48#ifndef GVF_PARAMETRIC_CONTROL_BETA
49#define GVF_PARAMETRIC_CONTROL_BETA 0.01
53#ifndef GVF_PARAMETRIC_CONTROL_KPSI
54#define GVF_PARAMETRIC_CONTROL_KPSI 1
129#if PERIODIC_TELEMETRY
150#ifdef FIXEDWING_FIRMWARE
174 X(0) = L * beta *
f1d - kx *
phi1;
175 X(1) = L * beta *
f2d - ky *
phi2;
183 J(2, 0) = (beta * L) * (beta *
f1dd + kx *
f1d);
184 J(2, 1) = (beta * L) * (beta *
f2dd + ky *
f2d);
200 Eigen::Matrix<float, 2, 3>
Fp;
202 Eigen::Matrix<float, 1, 2>
ht;
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();
238#ifdef FIXEDWING_FIRMWARE
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;
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);
310 Eigen::Matrix<float, 2, 4> F;
311 Eigen::Matrix<float, 2, 4>
Fp;
316 Eigen::Matrix<float, 1, 2>
ht;
321 F << 1.0, 0.0, 0.0, 0.0,
325 G = F.transpose() * F;
327 Gp = F.transpose() *
E * F;
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();
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 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).
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
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)
vector in East North Up coordinates Units: meters
static const struct usb_device_descriptor dev
int8_t register_periodic_telemetry(struct periodic_telemetry *_pt, uint8_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.