Merge pull request #1878 from paparazzi/obc_throttle_curves

[rpm-control] throttle curves with rpm control

conf/airframes/airframe.dtd

@@ -77,6 +77,7 @@ max CDATA #REQUIRED>
 
 <!ATTLIST curve
 throttle CDATA #REQUIRED
+rpm CDATA #IMPLIED
 collective CDATA #REQUIRED>
 
 <!ATTLIST axis

conf/airframes/tudelft/bs_helidd_indi.xml

@@ -79,7 +79,6 @@
   </section>
 
   <command_laws>
-    <call fun="throttle_curve_run(autopilot_get_motors_on(), values)"/>
     <call fun="swashplate_mixing_run(values)"/>
     <set servo="THROTTLE" value="throttle_curve.throttle"/>
     <set servo="FRONT" value="swashplate_mixing.commands[SW_FRONT]"/>

conf/airframes/tudelft/bs_helidd_pid.xml

@@ -78,7 +78,6 @@
   </section>
 
   <command_laws>
-    <call fun="throttle_curve_run(autopilot_get_motors_on(), values)"/>
     <call fun="swashplate_mixing_run(values)"/>
     <set servo="THROTTLE" value="throttle_curve.throttle"/>
     <set servo="FRONT" value="swashplate_mixing.commands[SW_FRONT]"/>

conf/airframes/tudelft/heli450.xml

@@ -63,7 +63,6 @@
   </section>
 
   <command_laws>
-    <call fun="throttle_curve_run(autopilot_get_motors_on(), values)"/>
     <call fun="swashplate_mixing_run(values)"/>
     <set servo="THROTTLE"    value="throttle_curve.throttle"/>
     <set servo="BACK"        value="swashplate_mixing.commands[SW_BACK]"/>

conf/airframes/tudelft/heliGeniusDD.xml

@@ -78,7 +78,6 @@
   </section>
 
   <command_laws>
-    <call fun="throttle_curve_run(autopilot_get_motors_on(), values)"/>
     <call fun="swashplate_mixing_run(values)"/>
     <set servo="THROTTLE" value="throttle_curve.throttle"/>
     <set servo="FRONT" value="swashplate_mixing.commands[SW_FRONT]"/>

conf/airframes/tudelft/logo600.xml

@@ -41,6 +41,7 @@ The xml in master currently configures the LOGO600 as a pure model aircraft: mea
     <module name="intermcu"      type="uart">
       <define name="REMAP_UART3" value="TRUE"/>
     </module>
+    <module name="heli" type="throttle_curve"/>
   </firmware>
 
   <firmware name="rotorcraft">
@@ -54,7 +55,6 @@ The xml in master currently configures the LOGO600 as a pure model aircraft: mea
     <module name="actuators"     type="spektrum">
       <configure name="ACTUATORS_SPEKTRUM_DEV2" value="UART6"/>
     </module>
-    <module name="heli" type="throttle_curve"/>
     <define name="RC_LOST_FBW_MODE" value="FBW_MODE_FAILSAFE"/><!-- Switch to Failsafe or to Autopilot on RC loss? -->
     <!--define name="RC_LOST_IN_AUTO_FBW_MODE" value="FBW_MODE_AUTO"/--><!-- Switch to AUTO2 with a working autopilot on RC loss? Warning: beware of fly-away: program a GeoFence. beware: AP can ARM the FBW: airframe is dangerous at all times.-->
     <define name="RC_LOST_IN_AUTO_FBW_MODE" value="FBW_MODE_FAILSAFE"/><!-- Behave as a model aircraft: failsafe on RC lost. -->
@@ -88,6 +88,7 @@ The xml in master currently configures the LOGO600 as a pure model aircraft: mea
 
   <commands>
     <axis name="THRUST"     failsafe_value="0"/>
+    <axis name="COLLECTIVE" failsafe_value="-9600"/>
     <axis name="ROLL"       failsafe_value="0"/>
     <axis name="PITCH"      failsafe_value="0"/>
     <axis name="YAW"        failsafe_value="0"/>
@@ -111,14 +112,13 @@ The xml in master currently configures the LOGO600 as a pure model aircraft: mea
   </heli_curves>
 
   <command_laws>
-    <call fun="throttle_curve_run(TRUE, values)"/>
     <let var="th_hold"          value="LessThan(RadioControlValues(RADIO_TH_HOLD), -4800)"/>
 
-    <set servo="THROTTLE"    value="($th_hold? -9600 : throttle_curve.throttle)"/>
+    <set servo="THROTTLE"    value="($th_hold? -9600 : @THRUST)"/>
     <set servo="ROLL"        value="@ROLL"/>
     <set servo="PITCH"       value="@PITCH"/>
     <set servo="YAW"         value="@YAW"/>
