diff --git a/conf/airframes/ENAC/cyfoam.xml b/conf/airframes/ENAC/cyfoam.xml index 6c4f1de314..31a5b4835a 100644 --- a/conf/airframes/ENAC/cyfoam.xml +++ b/conf/airframes/ENAC/cyfoam.xml @@ -15,7 +15,7 @@ - + diff --git a/conf/airframes/tudelft/cyfoam.xml b/conf/airframes/tudelft/cyfoam.xml index db3dab7afb..1639c0d026 100644 --- a/conf/airframes/tudelft/cyfoam.xml +++ b/conf/airframes/tudelft/cyfoam.xml @@ -188,7 +188,7 @@ - + diff --git a/conf/airframes/tudelft/rot_wing_25kg.xml b/conf/airframes/tudelft/rot_wing_25kg.xml index 9361b54fdc..b1930d4f4b 100644 --- a/conf/airframes/tudelft/rot_wing_25kg.xml +++ b/conf/airframes/tudelft/rot_wing_25kg.xml @@ -108,7 +108,7 @@ - + diff --git a/conf/airframes/tudelft/rot_wing_v3b.xml b/conf/airframes/tudelft/rot_wing_v3b.xml index 09eeda22cf..1db76e7782 100644 --- a/conf/airframes/tudelft/rot_wing_v3b.xml +++ b/conf/airframes/tudelft/rot_wing_v3b.xml @@ -108,7 +108,7 @@ - + diff --git a/conf/modules/ctrl_effectiveness_scheduling.xml b/conf/modules/ctrl_effectiveness_scheduling.xml deleted file mode 100644 index 98724f4e83..0000000000 --- a/conf/modules/ctrl_effectiveness_scheduling.xml +++ /dev/null @@ -1,22 +0,0 @@ - - - - - - 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/modules/eff_scheduling_cyfoam.xml b/conf/modules/eff_scheduling_cyfoam.xml new file mode 100644 index 0000000000..47bdc02d12 --- /dev/null +++ b/conf/modules/eff_scheduling_cyfoam.xml @@ -0,0 +1,24 @@ + + + + + + Interpolation of control effectiveness matrix for Cyfoam tailsitter. + + The configuration of Cyfoam is a tailsitter with 2 propellers + blowing a wing with 2 flaps. A minimum amont of thrust is thus + required to have enough control on moments, especially pitch moment. + + Two scheduling functions are available, controlled by the flag + INDI_FUNCTIONS_RC_CHANNEL, which indicates the RC channel to be used. + + +
+ +
+ + + + + + diff --git a/conf/modules/eff_scheduling_falcon.xml b/conf/modules/eff_scheduling_falcon.xml new file mode 100644 index 0000000000..fd8d00bc42 --- /dev/null +++ b/conf/modules/eff_scheduling_falcon.xml @@ -0,0 +1,24 @@ + + + + + Interpolation of control effectivenss matrix of the Falcon hybride plane + + The configuation of Falcon is a tailsitter with 4 propellers mounted as a + regular quadcopter and a wing in the middle with 2 ailevons. The ailevons + are only used for roll and pitch with sufficient forward speed. This module + is in charge of disabling them at low speed / high angle of attack when + they have no more efficiency. + + + +
+ +
+ + + + + + + diff --git a/conf/modules/eff_scheduling_generic.xml b/conf/modules/eff_scheduling_generic.xml new file mode 100644 index 0000000000..3e603d329f --- /dev/null +++ b/conf/modules/eff_scheduling_generic.xml @@ -0,0 +1,24 @@ + + + + + + Interpolation of control effectivenss matrix. + This is necessary if the vehicle has different operating points, + with significantly different control effectivenss. + Interpolation is linear between two points, usually hover and + forward conditions. + + If instead using online adaptation is an option, be sure to + not use this module at the same time! + + +
+ +
+ + + + + + diff --git a/conf/modules/ctrl_eff_sched_rot_wing.xml b/conf/modules/eff_scheduling_rot_wing.xml similarity index 88% rename from conf/modules/ctrl_eff_sched_rot_wing.xml rename to conf/modules/eff_scheduling_rot_wing.xml index 92b2376560..5e8bbcdfa9 100644 --- a/conf/modules/ctrl_eff_sched_rot_wing.xml +++ b/conf/modules/eff_scheduling_rot_wing.xml @@ -1,7 +1,8 @@ - + - The control effectiveness scheduler for the rotating wing quadplane drone type + T + he control effectiveness scheduler for the rotating wing quadplane drone type - it requires a servo called ROTATION_MECH
@@ -18,12 +19,12 @@
- +
- - + + - + diff --git a/sw/airborne/modules/ctrl/ctrl_effectiveness_scheduling.c b/sw/airborne/modules/ctrl/eff_scheduling_cyfoam.c similarity index 80% rename from sw/airborne/modules/ctrl/ctrl_effectiveness_scheduling.c rename to sw/airborne/modules/ctrl/eff_scheduling_cyfoam.c index df4bbddbb4..736b8f7caf 100644 --- a/sw/airborne/modules/ctrl/ctrl_effectiveness_scheduling.c +++ b/sw/airborne/modules/ctrl/eff_scheduling_cyfoam.c @@ -14,16 +14,15 @@ * 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. + * along with paparazzi; see the file COPYING. If not, see + * . */ /** @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 "modules/ctrl/eff_scheduling_cyfoam.h" #include "firmwares/rotorcraft/stabilization/stabilization_indi.h" #include "firmwares/rotorcraft/guidance/guidance_h.h" #include "generated/airframe.h" @@ -38,18 +37,23 @@ #error "You need to define an RC channel to switch between simple and advanced scheduling" #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_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 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) +void eff_scheduling_cyfoam_init(void) { //sum of G1 and G2 int8_t i; @@ -67,31 +71,7 @@ void ctrl_eff_scheduling_init(void) } } -void ctrl_eff_scheduling_periodic(void) -{ - if(radio_control.values[INDI_FUNCTIONS_RC_CHANNEL] > 0) { - ctrl_eff_scheduling_periodic_b(); - } else { - ctrl_eff_scheduling_periodic_a(); - } - -#ifdef INDI_THRUST_ON_PITCH_EFF - //State prioritization {W Roll, W pitch, W yaw, TOTAL THRUST} - if(radio_control.values[INDI_FUNCTIONS_RC_CHANNEL] > 0 && (actuator_state_filt_vect[0] < -7000) && (actuator_state_filt_vect[1] > 7000)) { - Bwls[1][2] = INDI_THRUST_ON_PITCH_EFF/INDI_G_SCALING; - Bwls[1][3] = INDI_THRUST_ON_PITCH_EFF/INDI_G_SCALING; - } else if(radio_control.values[INDI_FUNCTIONS_RC_CHANNEL] > 0 && (actuator_state_filt_vect[0] > 7000) && (actuator_state_filt_vect[1] < -7000)) { - Bwls[1][2] = -INDI_THRUST_ON_PITCH_EFF/INDI_G_SCALING; - Bwls[1][3] = -INDI_THRUST_ON_PITCH_EFF/INDI_G_SCALING; - } else { - Bwls[1][2] = 0.0; - Bwls[1][3] = 0.0; - } -#endif - -} - -void ctrl_eff_scheduling_periodic_a(void) +static void eff_scheduling_periodic_a(void) { // Go from transition percentage to ratio float ratio = FLOAT_OF_BFP(transition_percentage, INT32_PERCENTAGE_FRAC) / 100; @@ -105,7 +85,7 @@ void ctrl_eff_scheduling_periodic_a(void) } } -void ctrl_eff_scheduling_periodic_b(void) +static void eff_scheduling_periodic_b(void) { float airspeed = stateGetAirspeed_f(); struct FloatEulers eulers_zxy; @@ -149,3 +129,26 @@ void ctrl_eff_scheduling_periodic_b(void) } +void eff_scheduling_cyfoam_periodic(void) +{ + if (radio_control.values[INDI_FUNCTIONS_RC_CHANNEL] > 0) { + eff_scheduling_periodic_b(); + } else { + eff_scheduling_periodic_a(); + } + +#ifdef INDI_THRUST_ON_PITCH_EFF + //State prioritization {W Roll, W pitch, W yaw, TOTAL THRUST} + if(radio_control.values[INDI_FUNCTIONS_RC_CHANNEL] > 0 && (actuator_state_filt_vect[0] < -7000) && (actuator_state_filt_vect[1] > 7000)) { + Bwls[1][2] = INDI_THRUST_ON_PITCH_EFF/INDI_G_SCALING; + Bwls[1][3] = INDI_THRUST_ON_PITCH_EFF/INDI_G_SCALING; + } else if(radio_control.values[INDI_FUNCTIONS_RC_CHANNEL] > 0 && (actuator_state_filt_vect[0] > 7000) && (actuator_state_filt_vect[1] < -7000)) { + Bwls[1][2] = -INDI_THRUST_ON_PITCH_EFF/INDI_G_SCALING; + Bwls[1][3] = -INDI_THRUST_ON_PITCH_EFF/INDI_G_SCALING; + } else { + Bwls[1][2] = 0.0; + Bwls[1][3] = 0.0; + } +#endif +} + diff --git a/sw/airborne/modules/ctrl/eff_scheduling_cyfoam.h b/sw/airborne/modules/ctrl/eff_scheduling_cyfoam.h new file mode 100644 index 0000000000..da84985541 --- /dev/null +++ b/sw/airborne/modules/ctrl/eff_scheduling_cyfoam.h @@ -0,0 +1,39 @@ +/* + * Copyright (C) Gautier Hattenberger + * + * 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/ctrl/eff_scheduling_cyfoam.h + */ + +#ifndef EFF_SCHEDULING_CYFOAM_H +#define EFF_SCHEDULING_CYFOAM_H + +/** + * Initialises periodic loop; + */ +extern void eff_scheduling_cyfoam_init(void); + +/** + * Periodic function that interpolates between gain sets depending on the scheduling variable. + */ +extern void eff_scheduling_cyfoam_periodic(void); + +#endif /* EFF_SCHEDULING_CYFOAM_H */ + diff --git a/sw/airborne/modules/ctrl/eff_scheduling_falcon.c b/sw/airborne/modules/ctrl/eff_scheduling_falcon.c new file mode 100644 index 0000000000..f95e080736 --- /dev/null +++ b/sw/airborne/modules/ctrl/eff_scheduling_falcon.c @@ -0,0 +1,118 @@ +/* + * Copyright (C) 2023 Florian Sansou + * Copyright (C) 2023 Gautier Hattenberger + * + * 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/ctrl/eff_scheduling_falcon.c" + * Interpolation of control effectivenss matrix of the Falcon hybrid plane + */ + +#include "modules/ctrl/eff_scheduling_falcon.h" +#include "firmwares/rotorcraft/stabilization/stabilization_indi.h" +#include "firmwares/rotorcraft/guidance/guidance_h.h" +#include "state.h" + +#include "modules/datalink/downlink.h" + +// Airspeed at which only with motor +#ifndef EFF_SCHEDULING_FALCON_LOW_AIRSPEED +#define EFF_SCHEDULING_FALCON_LOW_AIRSPEED 8.0f +#endif + +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 +}; + +void eff_scheduling_falcon_init(void) +{ + for (int8_t i = 0; i < INDI_OUTPUTS; i++) { + for (int8_t j = 0; j < INDI_NUM_ACT; j++) { + g1g2[i][j] = g1g2_hover[i][j] / INDI_G_SCALING; + } + } +} + +void eff_scheduling_falcon_periodic(void) +{ + // calculate squared airspeed + float airspeed = stateGetAirspeed_f(); + + if (airspeed > EFF_SCHEDULING_FALCON_LOW_AIRSPEED) { + airspeed -= EFF_SCHEDULING_FALCON_LOW_AIRSPEED; //offset for start eff at zero! + struct FloatEulers eulers_zxy; + float_eulers_of_quat_zxy(&eulers_zxy, stateGetNedToBodyQuat_f()); + + float pitch_ratio = 0.0f; + if (eulers_zxy.theta > -M_PI_4) { + pitch_ratio = 0.0f; + } + else { + pitch_ratio = fabsf(1.0f - eulers_zxy.theta/(float)(-M_PI_4)); + } + Bound(pitch_ratio, 0.0f, 1.0f); + + + Bound(airspeed, 0.0f, 30.0f); + float airspeed2 = airspeed*airspeed; + + // not effect of elevon on body roll axis + g1g2[0][4] = 0; + g1g2[0][5] = 0; + + float pitch_eff = pitch_ratio * EFF_PITCH_A * airspeed2; + g1g2[1][4] = pitch_eff / 1000; // elevon_right + g1g2[1][5] = -pitch_eff / 1000; // elevon_left + + float yaw_eff = pitch_ratio * EFF_YAW_A * airspeed2; + g1g2[2][4] = -yaw_eff / 1000; // elevon_right + g1g2[2][5] = -yaw_eff / 1000; // elevon_left + + // No thrust generated by elevon, maybe take drag in accout for the future ? + g1g2[3][4] = 0; + g1g2[3][5] = 0; + } + else { + //Come back to motor control + g1g2[0][4] = 0; // elevon_left + g1g2[0][5] = 0; // elevon_right + + g1g2[1][4] = 0; // elevon_left + g1g2[1][5] = 0; // elevon_right + + g1g2[2][4] = 0; // elevon_left + g1g2[2][5] = 0; // elevon_right + + g1g2[3][4] = 0; // elevon_left + g1g2[3][5] = 0; // elevon_right + } +} + +extern void eff_scheduling_falcon_report(void) +{ + float f[6] = { + g1g2[1][4], g1g2[1][5], + g1g2[2][4], g1g2[2][5], + g1g2[1][0], g1g2[2][0] + }; + DOWNLINK_SEND_PAYLOAD_FLOAT(DefaultChannel, DefaultDevice, 6, f); +} + diff --git a/sw/airborne/modules/ctrl/eff_scheduling_falcon.h b/sw/airborne/modules/ctrl/eff_scheduling_falcon.h new file mode 100644 index 0000000000..5b0d1422b3 --- /dev/null +++ b/sw/airborne/modules/ctrl/eff_scheduling_falcon.h @@ -0,0 +1,33 @@ +/* + * Copyright (C) 2023 Florian Sansou + * Copyright (C) 2023 Gautier Hattenberger + * + * 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/ctrl/eff_scheduling_falcon.h" + * Interpolation of control effectivenss matrix of the Falcon hybrid plane + */ + +#ifndef EFF_SCHEDULING_FALCON_H +#define EFF_SCHEDULING_FALCON_H + +extern void eff_scheduling_falcon_init(void); +extern void eff_scheduling_falcon_periodic(void); +extern void eff_scheduling_falcon_report(void); + +#endif // EFF_SCHEDULING_FALCON_H diff --git a/sw/airborne/modules/ctrl/eff_scheduling_generic.c b/sw/airborne/modules/ctrl/eff_scheduling_generic.c new file mode 100644 index 0000000000..d12becf0c3 --- /dev/null +++ b/sw/airborne/modules/ctrl/eff_scheduling_generic.c @@ -0,0 +1,86 @@ +/* + * 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, see + * . + */ + +/** @file modules/ctrl/eff_scheduling_generic.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 "modules/radio_control/radio_control.h" + +#if STABILIZATION_INDI_ALLOCATION_PSEUDO_INVERSE +#error "You need to use WLS control allocation for this module" +#endif + +#ifndef INDI_FUNCTIONS_RC_CHANNEL +#error "You need to define an RC channel to switch between simple and advanced scheduling" +#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 + +void eff_scheduling_generic_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 eff_scheduling_generic_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/eff_scheduling_generic.h similarity index 56% rename from sw/airborne/modules/ctrl/ctrl_effectiveness_scheduling.h rename to sw/airborne/modules/ctrl/eff_scheduling_generic.h index 5f39dd0851..132bde767e 100644 --- a/sw/airborne/modules/ctrl/ctrl_effectiveness_scheduling.h +++ b/sw/airborne/modules/ctrl/eff_scheduling_generic.h @@ -14,32 +14,26 @@ * 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. + * along with paparazzi; see the file COPYING. If not, see + * . */ /** - * @file modules/ctrl/ctrl_effectiveness_scheduling.h + * @file modules/ctrl/eff_scheduling_generic.h */ -#ifndef CTRL_EFFECTIVENESS_SCHEDULING_H -#define CTRL_EFFECTIVENESS_SCHEDULING_H - -#include "generated/airframe.h" -#include "firmwares/rotorcraft/stabilization/stabilization_indi.h" +#ifndef EFF_SCHEDULING_GENERIC_H +#define EFF_SCHEDULING_GENERIC_H /** * Initialises periodic loop; */ -extern void ctrl_eff_scheduling_init(void); +extern void eff_scheduling_generic_init(void); /** * Periodic function that interpolates between gain sets depending on the scheduling variable. */ -extern void ctrl_eff_scheduling_periodic(void); -extern void ctrl_eff_scheduling_periodic_a(void); -extern void ctrl_eff_scheduling_periodic_b(void); +extern void eff_scheduling_generic_periodic(void); -#endif /* CTRL_EFFECTIVENESS_SCHEDULING_H */ +#endif /* EFF_SCHEDULING_GENERIC_H */ diff --git a/sw/airborne/modules/ctrl/ctrl_eff_sched_rot_wing.c b/sw/airborne/modules/ctrl/eff_scheduling_rot_wing.c similarity index 84% rename from sw/airborne/modules/ctrl/ctrl_eff_sched_rot_wing.c rename to sw/airborne/modules/ctrl/eff_scheduling_rot_wing.c index 1dbdf66484..c4561855bc 100644 --- a/sw/airborne/modules/ctrl/ctrl_eff_sched_rot_wing.c +++ b/sw/airborne/modules/ctrl/eff_scheduling_rot_wing.c @@ -18,12 +18,12 @@ * . */ -/** @file "modules/ctrl/ctrl_eff_sched_rot_wing.c" +/** @file "modules/ctrl/eff_scheduling_rot_wing.c" * @author Dennis van Wijngaarden * The control effectiveness scheduler for the rotating wing drone type */ -#include "modules/ctrl/ctrl_eff_sched_rot_wing.h" +#include "modules/ctrl/eff_scheduling_rot_wing.h" #include "generated/airframe.h" #include "state.h" @@ -145,17 +145,17 @@ struct rot_wing_eff_sched_param_t eff_sched_p = { struct rot_wing_eff_sched_var_t eff_sched_var; -inline void ctrl_eff_sched_rot_wing_update_wing_angle(void); -inline void ctrl_eff_sched_rot_wing_update_MMOI(void); -inline void ctrl_eff_sched_rot_wing_update_cmd(void); -inline void ctrl_eff_sched_rot_wing_update_airspeed(void); -inline void ctrl_eff_sched_rot_wing_update_hover_motor_effectiveness(void); -inline void ctrl_eff_sched_rot_wing_update_elevator_effectiveness(void); -inline void ctrl_eff_sched_rot_wing_update_rudder_effectiveness(void); -inline void ctrl_eff_sched_rot_wing_update_aileron_effectiveness(void); -inline void ctrl_eff_sched_rot_wing_update_flaperon_effectiveness(void); -inline void ctrl_eff_sched_rot_wing_update_pusher_effectiveness(void); -inline void ctrl_eff_sched_rot_wing_schedule_liftd(void); +inline void eff_scheduling_rot_wing_update_wing_angle(void); +inline void eff_scheduling_rot_wing_update_MMOI(void); +inline void eff_scheduling_rot_wing_update_cmd(void); +inline void eff_scheduling_rot_wing_update_airspeed(void); +inline void eff_scheduling_rot_wing_update_hover_motor_effectiveness(void); +inline void eff_scheduling_rot_wing_update_elevator_effectiveness(void); +inline void eff_scheduling_rot_wing_update_rudder_effectiveness(void); +inline void eff_scheduling_rot_wing_update_aileron_effectiveness(void); +inline void eff_scheduling_rot_wing_update_flaperon_effectiveness(void); +inline void eff_scheduling_rot_wing_update_pusher_effectiveness(void); +inline void eff_scheduling_rot_wing_schedule_liftd(void); inline float guidance_indi_get_liftd(float pitch UNUSED, float theta UNUSED); inline void stabilization_indi_set_wls_settings(float use_increment); @@ -183,18 +183,18 @@ static void wing_position_cb(uint8_t sender_id UNUSED, struct act_feedback_t *po } } -void ctrl_eff_sched_rot_wing_init(void) +void eff_scheduling_rot_wing_init(void) { // Initialize variables to quad values - eff_sched_var.Ixx = eff_sched_p.Ixx_body + eff_sched_p.Ixx_wing; + eff_sched_var.Ixx = eff_sched_p.Ixx_body + eff_sched_p.Ixx_wing; eff_sched_var.Iyy = eff_sched_p.Iyy_body + eff_sched_p.Iyy_wing; eff_sched_var.wing_rotation_rad = 0; // ABI input eff_sched_var.wing_rotation_deg = 0; - eff_sched_var.cosr = 1; - eff_sched_var.sinr = 0; - eff_sched_var.cosr2 = 1; + eff_sched_var.cosr = 1; + eff_sched_var.sinr = 0; + eff_sched_var.cosr2 = 1; eff_sched_var.sinr2 = 0; - eff_sched_var.sinr3 = 0; + eff_sched_var.sinr3 = 0; // Set moment derivative variables eff_sched_var.pitch_motor_dMdpprz = ROT_WING_EFF_SCHED_DM_DPPRZ_HOVER_PITCH; @@ -211,26 +211,26 @@ void ctrl_eff_sched_rot_wing_init(void) AbiBindMsgACT_FEEDBACK(WING_ROTATION_CAN_ROT_WING_ID, &wing_position_ev, wing_position_cb); } -void ctrl_eff_sched_rot_wing_periodic(void) +void eff_scheduling_rot_wing_periodic(void) { // your periodic code here. // freq = 10.0 Hz - ctrl_eff_sched_rot_wing_update_wing_angle(); - ctrl_eff_sched_rot_wing_update_MMOI(); - ctrl_eff_sched_rot_wing_update_cmd(); - ctrl_eff_sched_rot_wing_update_airspeed(); + eff_scheduling_rot_wing_update_wing_angle(); + eff_scheduling_rot_wing_update_MMOI(); + eff_scheduling_rot_wing_update_cmd(); + eff_scheduling_rot_wing_update_airspeed(); // Update the effectiveness values - ctrl_eff_sched_rot_wing_update_hover_motor_effectiveness(); - ctrl_eff_sched_rot_wing_update_elevator_effectiveness(); - ctrl_eff_sched_rot_wing_update_rudder_effectiveness(); - ctrl_eff_sched_rot_wing_update_aileron_effectiveness(); - ctrl_eff_sched_rot_wing_update_flaperon_effectiveness(); - ctrl_eff_sched_rot_wing_update_pusher_effectiveness(); - ctrl_eff_sched_rot_wing_schedule_liftd(); + eff_scheduling_rot_wing_update_hover_motor_effectiveness(); + eff_scheduling_rot_wing_update_elevator_effectiveness(); + eff_scheduling_rot_wing_update_rudder_effectiveness(); + eff_scheduling_rot_wing_update_aileron_effectiveness(); + eff_scheduling_rot_wing_update_flaperon_effectiveness(); + eff_scheduling_rot_wing_update_pusher_effectiveness(); + eff_scheduling_rot_wing_schedule_liftd(); } -void ctrl_eff_sched_rot_wing_update_wing_angle(void) +void eff_scheduling_rot_wing_update_wing_angle(void) { // Calculate sin and cosines of rotation eff_sched_var.wing_rotation_deg = eff_sched_var.wing_rotation_rad / M_PI * 180.; @@ -245,24 +245,24 @@ void ctrl_eff_sched_rot_wing_update_wing_angle(void) } -void ctrl_eff_sched_rot_wing_update_MMOI(void) +void eff_scheduling_rot_wing_update_MMOI(void) { eff_sched_var.Ixx = eff_sched_p.Ixx_body + eff_sched_var.cosr2 * eff_sched_p.Ixx_wing + eff_sched_var.sinr2 * eff_sched_p.Iyy_wing; - eff_sched_var.Iyy = eff_sched_p.Iyy_body + eff_sched_var.sinr2 * eff_sched_p.Ixx_wing + eff_sched_var.cosr2 * eff_sched_p.Iyy_wing; + eff_sched_var.Iyy = eff_sched_p.Iyy_body + eff_sched_var.sinr2 * eff_sched_p.Ixx_wing + eff_sched_var.cosr2 * eff_sched_p.Iyy_wing; // Bound inertia Bound(eff_sched_var.Ixx, 0.01, 100.); Bound(eff_sched_var.Iyy, 0.01, 100.); } -void ctrl_eff_sched_rot_wing_update_cmd(void) +void eff_scheduling_rot_wing_update_cmd(void) { eff_sched_var.cmd_elevator = actuators_pprz[SERVO_SERVO_ELEVATOR]; eff_sched_var.cmd_pusher_scaled = actuators_pprz[SERVO_MOTOR_PUSH] * 0.000853229; // Scaled with 8181 / 9600 / 1000 eff_sched_var.cmd_T_mean_scaled = (actuators_pprz[SERVO_MOTOR_FRONT] + actuators_pprz[SERVO_MOTOR_RIGHT] + actuators_pprz[SERVO_MOTOR_BACK] + actuators_pprz[SERVO_MOTOR_LEFT]) / 4. * 0.000853229; // Scaled with 8181 / 9600 / 1000 } -void ctrl_eff_sched_rot_wing_update_airspeed(void) +void eff_scheduling_rot_wing_update_airspeed(void) { eff_sched_var.airspeed = stateGetAirspeed_f(); Bound(eff_sched_var.airspeed, 0. , 30.); @@ -270,9 +270,9 @@ void ctrl_eff_sched_rot_wing_update_airspeed(void) Bound(eff_sched_var.airspeed2, 0. , 900.); } -void ctrl_eff_sched_rot_wing_update_hover_motor_effectiveness(void) +void eff_scheduling_rot_wing_update_hover_motor_effectiveness(void) { - // Pitch motor effectiveness + // Pitch motor effectiveness float pitch_motor_q_eff = eff_sched_var.pitch_motor_dMdpprz / eff_sched_var.Iyy; @@ -302,12 +302,12 @@ void ctrl_eff_sched_rot_wing_update_hover_motor_effectiveness(void) g1g2[1][3] = -roll_motor_q_eff; // pitch effectiveness left motor } -void ctrl_eff_sched_rot_wing_update_elevator_effectiveness(void) +void eff_scheduling_rot_wing_update_elevator_effectiveness(void) { float de = eff_sched_p.k_elevator_deflection[0] + eff_sched_p.k_elevator_deflection[1] * eff_sched_var.cmd_elevator; float dMyde = (eff_sched_p.k_elevator[0] * de * eff_sched_var.airspeed2 + - eff_sched_p.k_elevator[1] * eff_sched_var.cmd_pusher_scaled * eff_sched_var.cmd_pusher_scaled * eff_sched_var.airspeed + + eff_sched_p.k_elevator[1] * eff_sched_var.cmd_pusher_scaled * eff_sched_var.cmd_pusher_scaled * eff_sched_var.airspeed + eff_sched_p.k_elevator[2] * eff_sched_var.airspeed2) / 10000.; float dMydpprz = dMyde * eff_sched_p.k_elevator_deflection[1]; @@ -320,10 +320,10 @@ void ctrl_eff_sched_rot_wing_update_elevator_effectiveness(void) g1g2[1][5] = eff_y_elev; } -void ctrl_eff_sched_rot_wing_update_rudder_effectiveness(void) +void eff_scheduling_rot_wing_update_rudder_effectiveness(void) { - float dMzdr = (eff_sched_p.k_rudder[0] * eff_sched_var.cmd_pusher_scaled * eff_sched_var.cmd_T_mean_scaled + - eff_sched_p.k_rudder[1] * eff_sched_var.cmd_T_mean_scaled * eff_sched_var.airspeed2 * eff_sched_var.cosr + + float dMzdr = (eff_sched_p.k_rudder[0] * eff_sched_var.cmd_pusher_scaled * eff_sched_var.cmd_T_mean_scaled + + eff_sched_p.k_rudder[1] * eff_sched_var.cmd_T_mean_scaled * eff_sched_var.airspeed2 * eff_sched_var.cosr + eff_sched_p.k_rudder[2] * eff_sched_var.airspeed2) / 10000.; // Convert moment to effectiveness @@ -338,7 +338,7 @@ void ctrl_eff_sched_rot_wing_update_rudder_effectiveness(void) g1g2[2][4] = eff_z_rudder; } -void ctrl_eff_sched_rot_wing_update_aileron_effectiveness(void) +void eff_scheduling_rot_wing_update_aileron_effectiveness(void) { float dMxdpprz = (eff_sched_p.k_aileron * eff_sched_var.airspeed2 * eff_sched_var.sinr3) / 1000000.; float eff_x_aileron = dMxdpprz / eff_sched_var.Ixx; @@ -346,7 +346,7 @@ void ctrl_eff_sched_rot_wing_update_aileron_effectiveness(void) g1g2[0][6] = eff_x_aileron; } -void ctrl_eff_sched_rot_wing_update_flaperon_effectiveness(void) +void eff_scheduling_rot_wing_update_flaperon_effectiveness(void) { float dMxdpprz = (eff_sched_p.k_flaperon * eff_sched_var.airspeed2 * eff_sched_var.sinr3) / 1000000.; float eff_x_flap_aileron = dMxdpprz / eff_sched_var.Ixx; @@ -354,7 +354,7 @@ void ctrl_eff_sched_rot_wing_update_flaperon_effectiveness(void) g1g2[0][7] = eff_x_flap_aileron; } -void ctrl_eff_sched_rot_wing_update_pusher_effectiveness(void) +void eff_scheduling_rot_wing_update_pusher_effectiveness(void) { float rpmP = eff_sched_p.k_rpm_pprz_pusher[0] + eff_sched_p.k_rpm_pprz_pusher[1] * eff_sched_var.cmd_pusher_scaled + eff_sched_p.k_rpm_pprz_pusher[2] * eff_sched_var.cmd_pusher_scaled * eff_sched_var.cmd_pusher_scaled; @@ -367,9 +367,9 @@ void ctrl_eff_sched_rot_wing_update_pusher_effectiveness(void) g1g2[4][8] = eff_pusher; } -float ctrl_eff_sched_rot_wing_lift_d = 0.0f; +float eff_scheduling_rot_wing_lift_d = 0.0f; -void ctrl_eff_sched_rot_wing_schedule_liftd(void) +void eff_scheduling_rot_wing_schedule_liftd(void) { float lift_d_wing = (eff_sched_p.k_lift_wing[0] + eff_sched_p.k_lift_wing[1] * eff_sched_var.sinr2) * eff_sched_var.airspeed2 / eff_sched_p.m; float lift_d_fuselage = eff_sched_p.k_lift_fuselage * eff_sched_var.airspeed2 / eff_sched_p.m; @@ -380,12 +380,12 @@ void ctrl_eff_sched_rot_wing_schedule_liftd(void) lift_d = 0.0; } Bound(lift_d, -130., 0.); - ctrl_eff_sched_rot_wing_lift_d = lift_d; + eff_scheduling_rot_wing_lift_d = lift_d; } // Override standard LIFT_D function float guidance_indi_get_liftd(float pitch UNUSED, float theta UNUSED) { - return ctrl_eff_sched_rot_wing_lift_d; + return eff_scheduling_rot_wing_lift_d; } void stabilization_indi_set_wls_settings(float use_increment) @@ -423,4 +423,4 @@ void stabilization_indi_set_wls_settings(float use_increment) du_min_stab_indi[7] = min_pprz_cmd_flap_ail - use_increment*actuators_pprz[7]; #endif //ROTWING_V3B -} \ No newline at end of file +} diff --git a/sw/airborne/modules/ctrl/ctrl_eff_sched_rot_wing.h b/sw/airborne/modules/ctrl/eff_scheduling_rot_wing.h similarity index 94% rename from sw/airborne/modules/ctrl/ctrl_eff_sched_rot_wing.h rename to sw/airborne/modules/ctrl/eff_scheduling_rot_wing.h index 16953d8e64..4c6616ed56 100644 --- a/sw/airborne/modules/ctrl/ctrl_eff_sched_rot_wing.h +++ b/sw/airborne/modules/ctrl/eff_scheduling_rot_wing.h @@ -18,7 +18,7 @@ * . */ -/** @file "modules/ctrl/ctrl_eff_sched_rot_wing.h" +/** @file "modules/ctrl/eff_scheduling_rot_wing.h" * @author Dennis van Wijngaarden * The control effectiveness scheduler for the rotating wing drone type */ @@ -29,8 +29,8 @@ #include "std.h" struct rot_wing_eff_sched_param_t { - float Ixx_body; // body MMOI around roll axis [kgm²] - float Iyy_body; // body MMOI around pitch axis [kgm²] + float Ixx_body; // body MMOI around roll axis [kgm²] + float Iyy_body; // body MMOI around pitch axis [kgm²] float Izz; // total MMOI around yaw axis [kgm²] float Ixx_wing; // wing MMOI around the chordwise direction of the wing [kgm²] float Iyy_wing; // wing MMOI around the spanwise direction of the wing [kgm²] @@ -78,7 +78,8 @@ struct rot_wing_eff_sched_var_t { extern float rotation_angle_setpoint_deg; extern int16_t rotation_cmd; -extern void ctrl_eff_sched_rot_wing_init(void); -extern void ctrl_eff_sched_rot_wing_periodic(void); +extern void eff_scheduling_rot_wing_init(void); +extern void eff_scheduling_rot_wing_periodic(void); #endif // CTRL_EFF_SCHED_ROT_WING_H +