Plane: allow Battery Watt limiter to bring throttle to 0%

ArduPlane/servos.cpp

@@ -424,39 +424,48 @@ void Plane::throttle_voltage_comp(int8_t &min_throttle, int8_t &max_throttle) co
  */
 void Plane::throttle_watt_limiter(int8_t &min_throttle, int8_t &max_throttle)
 {
-    uint32_t now = millis();
+    const uint32_t now_ms = millis();
+    const float throttle_demand = SRV_Channels::get_output_scaled(SRV_Channel::k_throttle);
     if (battery.overpower_detected()) {
         // overpower detected, cut back on the throttle if we're maxing it out by calculating a limiter value
-        // throttle limit will attack by 10% per second
+        // throttle limit will attack by 50% per second
 
-        if (is_positive(SRV_Channels::get_output_scaled(SRV_Channel::k_throttle)) && // demanding too much positive thrust
-            throttle_watt_limit_max < max_throttle - 25 &&
-            now - throttle_watt_limit_timer_ms >= 1) {
-            // always allow for 25% throttle available regardless of battery status
-            throttle_watt_limit_timer_ms = now;
+        if (is_positive(throttle_demand) && throttle_watt_limit_max < max_throttle) {
+            // demanding too much positive thrust
+            throttle_watt_limit_timer_ms = now_ms;
             throttle_watt_limit_max++;
 
-        } else if (is_negative(SRV_Channels::get_output_scaled(SRV_Channel::k_throttle)) &&
-                   min_throttle < 0 && // reverse thrust is available
-                   throttle_watt_limit_min < -(min_throttle) - 25 &&
-                   now - throttle_watt_limit_timer_ms >= 1) {
-            // always allow for 25% throttle available regardless of battery status
-            throttle_watt_limit_timer_ms = now;
+        } else if (is_negative(throttle_demand) && min_throttle < 0 && throttle_watt_limit_min < -(min_throttle)) {
+            // demanding too much negative thrust
+            throttle_watt_limit_timer_ms = now_ms;
             throttle_watt_limit_min++;
         }
 
-    } else if (now - throttle_watt_limit_timer_ms >= 1000) {
-        // it has been 1 second since last over-current, check if we can resume higher throttle.
+    } else if (throttle_watt_limit_min == 0 && throttle_watt_limit_max == 0) {
+        // no limiting happening, nothing to do
+        return;
+    }
+
+    // check to recover the throttle limit since we're not overloading
+    const int8_t most_limited_of_min_and_max = MAX(throttle_watt_limit_min, throttle_watt_limit_max);
+    const int8_t most_throttle_of_min_and_max = MAX(-min_throttle, max_throttle);
+
+    // if we're limiting all the way (min or max == 0) then the delay is larger
+    const uint32_t recovery_delay_ms = most_limited_of_min_and_max >= most_throttle_of_min_and_max ? 5000 : 1000;
+
+    if (now_ms - throttle_watt_limit_timer_ms >= recovery_delay_ms) {
+        // it has been 1 second since last over-current, or 5s if throttle is pushed all the way down to 0%, check if we can resume higher throttle.
         // this throttle release is needed to allow raising the max_throttle as the battery voltage drains down
         // throttle limit will release by 1% per second
-        if (SRV_Channels::get_output_scaled(SRV_Channel::k_throttle) > throttle_watt_limit_max && // demanding max forward thrust
-            throttle_watt_limit_max > 0) { // and we're currently limiting it
-            throttle_watt_limit_timer_ms = now;
+
+        if (throttle_demand > throttle_watt_limit_max && throttle_watt_limit_max > 0) {
+            // we're not overloaded and demanding forward thrust beyond our currently limited max
+            throttle_watt_limit_timer_ms = now_ms;
             throttle_watt_limit_max--;
 
-        } else if (SRV_Channels::get_output_scaled(SRV_Channel::k_throttle) < throttle_watt_limit_min && // demanding max negative thrust
-                   throttle_watt_limit_min > 0) { // and we're limiting it
-            throttle_watt_limit_timer_ms = now;
+        } else if (throttle_demand < -throttle_watt_limit_min && throttle_watt_limit_min > 0) {
+            // we're not overloaded and demanding reverse thrust beyond our currently limited min
+            throttle_watt_limit_timer_ms = now_ms;
             throttle_watt_limit_min--;
         }
     }