From 3ded030fa82710d52e37f0828ecd4d95171749a9 Mon Sep 17 00:00:00 2001
From: Iampete1 <iampete@hotmail.co.uk>
Date: Fri, 24 Apr 2020 20:20:14 +0100
Subject: [PATCH] Plane: compensate forward throttle for battery voltage drop

---
 ArduPlane/Parameters.cpp | 23 +++++++++++++++++++++++
 ArduPlane/Parameters.h   |  5 +++++
 ArduPlane/Plane.h        |  1 +
 ArduPlane/servos.cpp     | 34 ++++++++++++++++++++++++++++++++--
 4 files changed, 61 insertions(+), 2 deletions(-)

diff --git a/ArduPlane/Parameters.cpp b/ArduPlane/Parameters.cpp
index 1893f0d65d..da0ee25bdc 100644
--- a/ArduPlane/Parameters.cpp
+++ b/ArduPlane/Parameters.cpp
@@ -1248,6 +1248,29 @@ const AP_Param::GroupInfo ParametersG2::var_info[] = {
     AP_SUBGROUPINFO(efi, "EFI", 22, ParametersG2, AP_EFI),
 #endif
 
+    // @Param: FWD_BAT_VOLT_MAX
+    // @DisplayName: Forward throttle battery voltage compensation maximum voltage
+    // @Description: Forward throttle battery voltage compensation maximum voltage (voltage above this will have no additional scaling effect on thrust).  Recommend 4.4 * cell count, 0 = Disabled
+    // @Range: 6 35
+    // @Units: V
+    // @User: Advanced
+    AP_GROUPINFO("FWD_BAT_VOLT_MAX", 23, ParametersG2, fwd_thr_batt_voltage_max, 0.0f),
+
+    // @Param: FWD_BAT_VOLT_MIN
+    // @DisplayName: Forward throttle battery voltage compensation minimum voltage
+    // @Description: Forward throttle battery voltage compensation minimum voltage (voltage below this will have no additional scaling effect on thrust).  Recommend 3.5 * cell count, 0 = Disabled
+    // @Range: 6 35
+    // @Units: V
+    // @User: Advanced
+    AP_GROUPINFO("FWD_BAT_VOLT_MIN", 24, ParametersG2, fwd_thr_batt_voltage_min, 0.0f),
+
+    // @Param: FWD_BAT_IDX
+    // @DisplayName: Forward throttle battery compensation index
+    // @Description: Which battery monitor should be used for doing compensation for the forward throttle
+    // @Values: 0:First battery, 1:Second battery
+    // @User: Advanced
+    AP_GROUPINFO("FWD_BAT_IDX", 25, ParametersG2, fwd_thr_batt_idx, 0),
+
     AP_GROUPEND
 };
 
diff --git a/ArduPlane/Parameters.h b/ArduPlane/Parameters.h
index 25daae421f..eabace77fb 100644
--- a/ArduPlane/Parameters.h
+++ b/ArduPlane/Parameters.h
@@ -566,6 +566,11 @@ public:
     AP_Int8 crow_flap_options;
     AP_Int8 crow_flap_aileron_matching;
 
+    // Forward throttle battery voltage compenstaion
+    AP_Float fwd_thr_batt_voltage_max;
+    AP_Float fwd_thr_batt_voltage_min;
+    AP_Int8  fwd_thr_batt_idx;
+
 #if EFI_ENABLED
     // EFI Engine Monitor
     AP_EFI efi;
diff --git a/ArduPlane/Plane.h b/ArduPlane/Plane.h
index 0e0a561c3a..189a129228 100644
--- a/ArduPlane/Plane.h
+++ b/ArduPlane/Plane.h
@@ -938,6 +938,7 @@ private:
     void servos_output(void);
     void servos_auto_trim(void);
     void servos_twin_engine_mix();
+    void throttle_voltage_comp();
     void throttle_watt_limiter(int8_t &min_throttle, int8_t &max_throttle);
     void update_is_flying_5Hz(void);
     void crash_detection_update(void);
diff --git a/ArduPlane/servos.cpp b/ArduPlane/servos.cpp
index d5b51a88fa..f20135d9ef 100644
--- a/ArduPlane/servos.cpp
+++ b/ArduPlane/servos.cpp
@@ -342,6 +342,33 @@ void Plane::set_servos_manual_passthrough(void)
     }
 }
 
+/*
+  Scale the throttle to conpensate for battery voltage drop
+ */
+void Plane::throttle_voltage_comp()
+{
+    // return if not enabled, or setup incorrectly
+    if (g2.fwd_thr_batt_voltage_min >= g2.fwd_thr_batt_voltage_max || !is_positive(g2.fwd_thr_batt_voltage_max)) {
+        return;
+    }
+
+    float batt_voltage_resting_estimate = AP::battery().voltage_resting_estimate(g2.fwd_thr_batt_idx);
+    // Return for a very low battery
+    if (batt_voltage_resting_estimate < 0.25f * g2.fwd_thr_batt_voltage_min) {
+        return;
+    }
+
+    // constrain read voltage to min and max params
+    batt_voltage_resting_estimate = constrain_float(batt_voltage_resting_estimate,g2.fwd_thr_batt_voltage_min,g2.fwd_thr_batt_voltage_max);
+
+    // Scale the throttle up to compensate for voltage drop
+    // Ratio = 1 when voltage = voltage max, ratio increases as voltage drops
+    const float ratio = g2.fwd_thr_batt_voltage_max / batt_voltage_resting_estimate;
+
+    SRV_Channels::set_output_scaled(SRV_Channel::k_throttle,
+                                        constrain_int16(SRV_Channels::get_output_scaled(SRV_Channel::k_throttle) * ratio, -100, 100));
+}
+
 /*
   calculate any throttle limits based on the watt limiter
  */
@@ -421,10 +448,13 @@ void Plane::set_servos_controlled(void)
     } else if (landing.is_flaring()) {
         min_throttle = 0;
     }
-    
+
+    // conpensate for battery voltage drop
+    throttle_voltage_comp();
+
     // apply watt limiter
     throttle_watt_limiter(min_throttle, max_throttle);
-    
+
     SRV_Channels::set_output_scaled(SRV_Channel::k_throttle,
                                     constrain_int16(SRV_Channels::get_output_scaled(SRV_Channel::k_throttle), min_throttle, max_throttle));