From 95e60fe50dd394e351c4570e1cc14dd1ecad27fb Mon Sep 17 00:00:00 2001 From: Ewoud Smeur Date: Fri, 1 Dec 2017 17:48:22 +0100 Subject: [PATCH] Cyfoam, ctrl effectiveness scheduling, JSBsim (#2145) * Cyfoam, ctrl effectiveness scheduling, JSBsim - Added Cyfoam airframe file - Added control effectiveness scheduling function - More advanced scheduling function should be added in the future - Added simple JSBsim files for Cyclone for a crude simulation * only calculate pseudo inverse when using it --- conf/airframes/ENAC/conf_enac.xml | 11 + conf/airframes/ENAC/cyfoam.xml | 305 ++++++++++++++ .../modules/ctrl_effectiveness_scheduling.xml | 22 + .../aircraft/Systems/aerodynamics_cyfoam.xml | 94 +++++ conf/simulator/jsbsim/aircraft/cyclone.xml | 384 ++++++++++++++++++ .../stabilization/stabilization_indi.c | 8 +- .../stabilization/stabilization_indi.h | 8 + .../ctrl/ctrl_effectiveness_scheduling.c | 79 ++++ .../ctrl/ctrl_effectiveness_scheduling.h | 43 ++ 9 files changed, 948 insertions(+), 6 deletions(-) create mode 100644 conf/airframes/ENAC/cyfoam.xml create mode 100644 conf/modules/ctrl_effectiveness_scheduling.xml create mode 100644 conf/simulator/jsbsim/aircraft/Systems/aerodynamics_cyfoam.xml create mode 100644 conf/simulator/jsbsim/aircraft/cyclone.xml create mode 100644 sw/airborne/modules/ctrl/ctrl_effectiveness_scheduling.c create mode 100644 sw/airborne/modules/ctrl/ctrl_effectiveness_scheduling.h diff --git a/conf/airframes/ENAC/conf_enac.xml b/conf/airframes/ENAC/conf_enac.xml index 9e50fb65c4..d63ab66b25 100644 --- a/conf/airframes/ENAC/conf_enac.xml +++ b/conf/airframes/ENAC/conf_enac.xml @@ -65,6 +65,17 @@ settings_modules="modules/rotorcraft_cam.xml modules/switch_servo.xml modules/ahrs_int_cmpl_quat.xml modules/stabilization_int_euler.xml modules/nav_basic_rotorcraft.xml modules/guidance_rotorcraft.xml modules/gps.xml modules/imu_common.xml" gui_color="#3cf2d51335a7" /> + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + +
+ + + + + + + + + + + + +
+ +
+ + +
+ +
+ + + + + +
+ +
+ + + + + + + +
+ +
+ + + + + + + + + + + + + + + + + + + + + + + + + + + + + + +
+ + +
+ + + + + +
+ +
+ + +
+ + + + + + + + +
+ + + +
+ + +
+ + + +
+ +
+ + + + + + + + + + + + + + + + +
+ +
+ + + + +
+ +
+ + + + +
+ + diff --git a/conf/modules/ctrl_effectiveness_scheduling.xml b/conf/modules/ctrl_effectiveness_scheduling.xml new file mode 100644 index 0000000000..98724f4e83 --- /dev/null +++ b/conf/modules/ctrl_effectiveness_scheduling.xml @@ -0,0 +1,22 @@ + + + + + + Interpolation of control effectivenss matrix. + This is necessary if the vehicle has different operating points, + with significantly different control effectivenss. + + If instead using online adaptation is an option, be sure to + not use this module at the same time! + + +
+ +
+ + + + + + diff --git a/conf/simulator/jsbsim/aircraft/Systems/aerodynamics_cyfoam.xml b/conf/simulator/jsbsim/aircraft/Systems/aerodynamics_cyfoam.xml new file mode 100644 index 0000000000..2cb33b886a --- /dev/null +++ b/conf/simulator/jsbsim/aircraft/Systems/aerodynamics_cyfoam.xml @@ -0,0 +1,94 @@ + + + + + + Lift due to alpha + + aero/qbar-psf + metrics/Sw-sqft + + aero/alpha-rad + + -1.87 -0.35 + -1.57 0.0 + -1.27 0.35 + -0.80 0.25 + 0.00 0.0 + 0.80 0.2 + 1.27 0.2 + 1.57 0.0 + 1.87 -0.2 + +
+ + + + + + + + + Drag at zero lift + + aero/qbar-psf + metrics/Sw-sqft + + aero/alpha-rad + + -1.57 0.4 + -1.27 0.6 + 0.00 1.500 + 1.27 0.6 + 1.57 0.4 + +
+ + + + + Induced drag + + aero/qbar-psf + metrics/Sw-sqft + aero/cl-squared + 0.8 + + + + + + + + + + + + + + + + + + + + + + + + + + + + Side force due to beta + + aero/qbar-psf + metrics/Sw-sqft + aero/beta-rad + 0.02 + + + + + + diff --git a/conf/simulator/jsbsim/aircraft/cyclone.xml b/conf/simulator/jsbsim/aircraft/cyclone.xml new file mode 100644 index 0000000000..088d501318 --- /dev/null +++ b/conf/simulator/jsbsim/aircraft/cyclone.xml @@ -0,0 +1,384 @@ + + + + + + Ewoud Smeur + 07-03-2017 + Version 0.9 - beta + Cyclone + + + + 115 + 35.4 + 7.09 + 0 + 0 + 0 + 0 + 90 + + 0 + 0 + 0 + + + 0 + 0 + 0 + + + 0 + 0 + 0 + + + + + 0.0132 + 0.00250 + 0.0150 + 0. + 0. + 0. + 2.4 + + 0 + 0 + 0 + + + + + + + -0.15 + 0 + -0.1 + + 0.8 + 0.5 + 500 + 100 + 1000 + 0.0 + NONE + 0 + + + + + 0.15 + 0 + -0.1 + + 0.8 + 0.5 + 500 + 100 + 1000 + 0.0 + NONE + 0 + + + + + 0. + 0.15 + -0.1 + + 0.8 + 0.5 + 500 + 100 + 1000 + 0.0 + NONE + 0 + + + + + 0. + -0.15 + -0.1 + + 0.8 + 0.5 + 500 + 100 + 1000 + 0.0 + NONE + 0 + + + + + + + + + fcs/ele_left + 54 + fcs/ele_left_lag + + + fcs/ele_right + 54 + fcs/ele_right_lag + + + fcs/mot_right + 31.7 + fcs/mot_right_lag + + + fcs/mot_left + 31.7 + fcs/mot_left_lag + + + + + + + fcs/ele_left + fcs/ele_left_lag + fcs/ele_right + fcs/ele_right_lag + fcs/mot_right + fcs/mot_right_lag + fcs/mot_left + fcs/mot_left_lag + + + + + + + + fcs/mot_right_lag + 2.2 + + + + 0 + 9.4 + 0 + + + 0 + 0 + -1 + + + + + + + fcs/mot_left_lag + 2.2 + + + + 0 + -9.4 + 0 + + + 0 + 0 + -1 + + + + + + + + + + + fcs/ele_left_lag + -0.5 + + + + + 0 + 0 + 1 + + + -1 + 0 + 0 + + + + + + + fcs/ele_left_lag + -0.5 + + + + 0 + 0 + 0 + + + 1 + 0 + 0 + + + + + + + fcs/ele_right_lag + 0.5 + + + + + 0 + 0 + 1 + + + -1 + 0 + 0 + + + + + + + fcs/ele_right_lag + 0.5 + + + + 0 + 0 + 0 + + + 1 + 0 + 0 + + + + + + + + + fcs/ele_left_lag + 4.2 + + + + + 0 + 1 + 0 + + + 1 + 0 + 0 + + + + + + + fcs/ele_left_lag + 4.2 + + + + 0 + 0 + 0 + + + -1 + 0 + 0 + + + + + + + fcs/ele_right_lag + 4.2 + + + + + 0 + 1 + 0 + + + 1 + 0 + 0 + + + + + + + fcs/ele_right_lag + 4.2 + + + + 0 + 0 + 0 + + + -1 + 0 + 0 + + + + + + + + + + + + + + + + + + + + + + + + diff --git a/sw/airborne/firmwares/rotorcraft/stabilization/stabilization_indi.c b/sw/airborne/firmwares/rotorcraft/stabilization/stabilization_indi.c index 0a5a20e1c4..f42351fa60 100644 --- a/sw/airborne/firmwares/rotorcraft/stabilization/stabilization_indi.c +++ b/sw/airborne/firmwares/rotorcraft/stabilization/stabilization_indi.c @@ -45,14 +45,8 @@ #include "wls/wls_alloc.h" #include -//only 4 actuators supported for now -#define INDI_NUM_ACT 4 -// outputs: roll, pitch, yaw, thrust -#define INDI_OUTPUTS 4 // Factor that the estimated G matrix is allowed to deviate from initial one #define INDI_ALLOWED_G_FACTOR 2.0 -// Scaling for the control effectiveness to make it readible -#define INDI_G_SCALING 1000.0 float du_min[INDI_NUM_ACT]; float du_max[INDI_NUM_ACT]; @@ -660,8 +654,10 @@ void lms_estimation(void) float_vect_copy(g1[0], g1_est[0], INDI_OUTPUTS * INDI_NUM_ACT); float_vect_copy(g2, g2_est, INDI_NUM_ACT); +#if STABILIZATION_INDI_ALLOCATION_PSEUDO_INVERSE // Calculate the inverse of (G1+G2) calc_g1g2_pseudo_inv(); +#endif } /** diff --git a/sw/airborne/firmwares/rotorcraft/stabilization/stabilization_indi.h b/sw/airborne/firmwares/rotorcraft/stabilization/stabilization_indi.h index 3267ef5947..ee9049d9a6 100644 --- a/sw/airborne/firmwares/rotorcraft/stabilization/stabilization_indi.h +++ b/sw/airborne/firmwares/rotorcraft/stabilization/stabilization_indi.h @@ -26,8 +26,16 @@ #include "firmwares/rotorcraft/stabilization/stabilization_attitude_common_int.h" #include "firmwares/rotorcraft/stabilization/stabilization_attitude_ref_quat_int.h" +//only 4 actuators supported for now +#define INDI_NUM_ACT 4 +// outputs: roll, pitch, yaw, thrust +#define INDI_OUTPUTS 4 +// Scaling for the control effectiveness to make it readible +#define INDI_G_SCALING 1000.0 + extern struct Int32Quat stab_att_sp_quat; ///< with #INT32_QUAT_FRAC extern struct Int32Eulers stab_att_sp_euler; ///< with #INT32_ANGLE_FRAC +extern float g1g2[INDI_OUTPUTS][INDI_NUM_ACT]; extern bool indi_use_adaptive; diff --git a/sw/airborne/modules/ctrl/ctrl_effectiveness_scheduling.c b/sw/airborne/modules/ctrl/ctrl_effectiveness_scheduling.c new file mode 100644 index 0000000000..93d0b2bbdb --- /dev/null +++ b/sw/airborne/modules/ctrl/ctrl_effectiveness_scheduling.c @@ -0,0 +1,79 @@ +/* + * Copyright (C) 2017 Ewoud Smeur + * + * 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, write to + * the Free Software Foundation, 59 Temple Place - Suite 330, + * Boston, MA 02111-1307, USA. + */ + +/** @file modules/ctrl/ctrl_effectiveness_scheduling.c + * Module that interpolates gainsets in flight based on the transition percentage + */ + +#include "modules/ctrl/ctrl_effectiveness_scheduling.h" +#include "firmwares/rotorcraft/stabilization/stabilization_indi.h" +#include "firmwares/rotorcraft/guidance/guidance_h.h" +#include "generated/airframe.h" +#include "state.h" +#include "subsystems/radio_control.h" + +#if STABILIZATION_INDI_ALLOCATION_PSEUDO_INVERSE +#error "You need to use WLS control allocation for this module" +#endif + + +static float g1g2_forward[INDI_OUTPUTS][INDI_NUM_ACT] = {FWD_G1_ROLL, + FWD_G1_PITCH, FWD_G1_YAW, FWD_G1_THRUST + }; + +static float g1g2_hover[INDI_OUTPUTS][INDI_NUM_ACT] = {STABILIZATION_INDI_G1_ROLL, + STABILIZATION_INDI_G1_PITCH, STABILIZATION_INDI_G1_YAW, STABILIZATION_INDI_G1_THRUST + }; + +static float g2_both[INDI_NUM_ACT] = STABILIZATION_INDI_G2; //scaled by INDI_G_SCALING + +//Get the specified gains in the gainlibrary +void ctrl_eff_scheduling_init(void) +{ + //sum of G1 and G2 + int8_t i; + int8_t j; + for (i = 0; i < INDI_OUTPUTS; i++) { + for (j = 0; j < INDI_NUM_ACT; j++) { + if (i != 2) { + g1g2_hover[i][j] = g1g2_hover[i][j] / INDI_G_SCALING; + g1g2_forward[i][j] = g1g2_forward[i][j] / INDI_G_SCALING; + } else { + g1g2_forward[i][j] = (g1g2_forward[i][j] + g2_both[j]) / INDI_G_SCALING; + g1g2_hover[i][j] = (g1g2_hover[i][j] + g2_both[j]) / INDI_G_SCALING; + } + } + } +} + +void ctrl_eff_scheduling_periodic(void) +{ + // Go from transition percentage to ratio + float ratio = FLOAT_OF_BFP(transition_percentage, INT32_PERCENTAGE_FRAC) / 100; + + int8_t i; + int8_t j; + for (i = 0; i < INDI_OUTPUTS; i++) { + for (j = 0; j < INDI_NUM_ACT; j++) { + g1g2[i][j] = (g1g2_hover[i][j] * (1.0 - ratio) + g1g2_forward[i][j] * ratio); + } + } +} diff --git a/sw/airborne/modules/ctrl/ctrl_effectiveness_scheduling.h b/sw/airborne/modules/ctrl/ctrl_effectiveness_scheduling.h new file mode 100644 index 0000000000..cda0a471ed --- /dev/null +++ b/sw/airborne/modules/ctrl/ctrl_effectiveness_scheduling.h @@ -0,0 +1,43 @@ +/* + * Copyright (C) 2017 Ewoud Smeur + * + * 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, write to + * the Free Software Foundation, 59 Temple Place - Suite 330, + * Boston, MA 02111-1307, USA. + */ + +/** + * @file modules/ctrl/ctrl_effectiveness_scheduling.h + */ + +#ifndef CTRL_EFFECTIVENESS_SCHEDULING_H +#define CTRL_EFFECTIVENESS_SCHEDULING_H + +#include "generated/airframe.h" +#include "firmwares/rotorcraft/stabilization/stabilization_indi.h" + +/** + * Initialises periodic loop; + */ +extern void ctrl_eff_scheduling_init(void); + +/** + * Periodic function that interpolates between gain sets depending on the scheduling variable. + */ +extern void ctrl_eff_scheduling_periodic(void); + +#endif /* CTRL_EFFECTIVENESS_SCHEDULING_H */ +