Sag compensated battery voltage and voltage based throttle compensation

src/main/build/debug.h

@@ -59,6 +59,7 @@ typedef enum {
     DEBUG_ALWAYS,
     DEBUG_STAGE2,
     DEBUG_WIND_ESTIMATOR,
+    DEBUG_SAG_COMP_VOLTAGE,
     DEBUG_COUNT
 } debugType_e;
 
@@ -75,4 +76,4 @@ void debugTracePrintBufferHex(bool synchronous, const void *buffer, size_t size)
 #define DEBUG_TRACE_SYNC(fmt, ...)
 #define DEBUG_TRACE_BUFFER_HEX(buf, size)
 #define DEBUG_TRACE_BUFFER_HEX_SYNC(buf, size)
-#endif
+#endif

src/main/cms/cms_menu_osd.c

@@ -148,6 +148,7 @@ static const OSD_Entry menuOsdElemsEntries[] =
 
     OSD_ELEMENT_ENTRY("RSSI", OSD_RSSI_VALUE),
     OSD_ELEMENT_ENTRY("MAIN BATTERY", OSD_MAIN_BATT_VOLTAGE),
+    OSD_ELEMENT_ENTRY("MAIN BATT SC", OSD_SAG_COMPENSATED_MAIN_BATT_VOLTAGE),
     OSD_ELEMENT_ENTRY("CELL VOLTAGE", OSD_MAIN_BATT_CELL_VOLTAGE),
     OSD_ELEMENT_ENTRY("CROSSHAIRS", OSD_CROSSHAIRS),
     OSD_ELEMENT_ENTRY("HORIZON", OSD_ARTIFICIAL_HORIZON),

src/main/fc/fc_tasks.c

@@ -106,8 +106,10 @@ void taskUpdateBattery(timeUs_t currentTimeUs)
 #ifdef USE_ADC
     if (feature(FEATURE_VBAT))
         batteryUpdate(BatMonitoringTimeSinceLastServiced);
-    if (feature(FEATURE_VBAT) && feature(FEATURE_CURRENT_METER))
+    if (feature(FEATURE_VBAT) && feature(FEATURE_CURRENT_METER)) {
         powerMeterUpdate(BatMonitoringTimeSinceLastServiced);
+        sagCompensatedVBatUpdate(currentTimeUs);
+    }
 #endif
     batMonitoringLastServiced = currentTimeUs;
 }

src/main/fc/settings.yaml

@@ -67,7 +67,7 @@ tables:
   - name: i2c_speed
     values: ["400KHZ", "800KHZ", "100KHZ", "200KHZ"]
   - name: debug_modes
-    values: ["NONE", "GYRO", "NOTCH", "NAV_LANDING", "FW_ALTITUDE", "AGL", "FLOW_RAW", "SBUS", "FPORT", "ALWAYS", "STAGE2", "WIND_ESTIMATOR"]
+    values: ["NONE", "GYRO", "NOTCH", "NAV_LANDING", "FW_ALTITUDE", "AGL", "FLOW_RAW", "SBUS", "FPORT", "ALWAYS", "STAGE2", "WIND_ESTIMATOR", "SAG_COMP_VOLTAGE"]
   - name: async_mode
     values: ["NONE", "GYRO", "ALL"]
   - name: aux_operator
@@ -427,6 +427,9 @@ groups:
       - name: motor_pwm_protocol
         field: motorPwmProtocol
         table: motor_pwm_protocol
+      - name: throttle_vbat_compensation
+        field: throttleVBatCompensation
+        type: bool
 
   - name: PG_FAILSAFE_CONFIG
     type: failsafeConfig_t

src/main/flight/mixer.c

@@ -50,6 +50,8 @@
 
 #include "rx/rx.h"
 
+#include "sensors/battery.h"
+
 
 //#define MIXER_DEBUG
 