-    <set servo="COLLECTIVE"  value="throttle_curve.collective"/>
+    <set servo="COLLECTIVE"  value="@COLLECTIVE"/>
     <set servo="TAIL_GAIN"   value="0"/>
     <set servo="AUX2"        value="0"/>
   </command_laws>

conf/airframes/tudelft/outback.xml

@@ -73,7 +73,6 @@
       <module name="intermcu"      type="uart"/>
       <module name="telemetry"     type="intermcu"/>
       <module name="opa_controller"/>
-      <module name="heli_throttle_curve"/>
       <module name="heli_swashplate_mixing"/>
       <module name="gps" type="ublox"/>
       <module name="gps_ubx_ucenter"/>
@@ -116,57 +115,79 @@
     <set command="ROLL"     value="@ROLL"/>
     <set command="PITCH"    value="@PITCH"/>
     <set command="YAW"      value="@YAW"/>
-    <set command="FMODE"    value="@FMODE"/>
   </rc_commands>
 
   <commands>
     <axis name="THRUST"     failsafe_value="0"/>
     <axis name="ROLL"       failsafe_value="0"/>
     <axis name="PITCH"      failsafe_value="0"/>
-    <axis name="YAW"        failsafe_value="0"/>
-    <axis name="FMODE"      failsafe_value="-9600"/>
-    <axis name="TH_HOLD"    failsafe_value="-9600"/>
-    <axis name="MODE"       failsafe_value="-9600"/>
+    <axis name="YAW"        failsafe_value="9600"/>
+    <axis name="COLLECTIVE" failsafe_value="-9600"/>
   </commands>
 
   <section name="MIXING" prefix="SW_MIXING_">
     <define name="TYPE"       value="HR120"/>
     <define name="TRIM_ROLL"  value="0"/>
-    <define name="TRIM_PITCH" value="0"/>
-    <define name="TRIM_COLL"  value="0"/>
+    <define name="TRIM_PITCH" value="-682"/>
+    <define name="TRIM_COLL" value="0"/>
+  </section>
+
+  <section name="THROTTLE_CURVE" prefix="THROTTLE_CURVE_">
+    <define name="RPM_FB_P" value="5.0"/>
+    <define name="RPM_FB_I" value="5.0"/>
   </section>
 
   <command_laws>
-    <call fun="throttle_curve_run(fbw_motors_on, values)"/>
+
     <call fun="swashplate_mixing_run(values)"/>
     <let var="th_hold"          value="LessThan(RadioControlValues(RADIO_TH_HOLD), -4800)"/>
-    <let var="man_mode"         value="LessThan(RadioControlValues(RADIO_MODE), -4800)"/>
+    <let var="forward_curve"    value="INTERMCU_GET_CMD_STATUS(INTERMCU_CMD_TIPPROPS)"/>
+    <call fun="INTERMCU_CLR_CMD_STATUS(INTERMCU_CMD_TIPPROPS)"/>
 
-    <set servo="THROTTLE"       value="($th_hold? -9600 : throttle_curve.throttle)"/>
+    <set servo="THROTTLE"       value="($th_hold? -9600 : @THRUST)"/>
     <set servo="SW_BACK"        value="swashplate_mixing.commands[SW_BACK]"/>
     <set servo="SW_LEFTFRONT"   value="swashplate_mixing.commands[SW_LEFTFRONT]"/>
     <set servo="SW_RIGHTFRONT"  value="swashplate_mixing.commands[SW_RIGHTFRONT]"/>
-    <set servo="TAIL_LEFT"      value="($th_hold? 0 : (-@YAW - throttle_curve.throttle*0.25) )"/>
-    <set servo="TAIL_RIGHT"     value="($th_hold? 0 : (-@YAW - throttle_curve.throttle*0.25) )"/>
-    <set servo="AIL_LEFTLOW"    value="($man_mode? ( @PITCH - @ROLL) : 0)"/>
-    <set servo="AIL_RIGHTLOW"   value="($man_mode? (-@PITCH - @ROLL) : 0)"/>
-    <set servo="AIL_LEFTUP"     value="($man_mode? ( @PITCH - @ROLL) : 0)"/>
-    <set servo="AIL_RIGHTUP"    value="($man_mode? (-@PITCH - @ROLL) : 0)"/>
+    <set servo="TAIL_LEFT"      value="(Or($th_hold, $forward_curve)? 0 : (-@YAW - @THRUST*0.25) )"/>
+    <set servo="TAIL_RIGHT"     value="(Or($th_hold, $forward_curve)? 0 : (-@YAW - @THRUST*0.25) )"/>
+    <set servo="AIL_LEFTLOW"    value=" 2*@PITCH + 3*@YAW"/>
+    <set servo="AIL_RIGHTLOW"   value="-2*@PITCH + 3*@YAW"/>
+    <set servo="AIL_LEFTUP"     value=" 2*@PITCH + 3*@YAW"/>
+    <set servo="AIL_RIGHTUP"    value="-2*@PITCH + 3*@YAW"/>
   </command_laws>
 
