 |
Paparazzi UAS
v6.1.0_stable
Paparazzi is a free software Unmanned Aircraft System.
|
Go to the documentation of this file.
28 #include <Eigen/Dense>
49 #if PERIODIC_TELEMETRY
101 #endif // PERIODIC TELEMETRY
118 #if PERIODIC_TELEMETRY
150 float x = pos_enu->
x;
151 float y = pos_enu->
y;
153 float phi1 = L * (
x - f1);
154 float phi2 = L * (
y - f2);
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);
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));
178 Eigen::Vector3f xi_dot;
182 xi_dot << vel_enu->
x, vel_enu->
y, w_dot;
186 Eigen::Matrix<float, 2, 3> Fp;
188 Eigen::Matrix<float, 1, 2> ht;
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();
208 float aux = ht * Fp *
X;
220 float f3d,
float f1dd,
float f2dd,
float f3dd)
239 float x = pos_enu->
x;
240 float y = pos_enu->
y;
241 float z = pos_enu->
z;
243 float phi1 = L * (
x - f1);
244 float phi2 = L * (
y - f2);
245 float phi3 = L * (
z - f3);
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);
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));
277 Eigen::Vector4f xi_dot;
281 xi_dot << vel_enu->
x, vel_enu->
y, vel_enu->
z, w_dot;
284 Eigen::Matrix<float, 2, 4> F;
285 Eigen::Matrix<float, 2, 4> Fp;
290 Eigen::Matrix<float, 1, 2> ht;
295 F << 1.0, 0.0, 0.0, 0.0,
299 G = F.transpose() * F;
301 Gp = F.transpose() *
E * F;
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();
307 float aux = ht * Fp *
X;
334 float f1, f2, f1d, f2d, f1dd, f2dd;
360 if (zl < 1 || zh < 1) {
376 float f1, f2, f3, f1d, f2d, f3d, f1dd, f2dd, f3dd;
393 float zl = alt_center - delta;
394 float zh = alt_center + delta;
403 float wz,
float dx,
float dy,
float dz,
float alpha)
426 float f1, f2, f3, f1d, f2d, f3d, f1dd, f2dd, f3dd;
436 float wz,
float dx,
float dy,
float dz,
float alpha)
438 gvf_parametric_3D_lissajous_XYZ(
waypoints[wp].
x,
waypoints[wp].
y, zo, cx, cy, cz, wx, wy, wz, dx, dy, dz,
alpha);
gvf_par_3d_ell_par gvf_parametric_3d_ellipse_par
void gvf_parametric_control_2D(float kx, float ky, float f1, float f2, float f1d, float f2d, float f1dd, float f2dd)
signed char int8_t
Typedef defining 8 bit char type.
unsigned int uint32_t
Typedef defining 32 bit unsigned int type.
unsigned char uint8_t
Typedef defining 8 bit unsigned char type.
static float stateGetHorizontalSpeedDir_f(void)
Get dir of horizontal ground speed (float).
#define GVF_PARAMETRIC_CONTROL_KPSI
#define GVF_PARAMETRIC_CONTROL_BETA
static void h(const real32_T x[7], const real32_T q[4], real32_T y[6])
bool gvf_parametric_3D_ellipse_wp(uint8_t wp, float r, float zl, float zh, float alpha)
static float ground_speed
static struct EnuCoor_f * stateGetPositionEnu_f(void)
Get position in local ENU coordinates (float).
bool gvf_parametric_2D_trefoil_wp(uint8_t wp, float w1, float w2, float ratio, float r, float alpha)
static const struct usb_device_descriptor dev
#define GVF_PARAMETRIC_CONTROL_KCLIMB
void gvf_parametric_3d_lissajous_info(float *f1, float *f2, float *f3, float *f1d, float *f2d, float *f3d, float *f1dd, float *f2dd, float *f3dd)
enum trajectories_parametric type
bool gvf_parametric_2D_trefoil_XY(float xo, float yo, float w1, float w2, float ratio, float r, float alpha)
2D TRAJECTORIES
#define GVF_PARAMETRIC_CONTROL_L
struct point waypoints[NB_WAYPOINT]
size == nb_waypoint, waypoint 0 is a dummy waypoint
#define GVF_PARAMETRIC_CONTROL_KROLL
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)
void gvf_parametric_2d_trefoil_info(float *f1, float *f2, float *f1d, float *f2d, float *f1dd, float *f2dd)
static void send_circle_parametric(struct transport_tx *trans, struct link_device *dev)
gvf_par_3d_lis_par gvf_parametric_3d_lissajous_par
uint32_t gvf_parametric_t0
gvf_parametric_con gvf_parametric_control
static void send_gvf_parametric(struct transport_tx *trans, struct link_device *dev)
void gvf_parametric_low_level_control_3D(float heading_rate, float climbing_rate)
#define HORIZONTAL_MODE_CIRCLE
uint32_t get_sys_time_msec(void)
Get the time in milliseconds since startup.
int8_t register_periodic_telemetry(struct periodic_telemetry *_pt, uint8_t _id, telemetry_cb _cb)
Register a telemetry callback function.
vector in East North Up coordinates Units: meters
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_2d_tre_par gvf_parametric_2d_trefoil_par
gvf_parametric_tra gvf_parametric_trajectory
static float stateGetHorizontalSpeedNorm_f(void)
Get norm of horizontal ground speed (float).
static struct EnuCoor_f * stateGetSpeedEnu_f(void)
Get ground speed in local ENU coordinates (float).
bool gvf_parametric_3D_ellipse_XYZ(float xo, float yo, float r, float zl, float zh, float alpha)
3D TRAJECTORIES
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_low_level_control_2D(float heading_rate)
bool gvf_parametric_3D_ellipse_wp_delta(uint8_t wp, float r, float alt_center, float delta, float alpha)
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_set_direction(int8_t s)
void gvf_parametric_init(void)
#define DefaultPeriodic
Set default periodic telemetry.