@@ -87,7 +89,7 @@ PG_RESET_TEMPLATE(mixerConfig_t, mixerConfig,
 #define DEFAULT_MIN_THROTTLE    1150
 #endif
 
-PG_REGISTER_WITH_RESET_TEMPLATE(motorConfig_t, motorConfig, PG_MOTOR_CONFIG, 1);
+PG_REGISTER_WITH_RESET_TEMPLATE(motorConfig_t, motorConfig, PG_MOTOR_CONFIG, 2);
 
 PG_RESET_TEMPLATE(motorConfig_t, motorConfig,
     .minthrottle = DEFAULT_MIN_THROTTLE,
@@ -96,7 +98,8 @@ PG_RESET_TEMPLATE(motorConfig_t, motorConfig,
     .maxthrottle = 1850,
     .mincommand = 1000,
     .motorAccelTimeMs = 0,
-    .motorDecelTimeMs = 0
+    .motorDecelTimeMs = 0,
+    .throttleVBatCompensation = false
 );
 
 static motorMixer_t currentMixer[MAX_SUPPORTED_MOTORS];
@@ -305,6 +308,10 @@ void mixTable(const float dT)
         throttleCommand = rcCommand[THROTTLE];
         throttleMin = motorConfig()->minthrottle;
         throttleMax = motorConfig()->maxthrottle;
+
+        // Throttle compensation based on battery voltage
+        if (motorConfig()->throttleVBatCompensation && STATE(FIXED_WING) && isAmperageConfigured() && feature(FEATURE_VBAT))
+            throttleCommand = MIN(throttleCommand * calculateThrottleCompensationFactor(), throttleMax);
     }
 
     throttleRange = throttleMax - throttleMin;

src/main/flight/mixer.h

@@ -80,6 +80,7 @@ typedef struct motorConfig_s {
     uint8_t  motorPwmProtocol;
     uint16_t motorAccelTimeMs;              // Time limit for motor to accelerate from 0 to 100% throttle [ms]
     uint16_t motorDecelTimeMs;              // Time limit for motor to decelerate from 0 to 100% throttle [ms]
+    bool     throttleVBatCompensation;
 } motorConfig_t;
 
 PG_DECLARE(motorConfig_t, motorConfig);

src/main/io/osd.c

@@ -1050,6 +1050,19 @@ static bool osdDrawSingleElement(uint8_t item)
         displayWriteWithAttr(osdDisplayPort, elemPosX + 1, elemPosY, buff, elemAttr);
         return true;
 
+    case OSD_SAG_COMPENSATED_MAIN_BATT_VOLTAGE:
+        osdFormatBatteryChargeSymbol(buff);
+        buff[1] = '\0';
+        osdUpdateBatteryCapacityOrVoltageTextAttributes(&elemAttr);
+        displayWriteWithAttr(osdDisplayPort, elemPosX, elemPosY, buff, elemAttr);
+        elemAttr = TEXT_ATTRIBUTES_NONE;
+        osdFormatCentiNumber(buff, getSagCompensatedBatteryVoltage(), 0, osdConfig()->main_voltage_decimals, 0, osdConfig()->main_voltage_decimals + 2);
+        strcat(buff, "V");
+        if ((getBatteryState() != BATTERY_NOT_PRESENT) && (getSagCompensatedBatteryVoltage() <= getBatteryWarningVoltage()))
+            TEXT_ATTRIBUTES_ADD_BLINK(elemAttr);
+        displayWriteWithAttr(osdDisplayPort, elemPosX + 1, elemPosY, buff, elemAttr);
+        return true;
+
     case OSD_CURRENT_DRAW:
         buff[0] = SYM_AMP;
         osdFormatCentiNumber(buff + 1, getAmperage(), 0, 2, 0, 3);
@@ -1895,6 +1908,8 @@ void pgResetFn_osdConfig(osdConfig_t *osdConfig)
 {
     osdConfig->item_pos[0][OSD_ALTITUDE] = OSD_POS(1, 0) | OSD_VISIBLE_FLAG;
     osdConfig->item_pos[0][OSD_MAIN_BATT_VOLTAGE] = OSD_POS(12, 0) | OSD_VISIBLE_FLAG;
+    osdConfig->item_pos[0][OSD_SAG_COMPENSATED_MAIN_BATT_VOLTAGE] = OSD_POS(12, 1);
+
     osdConfig->item_pos[0][OSD_RSSI_VALUE] = OSD_POS(23, 0) | OSD_VISIBLE_FLAG;
     //line 2
     osdConfig->item_pos[0][OSD_HOME_DIST] = OSD_POS(1, 1);

src/main/io/osd.h

@@ -83,6 +83,7 @@ typedef enum {
     OSD_DEBUG, // Number 46. Intentionally absent from configurator and CMS. Set it from CLI.
     OSD_WIND_SPEED_HORIZONTAL,
     OSD_WIND_SPEED_VERTICAL,
+    OSD_SAG_COMPENSATED_MAIN_BATT_VOLTAGE,
     OSD_ITEM_COUNT // MUST BE LAST
 } osd_items_e;
 

src/main/sensors/battery.c

@@ -42,6 +42,7 @@
 
 #include "io/beeper.h"
 
+#include "build/debug.h"
 
 #define ADCVREF 3300                 // in mV (3300 = 3.3V)
 
@@ -65,6 +66,8 @@ static bool batteryFullWhenPluggedIn = false;
 static uint16_t vbat = 0;                   // battery voltage in 0.1V steps (filtered)
 static uint16_t vbatLatestADC = 0;          // most recent unsmoothed raw reading from vbat ADC
 static uint16_t amperageLatestADC = 0;      // most recent raw reading from current ADC
+static uint16_t powerSupplyImpedance = 0;   // calculated impedance in milliohm
+static uint16_t sagCompensatedVBat = 0;     // calculated no load vbat
 
 static int32_t amperage = 0;               // amperage read by current sensor in centiampere (1/100th A)
 static int32_t power = 0;                  // power draw in cW (0.01W resolution)
@@ -245,6 +248,16 @@ uint16_t getBatteryVoltage(void)
     return vbat;
 }
 
+uint16_t getSagCompensatedBatteryVoltage(void)
+{
+    return sagCompensatedVBat;
+}
+
+float calculateThrottleCompensationFactor(void)
+{
+    return batteryFullVoltage / sagCompensatedVBat;
+}
+
 uint16_t getBatteryVoltageLatestADC(void)
 {
     return vbatLatestADC;
@@ -342,6 +355,42 @@ void powerMeterUpdate(int32_t timeDelta)
     mWhDrawn = mWhDrawnRaw / (3600 * 100);
 }
 
+void sagCompensatedVBatUpdate(timeUs_t currentTime)
+{
+    static timeUs_t recordTimestamp = 0;
+    static int32_t amperageRecord;
+    static uint16_t vbatRecord;
+
+    if (batteryState == BATTERY_NOT_PRESENT) {
+
+        recordTimestamp = 0;
+        powerSupplyImpedance = 0;
+        sagCompensatedVBat = vbat;
+
+    } else {
+
+        if (cmpTimeUs(currentTime, recordTimestamp) > 20000000)
+            recordTimestamp = 0;
+
+        if (!recordTimestamp) {
+            amperageRecord = amperage;
+            vbatRecord = vbat;
+            recordTimestamp = currentTime;
+        } else if ((amperage - amperageRecord >= 400) && ((int16_t)vbatRecord - vbat >= 10)) {
+            powerSupplyImpedance = (int32_t)(vbatRecord - vbat) * 1000 / (amperage - amperageRecord);
+            amperageRecord = amperage;
+            vbatRecord = vbat;
+            recordTimestamp = currentTime;
+        }
+
+        sagCompensatedVBat = MIN(batteryFullVoltage, vbat + powerSupplyImpedance * amperage / 1000);
+
+    }
+
+    DEBUG_SET(DEBUG_SAG_COMP_VOLTAGE, 0, powerSupplyImpedance);
+    DEBUG_SET(DEBUG_SAG_COMP_VOLTAGE, 1, sagCompensatedVBat);
+}
+
 uint8_t calculateBatteryPercentage(void)
 {
     if (batteryState == BATTERY_NOT_PRESENT)

src/main/sensors/battery.h

@@ -18,6 +18,7 @@
 #pragma once
 
 #include "config/parameter_group.h"
+#include "drivers/time.h"
 
 #ifndef VBAT_SCALE_DEFAULT
 #define VBAT_SCALE_DEFAULT 1100
@@ -88,6 +89,7 @@ void batteryInit(void);
 
 bool isBatteryVoltageConfigured(void);
 uint16_t getBatteryVoltage(void);
+uint16_t getSagCompensatedBatteryVoltage(void);
 uint16_t getBatteryVoltageLatestADC(void);
 uint16_t getBatteryWarningVoltage(void);
 uint8_t getBatteryCellCount(void);
@@ -102,7 +104,8 @@ int32_t getMAhDrawn(void);
 int32_t getMWhDrawn(void);
 
 void currentMeterUpdate(int32_t lastUpdateAt);
-
+void sagCompensatedVBatUpdate(timeUs_t currentTime);
 void powerMeterUpdate(int32_t lastUpdateAt);
 
 uint8_t calculateBatteryPercentage(void);
+float calculateThrottleCompensationFactor(void);