AP_ICEngine: Add redline governor and RPM filter

Adds a top end governor to protect the engine from exceeding design limits. Similar to idle governor.
Adds a LPF at 0.5 Hz for RPM.

ArduPlane/servos.cpp

@@ -913,7 +913,7 @@ void Plane::set_servos(void)
 
     uint8_t override_pct;
     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"
 #include <AP_RPM/AP_RPM.h>
 
@@ -135,7 +135,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
@@ -145,6 +145,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
 };
 
@@ -159,6 +167,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);
 }
 
 /*
@@ -219,6 +229,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:
@@ -267,7 +279,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
@@ -291,6 +302,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:
@@ -346,6 +376,31 @@ bool AP_ICEngine::throttle_override(uint8_t &percentage)
         percentage = (uint8_t)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
+        const float incoming_throttle = SRV_Channels::get_output_scaled(SRV_Channel::k_throttle);
+        if (redline.throttle_percentage < 1.0f) {
+            redline.throttle_percentage = incoming_throttle;
+        }
+        if (incoming_throttle < 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 = incoming_throttle;
+        } else if (incoming_throttle < redline.throttle_percentage) {
+            // the throttle is below the integrator set point
+            // remove the difference from the integrator
+            redline.governor_integrator -= redline.throttle_percentage - incoming_throttle;
+            redline.throttle_percentage = incoming_throttle;
+        } 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

@@ -59,6 +59,10 @@ private:
 
     enum ICE_State state;
 
+    // filter for RPM value
+    LowPassFilterFloat _rpm_filter;
+    float filtered_rpm_value;
+
     // enable library
     AP_Int8 enable;
 
@@ -121,12 +125,21 @@ private:
 
     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;
 };