diff --git a/conf/flight_plans/demo_gvf.xml b/conf/flight_plans/demo_gvf.xml index 7c065a8a95..be8a3a1233 100644 --- a/conf/flight_plans/demo_gvf.xml +++ b/conf/flight_plans/demo_gvf.xml @@ -29,10 +29,12 @@ + + @@ -78,6 +80,18 @@ + + + + + + + + + + + + diff --git a/conf/modules/gvf_parametric.xml b/conf/modules/gvf_parametric.xml new file mode 100644 index 0000000000..cb8ccf974f --- /dev/null +++ b/conf/modules/gvf_parametric.xml @@ -0,0 +1,87 @@ + + + + + Guidance algorithm for tracking 2D and 3D smooth trajectories that are defined by a parametric equation. + + For more details we refer to https://wiki.paparazziuav.org/wiki/Module/guidance_vector_field . + This algorithm needs the Eigen library, be sure you have it installed, e.g., git submodule update --init --recursive sw/ext/eigen + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + +
+ + + + + +
+ + + + + + + + + + + + + + + + + + + + + diff --git a/conf/telemetry/default_fixedwing_gvf.xml b/conf/telemetry/default_fixedwing_gvf.xml index 16c05b6129..ac6a9155fe 100644 --- a/conf/telemetry/default_fixedwing_gvf.xml +++ b/conf/telemetry/default_fixedwing_gvf.xml @@ -32,6 +32,7 @@ + diff --git a/sw/airborne/modules/guidance/gvf/gvf.c b/sw/airborne/modules/guidance/gvf/gvf.c index 5e10467d1d..d6e0dbd1ef 100644 --- a/sw/airborne/modules/guidance/gvf/gvf.c +++ b/sw/airborne/modules/guidance/gvf/gvf.c @@ -40,6 +40,9 @@ gvf_con gvf_control; gvf_tra gvf_trajectory; gvf_seg gvf_segment; +// Time variables to check if GVF is active +uint32_t gvf_t0 = 0; + #if PERIODIC_TELEMETRY #include "subsystems/datalink/telemetry.h" static void send_gvf(struct transport_tx *trans, struct link_device *dev) @@ -65,27 +68,39 @@ static void send_gvf(struct transport_tx *trans, struct link_device *dev) uint8_t traj_type = (uint8_t)gvf_trajectory.type; - pprz_msg_send_GVF(trans, dev, AC_ID, &gvf_control.error, &traj_type, - &gvf_control.s, &gvf_control.ke, plen, gvf_trajectory.p); + uint32_t now = get_sys_time_msec(); + uint32_t delta_T = now - gvf_t0; + + if (delta_T < 200) + pprz_msg_send_GVF(trans, dev, AC_ID, &gvf_control.error, &traj_type, + &gvf_control.s, &gvf_control.ke, plen, gvf_trajectory.p); } static void send_circle(struct transport_tx *trans, struct link_device *dev) { - if (gvf_trajectory.type == ELLIPSE && - (gvf_trajectory.p[2] == gvf_trajectory.p[3])) { - pprz_msg_send_CIRCLE(trans, dev, AC_ID, - &gvf_trajectory.p[0], &gvf_trajectory.p[1], - &gvf_trajectory.p[2]); - } + uint32_t now = get_sys_time_msec(); + uint32_t delta_T = now - gvf_t0; + + if (delta_T < 200) + if (gvf_trajectory.type == ELLIPSE && + (gvf_trajectory.p[2] == gvf_trajectory.p[3])) { + pprz_msg_send_CIRCLE(trans, dev, AC_ID, + &gvf_trajectory.p[0], &gvf_trajectory.p[1], + &gvf_trajectory.p[2]); + } } static void send_segment(struct transport_tx *trans, struct link_device *dev) { - if (gvf_trajectory.type == LINE && gvf_segment.seg == 1) { - pprz_msg_send_SEGMENT(trans, dev, AC_ID, - &gvf_segment.x1, &gvf_segment.y1, - &gvf_segment.x2, &gvf_segment.y2); - } + uint32_t now = get_sys_time_msec(); + uint32_t delta_T = now - gvf_t0; + + if (delta_T < 200) + if (gvf_trajectory.type == LINE && gvf_segment.seg == 1) { + pprz_msg_send_SEGMENT(trans, dev, AC_ID, + &gvf_segment.x1, &gvf_segment.y1, + &gvf_segment.x2, &gvf_segment.y2); + } } #endif @@ -139,6 +154,8 @@ void gvf_init(void) void gvf_control_2D(float ke, float kn, float e, struct gvf_grad *grad, struct gvf_Hess *hess) { + gvf_t0 = get_sys_time_msec(); + struct FloatEulers *att = stateGetNedToBodyEulers_f(); float ground_speed = stateGetHorizontalSpeedNorm_f(); float course = stateGetHorizontalSpeedDir_f(); diff --git a/sw/airborne/modules/guidance/gvf/gvf.h b/sw/airborne/modules/guidance/gvf/gvf.h index 63f79f223f..be824a9a27 100644 --- a/sw/airborne/modules/guidance/gvf/gvf.h +++ b/sw/airborne/modules/guidance/gvf/gvf.h @@ -32,8 +32,8 @@ #include "std.h" -/** @typedef gvf_conf -* @brief Parameters for the GVF +/** @typedef gvf_con +* @brief Control parameters for the GVF * @param ke Gain defining how agressive is the vector field * @param kn Gain for making converge the vehile to the vector field * @param error Error signal. It does not have any specific units. It depends on how the trajectory has been implemented. Check the specific wiki entry for each trajectory. diff --git a/sw/airborne/modules/guidance/gvf_parametric/gvf_parametric.cpp b/sw/airborne/modules/guidance/gvf_parametric/gvf_parametric.cpp new file mode 100644 index 0000000000..2f3b6fda63 --- /dev/null +++ b/sw/airborne/modules/guidance/gvf_parametric/gvf_parametric.cpp @@ -0,0 +1,440 @@ +/* + * Copyright (C) 2020 Hector Garcia de Marina + * + * This file is part of paparazzi. + * + * paparazzi is free software; you can redistribute it and/or modify + * it under the terms of the GNU General Public License as published by + * the Free Software Foundation; either version 2, or (at your option) + * any later version. + * + * paparazzi is distributed in the hope that it will be useful, + * but WITHOUT ANY WARRANTY; without even the implied warranty of + * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the + * GNU General Public License for more details. + * + * You should have received a copy of the GNU General Public License + * along with paparazzi; see the file COPYING. If not, see + * . + */ + +/** + * @file modules/guidance/gvf_parametric/gvf_parametric.cpp + * + * Guiding vector field algorithm for 2D and 3D complex trajectories. + */ + +#include +#include + +#include "gvf_parametric.h" +#include "gvf_parametric_low_level_control.h" +#include "./trajectories/gvf_parametric_3d_ellipse.h" +#include "./trajectories/gvf_parametric_3d_lissajous.h" +#include "./trajectories/gvf_parametric_2d_trefoil.h" + +#ifdef __cplusplus +extern "C" { +#endif + +#include "autopilot.h" + +// Control +uint32_t gvf_parametric_t0 = 0; // We need it for calculting the time lapse delta_T +gvf_parametric_con gvf_parametric_control; + +// Trajectory +gvf_parametric_tra gvf_parametric_trajectory; + +#if PERIODIC_TELEMETRY +#include "subsystems/datalink/telemetry.h" +static void send_gvf_parametric(struct transport_tx *trans, struct link_device *dev) +{ + // Do not know whether is a good idea to do this check here or to include + // this plen in gvf_trajectory + int plen; + int elen; + + switch (gvf_parametric_trajectory.type) { + case TREFOIL_2D: + plen = 7; + elen = 2; + break; + case ELLIPSE_3D: + plen = 6; + elen = 3; + break; + case LISSAJOUS_3D: + plen = 13; + elen = 3; + break; + default: + plen = 1; + elen = 3; + } + + uint8_t traj_type = (uint8_t)gvf_parametric_trajectory.type; + + uint32_t now = get_sys_time_msec(); + uint32_t delta_T = now - gvf_parametric_t0; + + float wb = gvf_parametric_control.w * gvf_parametric_control.beta; + + if (delta_T < 200) { + pprz_msg_send_GVF_PARAMETRIC(trans, dev, AC_ID, &traj_type, &gvf_parametric_control.s, &wb, plen, + gvf_parametric_trajectory.p_parametric, elen, gvf_parametric_trajectory.phi_errors); + } +} + +static void send_circle_parametric(struct transport_tx *trans, struct link_device *dev) +{ + uint32_t now = get_sys_time_msec(); + uint32_t delta_T = now - gvf_parametric_t0; + + if (delta_T < 200) + if (gvf_parametric_trajectory.type == ELLIPSE_3D) { + pprz_msg_send_CIRCLE(trans, dev, AC_ID, &gvf_parametric_trajectory.p_parametric[0], + &gvf_parametric_trajectory.p_parametric[1], &gvf_parametric_trajectory.p_parametric[2]); + } +} + +#endif // PERIODIC TELEMETRY + +#ifdef __cplusplus +} +#endif + +void gvf_parametric_init(void) +{ + gvf_parametric_control.w = 0; + gvf_parametric_control.delta_T = 0; + gvf_parametric_control.s = 1; + gvf_parametric_control.k_roll = GVF_PARAMETRIC_CONTROL_KROLL; + gvf_parametric_control.k_climb = GVF_PARAMETRIC_CONTROL_KCLIMB; + gvf_parametric_control.k_psi = GVF_PARAMETRIC_CONTROL_KPSI; + gvf_parametric_control.L = GVF_PARAMETRIC_CONTROL_L; + gvf_parametric_control.beta = GVF_PARAMETRIC_CONTROL_BETA; + +#if PERIODIC_TELEMETRY + register_periodic_telemetry(DefaultPeriodic, PPRZ_MSG_ID_GVF_PARAMETRIC, send_gvf_parametric); + register_periodic_telemetry(DefaultPeriodic, PPRZ_MSG_ID_CIRCLE, send_circle_parametric); +#endif + +} + +void gvf_parametric_set_direction(int8_t s) +{ + gvf_parametric_control.s = s; +} + +void gvf_parametric_control_2D(float kx, float ky, float f1, float f2, float f1d, float f2d, float f1dd, float f2dd) +{ + + uint32_t now = get_sys_time_msec(); + gvf_parametric_control.delta_T = now - gvf_parametric_t0; + gvf_parametric_t0 = now; + + if (gvf_parametric_control.delta_T > 300) { // We need at least two iterations for Delta_T + gvf_parametric_control.w = 0; // Reset w since we assume the algorithm starts + return; + } + + float L = gvf_parametric_control.L; + float beta = gvf_parametric_control.beta * gvf_parametric_control.s; + + Eigen::Vector3f X; + Eigen::Matrix3f J; + + // Error signals phi_x and phi_y + struct EnuCoor_f *pos_enu = stateGetPositionEnu_f(); + float x = pos_enu->x; + float y = pos_enu->y; + + float phi1 = L * (x - f1); + float phi2 = L * (y - f2); + + gvf_parametric_trajectory.phi_errors[0] = phi1; // Error signals for the telemetry + gvf_parametric_trajectory.phi_errors[1] = phi2; + + // Chi + X(0) = L * beta * f1d - kx * phi1; + X(1) = L * beta * f2d - ky * phi2; + X(2) = L + beta * (kx * phi1 * f1d + ky * phi2 * f2d); + X *= L; + + // Jacobian + J.setZero(); + J(0, 0) = -kx * L; + J(1, 1) = -ky * L; + J(2, 0) = (beta * L) * (beta * f1dd + kx * f1d); + J(2, 1) = (beta * L) * (beta * f2dd + ky * f2d); + J(2, 2) = beta * beta * (kx * (phi1 * f1dd - L * f1d * f1d) + ky * (phi2 * f2dd - L * f2d * f2d)); + J *= L; + + // Guidance algorithm + float ground_speed = stateGetHorizontalSpeedNorm_f(); + float w_dot = (ground_speed * X(2)) / sqrtf(X(0) * X(0) + X(1) * X(1)); + + Eigen::Vector3f xi_dot; + struct EnuCoor_f *vel_enu = stateGetSpeedEnu_f(); + float course = stateGetHorizontalSpeedDir_f(); + + xi_dot << vel_enu->x, vel_enu->y, w_dot; + + Eigen::Matrix3f G; + Eigen::Matrix3f Gp; + Eigen::Matrix Fp; + Eigen::Vector2f h; + Eigen::Matrix ht; + + G << 1, 0, 0, + 0, 1, 0, + 0, 0, 0; + Fp << 0, -1, 0, + 1, 0, 0; + Gp << 0, -1, 0, + 1, 0, 0, + 0, 0, 0; + + h << sinf(course), cosf(course); + ht = h.transpose(); + + Eigen::Matrix Xt = X.transpose(); + Eigen::Vector3f Xh = X / X.norm(); + Eigen::Matrix Xht = Xh.transpose(); + Eigen::Matrix3f I; + I.setIdentity(); + + float aux = ht * Fp * X; + float heading_rate = -1 / (Xt * G * X) * Xt * Gp * (I - Xh * Xht) * J * xi_dot - (gvf_parametric_control.k_psi * aux / + sqrtf(Xt * G * X)); + + // Virtual coordinate update, even if the vehicle is not in autonomous mode, the parameter w will get "closer" to + // the vehicle. So it is not only okei but advisable to update it. + gvf_parametric_control.w += w_dot * gvf_parametric_control.delta_T * 1e-3; + + gvf_parametric_low_level_control_2D(heading_rate); +} + +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) +{ + uint32_t now = get_sys_time_msec(); + gvf_parametric_control.delta_T = now - gvf_parametric_t0; + gvf_parametric_t0 = now; + + if (gvf_parametric_control.delta_T > 300) { // We need at least two iterations for Delta_T + gvf_parametric_control.w = 0; // Reset w since we assume the algorithm starts + return; + } + + float L = gvf_parametric_control.L; + float beta = gvf_parametric_control.beta * gvf_parametric_control.s; + + Eigen::Vector4f X; + Eigen::Matrix4f J; + + // Error signals phi_x phi_y and phi_z + struct EnuCoor_f *pos_enu = stateGetPositionEnu_f(); + float x = pos_enu->x; + float y = pos_enu->y; + float z = pos_enu->z; + + float phi1 = L * (x - f1); + float phi2 = L * (y - f2); + float phi3 = L * (z - f3); + + gvf_parametric_trajectory.phi_errors[0] = phi1 / L; // Error signals in meters for the telemetry + gvf_parametric_trajectory.phi_errors[1] = phi2 / L; + gvf_parametric_trajectory.phi_errors[2] = phi3 / L; + + // Chi + X(0) = -f1d * L * L * beta - kx * phi1; + X(1) = -f2d * L * L * beta - ky * phi2; + X(2) = -f3d * L * L * beta - kz * phi3; + X(3) = -L * L + beta * (kx * phi1 * f1d + ky * phi2 * f2d + kz * phi3 * f3d); + X *= L; + + // Jacobian + J.setZero(); + J(0, 0) = -kx * L; + J(1, 1) = -ky * L; + J(2, 2) = -kz * L; + J(3, 0) = kx * f1d * beta * L; + J(3, 1) = ky * f2d * beta * L; + J(3, 2) = kz * f3d * beta * L; + J(0, 3) = -(beta * L) * (beta * L * f1dd - kx * f1d); + J(1, 3) = -(beta * L) * (beta * L * f2dd - ky * f2d); + J(2, 3) = -(beta * L) * (beta * L * f3dd - kz * f3d); + J(3, 3) = beta * beta * (kx * (phi1 * f1dd - L * f1d * f1d) + ky * (phi2 * f2dd - L * f2d * f2d) + + kz * (phi3 * f3dd - L * f3d * f3d)); + J *= L; + + // Guidance algorithm + float ground_speed = stateGetHorizontalSpeedNorm_f(); + float w_dot = (ground_speed * X(3)) / sqrtf(X(0) * X(0) + X(1) * X(1)); + + Eigen::Vector4f xi_dot; + struct EnuCoor_f *vel_enu = stateGetSpeedEnu_f(); + float course = stateGetHorizontalSpeedDir_f(); + + xi_dot << vel_enu->x, vel_enu->y, vel_enu->z, w_dot; + + Eigen::Matrix2f E; + Eigen::Matrix F; + Eigen::Matrix Fp; + Eigen::Matrix4f G; + Eigen::Matrix4f Gp; + Eigen::Matrix4f I; + Eigen::Vector2f h; + Eigen::Matrix ht; + + h << sinf(course), cosf(course); + ht = h.transpose(); + I.setIdentity(); + F << 1.0, 0.0, 0.0, 0.0, + 0.0, 1.0, 0.0, 0.0; + E << 0.0, -1.0, + 1.0, 0.0; + G = F.transpose() * F; + Fp = E * F; + Gp = F.transpose() * E * F; + + Eigen::Matrix Xt = X.transpose(); + Eigen::Vector4f Xh = X / X.norm(); + Eigen::Matrix Xht = Xh.transpose(); + + float aux = ht * Fp * X; + + float heading_rate = -1 / (Xt * G * X) * Xt * Gp * (I - Xh * Xht) * J * xi_dot - (gvf_parametric_control.k_psi * aux / + sqrtf(Xt * G * X)); + float climbing_rate = (ground_speed * X(2)) / sqrtf(X(0) * X(0) + X(1) * X(1)); + + // Virtual coordinate update, even if the vehicle is not in autonomous mode, the parameter w will get "closer" to + // the vehicle. So it is not only okei but advisable to update it. + gvf_parametric_control.w += w_dot * gvf_parametric_control.delta_T * 1e-3; + + gvf_parametric_low_level_control_3D(heading_rate, climbing_rate); +} + +/** 2D TRAJECTORIES **/ +// 2D TREFOIL KNOT + +bool gvf_parametric_2D_trefoil_XY(float xo, float yo, float w1, float w2, float ratio, float r, float alpha) +{ + gvf_parametric_trajectory.type = TREFOIL_2D; + gvf_parametric_trajectory.p_parametric[0] = xo; + gvf_parametric_trajectory.p_parametric[1] = yo; + gvf_parametric_trajectory.p_parametric[2] = w1; + gvf_parametric_trajectory.p_parametric[3] = w2; + gvf_parametric_trajectory.p_parametric[4] = ratio; + gvf_parametric_trajectory.p_parametric[5] = r; + gvf_parametric_trajectory.p_parametric[6] = alpha; + + float f1, f2, f1d, f2d, f1dd, f2dd; + + gvf_parametric_2d_trefoil_info(&f1, &f2, &f1d, &f2d, &f1dd, &f2dd); + gvf_parametric_control_2D(gvf_parametric_2d_trefoil_par.kx, gvf_parametric_2d_trefoil_par.ky, f1, f2, f1d, f2d, f1dd, + f2dd); + + return true; +} + +bool gvf_parametric_2D_trefoil_wp(uint8_t wp, float w1, float w2, float ratio, float r, float alpha) +{ + gvf_parametric_2D_trefoil_XY(waypoints[wp].x, waypoints[wp].y, w1, w2, ratio, r, alpha); + return true; +} + +/** 3D TRAJECTORIES **/ +// 3D ELLIPSE + +bool gvf_parametric_3D_ellipse_XYZ(float xo, float yo, float r, float zl, float zh, float alpha) +{ + horizontal_mode = HORIZONTAL_MODE_CIRCLE; // Circle for the 2D GCS + + // Safety first! If the asked altitude is low + if (zl > zh) { + zl = zh; + } + if (zl < 1 || zh < 1) { + zl = 10; + zh = 10; + } + if (r < 1) { + r = 60; + } + + gvf_parametric_trajectory.type = ELLIPSE_3D; + gvf_parametric_trajectory.p_parametric[0] = xo; + gvf_parametric_trajectory.p_parametric[1] = yo; + gvf_parametric_trajectory.p_parametric[2] = r; + gvf_parametric_trajectory.p_parametric[3] = zl; + gvf_parametric_trajectory.p_parametric[4] = zh; + gvf_parametric_trajectory.p_parametric[5] = alpha; + + float f1, f2, f3, f1d, f2d, f3d, f1dd, f2dd, f3dd; + + gvf_parametric_3d_ellipse_info(&f1, &f2, &f3, &f1d, &f2d, &f3d, &f1dd, &f2dd, &f3dd); + gvf_parametric_control_3D(gvf_parametric_3d_ellipse_par.kx, gvf_parametric_3d_ellipse_par.ky, + gvf_parametric_3d_ellipse_par.kz, f1, f2, f3, f1d, f2d, f3d, f1dd, f2dd, f3dd); + + return true; +} + +bool gvf_parametric_3D_ellipse_wp(uint8_t wp, float r, float zl, float zh, float alpha) +{ + gvf_parametric_3D_ellipse_XYZ(waypoints[wp].x, waypoints[wp].y, r, zl, zh, alpha); + return true; +} + +bool gvf_parametric_3D_ellipse_wp_delta(uint8_t wp, float r, float alt_center, float delta, float alpha) +{ + float zl = alt_center - delta; + float zh = alt_center + delta; + + gvf_parametric_3D_ellipse_XYZ(waypoints[wp].x, waypoints[wp].y, r, zl, zh, alpha); + return true; +} + +// 3D Lissajous + +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) +{ + // Safety first! If the asked altitude is low + if ((zo - cz) < 1) { + zo = 10; + cz = 0; + } + + gvf_parametric_trajectory.type = LISSAJOUS_3D; + gvf_parametric_trajectory.p_parametric[0] = xo; + gvf_parametric_trajectory.p_parametric[1] = yo; + gvf_parametric_trajectory.p_parametric[2] = zo; + gvf_parametric_trajectory.p_parametric[3] = cx; + gvf_parametric_trajectory.p_parametric[4] = cy; + gvf_parametric_trajectory.p_parametric[5] = cz; + gvf_parametric_trajectory.p_parametric[6] = wx; + gvf_parametric_trajectory.p_parametric[7] = wy; + gvf_parametric_trajectory.p_parametric[8] = wz; + gvf_parametric_trajectory.p_parametric[9] = dx; + gvf_parametric_trajectory.p_parametric[10] = dy; + gvf_parametric_trajectory.p_parametric[11] = dz; + gvf_parametric_trajectory.p_parametric[12] = alpha; + + float f1, f2, f3, f1d, f2d, f3d, f1dd, f2dd, f3dd; + + gvf_parametric_3d_lissajous_info(&f1, &f2, &f3, &f1d, &f2d, &f3d, &f1dd, &f2dd, &f3dd); + gvf_parametric_control_3D(gvf_parametric_3d_lissajous_par.kx, gvf_parametric_3d_lissajous_par.ky, + gvf_parametric_3d_lissajous_par.kz, f1, f2, f3, f1d, f2d, f3d, f1dd, f2dd, f3dd); + + return true; +} + +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) +{ + gvf_parametric_3D_lissajous_XYZ(waypoints[wp].x, waypoints[wp].y, zo, cx, cy, cz, wx, wy, wz, dx, dy, dz, alpha); + return true; +} diff --git a/sw/airborne/modules/guidance/gvf_parametric/gvf_parametric.h b/sw/airborne/modules/guidance/gvf_parametric/gvf_parametric.h new file mode 100644 index 0000000000..7bcb611164 --- /dev/null +++ b/sw/airborne/modules/guidance/gvf_parametric/gvf_parametric.h @@ -0,0 +1,132 @@ +/* + * Copyright (C) 2020 Hector Garcia de Marina + * + * This file is part of paparazzi. + * + * paparazzi is free software; you can redistribute it and/or modify + * it under the terms of the GNU General Public License as published by + * the Free Software Foundation; either version 2, or (at your option) + * any later version. + * + * paparazzi is distributed in the hope that it will be useful, + * but WITHOUT ANY WARRANTY; without even the implied warranty of + * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the + * GNU General Public License for more details. + * + * You should have received a copy of the GNU General Public License + * along with paparazzi; see the file COPYING. If not, see + * . + */ + +/** + * @file modules/guidance/gvf_parametric/gvf_parametric.h + * + * Guiding vector field algorithm for 2D and 3D parametric trajectories. + */ + +#ifndef GVF_PARAMETRIC_H +#define GVF_PARAMETRIC_H + +#define GVF_PARAMETRIC_GRAVITY 9.806 + +/*! Default gain kroll for tuning the "coordinated turn" */ +#ifndef GVF_PARAMETRIC_CONTROL_KROLL +#define GVF_PARAMETRIC_CONTROL_KROLL 1 +#endif + +/*! Default gain kclimb for tuning the climbing setting point */ +#ifndef GVF_PARAMETRIC_CONTROL_KCLIMB +#define GVF_PARAMETRIC_CONTROL_KCLIMB 1 +#endif + +/*! Default scale for the error signals */ +#ifndef GVF_PARAMETRIC_CONTROL_L +#define GVF_PARAMETRIC_CONTROL_L 0.1 +#endif + +/*! Default scale for w */ +#ifndef GVF_PARAMETRIC_CONTROL_BETA +#define GVF_PARAMETRIC_CONTROL_BETA 0.01 +#endif + +/*! Default gain kpsi for tuning the alignment of the vehicle with the vector field */ +#ifndef GVF_PARAMETRIC_CONTROL_KPSI +#define GVF_PARAMETRIC_CONTROL_KPSI 1 +#endif + + +#ifdef __cplusplus +extern "C" { +#endif + +#include "modules/guidance/gvf_parametric/trajectories/gvf_parametric_3d_ellipse.h" +#include "modules/guidance/gvf_parametric/trajectories/gvf_parametric_3d_lissajous.h" +#include "modules/guidance/gvf_parametric/trajectories/gvf_parametric_2d_trefoil.h" + +/** @typedef gvf_parametric_con +* @brief Control parameters for the GVF_PARAMETRIC +* @param w Virtual coordinate from the parametrization of the trajectory +* @param delta_T Time between iterations needed for integrating w +* @param s Defines the direction to be tracked. It takes the values -1 or 1. +* @param k_roll Gain for tuning the coordinated turn. +* @param k_climb Gain for tuning the climbing setting point. +*/ +typedef struct { + float w; + float delta_T; + int8_t s; + float k_roll; + float k_climb; + float k_psi; + float L; + float beta; +} gvf_parametric_con; + +extern gvf_parametric_con gvf_parametric_control; + +// Parameters for the trajectories +enum trajectories_parametric { + TREFOIL_2D = 0, + ELLIPSE_3D = 1, + LISSAJOUS_3D = 2, + NONE_PARAMETRIC = 255, +}; + +typedef struct { + enum trajectories_parametric type; + float p_parametric[16]; + float phi_errors[3]; +} gvf_parametric_tra; + +extern gvf_parametric_tra gvf_parametric_trajectory; + +// Init function +extern void gvf_parametric_init(void); + +// Control functions +extern void gvf_parametric_set_direction(int8_t s); +extern void gvf_parametric_control_2D(float, float, float, float, float, float, float, float); +extern void gvf_parametric_control_3D(float, float, float, float, float, float, float, float, float, + float, float, float); + +// 2D Trefoil +extern bool gvf_parametric_2D_trefoil_XY(float, float, float, float, float, float, float); +extern bool gvf_parametric_2D_trefoil_wp(uint8_t, float, float, float, float, float); + +// 3D Ellipse +extern bool gvf_parametric_3D_ellipse_XYZ(float, float, float, float, float, float); +extern bool gvf_parametric_3D_ellipse_wp(uint8_t, float, float, float, float); +extern bool gvf_parametric_3D_ellipse_wp_delta(uint8_t, float, float, float, float); + +// 3D Lissajous +extern bool gvf_parametric_3D_lissajous_XYZ(float, float, float, float, float, float, float, float, float, float, float, + float, float); +extern bool gvf_parametric_3D_lissajous_wp_center(uint8_t, float, float, float, float, float, float, float, float, + float, float, float); + +#ifdef __cplusplus +} +#endif + + +#endif // GVF_PARAMETRIC_H diff --git a/sw/airborne/modules/guidance/gvf_parametric/gvf_parametric_low_level_control.c b/sw/airborne/modules/guidance/gvf_parametric/gvf_parametric_low_level_control.c new file mode 100644 index 0000000000..ed83bb52d8 --- /dev/null +++ b/sw/airborne/modules/guidance/gvf_parametric/gvf_parametric_low_level_control.c @@ -0,0 +1,78 @@ +/* + * Copyright (C) 2020 Hector Garcia de Marina + * + * This file is part of paparazzi. + * + * paparazzi is free software; you can redistribute it and/or modify + * it under the terms of the GNU General Public License as published by + * the Free Software Foundation; either version 2, or (at your option) + * any later version. + * + * paparazzi is distributed in the hope that it will be useful, + * but WITHOUT ANY WARRANTY; without even the implied warranty of + * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the + * GNU General Public License for more details. + * + * You should have received a copy of the GNU General Public License + * along with paparazzi; see the file COPYING. If not, see + * . + */ + +/** + * @file modules/guidance/gvf_parametric/gvf_parametric_low_level_control.c + * + * Firmware dependent file for the guiding vector field algorithm for 2D and 3D parametric trajectories. + */ + +#include "autopilot.h" +#include "gvf_parametric_low_level_control.h" +#include "gvf_parametric.h" + +#if defined(FIXEDWING_FIRMWARE) +#include "firmwares/fixedwing/stabilization/stabilization_attitude.h" +#include "firmwares/fixedwing/guidance/guidance_v_n.h" // gvf_parametric is only compatible with the new pprz controller! +#endif + +void gvf_parametric_low_level_control_2D(float heading_rate) +{ +#if defined(FIXEDWING_FIRMWARE) + if (autopilot_get_mode() == AP_MODE_AUTO2) { + // Lateral XY coordinates + lateral_mode = LATERAL_MODE_ROLL; + + struct FloatEulers *att = stateGetNedToBodyEulers_f(); + float ground_speed = stateGetHorizontalSpeedNorm_f(); + + h_ctl_roll_setpoint = + -gvf_parametric_control.k_roll * atanf(heading_rate * ground_speed / GVF_PARAMETRIC_GRAVITY / cosf(att->theta)); + BoundAbs(h_ctl_roll_setpoint, h_ctl_roll_max_setpoint); // Setting point for roll angle + } +#else +#error gvf_parametric does not support your firmware yet +#endif +} + +void gvf_parametric_low_level_control_3D(float heading_rate, float climbing_rate) +{ +#if defined(FIXEDWING_FIRMWARE) + if (autopilot_get_mode() == AP_MODE_AUTO2) { + // Vertical Z coordinate + v_ctl_mode = V_CTL_MODE_AUTO_CLIMB; + v_ctl_speed_mode = V_CTL_SPEED_THROTTLE; + + v_ctl_climb_setpoint = gvf_parametric_control.k_climb * climbing_rate; // Setting point for vertical speed + + // Lateral XY coordinates + lateral_mode = LATERAL_MODE_ROLL; + + struct FloatEulers *att = stateGetNedToBodyEulers_f(); + float ground_speed = stateGetHorizontalSpeedNorm_f(); + + h_ctl_roll_setpoint = + -gvf_parametric_control.k_roll * atanf(heading_rate * ground_speed / GVF_PARAMETRIC_GRAVITY / cosf(att->theta)); + BoundAbs(h_ctl_roll_setpoint, h_ctl_roll_max_setpoint); // Setting point for roll angle + } +#else +#error gvf_parametric does not support your firmware yet +#endif +} diff --git a/sw/airborne/modules/guidance/gvf_parametric/gvf_parametric_low_level_control.h b/sw/airborne/modules/guidance/gvf_parametric/gvf_parametric_low_level_control.h new file mode 100644 index 0000000000..dbed98ffe1 --- /dev/null +++ b/sw/airborne/modules/guidance/gvf_parametric/gvf_parametric_low_level_control.h @@ -0,0 +1,44 @@ +/* + * Copyright (C) 2020 Hector Garcia de Marina + * + * This file is part of paparazzi. + * + * paparazzi is free software; you can redistribute it and/or modify + * it under the terms of the GNU General Public License as published by + * the Free Software Foundation; either version 2, or (at your option) + * any later version. + * + * paparazzi is distributed in the hope that it will be useful, + * but WITHOUT ANY WARRANTY; without even the implied warranty of + * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the + * GNU General Public License for more details. + * + * You should have received a copy of the GNU General Public License + * along with paparazzi; see the file COPYING. If not, see + * . + */ + +/** + * @file modules/guidance/gvf_parametric/gvf_parametric_low_level_control.h + * + * Firmware dependent file for the guiding vector field algorithm for 2D and 3D parametric trajectories. + */ + +#ifndef GVF_PARAMETRIC_LOW_LEVEL_CONTROL_H +#define GVF_PARAMETRIC_LOW_LEVEL_CONTROL_H + +#ifdef __cplusplus +extern "C" { +#endif + +// Low level control functions +extern void gvf_parametric_low_level_control_2D(float); +extern void gvf_parametric_low_level_control_3D(float, float); + +#ifdef __cplusplus +} +#endif + + +#endif // GVF_PARAMETRIC_LOW_LEVEL_CONTROL_H + diff --git a/sw/airborne/modules/guidance/gvf_parametric/trajectories/gvf_parametric_2d_trefoil.c b/sw/airborne/modules/guidance/gvf_parametric/trajectories/gvf_parametric_2d_trefoil.c new file mode 100644 index 0000000000..7628231b91 --- /dev/null +++ b/sw/airborne/modules/guidance/gvf_parametric/trajectories/gvf_parametric_2d_trefoil.c @@ -0,0 +1,102 @@ +/* + * Copyright (C) 2020 Hector Garcia de Marina + * + * This file is part of paparazzi. + * + * paparazzi is free software; you can redistribute it and/or modify + * it under the terms of the GNU General Public License as published by + * the Free Software Foundation; either version 2, or (at your option) + * any later version. + * + * paparazzi is distributed in the hope that it will be useful, + * but WITHOUT ANY WARRANTY; without even the implied warranty of + * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the + * GNU General Public License for more details. + * + * You should have received a copy of the GNU General Public License + * along with paparazzi; see the file COPYING. If not, see + * . + */ + +/** + * @file modules/guidance/gvf_parametric/trajectories/gvf_parametric_2d_trefoil.c + * + * Guiding vector field algorithm for 2D and 3D complex trajectories. + * + * 2D trefoil know + */ + +#include "subsystems/navigation/common_nav.h" +#include "modules/guidance/gvf_parametric/gvf_parametric.h" +#include "gvf_parametric_2d_trefoil.h" + +/*! Default gain kx for the 2d trefoil knot trajectory */ +#ifndef GVF_PARAMETRIC_2D_TREFOIL_KX +#define GVF_PARAMETRIC_2D_TREFOIL_KX 0.001 +#endif + +/*! Default gain ky for the 2d trefoil knot trajectory */ +#ifndef GVF_PARAMETRIC_2D_TREFOIL_KY +#define GVF_PARAMETRIC_2D_TREFOIL_KY 0.001 +#endif + +/*! Default 1st frequency for the 2d trefoil trajectory*/ +#ifndef GVF_PARAMETRIC_2D_TREFOIL_W1 +#define GVF_PARAMETRIC_2D_TREFOIL_W1 0.02 +#endif + +/*! Default 2nd frequency for the 2d trefoil trajectory*/ +#ifndef GVF_PARAMETRIC_2D_TREFOIL_W2 +#define GVF_PARAMETRIC_2D_TREFOIL_W2 0.03 +#endif + +/*! Default ratio for the 2d trefoil trajectory*/ +#ifndef GVF_PARAMETRIC_2D_TREFOIL_RATIO +#define GVF_PARAMETRIC_2D_TREFOIL_RATIO 160 +#endif + +/*! Default radius of the circles for the 2d trefoil trajectory*/ +#ifndef GVF_PARAMETRIC_2D_TREFOIL_R +#define GVF_PARAMETRIC_2D_TREFOIL_R 80 +#endif + +/*! Default orientation for the 2d trefoil trajectory*/ +#ifndef GVF_PARAMETRIC_2D_TREFOIL_ALPHA +#define GVF_PARAMETRIC_2D_TREFOIL_ALPHA 0 +#endif + +gvf_par_2d_tre_par gvf_parametric_2d_trefoil_par = {GVF_PARAMETRIC_2D_TREFOIL_KX, GVF_PARAMETRIC_2D_TREFOIL_KY, GVF_PARAMETRIC_2D_TREFOIL_W1, GVF_PARAMETRIC_2D_TREFOIL_W2, GVF_PARAMETRIC_2D_TREFOIL_RATIO, GVF_PARAMETRIC_2D_TREFOIL_R, GVF_PARAMETRIC_2D_TREFOIL_ALPHA}; + +void gvf_parametric_2d_trefoil_info(float *f1, float *f2, float *f1d, float *f2d, float *f1dd, float *f2dd) +{ + float xo = gvf_parametric_trajectory.p_parametric[0]; + float yo = gvf_parametric_trajectory.p_parametric[1]; + float w1 = gvf_parametric_trajectory.p_parametric[2]; + float w2 = gvf_parametric_trajectory.p_parametric[3]; + float ratio = gvf_parametric_trajectory.p_parametric[4]; + float r = gvf_parametric_trajectory.p_parametric[5]; + float alpha_rad = gvf_parametric_trajectory.p_parametric[6]*M_PI/180; + + float w = gvf_parametric_control.w; + float wb = w * gvf_parametric_control.beta * gvf_parametric_control.s; + + // Parametric equations of the trajectory and the partial derivatives w.r.t. 'w' + float nrf1 = cosf(wb*w1)*(r*cosf(wb*w2) + ratio); + float nrf2 = sinf(wb*w1)*(r*cosf(wb*w2) + ratio); + + float nrf1d = -w1*sinf(wb*w1)*(r*cosf(wb*w2) + ratio) - cosf(wb*w1)*r*w2*sinf(wb*w2); + float nrf2d = w1*cosf(wb*w1)*(r*cosf(wb*w2) + ratio) - sinf(wb*w1)*r*w2*sinf(wb*w2); + + float nrf1dd = -w1*w1*cosf(wb*w1)*(r*cosf(wb*w2) + ratio) + w1*sinf(wb*w1)*r*w2*sinf(wb*w2) + w1*sinf(wb*w1)*r*w2*sinf(wb*w2) - cosf(wb*w1)*r*w2*w2*cosf(wb*w2); + float nrf2dd = -w1*w1*sinf(wb*w1)*(r*cosf(wb*w2) + ratio) - w1*cosf(wb*w1)*r*w2*sinf(wb*w2) - w1*cosf(wb*w1)*r*w2*sinf(wb*w2) - sinf(wb*w1)*r*w2*w2*cosf(wb*w2); + + *f1 = cosf(alpha_rad)*nrf1 - sinf(alpha_rad)*nrf2 + xo; + *f2 = sinf(alpha_rad)*nrf1 + cosf(alpha_rad)*nrf2 + yo; + + *f1d = cosf(alpha_rad)*nrf1d - sinf(alpha_rad)*nrf2d; + *f2d = sinf(alpha_rad)*nrf1d + cosf(alpha_rad)*nrf2d; + + *f1dd = cosf(alpha_rad)*nrf1dd - sinf(alpha_rad)*nrf2dd; + *f2dd = sinf(alpha_rad)*nrf1dd + cosf(alpha_rad)*nrf2dd; +} + diff --git a/sw/airborne/modules/guidance/gvf_parametric/trajectories/gvf_parametric_2d_trefoil.h b/sw/airborne/modules/guidance/gvf_parametric/trajectories/gvf_parametric_2d_trefoil.h new file mode 100644 index 0000000000..227576f307 --- /dev/null +++ b/sw/airborne/modules/guidance/gvf_parametric/trajectories/gvf_parametric_2d_trefoil.h @@ -0,0 +1,64 @@ +/* + * Copyright (C) 2020 Hector Garcia de Marina + * + * This file is part of paparazzi. + * + * paparazzi is free software; you can redistribute it and/or modify + * it under the terms of the GNU General Public License as published by + * the Free Software Foundation; either version 2, or (at your option) + * any later version. + * + * paparazzi is distributed in the hope that it will be useful, + * but WITHOUT ANY WARRANTY; without even the implied warranty of + * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the + * GNU General Public License for more details. + * + * You should have received a copy of the GNU General Public License + * along with paparazzi; see the file COPYING. If not, see + * . + */ + +/** + * @file modules/guidance/gvf_parametric/trajectories/gvf_parametric_2d_trefoil.h + * + * Guiding vector field algorithm for 2D and 3D complex trajectories. + * + * 2D trefoil knot + */ + +#ifndef GVF_PARAMETRIC_2D_TREFOIL_H +#define GVF_PARAMETRIC_2D_TREFOIL_H + +#ifdef __cplusplus +extern "C" { +#endif + +/** @typedef gvf_2d_tre_par +* @brief Parameters for the GVF parametric 2D trefoil knot +* @param kx Gain defining how agressive is the vector field in x coordinate +* @param ky Gain defining how agressive is the vector field in y coordinate +* @param w1 1st frequency +* @param w2 2nd frequency +* @param off Off-phase +* @param r Radius of the "circles" +* @param alpha Orientation/rotation of the trajectory in the XY plane +*/ +typedef struct { + float kx; + float ky; + float w1; + float w2; + float ratio; + float r; + float alpha; +} gvf_par_2d_tre_par; + +extern gvf_par_2d_tre_par gvf_parametric_2d_trefoil_par; + +extern void gvf_parametric_2d_trefoil_info(float *f1, float *f2, float *f1d, float *f2d, float *f1dd, float *f2dd); + +#ifdef __cplusplus +} +#endif + +#endif // GVF_PARAMETRIC_2D_TREFOIL_H diff --git a/sw/airborne/modules/guidance/gvf_parametric/trajectories/gvf_parametric_3d_ellipse.c b/sw/airborne/modules/guidance/gvf_parametric/trajectories/gvf_parametric_3d_ellipse.c new file mode 100644 index 0000000000..4f9e079184 --- /dev/null +++ b/sw/airborne/modules/guidance/gvf_parametric/trajectories/gvf_parametric_3d_ellipse.c @@ -0,0 +1,98 @@ +/* + * Copyright (C) 2020 Hector Garcia de Marina + * + * This file is part of paparazzi. + * + * paparazzi is free software; you can redistribute it and/or modify + * it under the terms of the GNU General Public License as published by + * the Free Software Foundation; either version 2, or (at your option) + * any later version. + * + * paparazzi is distributed in the hope that it will be useful, + * but WITHOUT ANY WARRANTY; without even the implied warranty of + * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the + * GNU General Public License for more details. + * + * You should have received a copy of the GNU General Public License + * along with paparazzi; see the file COPYING. If not, see + * . + */ + +/** + * @file modules/guidance/gvf_parametric/trajectories/gvf_parametric_3d_ellipse.c + * + * Guiding vector field algorithm for 2D and 3D complex trajectories. + * + * 3D ellipse (intersection between a cylinder and a tilted plane) + */ + +#include "subsystems/navigation/common_nav.h" +#include "modules/guidance/gvf_parametric/gvf_parametric.h" +#include "gvf_parametric_3d_ellipse.h" + +/*! Default gain kx for the 3d ellipse trajectory */ +#ifndef GVF_PARAMETRIC_3D_ELLIPSE_KX +#define GVF_PARAMETRIC_3D_ELLIPSE_KX 0.001 +#endif + +/*! Default gain ky for the 3d ellipse trajectory */ +#ifndef GVF_PARAMETRIC_3D_ELLIPSE_KY +#define GVF_PARAMETRIC_3D_ELLIPSE_KY 0.001 +#endif + +/*! Default gain kz for the 3d ellipse trajectory */ +#ifndef GVF_PARAMETRIC_3D_ELLIPSE_KZ +#define GVF_PARAMETRIC_3D_ELLIPSE_KZ 0.001 +#endif + +/*! Default radius of the cylinder */ +#ifndef GVF_PARAMETRIC_3D_ELLIPSE_R +#define GVF_PARAMETRIC_3D_ELLIPSE_R 80 +#endif + +/*! Default highest point for the ellipse trajectory */ +#ifndef GVF_PARAMETRIC_3D_ELLIPSE_ZL +#define GVF_PARAMETRIC_3D_ELLIPSE_ZL 40 +#endif + +/*! Default highest point for the ellipse trajectory */ +#ifndef GVF_PARAMETRIC_3D_ELLIPSE_ZH +#define GVF_PARAMETRIC_3D_ELLIPSE_ZH 40 +#endif + +/*! Default orientation in degrees for the lowest point of the ellipse */ +#ifndef GVF_PARAMETRIC_3D_ELLIPSE_ALPHA +#define GVF_PARAMETRIC_3D_ELLIPSE_ALPHA 0 +#endif + +gvf_par_3d_ell_par gvf_parametric_3d_ellipse_par = {GVF_PARAMETRIC_3D_ELLIPSE_KX, + GVF_PARAMETRIC_3D_ELLIPSE_KY, GVF_PARAMETRIC_3D_ELLIPSE_KZ, GVF_PARAMETRIC_3D_ELLIPSE_R, GVF_PARAMETRIC_3D_ELLIPSE_ZL, GVF_PARAMETRIC_3D_ELLIPSE_ZH, GVF_PARAMETRIC_3D_ELLIPSE_ALPHA + }; + +void gvf_parametric_3d_ellipse_info(float *f1, float *f2, float *f3, float *f1d, float *f2d, float *f3d, + float *f1dd, float *f2dd, float *f3dd) +{ + float xo = gvf_parametric_trajectory.p_parametric[0]; + float yo = gvf_parametric_trajectory.p_parametric[1]; + float r = gvf_parametric_trajectory.p_parametric[2]; + float zl = gvf_parametric_trajectory.p_parametric[3]; + float zh = gvf_parametric_trajectory.p_parametric[4]; + float alpha_rad = gvf_parametric_trajectory.p_parametric[5]*M_PI/180; + + float w = gvf_parametric_control.w; + float wb = w * gvf_parametric_control.beta * gvf_parametric_control.s; + + // Parametric equations of the trajectory and the partial derivatives w.r.t. 'w' + *f1 = r * cosf(wb) + xo; + *f2 = r * sinf(wb) + yo; + *f3 = 0.5 * (zh + zl + (zl - zh) * sinf(alpha_rad - wb)); + + *f1d = -r * sinf(wb); + *f2d = r * cosf(wb); + *f3d = -0.5 * (zl - zh) * cosf(alpha_rad - wb); + + *f1dd = -r * cosf(wb); + *f2dd = -r * sinf(wb); + *f3dd = -0.5 * (zl - zh) * sinf(alpha_rad - wb); +} + diff --git a/sw/airborne/modules/guidance/gvf_parametric/trajectories/gvf_parametric_3d_ellipse.h b/sw/airborne/modules/guidance/gvf_parametric/trajectories/gvf_parametric_3d_ellipse.h new file mode 100644 index 0000000000..c7edce3f85 --- /dev/null +++ b/sw/airborne/modules/guidance/gvf_parametric/trajectories/gvf_parametric_3d_ellipse.h @@ -0,0 +1,65 @@ +/* + * Copyright (C) 2020 Hector Garcia de Marina + * + * This file is part of paparazzi. + * + * paparazzi is free software; you can redistribute it and/or modify + * it under the terms of the GNU General Public License as published by + * the Free Software Foundation; either version 2, or (at your option) + * any later version. + * + * paparazzi is distributed in the hope that it will be useful, + * but WITHOUT ANY WARRANTY; without even the implied warranty of + * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the + * GNU General Public License for more details. + * + * You should have received a copy of the GNU General Public License + * along with paparazzi; see the file COPYING. If not, see + * . + */ + +/** + * @file modules/guidance/gvf_parametric/trajectories/gvf_parametric_3d_ellipse.h + * + * Guiding vector field algorithm for 2D and 3D complex trajectories. + * + * 3D ellipse (intersection between a cylinder and a tilted plane) + */ + +#ifndef GVF_PARAMETRIC_3D_ELLIPSE_H +#define GVF_PARAMETRIC_3D_ELLIPSE_H + +#ifdef __cplusplus +extern "C" { +#endif + +/** @typedef gvf_3d_ell_par +* @brief Parameters for the GVF parametric 3D ellipse +* @param kx Gain defining how agressive is the vector field in x coordinate +* @param ky Gain defining how agressive is the vector field in y coordinate +* @param kz Gain defining how agressive is the vector field in z coordinate +* @param r Radius of the cylinder in meters +* @param zl Altitude of the lowest point of the ellipse +* @param zh Altitude of the highest point of the ellipse +* @param alpha Heading of the lowest point zl in rads +*/ +typedef struct { + float kx; + float ky; + float kz; + float r; + float zl; + float zh; + float alpha; +} gvf_par_3d_ell_par; + +extern gvf_par_3d_ell_par gvf_parametric_3d_ellipse_par; + +extern void gvf_parametric_3d_ellipse_info(float *f1, float *f2, float *f3, float *f1d, float *f2d, float *f3d, + float *f1dd, float *f2dd, float *f3dd); + +#ifdef __cplusplus +} +#endif + +#endif // GVF_PARAMETRIC_3D_ELLIPSE_H diff --git a/sw/airborne/modules/guidance/gvf_parametric/trajectories/gvf_parametric_3d_lissajous.c b/sw/airborne/modules/guidance/gvf_parametric/trajectories/gvf_parametric_3d_lissajous.c new file mode 100644 index 0000000000..f51010e15d --- /dev/null +++ b/sw/airborne/modules/guidance/gvf_parametric/trajectories/gvf_parametric_3d_lissajous.c @@ -0,0 +1,143 @@ +/* + * Copyright (C) 2020 Hector Garcia de Marina + * + * This file is part of paparazzi. + * + * paparazzi is free software; you can redistribute it and/or modify + * it under the terms of the GNU General Public License as published by + * the Free Software Foundation; either version 2, or (at your option) + * any later version. + * + * paparazzi is distributed in the hope that it will be useful, + * but WITHOUT ANY WARRANTY; without even the implied warranty of + * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the + * GNU General Public License for more details. + * + * You should have received a copy of the GNU General Public License + * along with paparazzi; see the file COPYING. If not, see + * . + */ + +/** + * @file modules/guidance/gvf_parametric/trajectories/gvf_parametric_3d_lissajous.c + * + * Guiding vector field algorithm for 2D and 3D complex trajectories. + * + * 3D lissajous + */ + +#include "subsystems/navigation/common_nav.h" +#include "modules/guidance/gvf_parametric/gvf_parametric.h" +#include "gvf_parametric_3d_lissajous.h" + +/*! Default gain kx for the 3d lissajous trajectory */ +#ifndef GVF_PARAMETRIC_3D_LISSAJOUS_KX +#define GVF_PARAMETRIC_3D_LISSAJOUS_KX 0.001 +#endif + +/*! Default gain ky for the 3d lissajous trajectory */ +#ifndef GVF_PARAMETRIC_3D_LISSAJOUS_KY +#define GVF_PARAMETRIC_3D_LISSAJOUS_KY 0.001 +#endif + +/*! Default gain kz for the 3d lissajous trajectory */ +#ifndef GVF_PARAMETRIC_3D_LISSAJOUS_KZ +#define GVF_PARAMETRIC_3D_LISSAJOUS_KZ 0.001 +#endif + +/*! Default amplitude of the trajectory in the X plane */ +#ifndef GVF_PARAMETRIC_3D_LISSAJOUS_CX +#define GVF_PARAMETRIC_3D_LISSAJOUS_CX 80 +#endif + +/*! Default amplitude of the trajectory in the Y plane */ +#ifndef GVF_PARAMETRIC_3D_LISSAJOUS_CY +#define GVF_PARAMETRIC_3D_LISSAJOUS_CY 80 +#endif + +/*! Default amplitude of the trajectory in the Z plane */ +#ifndef GVF_PARAMETRIC_3D_LISSAJOUS_CZ +#define GVF_PARAMETRIC_3D_LISSAJOUS_CZ 10 +#endif + +/*! Default frequency of the trajectory in the X plane */ +#ifndef GVF_PARAMETRIC_3D_LISSAJOUS_WX +#define GVF_PARAMETRIC_3D_LISSAJOUS_WX 1 +#endif + +/*! Default frequency of the trajectory in the Y plane */ +#ifndef GVF_PARAMETRIC_3D_LISSAJOUS_WY +#define GVF_PARAMETRIC_3D_LISSAJOUS_WY 1 +#endif + +/*! Default frequency of the trajectory in the Z plane */ +#ifndef GVF_PARAMETRIC_3D_LISSAJOUS_WZ +#define GVF_PARAMETRIC_3D_LISSAJOUS_WZ 1 +#endif + +/*! Default offphase of the trajectory in the X plane */ +#ifndef GVF_PARAMETRIC_3D_LISSAJOUS_DX +#define GVF_PARAMETRIC_3D_LISSAJOUS_DX 0 +#endif + +/*! Default offphase of the trajectory in the Y plane */ +#ifndef GVF_PARAMETRIC_3D_LISSAJOUS_DY +#define GVF_PARAMETRIC_3D_LISSAJOUS_DY 0 +#endif + +/*! Default offphase of the trajectory in the Z plane */ +#ifndef GVF_PARAMETRIC_3D_LISSAJOUS_DZ +#define GVF_PARAMETRIC_3D_LISSAJOUS_DZ 0 +#endif + +/*! Default orientation in degrees for the trajectory in the XY plane */ +#ifndef GVF_PARAMETRIC_3D_LISSAJOUS_ALPHA +#define GVF_PARAMETRIC_3D_LISSAJOUS_ALPHA 0 +#endif + +gvf_par_3d_lis_par gvf_parametric_3d_lissajous_par = {GVF_PARAMETRIC_3D_LISSAJOUS_KX, GVF_PARAMETRIC_3D_LISSAJOUS_KY, GVF_PARAMETRIC_3D_LISSAJOUS_KZ, GVF_PARAMETRIC_3D_LISSAJOUS_CX, GVF_PARAMETRIC_3D_LISSAJOUS_CY, GVF_PARAMETRIC_3D_LISSAJOUS_CZ, GVF_PARAMETRIC_3D_LISSAJOUS_WX, GVF_PARAMETRIC_3D_LISSAJOUS_WY, GVF_PARAMETRIC_3D_LISSAJOUS_WZ, GVF_PARAMETRIC_3D_LISSAJOUS_DX, GVF_PARAMETRIC_3D_LISSAJOUS_DY, GVF_PARAMETRIC_3D_LISSAJOUS_DZ, GVF_PARAMETRIC_3D_LISSAJOUS_ALPHA}; + +void gvf_parametric_3d_lissajous_info(float *f1, float *f2, float *f3, float *f1d, float *f2d, float *f3d, + float *f1dd, float *f2dd, float *f3dd) +{ + float xo = gvf_parametric_trajectory.p_parametric[0]; + float yo = gvf_parametric_trajectory.p_parametric[1]; + float zo = gvf_parametric_trajectory.p_parametric[2]; + float cx = gvf_parametric_trajectory.p_parametric[3]; + float cy = gvf_parametric_trajectory.p_parametric[4]; + float cz = gvf_parametric_trajectory.p_parametric[5]; + float wx = gvf_parametric_trajectory.p_parametric[6]; + float wy = gvf_parametric_trajectory.p_parametric[7]; + float wz = gvf_parametric_trajectory.p_parametric[8]; + float deltax_rad = gvf_parametric_trajectory.p_parametric[9]*M_PI/180; + float deltay_rad = gvf_parametric_trajectory.p_parametric[10]*M_PI/180; + float deltaz_rad = gvf_parametric_trajectory.p_parametric[11]*M_PI/180; + float alpha_rad = gvf_parametric_trajectory.p_parametric[12]*M_PI/180; + + float w = gvf_parametric_control.w; + float wb = w * gvf_parametric_control.beta * gvf_parametric_control.s; + + // Parametric equations of the trajectory and the partial derivatives w.r.t. 'w' + + float nrf1 = cx*cosf(wx*wb + deltax_rad); + float nrf2 = cy*cosf(wy*wb + deltay_rad); + + *f1 = cosf(alpha_rad)*nrf1 - sinf(alpha_rad)*nrf2 + xo; + *f2 = sinf(alpha_rad)*nrf1 + cosf(alpha_rad)*nrf2 + yo; + *f3 = cz*cosf(wz*wb + deltaz_rad) + zo; + + float nrf1d = -wx*cx*sinf(wx*wb + deltax_rad); + float nrf2d = -wy*cy*sinf(wy*wb + deltay_rad); + + *f1d = cosf(alpha_rad)*nrf1d - sinf(alpha_rad)*nrf2d; + *f2d = sinf(alpha_rad)*nrf1d + cosf(alpha_rad)*nrf2d; + *f3d = -wz*cz*sinf(wz*wb + deltaz_rad); + + float nrf1dd = -wx*wx*cx*cosf(wx*wb + deltax_rad); + float nrf2dd = -wy*wy*cy*cosf(wy*wb + deltay_rad); + + *f1dd = cosf(alpha_rad)*nrf1dd - sinf(alpha_rad)*nrf2dd; + *f2dd = sinf(alpha_rad)*nrf1dd + cosf(alpha_rad)*nrf2dd; + *f3dd = -wz*wz*cz*cosf(wz*wb + deltaz_rad); +} + diff --git a/sw/airborne/modules/guidance/gvf_parametric/trajectories/gvf_parametric_3d_lissajous.h b/sw/airborne/modules/guidance/gvf_parametric/trajectories/gvf_parametric_3d_lissajous.h new file mode 100644 index 0000000000..eaaf337633 --- /dev/null +++ b/sw/airborne/modules/guidance/gvf_parametric/trajectories/gvf_parametric_3d_lissajous.h @@ -0,0 +1,77 @@ +/* + * Copyright (C) 2020 Hector Garcia de Marina + * + * This file is part of paparazzi. + * + * paparazzi is free software; you can redistribute it and/or modify + * it under the terms of the GNU General Public License as published by + * the Free Software Foundation; either version 2, or (at your option) + * any later version. + * + * paparazzi is distributed in the hope that it will be useful, + * but WITHOUT ANY WARRANTY; without even the implied warranty of + * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the + * GNU General Public License for more details. + * + * You should have received a copy of the GNU General Public License + * along with paparazzi; see the file COPYING. If not, see + * . + */ + +/** + * @file modules/guidance/gvf_parametric/trajectories/gvf_parametric_3d_lissajous.h + * + * Guiding vector field algorithm for 2D and 3D complex trajectories. + * + * 3D lissajous figures + */ + +#ifndef GVF_PARAMETRIC_3D_LISSAJOUS_H +#define GVF_PARAMETRIC_3D_LISSAJOUS_H + +#ifdef __cplusplus +extern "C" { +#endif + +/** @typedef gvf_3d_lis_par +* @brief Parameters for the GVF parametric 3D lissajous +* @param kx Gain defining how agressive is the vector field in x coordinate +* @param ky Gain defining how agressive is the vector field in y coordinate +* @param kz Gain defining how agressive is the vector field in z coordinate +* @param cx Amplitude of the trajectory in the X plane +* @param cy Amplitude of the trajectory in the Y plane +* @param cz Amplitude of the trajectory in the Z plane +* @param wx Frequency of the trajectory in the X plane +* @param wy Frequency of the trajectory in the Y plane +* @param wz Frequency of the trajectory in the Z plane +* @param dx Offphase of the trajectory in the X plane +* @param dy Offphase of the trajectory in the Y plane +* @param dz Offphase of the trajectory in the Z plane +* @param alpha Orientation/rotation of the trajectory in the XY plane +*/ +typedef struct { + float kx; + float ky; + float kz; + float cx; + float cy; + float cz; + float wx; + float wy; + float wz; + float dx; + float dy; + float dz; + float alpha; +} gvf_par_3d_lis_par; + +extern gvf_par_3d_lis_par gvf_parametric_3d_lissajous_par; + +extern void gvf_parametric_3d_lissajous_info(float *f1, float *f2, float *f3, float *f1d, float *f2d, float *f3d, + float *f1dd, float *f2dd, float *f3dd); + +#ifdef __cplusplus +} +#endif + +#endif // GVF_PARAMETRIC_3D_LISSAJOUS_H diff --git a/sw/ground_segment/python/gvf/gvf_parametric_3d_lissa_sim.py b/sw/ground_segment/python/gvf/gvf_parametric_3d_lissa_sim.py new file mode 100644 index 0000000000..18e59586f0 --- /dev/null +++ b/sw/ground_segment/python/gvf/gvf_parametric_3d_lissa_sim.py @@ -0,0 +1,189 @@ +# Simulator for the gvf_parametric algorithm in Paparazzi (fixed-wing). +# Here you can check the demanded climbing and heading rates for your aircraft +# and the expected trajectories as well. + +import numpy as np +from scipy import linalg as la + +import matplotlib.pyplot as plt +import matplotlib +from mpl_toolkits.mplot3d import Axes3D + +# Happy pdf for a happy submission without complains in paperplaza, arxiv, etc +font = {'size' : 20} + +matplotlib.rc('font', **font) + +matplotlib.rcParams['ps.useafm'] = True +matplotlib.rcParams['pdf.use14corefonts'] = True +matplotlib.rcParams['text.usetex'] = True + +# Simulation parameters +tf = 500 +dt = 5e-2 +time = np.linspace(0, tf, tf/dt) +it = 1 + +# Data log +X_h = np.zeros((time.size, 4)) +theta_h = np.zeros((time.size, 1)) +e1_h = np.zeros((time.size, 1)) +e2_h = np.zeros((time.size, 1)) +e3_h = np.zeros((time.size, 1)) +u_theta_h = np.zeros((time.size, 1)) +u_z_h = np.zeros((time.size, 1)) +u_w_h = np.zeros((time.size, 1)) + +# Initial conditions +X = np.array([[100], [-53], [40], [0]]) +X_dot = np.array([[0], [0], [0], [0]]) +theta = np.pi/4 + +X_h[0, :] = X.transpose() +theta_h[0] = theta + +# Desired trajectory +xo = 0 +yo = 0 +zo = 100 +cx = 150 +cy = 150 +cz = -10 +deltax = 0 +deltay = np.pi/2 +deltaz = 0 +wx = 1 +wy = 1 +wz = 1 + +alpha = np.pi/4 + +# Controller +L = 1e-1 +beta = 1e-2 +k1 = 1e-3 +k2 = 1e-3 +k3 = 1e-3 +ktheta = 0.5 + +# Vehicle +s = 15 + +for t in time[:-1]: + + x = X[0][0] + y = X[1][0] + z = X[2][0] + w = X[3][0] + + wb = w*beta + + #f + nrf1 = cx*np.cos(wx*wb + deltax) + nrf2 = cy*np.cos(wy*wb + deltay) + f3 = cz*np.cos(wz*wb + deltaz) + zo + + nrf1d = -wx*cx*np.sin(wx*wb + deltax) + nrf2d = -wy*cy*np.sin(wy*wb + deltay) + f3d = -wz*cz*np.sin(wz*wb + deltaz) + + nrf1dd = -wx*wx*cx*np.cos(wx*wb + deltax) + nrf2dd = -wy*wy*cy*np.cos(wy*wb + deltay) + f3dd = -wz*wz*cz*np.cos(wz*wb + deltaz) + + f1 = np.cos(alpha)*nrf1 - np.sin(alpha)*nrf2 + xo + f2 = np.sin(alpha)*nrf1 + np.cos(alpha)*nrf2 + yo + + f1d = np.cos(alpha)*nrf1d - np.sin(alpha)*nrf2d + f2d = np.sin(alpha)*nrf1d + np.cos(alpha)*nrf2d + + f1dd = np.cos(alpha)*nrf1dd - np.sin(alpha)*nrf2dd + f2dd = np.sin(alpha)*nrf1dd + np.cos(alpha)*nrf2dd + + #phi + phi1 = L*(x - f1) + phi2 = L*(y - f2) + phi3 = L*(z - f3) + + #Chi, J + Chi = L*np.array([[-f1d*L*L*beta -k1*phi1], + [-f2d*L*L*beta -k2*phi2], + [-f3d*L*L*beta -k3*phi3], + [-L*L + beta*(k1*phi1*f1d + k2*phi2*f2d + k3*phi3*f3d)]]) + + j44 = beta*beta*(k1*(phi1*f1dd-L*f1d*f1d) + k2*(phi2*f2dd-L*f2d*f2d) + k3*(phi3*f3dd-L*f3d*f3d)) + J = L*np.array([[-k1*L, 0, 0, -(beta*L)*(beta*L*f1dd-k1*f1d)], + [ 0, -k2*L, 0, -(beta*L)*(beta*L*f2dd-k2*f2d)], + [ 0, 0, -k3*L, -(beta*L)*(beta*L*f3dd-k3*f3d)], + [beta*L*k1*f1d, beta*L*k2*f2d, beta*L*k3*f3d, j44]]) + + #G, Fp, Gp + G = np.array([[1,0,0,0], + [0,1,0,0], + [0,0,0,0], + [0,0,0,0]]) + + Fp = np.array([[0, -1, 0, 0], + [1, 0, 0, 0]]) + + Gp = np.array([[0, -1, 0, 0], + [1, 0, 0, 0], + [0, 0, 0, 0], + [0, 0, 0, 0]]) + + h = np.array([[np.cos(theta)],[np.sin(theta)]]) + ht = h.transpose() + + Chit = Chi.transpose() + Chinorm = np.sqrt(Chi.transpose().dot(Chi))[0][0] + Chih = Chi / Chinorm + + u_theta = (-(1/(Chit.dot(G).dot(Chi))*Chit.dot(Gp).dot(np.eye(4) - Chih.dot(Chih.transpose())).dot(J).dot(X_dot)) - ktheta*ht.dot(Fp).dot(Chi) / np.sqrt(Chit.dot(G).dot(Chi)))[0][0] + + u_zeta = Chi[2][0]*s / np.sqrt(Chi[0][0]*Chi[0][0] + Chi[1][0]*Chi[1][0]) + u_w = Chi[3][0]*s / np.sqrt(Chi[0][0]*Chi[0][0] + Chi[1][0]*Chi[1][0]) + + # Euler integration + theta = theta + u_theta*dt + X_dot = np.array([[s*np.cos(theta)],[s*np.sin(theta)], [u_zeta], [u_w]]) + X = X + X_dot*dt + + + # Log + X_h[it, :] = X.transpose() + theta_h[it] = theta + e1_h[it] = phi1 + e2_h[it] = phi2 + e3_h[it] = phi3 + u_theta_h[it] = u_theta + u_z_h[it] = u_zeta + u_w_h[it] = u_w + + it = it + 1 + +# Plots + +fig3d = plt.figure() +ax3d = fig3d.gca(projection='3d') +ax3d.plot(X_h[:,0], X_h[:,1], X_h[:,2]) + +f, (axe1,axe2,axe3) = plt.subplots(3,1) +axe1.plot(time[1:], e1_h[1:]) +axe2.plot(time[1:], e2_h[1:]) +axe3.plot(time[1:], e3_h[1:]) +axe3.set_xlabel("Time [s]") +axe1.set_xlabel("error X") +axe2.set_xlabel("error Y") +axe3.set_xlabel("error Z") + +f, (axut,axuz,axuw) = plt.subplots(3,1) +axut.plot(time[1:], u_theta_h[1:]) +axuz.plot(time[1:], u_z_h[1:]) +axuw.plot(time[1:], u_w_h[1:]) +axuw.set_xlabel("Time [s]") +axut.set_ylabel("Heading rate [rad/s]") +axuz.set_ylabel("Climbing rate [m/s]") +axuw.set_ylabel("Virual coord rate") + +plt.show() + diff --git a/sw/ground_segment/python/gvf/gvfframe.py b/sw/ground_segment/python/gvf/gvfframe.py index 4b3bbf66a3..00eb36d625 100644 --- a/sw/ground_segment/python/gvf/gvfframe.py +++ b/sw/ground_segment/python/gvf/gvfframe.py @@ -4,6 +4,7 @@ import time from scipy import linalg as la from matplotlib.path import Path from matplotlib.backends.backend_wxagg import FigureCanvasWxAgg as FigureCanvas +from mpl_toolkits.mplot3d import Axes3D import matplotlib.pyplot as pl import matplotlib.patches as patches import numpy as np @@ -33,12 +34,13 @@ class GVFFrame(wx.Frame): self.course = 0 self.yaw = 0 self.XY = np.array([0, 0]) + self.altitude = 0 + self.ground_altitude = -1 # Desired trajectory - self.timer_traj = 0 # We do not update the traj every time we receive a msg - self.timer_traj_lim = 7 # (7+1) * 0.25secs self.s = 0 self.ke = 0 + self.gvf_error = 0 self.map_gvf = map2d(np.array([0, 0]), 150000) self.traj = None @@ -59,16 +61,18 @@ class GVFFrame(wx.Frame): if int(ac_id) == self.ac_id: if msg.name == 'GPS': self.course = int(msg.get_field(3))*np.pi/1800 + self.altitude = float(msg.get_field(4))/1000 if msg.name == 'NAVIGATION': self.XY[0] = float(msg.get_field(2)) self.XY[1] = float(msg.get_field(3)) + if msg.name == 'NAVIGATION_REF': + self.ground_altitude = float(msg.get_field(3)) if msg.name == 'ATTITUDE': self.yaw = float(msg.get_field(1)) if msg.name == 'GVF': self.gvf_error = float(msg.get_field(0)) # Straight line - if int(msg.get_field(1)) == 0 \ - and self.timer_traj == self.timer_traj_lim: + if int(msg.get_field(1)) == 0: self.s = int(msg.get_field(2)) self.ke = float(msg.get_field(3)) param = [float(x) for x in msg.get_field(4)] @@ -81,8 +85,7 @@ class GVFFrame(wx.Frame): self.s, self.ke) # Ellipse - if int(msg.get_field(1)) == 1 \ - and self.timer_traj == self.timer_traj_lim: + elif int(msg.get_field(1)) == 1: self.s = int(msg.get_field(2)) self.ke = float(msg.get_field(3)) param = [float(x) for x in msg.get_field(4)] @@ -96,8 +99,7 @@ class GVFFrame(wx.Frame): self.map_gvf.area, self.s, self.ke) # Sin - if int(msg.get_field(1)) == 2 \ - and self.timer_traj == self.timer_traj_lim: + elif int(msg.get_field(1)) == 2: self.s = int(msg.get_field(2)) self.ke = float(msg.get_field(3)) param = [float(x) for x in msg.get_field(4)] @@ -112,34 +114,83 @@ class GVFFrame(wx.Frame): self.traj.vector_field(self.traj.XYoff, \ self.map_gvf.area, self.s, self.ke) - self.timer_traj = self.timer_traj + 1 - if self.timer_traj > self.timer_traj_lim: - self.timer_traj = 0 + if msg.name == 'GVF_PARAMETRIC': + # Trefoil 2D + if int(msg.get_field(0)) == 0: + self.s = int(msg.get_field(1)) + self.wb = float(msg.get_field(2)) + param = [float(x) for x in msg.get_field(3)] + xo = param[0] + yo = param[1] + w1 = param[2] + w2 = param[3] + ratio = param[4] + r = param[5] + alpha = param[6] + phi = [float(x) for x in msg.get_field(4)] + phi_x = phi[0] + phi_y = phi[1] + self.traj = traj_param_trefoil_2D(np.array([xo, yo]), w1, w2, ratio, r, alpha, self.wb) - def draw_gvf(self, XY, yaw, course): + # Ellipse 3D + elif int(msg.get_field(0)) == 1: + self.s = int(msg.get_field(1)) + self.wb = float(msg.get_field(2)) + param = [float(x) for x in msg.get_field(3)] + xo = param[0] + yo = param[1] + r = param[2] + zl = param[3] + zh = param[4] + alpha = param[5] + phi = [float(x) for x in msg.get_field(4)] + phi_x = phi[0] + phi_y = phi[1] + phi_z = phi[2] + self.traj = traj_param_ellipse_3D(np.array([xo,yo]), r, zl, zh, alpha, self.wb) + + # Lissajous 3D + elif int(msg.get_field(0)) == 2: + self.s = int(msg.get_field(1)) + self.wb = float(msg.get_field(2)) + param = [float(x) for x in msg.get_field(3)] + xo = param[0] + yo = param[1] + zo = param[2] + cx = param[3] + cy = param[4] + cz = param[5] + wx = param[6] + wy = param[7] + wz = param[8] + dx = param[9] + dy = param[10] + dz = param[11] + alpha = param[12] + phi = [float(x) for x in msg.get_field(4)] + phi_x = phi[0] + phi_y = phi[1] + phi_z = phi[2] + self.traj = traj_param_lissajous_3D(np.array([xo,yo]), zo, cx, cy, cz, \ + wx, wy, wz, dx, dy, dz, alpha, self.wb) + + def draw_gvf(self, XY, yaw, course, altitude): if self.traj is not None: - self.map_gvf.draw(XY, yaw, course, self.traj) + self.map_gvf.draw(XY, yaw, course, altitude, self.traj) def OnClose(self, event): self.interface.shutdown() self.Destroy() def OnRedrawTimer(self, event): - self.draw_gvf(self.XY, self.yaw, self.course) + self.draw_gvf(self.XY, self.yaw, self.course, self.altitude-self.ground_altitude) self.canvas.draw() class map2d: def __init__(self, XYoff, area): self.XYoff = XYoff self.area = area - self.fig, self.ax = pl.subplots() - self.ax.set_xlabel('South [m]') - self.ax.set_ylabel('West [m]') - self.ax.set_title('2D Map') - self.ax.annotate('HOME', xy = (0, 0)) - self.ax.set_xlim(XYoff[0]-0.5*np.sqrt(area), XYoff[0]+0.5*np.sqrt(area)) - self.ax.set_ylim(XYoff[1]-0.5*np.sqrt(area), XYoff[1]+0.5*np.sqrt(area)) - self.ax.axis('equal') + self.fig = pl.figure() def vehicle_patch(self, XY, yaw): Rot = np.array([[np.cos(yaw), np.sin(yaw)],[-np.sin(yaw), np.cos(yaw)]]) @@ -167,42 +218,113 @@ class map2d: return patches.PathPatch(path, facecolor='red', lw=2) - def draw(self, XY, yaw, course, traj): - self.ax.clear() - self.ax.plot(traj.traj_points[0, :], traj.traj_points[1, :]) - self.ax.quiver(traj.mapgrad_X, traj.mapgrad_Y, \ - traj.mapgrad_U, traj.mapgrad_V, color='Teal', \ - pivot='mid', width=0.002) - self.ax.add_patch(self.vehicle_patch(XY, yaw)) # In radians - apex = 45*np.pi/180 # 30 degrees apex angle - b = np.sqrt(2*(self.area/2000) / np.sin(apex)) - h = b*np.cos(apex/2) - self.ax.arrow(XY[0], XY[1], \ - h*np.sin(course), h*np.cos(course),\ - head_width=5, head_length=10, fc='k', ec='k') - self.ax.annotate('HOME', xy = (0, 0)) - if isinstance(traj, traj_ellipse): - self.ax.annotate('ELLIPSE', xy = (traj.XYoff[0], traj.XYoff[1])) - self.ax.plot(0, 0, 'kx', ms=10, mew=2) - self.ax.plot(traj.XYoff[0], traj.XYoff[1], 'kx', ms=10, mew=2) - elif isinstance(traj, traj_sin): - self.ax.annotate('SIN', xy = (traj.XYoff[0], traj.XYoff[1])) - self.ax.plot(0, 0, 'kx', ms=10, mew=2) - self.ax.plot(traj.XYoff[0], traj.XYoff[1], 'kx', ms=10, mew=2) - elif isinstance(traj, traj_line): - self.ax.annotate('LINE', xy = (traj.XYoff[0], traj.XYoff[1])) - self.ax.plot(0, 0, 'kx', ms=10, mew=2) - self.ax.plot(traj.XYoff[0], traj.XYoff[1], 'kx', ms=10, mew=2) + def draw(self, XY, yaw, course, altitude, traj): + self.fig.clf() + if traj.dim == 2: + ax = self.fig.add_subplot(111) + ax.plot(traj.traj_points[0, :], traj.traj_points[1, :]) + ax.quiver(traj.mapgrad_X, traj.mapgrad_Y, \ + traj.mapgrad_U, traj.mapgrad_V, color='Teal', \ + pivot='mid', width=0.002) + ax.add_patch(self.vehicle_patch(XY, yaw)) # In radians + apex = 45*np.pi/180 # 30 degrees apex angle + b = np.sqrt(2*(self.area/2000) / np.sin(apex)) + h = b*np.cos(apex/2) + ax.arrow(XY[0], XY[1], \ + h*np.sin(course), h*np.cos(course),\ + head_width=5, head_length=10, fc='k', ec='k') + ax.annotate('HOME', xy = (0, 0)) + if isinstance(traj, traj_ellipse): + ax.annotate('ELLIPSE', xy = (traj.XYoff[0], traj.XYoff[1])) + ax.plot(0, 0, 'kx', ms=10, mew=2) + ax.plot(traj.XYoff[0], traj.XYoff[1], 'kx', ms=10, mew=2) + elif isinstance(traj, traj_sin): + ax.annotate('SIN', xy = (traj.XYoff[0], traj.XYoff[1])) + ax.plot(0, 0, 'kx', ms=10, mew=2) + ax.plot(traj.XYoff[0], traj.XYoff[1], 'kx', ms=10, mew=2) + elif isinstance(traj, traj_line): + ax.annotate('LINE', xy = (traj.XYoff[0], traj.XYoff[1])) + ax.plot(0, 0, 'kx', ms=10, mew=2) + ax.plot(traj.XYoff[0], traj.XYoff[1], 'kx', ms=10, mew=2) + elif isinstance(traj, traj_param_trefoil_2D): + ax.annotate('TREFOIL', xy = (traj.XYoff[0], traj.XYoff[1])) + ax.plot(0, 0, 'kx', ms=10, mew=2) + ax.plot(traj.XYoff[0], traj.XYoff[1], 'kx', ms=10, mew=2) + ax.plot(traj.wpoint[0], traj.wpoint[1], 'rx', ms=10, mew=2) - self.ax.set_xlabel('South [m]') - self.ax.set_ylabel('West [m]') - self.ax.set_title('2D Map') - self.ax.set_xlim(self.XYoff[0]-0.5*np.sqrt(self.area), \ - self.XYoff[0]+0.5*np.sqrt(self.area)) - self.ax.set_ylim(self.XYoff[1]-0.5*np.sqrt(self.area), \ - self.XYoff[1]+0.5*np.sqrt(self.area)) - self.ax.axis('equal') - self.ax.grid() + ax.set_xlabel('South [m]') + ax.set_ylabel('West [m]') + ax.set_title('2D Map') + ax.set_xlim(self.XYoff[0]-0.5*np.sqrt(self.area), \ + self.XYoff[0]+0.5*np.sqrt(self.area)) + ax.set_ylim(self.XYoff[1]-0.5*np.sqrt(self.area), \ + self.XYoff[1]+0.5*np.sqrt(self.area)) + ax.axis('equal') + ax.grid() + + if traj.dim == 3: + a3d = self.fig.add_subplot(2,2,1, projection='3d') + axy = self.fig.add_subplot(2,2,2) + axz = self.fig.add_subplot(2,2,3) + ayz = self.fig.add_subplot(2,2,4) + + a3d.set_title('3D Map') + axy.set_title('XY Map') + axz.set_title('XZ Map') + ayz.set_title('YZ Map') + + # 3D + a3d.plot(traj.traj_points[0, :], traj.traj_points[1, :], traj.traj_points[2, :]) + if altitude != -1: + a3d.plot([XY[0]], [XY[1]], [altitude], marker='o', markerfacecolor='r', markeredgecolor='r') + a3d.plot([traj.wpoint[0]], [traj.wpoint[1]], [traj.wpoint[2]], marker='x', markerfacecolor='r', markeredgecolor='r') + + a3d.axis('equal') + if traj.deltaz < 0: + a3d.set_zlim(traj.zo+1.5*traj.deltaz, traj.zo-1.5*traj.deltaz) + else: + a3d.set_zlim(traj.zo-1.5*traj.deltaz, traj.zo+1.5*traj.deltaz) + + # XY + axy.plot(traj.traj_points[0, :], traj.traj_points[1, :]) + axy.add_patch(self.vehicle_patch(XY, yaw)) # In radians + apex = 45*np.pi/180 # 30 degrees apex angle + b = np.sqrt(2*(self.area/2000) / np.sin(apex)) + h = b*np.cos(apex/2) + axy.arrow(XY[0], XY[1], \ + h*np.sin(course), h*np.cos(course),\ + head_width=5, head_length=10, fc='k', ec='k') + axy.annotate('HOME', xy = (0, 0)) + axy.plot(traj.wpoint[0], traj.wpoint[1], 'rx', ms=10, mew=2) + if isinstance(traj, traj_param_ellipse_3D): + axy.annotate('ELLIPSE_3D', xy = (traj.XYoff[0], traj.XYoff[1])) + axy.plot(0, 0, 'kx', ms=10, mew=2) + axy.plot(traj.XYoff[0], traj.XYoff[1], 'kx', ms=10, mew=2) + elif isinstance(traj, traj_param_lissajous_3D): + axy.annotate('LISSA_3D', xy = (traj.XYoff[0], traj.XYoff[1])) + axy.plot(0, 0, 'kx', ms=10, mew=2) + axy.plot(traj.XYoff[0], traj.XYoff[1], 'kx', ms=10, mew=2) + + axy.axis('equal') + + # XZ + axz.plot(traj.traj_points[0, :], traj.traj_points[2, :]) + if altitude != -1: + axz.plot([XY[0]], [altitude], 'ro') + axz.plot(traj.wpoint[0], traj.wpoint[2], 'rx', ms=10, mew=2) + if traj.deltaz < 0: + axz.set_ylim(traj.zo+1.5*traj.deltaz, traj.zo-1.5*traj.deltaz) + else: + axz.set_ylim(traj.zo-1.5*traj.deltaz, traj.zo+1.5*traj.deltaz) + # YZ + ayz.plot(traj.traj_points[1, :], traj.traj_points[2, :]) + if altitude != -1: + ayz.plot([XY[1]], [altitude], 'ro') + ayz.plot(traj.wpoint[1], traj.wpoint[2], 'rx', ms=10, mew=2) + if traj.deltaz < 0: + axz.set_ylim(traj.zo+1.5*traj.deltaz, traj.zo-1.5*traj.deltaz) + else: + ayz.set_ylim(traj.zo-1.5*traj.deltaz, traj.zo+1.5*traj.deltaz) class traj_line: def float_range(self, start, end, step): @@ -211,6 +333,7 @@ class traj_line: start += step def __init__(self, Xminmax, a, b, alpha): + self.dim = 2 self.XYoff = np.array([a, b]) self.Xminmax = Xminmax self.a, self.b, self.alpha = a, b, alpha @@ -265,6 +388,7 @@ class traj_ellipse: start += step def __init__(self, XYoff, rot, a, b): + self.dim = 2 self.XYoff = XYoff self.a, self.b = a, b self.rot = rot @@ -324,6 +448,7 @@ class traj_sin: start += step def __init__(self, Xminmax, a, b, alpha, w, off, A): + self.dim = 2 self.XYoff = np.array([a, b]) self.Xminmax = Xminmax self.a, self.b, self.alpha, self.w, self.off, self.A = \ @@ -382,3 +507,125 @@ class traj_sin: self.mapgrad_U = self.mapgrad_U/norm self.mapgrad_V = self.mapgrad_V/norm + +class traj_param_trefoil_2D: + def float_range(self, start, end, step): + while start <= end: + yield start + start += step + + def __init__(self, XYoff, w1, w2, ratio, r, alpha, wb): + self.dim = 2 + self.XYoff, self.w1, self.w2, self.ratio, self.r, self.alpha, self.wb = XYoff, w1, w2, ratio, r, alpha, wb + self.mapgrad_X = [] + self.mapgrad_Y = [] + self.mapgrad_U = [] + self.mapgrad_V = [] + + self.alpha = alpha*np.pi/180 + + self.wpoint = self.param_point(self.wb) + + num_points = 1000 + self.traj_points = np.zeros((2, num_points)) + + i = 0 + range_points = 1000.0 + for t in self.float_range(0, range_points, range_points/num_points): + self.traj_points[:, i] = self.param_point(t) + i = i + 1 + if i >= num_points: + break + + def param_point(self, t): + xnr = np.cos(t*self.w1)*(self.r*np.cos(t*self.w2) + self.ratio) + ynr = np.sin(t*self.w1)*(self.r*np.cos(t*self.w2) + self.ratio) + + x = np.cos(self.alpha)*xnr - np.sin(self.alpha)*ynr + y = np.sin(self.alpha)*xnr + np.cos(self.alpha)*ynr + + return self.XYoff + np.array([x,y]) + +class traj_param_ellipse_3D: + def float_range(self, start, end, step): + while start <= end: + yield start + start += step + + def __init__(self, XYoff, r, zl, zh, alpha, wb): + self.dim = 3 + self.XYoff, self.r, self.zl, self.zh, self.alpha, self.wb = XYoff, r, zl, zh, alpha, wb + self.mapgrad_X = [] + self.mapgrad_Y = [] + self.mapgrad_U = [] + self.mapgrad_V = [] + + self.deltaz = self.zh - self.zl # For the 3D plot + self.zo = self.zl + self.deltaz # For the 3D plot + self.alpha = self.alpha*np.pi/180 + + self.wpoint = self.param_point(self.wb) + + num_points = 100 + self.traj_points = np.zeros((3, num_points)) + + i = 0 + range_points = 2*np.pi + 0.1 + for t in self.float_range(0, range_points, range_points/num_points): + self.traj_points[:, i] = self.param_point(t) + i = i + 1 + if i >= num_points: + break + + def param_point(self, t): + x = self.r * np.cos(t) + self.XYoff[0] + y = self.r * np.sin(t) + self.XYoff[1] + z = 0.5 * (self.zh + self.zl + (self.zl - self.zh) * np.sin(self.alpha - t)) + + return np.array([x,y,z]) + +class traj_param_lissajous_3D: + def float_range(self, start, end, step): + while start <= end: + yield start + start += step + + def __init__(self, XYoff, zo, cx, cy, cz, wx, wy, wz, dx, dy, dz, alpha, wb): + self.dim = 3 + self.XYoff, self.zo, self.cx, self.cy, self.cz, self.wx, self.wy, self.wz, self.dx, self.dy, self.dz, \ + self.alpha, self.wb = XYoff, zo, cx, cy, cz, wx, wy, wz, dx, dy, dz, alpha, wb + self.mapgrad_X = [] + self.mapgrad_Y = [] + self.mapgrad_U = [] + self.mapgrad_V = [] + + self.deltaz = self.cz # For the 3D plot + self.alpha = self.alpha*np.pi/180 + self.dx = self.dx*np.pi/180 + self.dy = self.dy*np.pi/180 + self.dz = self.dz*np.pi/180 + + self.wpoint = self.param_point(self.wb) + + smallest_w = min([self.wx, self.wy, self.wz]) + + num_points = 100 + self.traj_points = np.zeros((3, num_points)) + + i = 0 + range_points = 3*np.pi / smallest_w + for t in self.float_range(0, range_points, range_points/num_points): + self.traj_points[:, i] = self.param_point(t) + i = i + 1 + if i >= num_points: + break + + def param_point(self, t): + xnr = self.cx*np.cos(self.wx*t + self.dx) + ynr = self.cy*np.cos(self.wy*t + self.dy) + z = self.cz*np.cos(self.wz*t + self.dz) + self.zo + + x = np.cos(self.alpha)*xnr - np.sin(self.alpha)*ynr + self.XYoff[0] + y = np.sin(self.alpha)*xnr + np.cos(self.alpha)*ynr + self.XYoff[1] + + return np.array([x,y,z])