Fix battery voltage throttle compensation

docs/Battery.md

@@ -311,4 +311,14 @@ If `---` is displayed during flight instead of the remaining flight time/distanc
 
 ## Automatic throttle compensation based on battery voltage
 
-Automatic throttle compensation based on battery voltage can be used by enabling the `THR_VBAT_COMP` feature. It is working like this: used_throttle = requested_throttle * battery_max_voltage / sag_compensated_voltage.
+This features aims to compensate the throttle to get constant thrust with the same throttle request despite the battery voltage going down during flight. It can be used by enabling the `THR_VBAT_COMP` feature. This feature needs the sag compensated voltage which needs a current sensor (real or virtual) to be calculated.
+
+It is working like this: `used_throttle = requested_throttle * (1 + (battery_full_voltage / sag_compensated_voltage - 1) * thr_comp_weight)`.
+
+The default `thr_comp_weight` of 1 should be close to idal but if you want to tune this feature you need to find the difference in throttle value to achieve the same thrust (same power) when your battery is full and when your battery is almost empty then set `thr_comp_weight` to `(empty_battery_throttle / full_battery_throttle - 1) / (battery_full_voltage / battery_empty_sag_compensated_voltage - 1)`
+
+Example:
+  If the drawn power is 100W when the battery is full (12.6V) with 53% throttle and the drawn power is 100W with 58% throttle when the battery is almost empty with the sag compensated voltage being 11.0V `thr_comp_weight` needs to be set to this value to compensate the throttle automatically:
+  `(58 / 53 - 1) / (12.6 / 11.0 - 1) = 0.649`
+
+Known limitation: it doesn't work in 3D mode (3D feature)

docs/Cli.md

@@ -420,6 +420,7 @@ Re-apply any new defaults as desired.
 |  vtx_power  | 1 | VTX RF power level to use. The exact number of mw depends on the VTX hardware. |
 | motor_accel_time | 0 | Minimum time for the motor(s) to accelerate from 0 to 100% throttle (ms) [0-1000] |
 | motor_decel_time | 0 | Minimum time for the motor(s) to deccelerate from 100 to 0% throttle (ms) [0-1000] |
+| thr_comp_weight | 0.692 | Weight used for the throttle compensation based on battery voltage. See the [battery documentation](Battery.md#automatic-throttle-compensation-based-on-battery-voltage) |
 
 This Markdown table is made by MarkdwonTableMaker addon for google spreadsheet.
 Original Spreadsheet used to make this table can be found here https://docs.google.com/spreadsheets/d/1ubjYdMGmZ2aAMUNYkdfe3hhIF7wRfIjcuPOi_ysmp00/edit?usp=sharing

src/main/fc/settings.yaml

@@ -545,6 +545,10 @@ groups:
       - name: rth_energy_margin
         min: 0
         max: 100
+      - name: thr_comp_weight
+        field: throttle_compensation_weight
+        min: 0
+        max: 2
 
   - name: PG_BATTERY_PROFILES
     type: batteryProfile_t

src/main/flight/mixer.c

@@ -312,7 +312,7 @@ void mixTable(const float dT)
 
         // Throttle compensation based on battery voltage
         if (feature(FEATURE_THR_VBAT_COMP) && feature(FEATURE_VBAT) && isAmperageConfigured())
-            throttleCommand = MIN(throttleCommand * calculateThrottleCompensationFactor(), throttleMax);
+            throttleCommand = MIN(throttleMin + (throttleCommand - throttleMin) * calculateThrottleCompensationFactor(), throttleMax);
     }
 
     throttleRange = throttleMax - throttleMin;

src/main/sensors/battery.c

@@ -133,7 +133,9 @@ PG_RESET_TEMPLATE(batteryMetersConfig_t, batteryMetersConfig,
 
     .cruise_power = 0,
     .idle_power = 0,
-    .rth_energy_margin = 5
+    .rth_energy_margin = 5,
+
+    .throttle_compensation_weight = 1.0f
 
 );
 
@@ -358,7 +360,7 @@ uint16_t getBatterySagCompensatedVoltage(void)
 
 float calculateThrottleCompensationFactor(void)
 {
-    return batteryFullVoltage / sagCompensatedVBat;
+    return 1.0f + ((float)batteryFullVoltage / sagCompensatedVBat - 1.0f) * batteryMetersConfig()->throttle_compensation_weight;
 }
 
 uint16_t getBatteryVoltageLatestADC(void)

src/main/sensors/battery.h

@@ -76,6 +76,8 @@ typedef struct batteryMetersConfig_s {
     uint16_t idle_power;        // power drawn by the system when the motor(s) are stopped (cW)
     uint8_t rth_energy_margin;  // Energy that should be left after RTH (%), used for remaining time/distance before RTH
 
+    float throttle_compensation_weight;
+
 } batteryMetersConfig_t;
 
 typedef struct batteryProfile_s {