[rpm-control] throttle curves with rpm control

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>
+

sw/airborne/modules/helicopter/throttle_curve.c

@@ -26,59 +26,177 @@
 
 #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 */
+#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
+
+/* Register the RPM callback */
+#ifndef THROTTLE_CURVE_RPM_ID
+#define THROTTLE_CURVE_RPM_ID ABI_BROADCAST
+#endif
+static abi_event rpm_ev;
+static void rpm_cb(uint8_t sender_id, uint16_t rpm);
+
 /* 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)
+{
+  throttle_curve.rpm_meas = rpm;
+  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, -1, 1);
+      throttle_curve.rpm += rpm_diff;
+    }
+    else
+      throttle_curve.rpm = 0xFFFF;
+  }
+
+  // Trim in curve 3 hack
+  if(throttle_curve.mode == 2) {
+    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_motors_on) {
+    cmds[COMMAND_THRUST] = 0;
+    throttle_curve.rpm_err_sum = 0;
+  }
+
+  // disable the tip propellers when in curve 2
+  if (throttle_curve.mode == 2) {
+    INTERMCU_SET_CMD_STATUS(INTERMCU_CMD_TIPPROPS);
+  } else {
+    INTERMCU_CLR_CMD_STATUS(INTERMCU_CMD_TIPPROPS);
   }
 }
 
@@ -87,5 +205,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/airborne/modules/nav/nav_heli_spinup.c

@@ -44,7 +44,7 @@ void nav_heli_spinup_setup(uint16_t duration, float throttle)
   nav_throttle = 0;
   nav_cmd_roll = 0;
   nav_cmd_pitch = 0;
-  nav_cmd_yaw= 0;
+  nav_cmd_yaw = 0;
   horizontal_mode = HORIZONTAL_MODE_MANUAL;
   vertical_mode = VERTICAL_MODE_MANUAL;
 }
@@ -61,7 +61,7 @@ bool nav_heli_spinup_run(void)
 
   nav_cmd_roll = 0;
   nav_cmd_pitch = 0;
-  nav_cmd_yaw= 0;
+  nav_cmd_yaw = 0;
   horizontal_mode = HORIZONTAL_MODE_MANUAL;
   vertical_mode = VERTICAL_MODE_MANUAL;
   nav_throttle = stage_time * nav_heli_spinup.throttle / nav_heli_spinup.duration;