From ceeef947d5c42e474692be872cdda4e24edb975e Mon Sep 17 00:00:00 2001 From: Michael Keller Date: Mon, 7 Nov 2016 09:49:43 +1300 Subject: [PATCH] Fixed problems with battery low / critical warnings coming on when connecting battery. Also fixed problems with beeper settings for 1S setups. Introduced consistent usage of 'batteryCellCount = 0' to indicate 'no battery present'. --- src/main/io/beeper.c | 2 +- src/main/sensors/battery.c | 64 ++++++++++++++++++---------------- src/main/telemetry/frsky.c | 2 +- src/main/telemetry/smartport.c | 4 +-- 4 files changed, 38 insertions(+), 34 deletions(-) diff --git a/src/main/io/beeper.c b/src/main/io/beeper.c index 7718f49dab..5ddd3c3ea8 100644 --- a/src/main/io/beeper.c +++ b/src/main/io/beeper.c @@ -186,7 +186,7 @@ static const beeperTableEntry_t *currentBeeperEntry = NULL; */ void beeper(beeperMode_e mode) { - if (mode == BEEPER_SILENCE || ((getBeeperOffMask() & (1 << (BEEPER_USB-1))) && (feature(FEATURE_VBAT) && (batteryCellCount < 2)))) { + if (mode == BEEPER_SILENCE || ((getBeeperOffMask() & (1 << (BEEPER_USB-1))) && (feature(FEATURE_VBAT) && (batteryCellCount == 0)))) { beeperSilence(); return; } diff --git a/src/main/sensors/battery.c b/src/main/sensors/battery.c index a836e4a350..213d95d5b1 100644 --- a/src/main/sensors/battery.c +++ b/src/main/sensors/battery.c @@ -43,7 +43,7 @@ #define VBATT_LPF_FREQ 0.4f // Battery monitoring stuff -uint8_t batteryCellCount = 3; // cell count +uint8_t batteryCellCount; uint16_t batteryWarningVoltage; uint16_t batteryCriticalVoltage; @@ -90,33 +90,37 @@ void updateBattery(void) updateBatteryVoltage(); /* battery has just been connected*/ - if (batteryState == BATTERY_NOT_PRESENT && vbat > batteryConfig->batterynotpresentlevel ) - { - /* Actual battery state is calculated below, this is really BATTERY_PRESENT */ - batteryState = BATTERY_OK; - /* wait for VBatt to stabilise then we can calc number of cells - (using the filtered value takes a long time to ramp up) - We only do this on the ground so don't care if we do block, not - worse than original code anyway*/ - delay(VBATTERY_STABLE_DELAY); - updateBatteryVoltage(); - - unsigned cells = (batteryAdcToVoltage(vbatLatestADC) / batteryConfig->vbatmaxcellvoltage) + 1; - if (cells > 8) { - // something is wrong, we expect 8 cells maximum (and autodetection will be problematic at 6+ cells) - cells = 8; + if (batteryState == BATTERY_NOT_PRESENT) { + if (!ARMING_FLAG(ARMED)) { + /* wait for VBatt to stabilise then we can calc number of cells + (using the filtered value takes a long time to ramp up) + We only do this on the ground so don't care if we do block, not + worse than original code anyway*/ + delay(VBATTERY_STABLE_DELAY); + updateBatteryVoltage(); + } + + if (vbat > batteryConfig->batterynotpresentlevel || ARMING_FLAG(ARMED)) { + /* Actual battery state is calculated below, this is really BATTERY_PRESENT */ + batteryState = BATTERY_OK; + + unsigned cells = (batteryAdcToVoltage(vbatLatestADC) / batteryConfig->vbatmaxcellvoltage) + 1; + if (cells > 8) { + // something is wrong, we expect 8 cells maximum (and autodetection will be problematic at 6+ cells) + cells = 8; + } + batteryCellCount = cells; + batteryWarningVoltage = batteryCellCount * batteryConfig->vbatwarningcellvoltage; + batteryCriticalVoltage = batteryCellCount * batteryConfig->vbatmincellvoltage; + } + /* battery has been disconnected - can take a while for filter cap to disharge so we use a threshold of batteryConfig->batterynotpresentlevel */ + } else { + if (vbat <= batteryConfig->batterynotpresentlevel && !ARMING_FLAG(ARMED)) { + batteryState = BATTERY_NOT_PRESENT; + batteryCellCount = 0; + batteryWarningVoltage = 0; + batteryCriticalVoltage = 0; } - batteryCellCount = cells; - batteryWarningVoltage = batteryCellCount * batteryConfig->vbatwarningcellvoltage; - batteryCriticalVoltage = batteryCellCount * batteryConfig->vbatmincellvoltage; - } - /* battery has been disconnected - can take a while for filter cap to disharge so we use a threshold of VBATT_PRESENT_THRESHOLD */ - else if (batteryState != BATTERY_NOT_PRESENT && vbat <= batteryConfig->batterynotpresentlevel && !ARMING_FLAG(ARMED)) - { - batteryState = BATTERY_NOT_PRESENT; - batteryCellCount = 0; - batteryWarningVoltage = 0; - batteryCriticalVoltage = 0; } switch(batteryState) @@ -166,7 +170,7 @@ void batteryInit(batteryConfig_t *initialBatteryConfig) { batteryConfig = initialBatteryConfig; batteryState = BATTERY_NOT_PRESENT; - batteryCellCount = 1; + batteryCellCount = 0; batteryWarningVoltage = 0; batteryCriticalVoltage = 0; } @@ -216,7 +220,7 @@ void updateCurrentMeter(int32_t lastUpdateAt, rxConfig_t *rxConfig, uint16_t dea float calculateVbatPidCompensation(void) { float batteryScaler = 1.0f; - if (feature(FEATURE_VBAT) && batteryCellCount > 1) { + if (feature(FEATURE_VBAT) && batteryCellCount > 0) { // Up to 33% PID gain. Should be fine for 4,2to 3,3 difference batteryScaler = constrainf((( (float)batteryConfig->vbatmaxcellvoltage * batteryCellCount ) / (float) vbat), 1.0f, 1.33f); } @@ -225,7 +229,7 @@ float calculateVbatPidCompensation(void) { uint8_t calculateBatteryPercentage(void) { - return constrain((((uint32_t)vbat - (batteryConfig->vbatmincellvoltage * batteryCellCount)) * 100) / ((batteryConfig->vbatmaxcellvoltage - batteryConfig->vbatmincellvoltage) * batteryCellCount), 0, 100); + return batteryCellCount > 0 ? constrain((((uint32_t)vbat - (batteryConfig->vbatmincellvoltage * batteryCellCount)) * 100) / ((batteryConfig->vbatmaxcellvoltage - batteryConfig->vbatmincellvoltage) * batteryCellCount), 0, 100) : 0; } uint8_t calculateBatteryCapacityRemainingPercentage(void) diff --git a/src/main/telemetry/frsky.c b/src/main/telemetry/frsky.c index a2f452ed1a..cfc3f32fc2 100644 --- a/src/main/telemetry/frsky.c +++ b/src/main/telemetry/frsky.c @@ -529,7 +529,7 @@ void handleFrSkyTelemetry(rxConfig_t *rxConfig, uint16_t deadband3d_throttle) sendTemperature1(); sendThrottleOrBatterySizeAsRpm(rxConfig, deadband3d_throttle); - if (feature(FEATURE_VBAT)) { + if (feature(FEATURE_VBAT) && batteryCellCount > 0) { sendVoltage(); sendVoltageAmp(); sendAmperage(); diff --git a/src/main/telemetry/smartport.c b/src/main/telemetry/smartport.c index b8c446721d..59867574c3 100644 --- a/src/main/telemetry/smartport.c +++ b/src/main/telemetry/smartport.c @@ -634,7 +634,7 @@ void handleSmartPortTelemetry(void) break; #endif case FSSP_DATAID_VFAS : - if (feature(FEATURE_VBAT)) { + if (feature(FEATURE_VBAT) && batteryCellCount > 0) { uint16_t vfasVoltage; if (telemetryConfig->frsky_vfas_cell_voltage) { vfasVoltage = vbat / batteryCellCount; @@ -810,7 +810,7 @@ void handleSmartPortTelemetry(void) break; #endif case FSSP_DATAID_A4 : - if (feature(FEATURE_VBAT)) { + if (feature(FEATURE_VBAT) && batteryCellCount > 0) { smartPortSendPackage(id, vbat * 10 / batteryCellCount ); // given in 0.1V, convert to volts smartPortHasRequest = 0; }