28 #include <Eigen/Dense>
56 #if PERIODIC_TELEMETRY
74 static void send_circle_parametric(
struct transport_tx *trans,
struct link_device *
dev)
104 #if PERIODIC_TELEMETRY
142 float x = pos_enu->
x;
143 float y = pos_enu->
y;
145 float phi1 = L * (
x - f1);
146 float phi2 = L * (
y - f2);
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);
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));
171 Eigen::Vector3f xi_dot;
175 xi_dot << vel_enu->
x, vel_enu->
y, w_dot;
179 Eigen::Matrix<float, 2, 3> Fp;
181 Eigen::Matrix<float, 1, 2> ht;
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();
201 float aux = ht * Fp *
X;
213 float f3d,
float f1dd,
float f2dd,
float f3dd)
236 float x = pos_enu->
x;
237 float y = pos_enu->
y;
238 float z = pos_enu->
z;
240 float phi1 = L * (
x - f1);
241 float phi2 = L * (
y - f2);
242 float phi3 = L * (
z - f3);
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);
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));
276 Eigen::Vector4f xi_dot;
280 xi_dot << vel_enu->
x, vel_enu->
y, vel_enu->
z, w_dot;
283 Eigen::Matrix<float, 2, 4> F;
284 Eigen::Matrix<float, 2, 4> Fp;
289 Eigen::Matrix<float, 1, 2> ht;
294 F << 1.0, 0.0, 0.0, 0.0,
298 G = F.transpose() * F;
300 Gp = F.transpose() *
E * F;
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();
306 float aux = ht * Fp *
X;
335 float f1, f2, f1d, f2d, f1dd, f2dd;
364 if (zl < 1 || zh < 1) {
382 float f1, f2, f3, f1d, f2d, f3d, f1dd, f2dd, f3dd;
402 float zl = alt_center - delta;
403 float zh = alt_center + delta;
412 float wz,
float dx,
float dy,
float dz,
float alpha)
437 float f1, f2, f3, f1d, f2d, f3d, f1dd, f2dd, f3dd;
447 float wz,
float dx,
float dy,
float dz,
float alpha)
452 gvf_parametric_3D_lissajous_XYZ(
waypoints[wp].
x,
waypoints[wp].
y, zo, cx, cy, cz, wx, wy, wz, dx, dy, dz,
alpha);
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.
struct point waypoints[NB_WAYPOINT]
size == nb_waypoint, waypoint 0 is a dummy waypoint
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_parametric_con gvf_parametric_control
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)
void gvf_parametric_set_direction(int8_t s)
Guiding vector field algorithm for 2D and 3D parametric trajectories.
#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.
#define HORIZONTAL_MODE_CIRCLE
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.