-
-  <!-- Pitch RODS black laminated props: 52mm top links 65mm bottom links -->
+  <!-- v3 Pitch RODS black laminated props with straight angle root and 25deg blade-grip handles: 52mm top links 65mm bottom links -->
   <heli_curves>
-    <curve throttle="0,7000,7000" collective="-7500,-5000,-2500"/>
-    <curve throttle="7500,9000,9600" collective="-7500,-5200,-2000"/>
-    <curve throttle="7500,7500,7500" collective="-7000,500,8000"/>
+    <curve throttle="0,6000,0,0,0" collective="-9600,-8150,-6700,-5250,-3800"/>
+    <curve throttle="5000,6500,8000" rpm="1650,1650,1650" collective="-9600,-6500,-2300"/><!-- 1 blue bat: -5650 collective with 8000 throttle FF with 1650 RPM -->
+    <curve throttle="8600,8600,8600,8600,9600" collective="-9600,-7300,-5000,-2300,500"/>
   </heli_curves>
 
-  <!-- Pitch RODS white symmetric props: 56mm top links 65mm bottom links -->
+  <!-- RPM to PPRZ calculation (governer low, fixed RPM) Castle Creations BEC-->
+  <!-- RPM * 0.000214141414 + 1.2841 = ms -->
+  <!-- 1500 RPM: 1.602ms    1.602 - 1.1 = 0.502ms    9600 / 800 * 502 = 6024 PPRZ -->
+  <!-- 1650 RPM: 1.641ms    1.641 - 1.1 = 0.541ms    9600 / 800 * 541 = 6492 PPRZ -->
+  <!-- 1800 RPM: 1.672ms    1.672 - 1.1 = 0.572ms    9600 / 800 * 572 = 6864 PPRZ -->
+
+  <!-- v2 Pitch RODS black laminated props with straight angle root and straight blade-grip handles: 63mm top links 65mm bottom links -->
   <!--heli_curves>
-    <curve throttle="0,7000,7000" collective="0,2000,4000"/>
-    <curve throttle="5000,7000,9000" collective="0,2000,4000"/>
-    <curve throttle="7500,8500,9600" collective="0,2000,4000"/>
+    <curve throttle="0,6492,6492" collective="-5000,-2500,0"/>
+    <curve throttle="6492,6492,6492" collective="-5000,-2700,1500"/>
+    <curve throttle="6492,6492,6024" collective="-5000,-2700,3500"/>
+  </heli_curves-->
+
+  <!-- v1 Pitch RODS black laminated props with twisted angle root and straight blade-grip handles: 52mm top links 65mm bottom links -->
+  <!--heli_curves>
+    <curve throttle="0,6492,6492" collective="-7500,-5000,-2500"/>
+    <curve throttle="6492,6492,6492" collective="-7500,-5200,-1000"/>
+    <curve throttle="6492,6492,6024" collective="-7500,-5200,2000"/>
+  </heli_curves-->
+
+  <!-- v0 Pitch RODS white symmetric props: 56mm top links 65mm bottom links -->
+  <!--heli_curves>
+    <curve throttle="0,6024,6024" collective="0,2000,4000"/>
+    <curve throttle="6024,6024,6024" collective="0,2000,4000"/>
+    <curve throttle="6492,6492,6492" collective="0,2000,4000"/>
   </heli_curves-->
 
   <section name="MISC">

conf/airframes/tudelft/walkera_genius_v2.xml

@@ -165,7 +165,6 @@ Using Lisa/S V1.0 board file as it is software compatible.
     <let var="hoverstick" value=".1*MAX_PPRZ"/>
     <let var="halfway" value="(@THRUST >= ($hoverstick) ? 1 : 0)"/>
 
-    <call fun="throttle_curve_run(autopilot_get_motors_on(), values)"/>
     <call fun="swashplate_mixing_run(values)"/>
     <set servo="CIC_FRONT" value="-swashplate_mixing.commands[SW_FRONT]"/>
     <set servo="CIC_LEFT" value="-swashplate_mixing.commands[SW_LEFTBACK]"/>

