-
Notifications
You must be signed in to change notification settings - Fork 1.1k
Commit
This commit does not belong to any branch on this repository, and may belong to a fork outside of the repository.
[scheduling] harmonize efficiency scheduling modules
- harmonize name - make a 'generic' module with basic linear scheduling - old ctrl_effectiveness_scheduling was very specific to cyfoam, so renamed - add falcon scheduler, related airframe will come later
- Loading branch information
1 parent
7b54c09
commit 19a7e57
Showing
12 changed files
with
389 additions
and
76 deletions.
There are no files selected for viewing
This file contains bidirectional Unicode text that may be interpreted or compiled differently than what appears below. To review, open the file in an editor that reveals hidden Unicode characters.
Learn more about bidirectional Unicode characters
This file contains bidirectional Unicode text that may be interpreted or compiled differently than what appears below. To review, open the file in an editor that reveals hidden Unicode characters.
Learn more about bidirectional Unicode characters
This file was deleted.
Oops, something went wrong.
This file contains bidirectional Unicode text that may be interpreted or compiled differently than what appears below. To review, open the file in an editor that reveals hidden Unicode characters.
Learn more about bidirectional Unicode characters
| Original file line number | Diff line number | Diff line change |
|---|---|---|
| @@ -0,0 +1,20 @@ | ||
| <!DOCTYPE module SYSTEM "module.dtd"> | ||
|
|
||
| <module name="eff_scheduling_cyfoam" dir="ctrl"> | ||
| <doc> | ||
| <description> | ||
| Interpolation of control effectiveness matrix for Cyfoam tailsitter. | ||
|
|
||
| Two scheduling functions are available, controlled by the flag | ||
| INDI_FUNCTIONS_RC_CHANNEL, which indicates the RC channel to be used. | ||
| </description> | ||
| </doc> | ||
| <header> | ||
| <file name="eff_scheduling_cyfoam.h"/> | ||
| </header> | ||
| <init fun="eff_scheduling_cyfoam_init()"/> | ||
| <periodic fun="eff_scheduling_cyfoam_periodic()" freq="20"/> | ||
| <makefile> | ||
| <file name="eff_scheduling_cyfoam.c"/> | ||
| </makefile> | ||
| </module> |
This file contains bidirectional Unicode text that may be interpreted or compiled differently than what appears below. To review, open the file in an editor that reveals hidden Unicode characters.
Learn more about bidirectional Unicode characters
| Original file line number | Diff line number | Diff line change |
|---|---|---|
| @@ -0,0 +1,18 @@ | ||
| <!DOCTYPE module SYSTEM "module.dtd"> | ||
| <module name="eff_scheduling_falcon" dir="ctrl"> | ||
| <doc> | ||
| <description> | ||
| Interpolation of control effectivenss matrix of the Falcon hybride plane | ||
| </description> | ||
| </doc> | ||
|
|
||
| <header> | ||
| <file name="eff_scheduling_falcon.h"/> | ||
| </header> | ||
| <init fun="eff_scheduling_falcon_init()"/> | ||
| <periodic fun="eff_scheduling_falcon_periodic()" freq="20.0"/> | ||
| <periodic fun="eff_scheduling_falcon_report()" freq="0.5" autorun="FALSE"/> <!-- TRUE FALSE--> | ||
| <makefile target="ap|nps" firmware="rotorcraft" > | ||
| <file name="eff_scheduling_falcon.c"/> | ||
| </makefile> | ||
| </module> |
This file contains bidirectional Unicode text that may be interpreted or compiled differently than what appears below. To review, open the file in an editor that reveals hidden Unicode characters.
Learn more about bidirectional Unicode characters
| Original file line number | Diff line number | Diff line change |
|---|---|---|
| @@ -0,0 +1,24 @@ | ||
| <!DOCTYPE module SYSTEM "module.dtd"> | ||
|
|
||
| <module name="eff_scheduling_generic" dir="ctrl"> | ||
| <doc> | ||
| <description> | ||
| 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! | ||
| </description> | ||
| </doc> | ||
| <header> | ||
| <file name="eff_scheduling_generic.h"/> | ||
| </header> | ||
| <init fun="eff_scheduling_generic_init()"/> | ||
| <periodic fun="eff_scheduling_generic_periodic()" freq="20"/> | ||
| <makefile> | ||
| <file name="eff_scheduling_generic.c"/> | ||
| </makefile> | ||
| </module> |
This file contains bidirectional Unicode text that may be interpreted or compiled differently than what appears below. To review, open the file in an editor that reveals hidden Unicode characters.
Learn more about bidirectional Unicode characters
This file contains bidirectional Unicode text that may be interpreted or compiled differently than what appears below. To review, open the file in an editor that reveals hidden Unicode characters.
Learn more about bidirectional Unicode characters
| Original file line number | Diff line number | Diff line change |
|---|---|---|
| @@ -0,0 +1,39 @@ | ||
| /* | ||
| * Copyright (C) Gautier Hattenberger <gautier.hattenberger@enac.fr> | ||
| * | ||
| * 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 | ||
| * <http://www.gnu.org/licenses/>. | ||
| */ | ||
|
|
||
| /** | ||
| * @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 */ | ||
|
|
This file contains bidirectional Unicode text that may be interpreted or compiled differently than what appears below. To review, open the file in an editor that reveals hidden Unicode characters.
Learn more about bidirectional Unicode characters
| Original file line number | Diff line number | Diff line change |
|---|---|---|
| @@ -0,0 +1,118 @@ | ||
| /* | ||
| * Copyright (C) 2023 Florian Sansou <florian.sansou@enac.fr> | ||
| * Copyright (C) 2023 Gautier Hattenberger <gautier.hattenberger.fr> | ||
| * | ||
| * 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 | ||
| * <http://www.gnu.org/licenses/>. | ||
| */ | ||
|
|
||
| /** @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); | ||
| } | ||
|
|
Oops, something went wrong.