AP_ICEngine: Add redline RPM warning message and governor

ArduPlane/servos.cpp

@@ -911,9 +911,9 @@ void Plane::set_servos(void)
         }
     }
 
-    uint8_t override_pct;
+    float override_pct = SRV_Channels::get_output_scaled(SRV_Channel::k_throttle);
     if (g2.ice_control.throttle_override(override_pct)) {
-        // the ICE controller wants to override the throttle for starting
+        // the ICE controller wants to override the throttle for starting, idle, or redline
         SRV_Channels::set_output_scaled(SRV_Channel::k_throttle, override_pct);
     }
 

libraries/AP_ICEngine/AP_ICEngine.cpp

@@ -20,7 +20,7 @@
 #include <AP_Scheduler/AP_Scheduler.h>
 #include <AP_Notify/AP_Notify.h>
 #include <RC_Channel/RC_Channel.h>
-
+#include <Filter/LowPassFilter.h>
 #include "AP_ICEngine.h"
 
 extern const AP_HAL::HAL& hal;
@@ -134,7 +134,7 @@ const AP_Param::GroupInfo AP_ICEngine::var_info[] = {
     // @Param: OPTIONS
     // @DisplayName: ICE options
     // @Description: Options for ICE control
-    // @Bitmask: 0:DisableIgnitionRCFailsafe
+    // @Bitmask: 0:DisableIgnitionRCFailsafe,1:DisableRedlineRPMGovernor
     AP_GROUPINFO("OPTIONS", 15, AP_ICEngine, options, 0),
 
     // @Param: STARTCHN_MIN
@@ -144,6 +144,14 @@ const AP_Param::GroupInfo AP_ICEngine::var_info[] = {
     // @Range: 0 1300
     AP_GROUPINFO("STARTCHN_MIN", 16, AP_ICEngine, start_chan_min_pwm, 0),
 
+    // @Param: REDLINE_RPM
+    // @DisplayName: RPM of the redline limit for the engine
+    // @Description: Maximum RPM for the engine provided by the manufacturer. A value of 0 disables this feature. See ICE_OPTIONS to enable or disable the governor.
+    // @User: Advanced
+    // @Range: 0 2000000
+    // @Units: RPM
+    AP_GROUPINFO("REDLINE_RPM", 17, AP_ICEngine, redline_rpm, 0),
+
     AP_GROUPEND
 };
 
@@ -158,6 +166,8 @@ AP_ICEngine::AP_ICEngine(const AP_RPM &_rpm) :
         AP_HAL::panic("AP_ICEngine must be singleton");
     }
     _singleton = this;
