[scheduling] harmonize efficiency scheduling modules (#3182)

- 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
paparazzi · Nov 25, 2023 · 68ddea1 · 68ddea1
1 parent 44a3abb
commit 68ddea1
Show file tree

Hide file tree

Showing 17 changed files with 465 additions and 140 deletions.
diff --git a/conf/airframes/ENAC/cyfoam.xml b/conf/airframes/ENAC/cyfoam.xml
@@ -15,7 +15,7 @@
         <define name="RADIO_KILL_SWITCH" value="RADIO_GAIN1"/>
       </module>
 
-      <module name="ctrl_effectiveness_scheduling">
+      <module name="eff_scheduling_cyfoam">
         <define name="SQUARED_ROLL_EFF" value="0.0018"/>
         <define name="PITCH_EFF_AT_60" value="4.0"/>
         <define name="YAW_EFF_AT_60" value="8.0"/>

diff --git a/conf/airframes/tudelft/cyfoam.xml b/conf/airframes/tudelft/cyfoam.xml
@@ -188,7 +188,7 @@
         <define name="RADIO_KILL_SWITCH" value="5"/>
       </module>
 
-      <module name="ctrl_effectiveness_scheduling">
+      <module name="eff_scheduling_cyfoam">
         <define name="SQUARED_ROLL_EFF" value="0.0018"/>
         <define name="PITCH_EFF_AT_60" value="4.0"/>
         <define name="YAW_EFF_AT_60" value="8.0"/>

diff --git a/conf/airframes/tudelft/rot_wing_25kg.xml b/conf/airframes/tudelft/rot_wing_25kg.xml
@@ -108,7 +108,7 @@
 
         <module name="stabilization" type="rate_indi"/>
 
-        <module name="ctrl_eff_sched_rot_wing"/>
+        <module name="eff_scheduling_rot_wing"/>
 
         <module name="guidance" type="indi_hybrid_quadplane"/>
         <module name="nav" type="hybrid">

diff --git a/conf/airframes/tudelft/rot_wing_v3b.xml b/conf/airframes/tudelft/rot_wing_v3b.xml
@@ -108,7 +108,7 @@
             <define name="WLS_N_V" value="5"/>
         </module>
 
-        <module name="ctrl_eff_sched_rot_wing"/>
+        <module name="eff_scheduling_rot_wing"/>
 
         <module name="guidance" type="indi_hybrid_quadplane"/>
         <module name="nav" type="hybrid">

diff --git a/conf/modules/ctrl_effectiveness_scheduling.xml b/conf/modules/ctrl_effectiveness_scheduling.xml
diff --git a/conf/modules/eff_scheduling_cyfoam.xml b/conf/modules/eff_scheduling_cyfoam.xml
@@ -0,0 +1,24 @@
+<!DOCTYPE module SYSTEM "module.dtd">
+
+<module name="eff_scheduling_cyfoam" dir="ctrl">
+  <doc>
+    <description>
+      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.
+    </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>
diff --git a/conf/modules/eff_scheduling_falcon.xml b/conf/modules/eff_scheduling_falcon.xml
@@ -0,0 +1,24 @@
+<!DOCTYPE module SYSTEM "module.dtd">
+<module name="eff_scheduling_falcon" dir="ctrl">
+  <doc>
+    <description>
+      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.
+    </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>
diff --git a/conf/modules/eff_scheduling_generic.xml b/conf/modules/eff_scheduling_generic.xml
@@ -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>
diff --git a/conf/modules/ctrl_eff_sched_rot_wing.xml → conf/modules/eff_scheduling_rot_wing.xml b/conf/modules/ctrl_eff_sched_rot_wing.xml → conf/modules/eff_scheduling_rot_wing.xml
@@ -1,7 +1,8 @@
 <!DOCTYPE module SYSTEM "module.dtd">
-<module name="ctrl_eff_sched_rot_wing" dir="ctrl">
+<module name="eff_scheduling_rot_wing" dir="ctrl">
   <doc>
-    <description>The control effectiveness scheduler for the rotating wing quadplane drone type
+    <description>T
+      he control effectiveness scheduler for the rotating wing quadplane drone type
      - it requires a servo called ROTATION_MECH
     </description>
     <section name="ROT_WING" prefix="ROT_WING_EFF_SCHED_">
@@ -18,12 +19,12 @@
     </section>
   </doc>
   <header>
-    <file name="ctrl_eff_sched_rot_wing.h"/>
+    <file name="eff_scheduling_rot_wing.h"/>
   </header>
-  <init fun="ctrl_eff_sched_rot_wing_init()"/>
-  <periodic fun="ctrl_eff_sched_rot_wing_periodic()" freq="10.0"/>
+  <init fun="eff_scheduling_rot_wing_init()"/>
+  <periodic fun="eff_scheduling_rot_wing_periodic()" freq="10.0"/>
   <makefile>
-    <file name="ctrl_eff_sched_rot_wing.c"/>
+    <file name="eff_scheduling_rot_wing.c"/>
     <test>
       <define name="ACTUATORS_NB" value="10"/>
       <define name="SERVO_ROTATION_MECH" value="8"/>

diff --git a/...ules/ctrl/ctrl_effectiveness_scheduling.c → ...orne/modules/ctrl/eff_scheduling_cyfoam.c b/...ules/ctrl/ctrl_effectiveness_scheduling.c → ...orne/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
+ * <http://www.gnu.org/licenses/>.
  */
 
 /** @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
@@ -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 */
+