conf/conf_tests.xml

@@ -32,6 +32,17 @@
    settings_modules="modules/nav_basic_fw.xml modules/gps.xml modules/ahrs_float_dcm.xml modules/imu_common.xml modules/guidance_basic_fw.xml modules/stabilization_attitude_fw.xml"
    gui_color="blue"
   />
+  <aircraft
+   name="DelftaCopter"
+   ac_id="48"
+   airframe="airframes/tudelft/outback.xml"
+   radio="radios/dummy.xml"
+   telemetry="telemetry/tudelft/outback.xml"
+   flight_plan="flight_plans/rotorcraft_basic.xml"
+   settings="settings/rotorcraft_basic.xml"
+   settings_modules="modules/heli_throttle_curve.xml modules/gps_ubx_ucenter.xml modules/logger_sd_spi_direct.xml modules/temp_adc.xml modules/air_data.xml modules/geo_mag.xml modules/opa_controller.xml modules/guidance_indi.xml modules/ahrs_int_cmpl_quat.xml modules/stabilization_rate.xml modules/stabilization_int_quat.xml modules/nav_basic_rotorcraft.xml modules/guidance_rotorcraft.xml modules/gps.xml modules/imu_common.xml"
+   gui_color="#ffffdffac31f"
+  />
   <aircraft
    name="Bixler_Mission"
    ac_id="46"
@@ -42,7 +53,7 @@
    settings="settings/fixedwing_basic.xml settings/nps.xml"
    settings_modules="modules/nav_basic_fw.xml modules/gps.xml modules/ahrs_float_dcm.xml modules/imu_common.xml modules/guidance_basic_fw.xml modules/stabilization_attitude_fw.xml"
    gui_color="#8fe1fffff553"
-  />
+   />
   <aircraft
    name="DualBoard_AP_FBW"
    ac_id="3"
@@ -175,6 +186,17 @@
    settings_modules="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="white"
   />
+  <aircraft
+   name="Logo600"
+   ac_id="49"
+   airframe="airframes/tudelft/logo600.xml"
+   radio="radios/dummy.xml"
+   telemetry="telemetry/default_rotorcraft.xml"
+   flight_plan="flight_plans/dummy.xml"
+   settings="settings/rotorcraft_basic.xml"
+   settings_modules="modules/air_data.xml modules/geo_mag.xml modules/heli_throttle_curve.xml modules/guidance_indi.xml modules/ahrs_int_cmpl_quat.xml modules/stabilization_int_quat.xml modules/nav_basic_rotorcraft.xml modules/guidance_rotorcraft.xml modules/gps_ubx_ucenter.xml modules/gps.xml modules/imu_common.xml"
+   gui_color="#ffffdffac31f"
+  />
   <aircraft
    name="MentorEnergy"
    ac_id="13"

conf/modules/heli_swashplate_mixing.xml

@@ -4,7 +4,6 @@
   <doc>
     <description>Helicopter Swashplate Mixing</description>
   </doc>
-  <depends>heli_throttle_curve.xml</depends>
   <header>
     <file name="swashplate_mixing.h"/>
   </header>

conf/modules/heli_throttle_curve.xml

@@ -3,13 +3,28 @@
 <module name="throttle_curve" dir="helicopter">
   <doc>
     <description>Throttle Curve Mixers</description>
+    <define name="THROTTLE_CURVE_RPM_FB_P" value="0" description="The RPM controller feeadback P gain of the PI controller"/>
+    <define name="THROTTLE_CURVE_RPM_FB_I" value="0" description="The RPM controller feeadback I gain of the PI controller"/>
+    <define name="THROTTLE_CURVE_RPM_INC_LIMIT" value="512" description="Amount of RPM the controller can increase or decrease per second"/>
+    <define name="THROTTLE_CURVE_RPM_ID" value="ABI_BROADCAST" description="The ABI sender ID to listen for RPM messages"/>
+    <define name="THROTTLE_CURVE_RPM_ACT" value="0" description="The ABI RPM message motor index"/>
   </doc>
+  <settings>
+    <dl_settings>
+      <dl_settings NAME="ThrottleCurve">
+         <dl_setting var="throttle_curve.rpm_fb_p" min="0.00" step="0.005" max="100" shortname="rpm_fb_p" param="THROTTLE_CURVE_RPM_FB_P" />
+         <dl_setting var="throttle_curve.rpm_fb_i" min="0.00" step="0.005" max="100" shortname="rpm_fb_i" param="THROTTLE_CURVE_RPM_FB_I" />
+         <dl_setting var="throttle_curve.throttle_trim" min="-9600" step="0" max="9600" shortname="throttle_trim" />
+      </dl_settings>
+    </dl_settings>
+  </settings>
   <header>
     <file name="throttle_curve.h"/>
   </header>
   <init fun="throttle_curve_init()"/>
