From ed6aa4ed17e6d2d8386964375c7224d40467bd23 Mon Sep 17 00:00:00 2001
From: Tom Pittenger <magicrub@gmail.com>
Date: Tue, 5 Jan 2016 15:34:21 -0800
Subject: [PATCH] Plane: added throttle limiting via max power
 (current*voltage)

---
 ArduPlane/Attitude.cpp | 43 ++++++++++++++++++++++++++++++++++++++++++
 ArduPlane/Plane.h      |  5 +++++
 2 files changed, 48 insertions(+)

diff --git a/ArduPlane/Attitude.cpp b/ArduPlane/Attitude.cpp
index a48fba41a1..6d4332c891 100644
--- a/ArduPlane/Attitude.cpp
+++ b/ArduPlane/Attitude.cpp
@@ -917,6 +917,49 @@ void Plane::set_servos(void)
                 }
             }
         }
+
+        uint32_t now = millis();
+        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
+
+            if (channel_throttle->servo_out > 0 && // 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;
+                throttle_watt_limit_max++;
+
+            } else if (channel_throttle->servo_out < 0 &&
+                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;
+                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.
+            // 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 (channel_throttle->servo_out > 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;
+                throttle_watt_limit_max--;
+
+            } else if (channel_throttle->servo_out < throttle_watt_limit_min && // demanding max negative thrust
+                throttle_watt_limit_min > 0) { // and we're limiting it
+                throttle_watt_limit_timer_ms = now;
+                throttle_watt_limit_min--;
+            }
+        }
+
+        max_throttle = constrain_int16(max_throttle, 0, max_throttle - throttle_watt_limit_max);
+        if (min_throttle < 0) {
+            min_throttle = constrain_int16(min_throttle, min_throttle + throttle_watt_limit_min, 0);
+        }
+
         channel_throttle->servo_out = constrain_int16(channel_throttle->servo_out, 
                                                       min_throttle,
                                                       max_throttle);
diff --git a/ArduPlane/Plane.h b/ArduPlane/Plane.h
index 402a47e870..a0193b8bfa 100644
--- a/ArduPlane/Plane.h
+++ b/ArduPlane/Plane.h
@@ -527,6 +527,11 @@ private:
     // this controls throttle suppression in auto modes
     bool throttle_suppressed;
 
+    // reduce throttle to eliminate battery over-current
+    int8_t  throttle_watt_limit_max;
+    int8_t  throttle_watt_limit_min; // for reverse thrust
+    uint32_t throttle_watt_limit_timer_ms;
+
     AP_SpdHgtControl::FlightStage flight_stage = AP_SpdHgtControl::FLIGHT_NORMAL;
 
     // probability of aircraft is currently in flight. range from 0 to