diff --git a/src/main/io/ledstrip.c b/src/main/io/ledstrip.c index 5dcac28ce..df26efc3b 100644 --- a/src/main/io/ledstrip.c +++ b/src/main/io/ledstrip.c @@ -656,7 +656,7 @@ void applyLedWarningLayer(uint8_t updateNow) if (updateNow && warningFlashCounter == 0) { warningFlags = WARNING_FLAG_NONE; - if (feature(FEATURE_VBAT) && calculateBatteryState() != BATTERY_OK) { + if (feature(FEATURE_VBAT) && getBatteryState() != BATTERY_OK) { warningFlags |= WARNING_FLAG_LOW_BATTERY; } if (feature(FEATURE_FAILSAFE) && failsafeIsActive()) { diff --git a/src/main/io/serial_cli.c b/src/main/io/serial_cli.c index fef8db196..2709a5aa2 100644 --- a/src/main/io/serial_cli.c +++ b/src/main/io/serial_cli.c @@ -2042,8 +2042,8 @@ static void cliStatus(char *cmdline) { UNUSED(cmdline); - printf("System Uptime: %d seconds, Voltage: %d * 0.1V (%dS battery)\r\n", - millis() / 1000, vbat, batteryCellCount); + printf("System Uptime: %d seconds, Voltage: %d * 0.1V (%dS battery - %s)\r\n", + millis() / 1000, vbat, batteryCellCount,getBatteryStateString( )); printf("CPU Clock=%dMHz", (SystemCoreClock / 1000000)); diff --git a/src/main/mw.c b/src/main/mw.c index 059823dd9..766ebb247 100644 --- a/src/main/mw.c +++ b/src/main/mw.c @@ -88,8 +88,10 @@ enum { ALIGN_MAG = 2 }; -/* for VBAT monitoring frequency */ -#define VBATFREQ 6 // to read battery voltage - nth number of loop iterations +/* VBAT monitoring interval (in microseconds) - 1s*/ +#define VBATINTERVAL (6 * 3500) +/* IBat monitoring interval (in microseconds) - 6 default looptimes */ +#define IBATINTERVAL (6 * 3500) uint32_t currentTime = 0; uint32_t previousTime = 0; @@ -174,10 +176,9 @@ void annexCode(void) int32_t tmp, tmp2; int32_t axis, prop1 = 0, prop2; - static batteryState_e batteryState = BATTERY_OK; - static uint8_t vbatTimer = 0; - static int32_t vbatCycleTime = 0; - + static uint32_t vbatLastServiced = 0; + static uint32_t ibatLastServiced = 0; + uint32_t ibatTimeSinceLastServiced; // PITCH & ROLL only dynamic PID adjustment, depending on throttle value if (rcData[THROTTLE] < currentControlRateProfile->tpa_breakpoint) { prop2 = 100; @@ -247,24 +248,20 @@ void annexCode(void) rcCommand[PITCH] = rcCommand_PITCH; } - if (feature(FEATURE_VBAT | FEATURE_CURRENT_METER)) { - vbatCycleTime += cycleTime; - if (!(++vbatTimer % VBATFREQ)) { + if (feature(FEATURE_VBAT)) { + /* currentTime will rollover @ 70 minutes */ + if ((currentTime - vbatLastServiced) >= VBATINTERVAL) { + vbatLastServiced = currentTime; + updateBattery(); + } + } - if (feature(FEATURE_VBAT)) { - updateBatteryVoltage(); - batteryState = calculateBatteryState(); - //handle beepers for battery levels - if (batteryState == BATTERY_CRITICAL) - beeper(BEEPER_BAT_CRIT_LOW); //critically low battery - else if (batteryState == BATTERY_WARNING) - beeper(BEEPER_BAT_LOW); //low battery - } - - if (feature(FEATURE_CURRENT_METER)) { - updateCurrentMeter(vbatCycleTime, &masterConfig.rxConfig, masterConfig.flight3DConfig.deadband3d_throttle); - } - vbatCycleTime = 0; + if (feature(FEATURE_CURRENT_METER)) { + /* currentTime will rollover @ 70 minutes */ + ibatTimeSinceLastServiced = (currentTime - ibatLastServiced); + if (ibatTimeSinceLastServiced >= IBATINTERVAL) { + ibatLastServiced = currentTime; + updateCurrentMeter((ibatTimeSinceLastServiced / 1000), &masterConfig.rxConfig, masterConfig.flight3DConfig.deadband3d_throttle); } } diff --git a/src/main/sensors/battery.c b/src/main/sensors/battery.c index 8e32ef6cb..dc83519b2 100644 --- a/src/main/sensors/battery.c +++ b/src/main/sensors/battery.c @@ -31,13 +31,18 @@ #include "rx/rx.h" #include "io/rc_controls.h" +#include "flight/lowpass.h" +#include "io/beeper.h" + +#define VBATT_DETECT 10 +#define VBATT_LPF_FREQ 10 // Battery monitoring stuff uint8_t batteryCellCount = 3; // cell count uint16_t batteryWarningVoltage; uint16_t batteryCriticalVoltage; -uint8_t vbat = 0; // battery voltage in 0.1V steps +uint16_t vbat = 0; // battery voltage in 0.1V steps (filtered) uint16_t vbatLatestADC = 0; // most recent unsmoothed raw reading from vbat ADC uint16_t amperageLatestADC = 0; // most recent raw reading from current ADC @@ -46,6 +51,9 @@ int32_t mAhDrawn = 0; // milliampere hours drawn from the battery batteryConfig_t *batteryConfig; +static batteryState_e batteryState; +static lowpass_t lowpassFilter; + uint16_t batteryAdcToVoltage(uint16_t src) { // calculate battery voltage based on ADC reading @@ -53,53 +61,106 @@ uint16_t batteryAdcToVoltage(uint16_t src) return ((uint32_t)src * batteryConfig->vbatscale * 33 + (0xFFF * 5)) / (0xFFF * 10); } -#define BATTERY_SAMPLE_COUNT 8 - -void updateBatteryVoltage(void) +static void updateBatteryVoltage(void) { - static uint16_t vbatSamples[BATTERY_SAMPLE_COUNT]; - static uint8_t currentSampleIndex = 0; - uint8_t index; - uint16_t vbatSampleTotal = 0; + uint16_t vbatSample; + uint16_t vbatFiltered; // store the battery voltage with some other recent battery voltage readings - vbatSamples[(currentSampleIndex++) % BATTERY_SAMPLE_COUNT] = vbatLatestADC = adcGetChannel(ADC_BATTERY); - - // calculate vbat based on the average of recent readings - for (index = 0; index < BATTERY_SAMPLE_COUNT; index++) { - vbatSampleTotal += vbatSamples[index]; - } - vbat = batteryAdcToVoltage(vbatSampleTotal / BATTERY_SAMPLE_COUNT); + vbatSample = vbatLatestADC = adcGetChannel(ADC_BATTERY); + vbatFiltered = (uint16_t)lowpassFixed(&lowpassFilter, vbatSample, VBATT_LPF_FREQ); + vbat = batteryAdcToVoltage(vbatFiltered); } -batteryState_e calculateBatteryState(void) +#define VBATTERY_STABLE_DELAY 40 +/* Batt Hysteresis of +/-100mV */ +#define VBATT_HYSTERESIS 1 + +void updateBattery(void) { - if (vbat <= batteryCriticalVoltage) { - return BATTERY_CRITICAL; + updateBatteryVoltage(); + + /* battery has just been connected*/ + if(batteryState == BATTERY_NOTPRESENT && vbat > VBATT_DETECT) + { + /* 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; + batteryCellCount = cells; + batteryWarningVoltage = batteryCellCount * batteryConfig->vbatwarningcellvoltage; + batteryCriticalVoltage = batteryCellCount * batteryConfig->vbatmincellvoltage; } - if (vbat <= batteryWarningVoltage) { - return BATTERY_WARNING; + /* battery has been disconnected - can take a while for filter cap to disharge so we use a threshold of VBATT_DETECT */ + else if(batteryState != BATTERY_NOTPRESENT && vbat <= VBATT_DETECT) + { + batteryState = BATTERY_NOTPRESENT; + batteryCellCount = 0; + batteryWarningVoltage = 0; + batteryCriticalVoltage = 0; + } + + switch(batteryState) + { + case BATTERY_OK: + if(vbat <= (batteryWarningVoltage - VBATT_HYSTERESIS)){ + batteryState = BATTERY_WARNING; + beeper(BEEPER_BAT_LOW); + } + break; + case BATTERY_WARNING: + if(vbat <= (batteryCriticalVoltage - VBATT_HYSTERESIS)){ + batteryState = BATTERY_CRITICAL; + beeper(BEEPER_BAT_CRIT_LOW); + } + else if(vbat > (batteryWarningVoltage + VBATT_HYSTERESIS)){ + batteryState = BATTERY_OK; + } + else{ + beeper(BEEPER_BAT_LOW); + } + break; + case BATTERY_CRITICAL: + if(vbat > (batteryCriticalVoltage + VBATT_HYSTERESIS)){ + batteryState = BATTERY_WARNING; + beeper(BEEPER_BAT_LOW); + } + else{ + beeper(BEEPER_BAT_CRIT_LOW); + } + break; + case BATTERY_NOTPRESENT: + break; } - return BATTERY_OK; +} + +batteryState_e getBatteryState(void) +{ + return batteryState; +} + +const char * batteryStateStrings[] = {"OK", "WARNING", "CRITICAL", "NOT PRESENT"}; + +const char * getBatteryStateString(void) +{ + return batteryStateStrings[batteryState]; } void batteryInit(batteryConfig_t *initialBatteryConfig) { batteryConfig = initialBatteryConfig; - - uint32_t i; - - for (i = 0; i < BATTERY_SAMPLE_COUNT; i++) { - updateBatteryVoltage(); - delay((32 / BATTERY_SAMPLE_COUNT) * 10); - } - - unsigned cells = (vbat / 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; + batteryState = BATTERY_NOTPRESENT; + batteryCellCount = 1; + batteryWarningVoltage = 0; + batteryCriticalVoltage = 0; } #define ADCVREF 3300 // in mV diff --git a/src/main/sensors/battery.h b/src/main/sensors/battery.h index fdbc88162..8214c7203 100644 --- a/src/main/sensors/battery.h +++ b/src/main/sensors/battery.h @@ -48,10 +48,12 @@ typedef struct batteryConfig_s { typedef enum { BATTERY_OK = 0, BATTERY_WARNING, - BATTERY_CRITICAL + BATTERY_CRITICAL, + BATTERY_NOTPRESENT } batteryState_e; -extern uint8_t vbat; +extern uint16_t vbat; +extern uint16_t vbatRaw; extern uint16_t vbatLatestADC; extern uint8_t batteryCellCount; extern uint16_t batteryWarningVoltage; @@ -60,8 +62,9 @@ extern int32_t amperage; extern int32_t mAhDrawn; uint16_t batteryAdcToVoltage(uint16_t src); -batteryState_e calculateBatteryState(void); -void updateBatteryVoltage(void); +batteryState_e getBatteryState(void); +const char * getBatteryStateString(void); +void updateBattery(void); void batteryInit(batteryConfig_t *initialBatteryConfig); void updateCurrentMeter(int32_t lastUpdateAt, rxConfig_t *rxConfig, uint16_t deadband3d_throttle); diff --git a/src/main/telemetry/hott.c b/src/main/telemetry/hott.c index 3ec64bf4c..65f2b79da 100644 --- a/src/main/telemetry/hott.c +++ b/src/main/telemetry/hott.c @@ -217,9 +217,12 @@ static bool shouldTriggerBatteryAlarmNow(void) static inline void updateAlarmBatteryStatus(HOTT_EAM_MSG_t *hottEAMMessage) { + batteryState_e batteryState; + if (shouldTriggerBatteryAlarmNow()){ lastHottAlarmSoundTime = millis(); - if (vbat <= batteryWarningVoltage){ + batteryState = getBatteryState(); + if (batteryState == BATTERY_WARNING || batteryState == BATTERY_CRITICAL){ hottEAMMessage->warning_beeps = 0x10; hottEAMMessage->alarm_invers1 = HOTT_EAM_ALARM1_FLAG_BATTERY_1; } diff --git a/src/test/unit/battery_unittest.cc b/src/test/unit/battery_unittest.cc index b63af366c..388ea6e2a 100644 --- a/src/test/unit/battery_unittest.cc +++ b/src/test/unit/battery_unittest.cc @@ -24,6 +24,8 @@ extern "C" { #include "sensors/battery.h" #include "io/rc_controls.h" + #include "flight/lowpass.h" + #include "io/beeper.h" } #include "unittest_macros.h" @@ -83,6 +85,109 @@ TEST(BatteryTest, BatteryADCToVoltage) } } +uint16_t currentADCReading; + + +typedef struct batteryAdcToBatteryStateExpectation_s +{ + uint16_t adcReading; + uint16_t expectedVoltageInDeciVoltSteps; + batteryState_e expectedBatteryState; + uint8_t scale; +} batteryAdcToBatteryStateExpectation_t; + +/* Test the battery state and hysteresis code */ +TEST(BatteryTest, BatteryState) +{ + // batteryInit() reads a bunch of fields including vbatscale, so set up the config with useful initial values: + batteryConfig_t batteryConfig = { + .vbatscale = VBAT_SCALE_DEFAULT, + .vbatmaxcellvoltage = 43, + .vbatmincellvoltage = 33, + .vbatwarningcellvoltage = 35, + .currentMeterScale = 400, + .currentMeterOffset = 0, + .currentMeterType = CURRENT_SENSOR_NONE, + .multiwiiCurrentMeterOutput = 0, + .batteryCapacity = 2200, + }; + + batteryInit(&batteryConfig); + + batteryAdcToBatteryStateExpectation_t batteryAdcToBatteryStateExpectations[] = { + {1420, 126, BATTERY_OK, ELEVEN_TO_ONE_VOLTAGE_DIVIDER}, + /* fall down to battery warning level */ + {1185, 105, BATTERY_OK, ELEVEN_TO_ONE_VOLTAGE_DIVIDER}, + {1175, 104, BATTERY_WARNING, ELEVEN_TO_ONE_VOLTAGE_DIVIDER}, + /* creep back up to battery ok */ + {1185, 105, BATTERY_WARNING, ELEVEN_TO_ONE_VOLTAGE_DIVIDER}, + {1195, 106, BATTERY_WARNING, ELEVEN_TO_ONE_VOLTAGE_DIVIDER}, + {1207, 107, BATTERY_OK, ELEVEN_TO_ONE_VOLTAGE_DIVIDER}, + /* fall down to battery critical level */ + {1175, 104, BATTERY_WARNING, ELEVEN_TO_ONE_VOLTAGE_DIVIDER}, + {1108, 98, BATTERY_CRITICAL, ELEVEN_TO_ONE_VOLTAGE_DIVIDER}, + /* creep back up to battery warning */ + {1115, 99, BATTERY_CRITICAL, ELEVEN_TO_ONE_VOLTAGE_DIVIDER}, + {1130, 100, BATTERY_CRITICAL, ELEVEN_TO_ONE_VOLTAGE_DIVIDER}, + {1145, 101, BATTERY_WARNING, ELEVEN_TO_ONE_VOLTAGE_DIVIDER}, + + }; + uint8_t testIterationCount = sizeof(batteryAdcToBatteryStateExpectations) / sizeof(batteryAdcToBatteryStateExpectation_t); + + // expect + for (uint8_t index = 0; index < testIterationCount; index ++) { + batteryAdcToBatteryStateExpectation_t *batteryAdcToBatteryStateExpectation = &batteryAdcToBatteryStateExpectations[index]; + batteryConfig.vbatscale = batteryAdcToBatteryStateExpectation->scale; + currentADCReading = batteryAdcToBatteryStateExpectation->adcReading; + updateBattery( ); + batteryState_e batteryState = getBatteryState(); + EXPECT_EQ(batteryAdcToBatteryStateExpectation->expectedBatteryState, batteryState); + } +} + +typedef struct batteryAdcToCellCountExpectation_s +{ + uint16_t adcReading; + uint16_t expectedVoltageInDeciVoltSteps; + uint8_t scale; + uint8_t cellCount; +} batteryAdcToCellCountExpectation_t; + +/* Test the cell count is correctly detected if we start at 0V */ +TEST(BatteryTest, CellCount) +{ + // batteryInit() reads a bunch of fields including vbatscale, so set up the config with useful initial values: + batteryConfig_t batteryConfig = { + .vbatscale = VBAT_SCALE_DEFAULT, + .vbatmaxcellvoltage = 43, + .vbatmincellvoltage = 33, + .vbatwarningcellvoltage = 35, + .currentMeterScale = 400, + .currentMeterOffset = 0, + .currentMeterType = CURRENT_SENSOR_NONE, + .multiwiiCurrentMeterOutput = 0, + .batteryCapacity = 2200, + }; + + batteryInit(&batteryConfig); + + batteryAdcToCellCountExpectation_t batteryAdcToCellCountExpectations[] = { + {0, 0, ELEVEN_TO_ONE_VOLTAGE_DIVIDER, 1}, + {1420, 126, ELEVEN_TO_ONE_VOLTAGE_DIVIDER, 3}, + }; + uint8_t testIterationCount = sizeof(batteryAdcToCellCountExpectations) / sizeof(batteryAdcToCellCountExpectation_t); + + // expect + for (uint8_t index = 0; index < testIterationCount; index ++) { + batteryAdcToCellCountExpectation_t *batteryAdcToCellCountExpectation = &batteryAdcToCellCountExpectations[index]; + batteryConfig.vbatscale = batteryAdcToCellCountExpectation->scale; + currentADCReading = batteryAdcToCellCountExpectation->adcReading; + updateBattery( ); + EXPECT_EQ(batteryAdcToCellCountExpectation->cellCount, batteryCellCount); + } +} + + // STUBS extern "C" { @@ -90,6 +195,7 @@ extern "C" { uint8_t armingFlags = 0; int16_t rcCommand[4] = {0,0,0,0}; + bool feature(uint32_t mask) { UNUSED(mask); @@ -106,7 +212,7 @@ throttleStatus_e calculateThrottleStatus(rxConfig_t *rxConfig, uint16_t deadband uint16_t adcGetChannel(uint8_t channel) { UNUSED(channel); - return 0; + return currentADCReading; } void delay(uint32_t ms) @@ -114,4 +220,17 @@ void delay(uint32_t ms) UNUSED(ms); return; } + +int32_t lowpassFixed(lowpass_t *filter, int32_t in, int16_t freq) +{ + UNUSED(filter); + UNUSED(freq); + return in; +} + +void beeper(beeperMode_e mode) +{ + UNUSED(mode); +} + } diff --git a/src/test/unit/ledstrip_unittest.cc b/src/test/unit/ledstrip_unittest.cc index c98ace2fa..4e518163c 100644 --- a/src/test/unit/ledstrip_unittest.cc +++ b/src/test/unit/ledstrip_unittest.cc @@ -355,7 +355,7 @@ int16_t rcCommand[4]; int16_t rcData[MAX_SUPPORTED_RC_CHANNEL_COUNT]; uint32_t rcModeActivationMask; -batteryState_e calculateBatteryState(void) { +batteryState_e getBatteryState(void) { return BATTERY_OK; } diff --git a/src/test/unit/rc_controls_unittest.cc b/src/test/unit/rc_controls_unittest.cc index 0050e70e1..3c5bebccb 100644 --- a/src/test/unit/rc_controls_unittest.cc +++ b/src/test/unit/rc_controls_unittest.cc @@ -255,6 +255,18 @@ protected: rxConfig.mincheck = DEFAULT_MIN_CHECK; rxConfig.maxcheck = DEFAULT_MAX_CHECK; rxConfig.midrc = 1500; + + controlRateConfig.rcRate8 = 90; + controlRateConfig.rcExpo8 = 0; + controlRateConfig.thrMid8 = 0; + controlRateConfig.thrExpo8 = 0; + controlRateConfig.rcYawExpo8 = 0; + controlRateConfig.rates[0] = 0; + controlRateConfig.rates[1] = 0; + controlRateConfig.rates[2] = 0; + controlRateConfig.dynThrPID = 0; + controlRateConfig.tpa_breakpoint = 0; + } }; diff --git a/src/test/unit/telemetry_hott_unittest.cc b/src/test/unit/telemetry_hott_unittest.cc index 2606e6791..4fdf31705 100644 --- a/src/test/unit/telemetry_hott_unittest.cc +++ b/src/test/unit/telemetry_hott_unittest.cc @@ -163,7 +163,7 @@ int32_t GPS_coord[2]; uint16_t GPS_speed; // speed in 0.1m/s uint16_t GPS_distanceToHome; // distance to home point in meters uint16_t GPS_altitude; // altitude in 0.1m -uint8_t vbat; +uint16_t vbat; int16_t GPS_directionToHome; // direction to home or hol point in degrees int32_t amperage; @@ -232,5 +232,9 @@ portSharing_e determinePortSharing(serialPortConfig_t *, serialPortFunction_e) { return PORTSHARING_NOT_SHARED; } +batteryState_e getBatteryState(void) { + return BATTERY_OK; +} + }