-  <makefile target="ap|fbw">
+  <makefile target="ap">
     <file name="throttle_curve.c"/>
+    <define name="USE_THROTTLE_CURVES" value="TRUE"/>
   </makefile>
 </module>
 

conf/modules/rpm_control.xml

@@ -0,0 +1,16 @@
+<!DOCTYPE module SYSTEM "module.dtd">
+
+<module name="rpm_control" dir="ctrl">
+  <doc>
+    <description>RPM controller with feedback</description>
+  </doc>
+  <header>
+    <file name="rpm_control.h"/>
+  </header>
+  <init fun="rpm_control_init()"/>
+  <periodic fun="rpm_control_periodic()" freq="512" autorun="TRUE"/>
+  <makefile>
+    <file name="rpm_control.c"/>
+  </makefile>
+</module>
+

conf/telemetry/tudelft/outback.xml

@@ -30,6 +30,7 @@
       <message name="TEMP_ADC"               period="3.0"/>
       <message name="FBW_STATUS"             period="2.0"/>
       <message name="MOTOR"                  period="0.5"/>
+      <message name="THROTTLE_CURVE"         period="0.5"/>
     </mode>
 
     <mode name="ppm">

conf/userconf/tudelft/conf.xml

@@ -173,7 +173,7 @@
    telemetry="telemetry/tudelft/outback.xml"
    flight_plan="flight_plans/rotorcraft_basic.xml"
    settings="settings/rotorcraft_basic.xml"
-   settings_modules="modules/gps_ubx_ucenter.xml modules/logger_sd_spi_direct.xml modules/temp_adc.xml modules/air_data.xml modules/geo_mag.xml modules/opa_controller.xml modules/ahrs_int_cmpl_quat.xml modules/stabilization_rate.xml modules/stabilization_int_quat.xml modules/nav_basic_rotorcraft.xml modules/guidance_rotorcraft.xml modules/gps.xml modules/imu_common.xml"
+   settings_modules="modules/heli_throttle_curve.xml modules/gps_ubx_ucenter.xml modules/logger_sd_spi_direct.xml modules/temp_adc.xml modules/air_data.xml modules/geo_mag.xml modules/opa_controller.xml modules/guidance_indi.xml modules/ahrs_int_cmpl_quat.xml modules/stabilization_rate.xml modules/stabilization_int_quat.xml modules/nav_basic_rotorcraft.xml modules/guidance_rotorcraft.xml modules/gps.xml modules/imu_common.xml"
    gui_color="#ffffdffac31f"
    release="dc8a90d35290784bcc817edff08d8da2df64bcef"
   />
@@ -276,7 +276,7 @@
    telemetry="telemetry/default_rotorcraft.xml"
    flight_plan="flight_plans/dummy.xml"
    settings="settings/rotorcraft_basic.xml"
-   settings_modules="modules/air_data.xml modules/geo_mag.xml modules/ahrs_int_cmpl_quat.xml modules/stabilization_int_quat.xml modules/nav_basic_rotorcraft.xml modules/guidance_rotorcraft.xml modules/gps_ubx_ucenter.xml modules/gps.xml modules/imu_common.xml"
+   settings_modules="modules/air_data.xml modules/geo_mag.xml modules/heli_throttle_curve.xml modules/guidance_indi.xml modules/ahrs_int_cmpl_quat.xml modules/stabilization_int_quat.xml modules/nav_basic_rotorcraft.xml modules/guidance_rotorcraft.xml modules/gps_ubx_ucenter.xml modules/gps.xml modules/imu_common.xml"
    gui_color="#ffffdffac31f"
    release="0fba1ce80a46d8dce23c570e6547595c89988fad"
   />

sw/airborne/firmwares/rotorcraft/main_ap.c

@@ -247,6 +247,11 @@ void main_periodic(void)
   autopilot_periodic();
   /* set actuators     */
   //actuators_set(autopilot_get_motors_on());
+
+#if USE_THROTTLE_CURVES
+  throttle_curve_run(commands, autopilot_get_mode());
+#endif
+
 #ifndef INTER_MCU_AP
   SetActuatorsFromCommands(commands, autopilot_get_mode());
 #else

sw/airborne/modules/helicopter/throttle_curve.c

@@ -26,59 +26,179 @@
 
 #include "throttle_curve.h"
 #include "subsystems/commands.h"
