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));
176 float w_dot = (ground_speed *
X(2)) / sqrtf(
X(0) *
X(0) +
X(1) *
X(1));
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));
275 float w_dot = (ground_speed *
X(3)) / sqrtf(
X(0) *
X(0) +
X(1) *
X(1));
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;
311 float climbing_rate = (ground_speed *
X(2)) / sqrtf(
X(0) *
X(0) +
X(1) *
X(1));
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);