+
+    _rpm_filter.set_cutoff_frequency(1 / AP::scheduler().get_loop_period_s(), 0.5f);
 }
 
 /*
@@ -218,6 +228,8 @@ void AP_ICEngine::update(void)
         should_run = false;
     }
 
+    float rpm_value;
+
     // switch on current state to work out new state
     switch (state) {
     case ICE_OFF:
@@ -266,7 +278,6 @@ void AP_ICEngine::update(void)
             gcs().send_text(MAV_SEVERITY_INFO, "Stopped engine");
         } else if (rpm_instance > 0) {
             // check RPM to see if still running
-            float rpm_value;
             if (!rpm.get_rpm(rpm_instance-1, rpm_value) ||
                 rpm_value < rpm_threshold) {
                 // engine has stopped when it should be running
@@ -290,6 +301,25 @@ void AP_ICEngine::update(void)
         }
     }
 
+    // check against redline RPM
+    if (rpm_instance > 0 && redline_rpm > 0 && rpm.get_rpm(rpm_instance-1, rpm_value)) {
+        // update the filtered RPM value
+        filtered_rpm_value =  _rpm_filter.apply(rpm_value);
+        if (!redline.flag && filtered_rpm_value > redline_rpm) {
+            // redline governor is off. rpm is too high. enable the governor
+            gcs().send_text(MAV_SEVERITY_INFO, "Engine: Above redline RPM");
+            redline.flag = true;
+        } else if (redline.flag && filtered_rpm_value < redline_rpm * 0.9f) {
+            // redline governor is on. rpm is safely below. disable the governor
+            redline.flag = false;
+            // reset redline governor
+            redline.throttle_percentage = 0.0f;
+            redline.governor_integrator = 0.0f;
+        }
+    } else {
+        redline.flag = false;
+    }
+
     /* now set output channels */
     switch (state) {
     case ICE_OFF:
@@ -326,7 +356,7 @@ void AP_ICEngine::update(void)
   check for throttle override. This allows the ICE controller to force
   the correct starting throttle when starting the engine and maintain idle when disarmed
  */
-bool AP_ICEngine::throttle_override(uint8_t &percentage)
+bool AP_ICEngine::throttle_override(float &percentage)
 {
     if (!enable) {
         return false;
@@ -335,14 +365,38 @@ bool AP_ICEngine::throttle_override(uint8_t &percentage)
     if (state == ICE_RUNNING &&
         idle_percent > 0 &&
         idle_percent < 100 &&
-        (int16_t)idle_percent > SRV_Channels::get_output_scaled(SRV_Channel::k_throttle))
+        idle_percent > percentage)
     {
-        percentage = (uint8_t)idle_percent;
+        percentage = idle_percent;
         return true;
     }
 
     if (state == ICE_STARTING || state == ICE_START_DELAY) {
-        percentage = (uint8_t)start_percent.get();
+        percentage = start_percent.get();
+        return true;
+    }
+
+    if (redline.flag && !(options & uint16_t(Options::DISABLE_REDLINE_GOVERNOR))) {
+        // limit the throttle from increasing above what the current output is
+        if (redline.throttle_percentage < 1.0f) {
+            redline.throttle_percentage = percentage;
+        }
+        if (percentage < redline.throttle_percentage - redline.governor_integrator) {
+            // the throttle before the override is much lower than what the integrator is at
+            // reset the integrator
+            redline.governor_integrator = 0;
+            redline.throttle_percentage = percentage;
+        } else if (percentage < redline.throttle_percentage) {
+            // the throttle is below the integrator set point
+            // remove the difference from the integrator
+            redline.governor_integrator -= redline.throttle_percentage - percentage;
+            redline.throttle_percentage = percentage;
+        } else if (filtered_rpm_value > redline_rpm) {
+            // reduce the throttle if still over the redline RPM
+            const float redline_setpoint_step = idle_slew * AP::scheduler().get_loop_period_s();
+            redline.governor_integrator += redline_setpoint_step;
+        }
+        percentage = redline.throttle_percentage - redline.governor_integrator;
         return true;
     }
     return false;

libraries/AP_ICEngine/AP_ICEngine.h

@@ -32,7 +32,7 @@ class AP_ICEngine {
     void update(void);
 
     // check for throttle override
-    bool throttle_override(uint8_t &percent);
+    bool throttle_override(float &percent);
 
     enum ICE_State {
         ICE_OFF=0,
@@ -60,6 +60,10 @@ class AP_ICEngine {
 
     enum ICE_State state;
 
+    // filter for RPM value
+    LowPassFilterFloat _rpm_filter;
+    float filtered_rpm_value;
+
     // enable library
     AP_Int8 enable;
 
@@ -122,12 +126,21 @@ class AP_ICEngine {
 
     enum class Options : uint16_t {
         DISABLE_IGNITION_RC_FAILSAFE=(1U<<0),
+        DISABLE_REDLINE_GOVERNOR = (1U << 1),
     };
     AP_Int16 options;
 
     // start_chan debounce
     uint16_t start_chan_last_value = 1500;
     uint32_t start_chan_last_ms;
+
+    // redline rpm
+    AP_Int32 redline_rpm;
+    struct {
+        bool flag;
+        float governor_integrator;
+        float throttle_percentage;
+    } redline;
 };