+#include "autopilot.h"
+#include "subsystems/radio_control.h"
+#include "subsystems/abi.h"
 
 /* The switching values for the Throttle Curve Mode switch */
 #define THROTTLE_CURVE_SWITCH_VAL (MAX_PPRZ*2/THROTTLE_CURVES_NB)
 
+/* Default RPM feedback gains and limits */
+#ifndef THROTTLE_CURVE_RPM_FB_P
+#define THROTTLE_CURVE_RPM_FB_P 0.0
+#endif
+
+#ifndef THROTTLE_CURVE_RPM_FB_I
+#define THROTTLE_CURVE_RPM_FB_I 0.0
+#endif
+
+#ifndef THROTTLE_CURVE_RPM_INC_LIMIT
+#define THROTTLE_CURVE_RPM_INC_LIMIT 512
+#endif
+
+/* Register the RPM callback */
+#ifndef THROTTLE_CURVE_RPM_ID
+#define THROTTLE_CURVE_RPM_ID ABI_BROADCAST
+#endif
+#ifndef THROTTLE_CURVE_RPM_ACT
+#define THROTTLE_CURVE_RPM_ACT 0
+#endif
+static abi_event rpm_ev;
+static void rpm_cb(uint8_t sender_id, uint16_t *rpm, uint8_t num_act);
+
 /* Initialize the throttle curves from the airframe file */
 struct throttle_curve_t throttle_curve = {
   .nb_curves = THROTTLE_CURVES_NB,
   .curves = THROTTLE_CURVES
 };
 
+#if PERIODIC_TELEMETRY
+#include "subsystems/datalink/telemetry.h"
+
+static void throttle_curve_send_telem(struct transport_tx *trans, struct link_device *dev)
+{
+  pprz_msg_send_THROTTLE_CURVE(trans, dev, AC_ID, &throttle_curve.mode, &throttle_curve.throttle,
+                               &throttle_curve.collective,
+                               &throttle_curve.rpm, &throttle_curve.rpm_meas, &throttle_curve.rpm_err_sum);
+}
+#endif
+
 /**
  * Initialize the default throttle curve values
  */
 void throttle_curve_init(void)
 {
   throttle_curve.mode       = THROTTLE_CURVE_MODE_INIT;
+  throttle_curve.nav_mode   = THROTTLE_CURVE_MODE_INIT;
   throttle_curve.throttle   = throttle_curve.curves[THROTTLE_CURVE_MODE_INIT].throttle[0];
   throttle_curve.collective = throttle_curve.curves[THROTTLE_CURVE_MODE_INIT].collective[0];
+  throttle_curve.rpm_fb_p = THROTTLE_CURVE_RPM_FB_P;
+  throttle_curve.rpm_fb_i = THROTTLE_CURVE_RPM_FB_I;
+  throttle_curve.rpm_err_sum = 0;
+  throttle_curve.rpm_measured = false;
+  throttle_curve.throttle_trim = 0;
+  throttle_curve.coll_trim = 0;
+
+  AbiBindMsgRPM(THROTTLE_CURVE_RPM_ID, &rpm_ev, rpm_cb);
+
+#if PERIODIC_TELEMETRY
+  register_periodic_telemetry(DefaultPeriodic, PPRZ_MSG_ID_THROTTLE_CURVE, throttle_curve_send_telem);
+#endif
+}
+
+/**
+ * RPM callback for RPM based control throttle curves
+ */
+static void rpm_cb(uint8_t __attribute__((unused)) sender_id, uint16_t *rpm, uint8_t num_act)
+{
+  if(num_act <= THROTTLE_CURVE_RPM_ACT)
+    return;
+    
+  throttle_curve.rpm_meas = rpm[THROTTLE_CURVE_RPM_ACT];
+  throttle_curve.rpm_measured = true;
 }
 
 /**
  * Run the throttle curve and generate the output throttle and pitch
  * This depends on the FMODE(flight mode) and TRHUST command
  */
