From e782ca55e676f3e22a263508ff8c63ac709bb3ed Mon Sep 17 00:00:00 2001 From: mikeller Date: Sun, 27 Nov 2016 20:24:50 +1300 Subject: [PATCH] Unified 'calculateBatteryPercentage' and 'calculateBatteryCapacityRemainingPercentage'. Fixed tests. Changed consumption calculations to be based on latest amperage value. --- src/main/io/dashboard.c | 2 +- src/main/io/ledstrip.c | 2 +- src/main/sensors/battery.c | 73 +++++++++++++----------- src/main/sensors/battery.h | 1 - src/main/telemetry/crsf.c | 2 +- src/main/telemetry/frsky.c | 2 +- src/test/unit/telemetry_crsf_unittest.cc | 1 + 7 files changed, 44 insertions(+), 39 deletions(-) diff --git a/src/main/io/dashboard.c b/src/main/io/dashboard.c index c0cca5a918..65db36e969 100644 --- a/src/main/io/dashboard.c +++ b/src/main/io/dashboard.c @@ -473,7 +473,7 @@ void showBatteryPage(void) i2c_OLED_set_line(rowIndex++); i2c_OLED_send_string(lineBuffer); - uint8_t capacityPercentage = calculateBatteryCapacityRemainingPercentage(); + uint8_t capacityPercentage = calculateBatteryPercentage(); i2c_OLED_set_line(rowIndex++); drawHorizonalPercentageBar(SCREEN_CHARACTER_COLUMN_COUNT, capacityPercentage); } diff --git a/src/main/io/ledstrip.c b/src/main/io/ledstrip.c index 00df39835c..e6bad4d189 100644 --- a/src/main/io/ledstrip.c +++ b/src/main/io/ledstrip.c @@ -480,7 +480,7 @@ static void applyLedFixedLayers() case LED_FUNCTION_BATTERY: color = HSV(RED); - hOffset += scaleRange(calculateBatteryCapacityRemainingPercentage(), 0, 100, -30, 120); + hOffset += scaleRange(calculateBatteryPercentage(), 0, 100, -30, 120); break; case LED_FUNCTION_RSSI: diff --git a/src/main/sensors/battery.c b/src/main/sensors/battery.c index 8e02a16a86..3a8a59a8a1 100644 --- a/src/main/sensors/battery.c +++ b/src/main/sensors/battery.c @@ -235,18 +235,33 @@ static void updateBatteryCurrent(void) } uint16_t iBatSample = adcGetChannel(ADC_CURRENT); - amperage = currentSensorToCentiamps(biquadFilterApply(&iBatFilter, iBatSample)); amperageLatest = currentSensorToCentiamps(iBatSample); + amperage = currentSensorToCentiamps(biquadFilterApply(&iBatFilter, iBatSample)); } static void updateCurrentDrawn(int32_t lastUpdateAt) { static float mAhDrawnF = 0.0f; // used to get good enough resolution - mAhDrawnF = mAhDrawnF + (amperage * lastUpdateAt / (100.0f * 1000 * 3600)); + mAhDrawnF = mAhDrawnF + (amperageLatest * lastUpdateAt / (100.0f * 1000 * 3600)); mAhDrawn = mAhDrawnF; } +void updateConsumptionWarning(void) +{ + if (batteryConfig->useConsumptionAlerts && batteryConfig->batteryCapacity > 0 && getBatteryState() != BATTERY_NOT_PRESENT) { + if (calculateBatteryPercentage() == 0) { + vBatState = BATTERY_CRITICAL; + } else if (calculateBatteryPercentage() <= batteryConfig->consumptionWarningPercentage) { + consumptionState = BATTERY_WARNING; + } else { + consumptionState = BATTERY_OK; + } + + updateBatteryAlert(); + } +} + void updateCurrentMeter(int32_t lastUpdateAt, rxConfig_t *rxConfig, uint16_t deadband3d_throttle) { switch(batteryConfig->currentMeterType) { @@ -255,9 +270,11 @@ void updateCurrentMeter(int32_t lastUpdateAt, rxConfig_t *rxConfig, uint16_t dea updateCurrentDrawn(lastUpdateAt); + updateConsumptionWarning(); + break; case CURRENT_SENSOR_VIRTUAL: - amperage = (int32_t)batteryConfig->currentMeterOffset; + amperageLatest = (int32_t)batteryConfig->currentMeterOffset; if (ARMING_FLAG(ARMED)) { throttleStatus_e throttleStatus = calculateThrottleStatus(rxConfig, deadband3d_throttle); int throttleOffset = (int32_t)rcCommand[THROTTLE] - 1000; @@ -265,48 +282,33 @@ void updateCurrentMeter(int32_t lastUpdateAt, rxConfig_t *rxConfig, uint16_t dea throttleOffset = 0; } int throttleFactor = throttleOffset + (throttleOffset * throttleOffset / 50); - amperage += throttleFactor * (int32_t)batteryConfig->currentMeterScale / 1000; + amperageLatest += throttleFactor * (int32_t)batteryConfig->currentMeterScale / 1000; } + amperage = amperageLatest; updateCurrentDrawn(lastUpdateAt); + updateConsumptionWarning(); + break; case CURRENT_SENSOR_ESC: #ifdef USE_ESC_TELEMETRY if (isEscTelemetryActive()) { - amperage = getEscTelemetryCurrent(); + amperageLatest = getEscTelemetryCurrent(); + amperage = amperageLatest; mAhDrawn = getEscTelemetryConsumption(); + + updateConsumptionWarning(); } break; #endif case CURRENT_SENSOR_NONE: amperage = 0; + amperageLatest = 0; break; } - - if (batteryConfig->useConsumptionAlerts) { - switch(consumptionState) { - case BATTERY_OK: - if (calculateBatteryCapacityRemainingPercentage() <= batteryConfig->consumptionWarningPercentage) { - consumptionState = BATTERY_WARNING; - } - - break; - case BATTERY_WARNING: - if (calculateBatteryCapacityRemainingPercentage() == 0) { - consumptionState = BATTERY_CRITICAL; - } - - break; - case BATTERY_CRITICAL: - case BATTERY_NOT_PRESENT: - break; - } - - updateBatteryAlert(); - } } float calculateVbatPidCompensation(void) { @@ -320,12 +322,15 @@ float calculateVbatPidCompensation(void) { uint8_t calculateBatteryPercentage(void) { - return batteryCellCount > 0 ? constrain((((uint32_t)vbat - (batteryConfig->vbatmincellvoltage * batteryCellCount)) * 100) / ((batteryConfig->vbatmaxcellvoltage - batteryConfig->vbatmincellvoltage) * batteryCellCount), 0, 100) : 0; -} + uint8_t batteryPercentage = 0; + if (batteryCellCount > 0) { + uint16_t batteryCapacity = batteryConfig->batteryCapacity; + if (batteryCapacity > 0) { + batteryPercentage = constrain(((float)batteryCapacity - mAhDrawn) * 100 / batteryCapacity, 0, 100); + } else { + batteryPercentage = constrain((((uint32_t)vbat - (batteryConfig->vbatmincellvoltage * batteryCellCount)) * 100) / ((batteryConfig->vbatmaxcellvoltage - batteryConfig->vbatmincellvoltage) * batteryCellCount), 0, 100); + } + } -uint8_t calculateBatteryCapacityRemainingPercentage(void) -{ - uint16_t batteryCapacity = batteryConfig->batteryCapacity; - - return constrain((batteryCapacity - constrain(mAhDrawn, 0, 0xFFFF)) * 100.0f / batteryCapacity , 0, 100); + return batteryPercentage; } diff --git a/src/main/sensors/battery.h b/src/main/sensors/battery.h index 872e017000..e7821a475d 100644 --- a/src/main/sensors/battery.h +++ b/src/main/sensors/battery.h @@ -93,4 +93,3 @@ int32_t currentMeterToCentiamps(uint16_t src); float calculateVbatPidCompensation(void); uint8_t calculateBatteryPercentage(void); -uint8_t calculateBatteryCapacityRemainingPercentage(void); diff --git a/src/main/telemetry/crsf.c b/src/main/telemetry/crsf.c index a5f619d6dd..802d25a14a 100644 --- a/src/main/telemetry/crsf.c +++ b/src/main/telemetry/crsf.c @@ -185,7 +185,7 @@ void crsfFrameBatterySensor(sbuf_t *dst) #else crsfSerialize16(dst, amperage / 10); const uint32_t batteryCapacity = batteryConfig->batteryCapacity; - const uint8_t batteryRemainingPercentage = calculateBatteryCapacityRemainingPercentage(); + const uint8_t batteryRemainingPercentage = calculateBatteryPercentage(); #endif crsfSerialize8(dst, (batteryCapacity >> 16)); crsfSerialize8(dst, (batteryCapacity >> 8)); diff --git a/src/main/telemetry/frsky.c b/src/main/telemetry/frsky.c index e597a153a0..df2f812ba8 100644 --- a/src/main/telemetry/frsky.c +++ b/src/main/telemetry/frsky.c @@ -428,7 +428,7 @@ static void sendFuelLevel(void) sendDataHead(ID_FUEL_LEVEL); if (batteryConfig->batteryCapacity > 0) { - serialize16((uint16_t)calculateBatteryCapacityRemainingPercentage()); + serialize16((uint16_t)calculateBatteryPercentage()); } else { serialize16((uint16_t)constrain(mAhDrawn, 0, 0xFFFF)); } diff --git a/src/test/unit/telemetry_crsf_unittest.cc b/src/test/unit/telemetry_crsf_unittest.cc index 6f1762c572..3952f3d9e8 100644 --- a/src/test/unit/telemetry_crsf_unittest.cc +++ b/src/test/unit/telemetry_crsf_unittest.cc @@ -311,6 +311,7 @@ portSharing_e determinePortSharing(serialPortConfig_t *, serialPortFunction_e) { uint8_t batteryCapacityRemainingPercentage(void) {return 67;} uint8_t calculateBatteryCapacityRemainingPercentage(void) {return 67;} +uint8_t calculateBatteryPercentage(void) {return 67;} batteryState_e getBatteryState(void) {return BATTERY_OK;} bool isAirmodeActive(void) {return airMode;}