-void throttle_curve_run(bool motors_on, pprz_t in_cmd[])
+void throttle_curve_run(pprz_t cmds[], uint8_t ap_mode)
 {
   // Calculate the mode value from the switch
-  int8_t mode = ((float)(in_cmd[COMMAND_FMODE] + MAX_PPRZ) / THROTTLE_CURVE_SWITCH_VAL);
-  Bound(mode, 0, THROTTLE_CURVES_NB - 1);
-  throttle_curve.mode = mode;
+  if (ap_mode != AP_MODE_NAV) {
+    int8_t mode = ((float)(radio_control.values[RADIO_FMODE] + MAX_PPRZ) / THROTTLE_CURVE_SWITCH_VAL);
+    Bound(mode, 0, THROTTLE_CURVES_NB - 1);
+    throttle_curve.mode = mode;
+  } else {
+    throttle_curve.mode = throttle_curve.nav_mode;
+  }
+
+  // Failsafe curve
+  if (ap_mode == AP_MODE_FAILSAFE) {
+    throttle_curve.mode = 0;
+  }
 
   // Check if we have multiple points or a single point
-  struct curve_t curve = throttle_curve.curves[mode];
+  struct curve_t curve = throttle_curve.curves[throttle_curve.mode];
   if (curve.nb_points == 1) {
     throttle_curve.throttle = curve.throttle[0];
     throttle_curve.collective = curve.collective[0];
+    throttle_curve.rpm = curve.rpm[0];
   } else {
     // Calculate the left point on the curve we need to use
     uint16_t curve_range = (MAX_PPRZ / (curve.nb_points - 1));
-    int8_t curve_p = ((float)in_cmd[COMMAND_THRUST] / curve_range);
+    int8_t curve_p = ((float)cmds[COMMAND_THRUST] / curve_range);
     Bound(curve_p, 0, curve.nb_points - 1);
 
-    // Calculate the throttle and pitch value
-    uint16_t x = in_cmd[COMMAND_THRUST] - curve_p * curve_range;
+    // Calculate the throttle, pitch and rpm value
+    uint16_t x = cmds[COMMAND_THRUST] - curve_p * curve_range;
     throttle_curve.throttle = curve.throttle[curve_p]
                               + ((curve.throttle[curve_p + 1] - curve.throttle[curve_p]) * x / curve_range);
     throttle_curve.collective = curve.collective[curve_p]
                                 + ((curve.collective[curve_p + 1] - curve.collective[curve_p]) * x / curve_range);
+    if (curve.rpm[0] != 0xFFFF) {
+      if (throttle_curve.rpm == 0xFFFF) {
+        throttle_curve.rpm = throttle_curve.rpm_meas;
+      }
+      uint16_t new_rpm = curve.rpm[curve_p]
+                         + ((curve.rpm[curve_p + 1] - curve.rpm[curve_p]) * x / curve_range);
+      int32_t rpm_diff = new_rpm - throttle_curve.rpm;
+      Bound(rpm_diff, -(THROTTLE_CURVE_RPM_INC_LIMIT/512), THROTTLE_CURVE_RPM_INC_LIMIT/512);
+      throttle_curve.rpm += rpm_diff;
+    } else {
+      throttle_curve.rpm = 0xFFFF;
+    }
   }
 
+  // Trim
+  int32_t trimmed_throttle = throttle_curve.throttle + throttle_curve.throttle_trim;
+  Bound(trimmed_throttle, 0, MAX_PPRZ);
+  throttle_curve.throttle = trimmed_throttle;
+
+  int32_t trimmed_collective = throttle_curve.collective + throttle_curve.coll_trim;
+  Bound(trimmed_collective, -MAX_PPRZ, MAX_PPRZ);
+  throttle_curve.collective = trimmed_collective;
+
+  // Update RPM feedback
+  if (curve.rpm[0] != 0xFFFF && throttle_curve.rpm_measured) {
+    // Calculate RPM error
+    int32_t rpm_err = (throttle_curve.rpm - throttle_curve.rpm_meas);
+
+    // Calculate integrated error
+    throttle_curve.rpm_err_sum += rpm_err * throttle_curve.rpm_fb_i / 512.0f;
+    Bound(throttle_curve.rpm_err_sum, -throttle_curve.throttle, (MAX_PPRZ - throttle_curve.throttle));
+
+    // Calculate feedback command
+    int32_t rpm_feedback = rpm_err * throttle_curve.rpm_fb_p + throttle_curve.rpm_err_sum;
+    Bound(rpm_feedback, -MAX_PPRZ, MAX_PPRZ);
+
+    // Apply feedback command
+    int32_t new_throttle = throttle_curve.throttle + rpm_feedback;
+    Bound(new_throttle, 0, MAX_PPRZ);
+    throttle_curve.throttle = new_throttle;
+    throttle_curve.rpm_measured = false;
+  } else if (curve.rpm[0] == 0xFFFF) {
+    throttle_curve.rpm_err_sum = 0;
+  }
+
+  // Set the commands
+  cmds[COMMAND_THRUST] = throttle_curve.throttle; //Reuse for now
+  cmds[COMMAND_COLLECTIVE] = throttle_curve.collective;
+
   // Only set throttle if motors are on
-  if (!motors_on) {
-    throttle_curve.throttle = 0;
+  if (!autopilot_get_motors_on()) {
+    cmds[COMMAND_THRUST] = 0;
+    throttle_curve.rpm_err_sum = 0;
   }
 }
 
@@ -87,5 +207,7 @@ void throttle_curve_run(bool motors_on, pprz_t in_cmd[])
  */
 void nav_throttle_curve_set(uint8_t mode)
 {
-  commands[COMMAND_FMODE] = mode * THROTTLE_CURVE_SWITCH_VAL - MAX_PPRZ;
+  int16_t new_mode = mode;
+  Bound(new_mode, 0, THROTTLE_CURVES_NB - 1);
+  throttle_curve.nav_mode = new_mode;
 }

sw/airborne/modules/helicopter/throttle_curve.h

@@ -35,23 +35,35 @@
 struct curve_t {
   uint8_t nb_points;                          ///< The number of points in the curve
   uint16_t throttle[THROTTLE_POINTS_NB];      ///< Throttle points in the curve
+  uint16_t rpm[THROTTLE_POINTS_NB];           ///< RPM points in the curve
   int16_t collective[THROTTLE_POINTS_NB];     ///< The collective points in the curve
 };
 
 /* Main throttle curve structure */
 struct throttle_curve_t {
   uint8_t mode;                               ///< Flight mode
+  uint8_t nav_mode;                               ///< Nav Flight mode
   uint8_t nb_curves;                          ///< The number of throttle/pitch curves
   struct curve_t curves[THROTTLE_CURVES_NB];  ///< Throttle/pitch curves
 
   uint16_t throttle;                          ///< Output thrust(throttle) of the throttle curve
   int16_t collective;                         ///< Output collective of the throttle curve
+  uint16_t rpm;                               ///< Output RPM of the throttle curve
+
+  uint16_t rpm_meas;                          ///< RPM measured
+  bool rpm_measured;                          ///< Whenever the RPM is measured
+  float rpm_err_sum;                          ///< Summed RPM error
+  float rpm_fb_p;                             ///< RPM feedback p gain
+  float rpm_fb_i;
+
+  int32_t throttle_trim;                             ///< RPM feedback i gain
+  int32_t coll_trim;                          ///< Collective trim
 };
 extern struct throttle_curve_t throttle_curve;
 
 /* External functions */
 extern void throttle_curve_init(void);
-void throttle_curve_run(bool motors_on, pprz_t in_cmd[]);
+void throttle_curve_run(pprz_t in_cmd[], uint8_t autopilot_mode);
 void nav_throttle_curve_set(uint8_t mode);
 
 #endif

sw/tools/generators/gen_airframe.ml

@@ -289,14 +289,28 @@ let parse_command = fun command no ->
   let failsafe_value = int_of_string (ExtXml.attrib command "failsafe_value") in
   { failsafe_value = failsafe_value; foo = 0}
 
-let parse_heli_curves = fun heli_surves ->
-  let a = fun s -> ExtXml.attrib heli_surves s in
-  match Xml.tag heli_surves with
+let parse_heli_curves = fun heli_curves ->
+  let a = fun s -> ExtXml.attrib heli_curves s in
+  match Xml.tag heli_curves with
       "curve" ->
         let throttle = a "throttle" in
+        let rpm = ExtXml.attrib_or_default heli_curves "rpm" "" in
         let collective = a "collective" in
-        printf "  {.nb_points = %i, \\\n" (List.length (Str.split (Str.regexp ",") throttle));
+        let nb_throttle = List.length (Str.split (Str.regexp ",") throttle) in
+        let nb_rpm = List.length (Str.split (Str.regexp ",") rpm) in
+        let nb_collective = List.length (Str.split (Str.regexp ",") collective) in
+        if nb_throttle < 1 then
+          failwith (Printf.sprintf "Need at least one value in throttle curve for a throttle ('%s', '%s')" throttle collective);
+        if nb_throttle <> nb_collective then
+          failwith (Printf.sprintf "Amount of throttle points not the same as collective in throttle curve ('%s', '%s')" throttle collective);
+        if nb_throttle <> nb_rpm && nb_rpm <> 0 then
+          failwith (Printf.sprintf "Amount of throttle points not the same as rpm in throttle curve ('%s', '%s', '%s')" throttle collective rpm);
+        printf "  {.nb_points = %i, \\\n" nb_throttle;
         printf "   .throttle = {%s}, \\\n" throttle;
+        if nb_rpm <> 0 then
+          printf "   .rpm = {%s}, \\\n" rpm
+        else
+          printf "   .rpm = {0xFFFF}, \\\n";
         printf "   .collective = {%s}}, \\\n" collective
     | _ -> xml_error "mixer"