rusefi
/
atbetaflight
mirror of https://github.com/rusefi/atbetaflight.git
1
0
Fork 0
Code Issues Packages Projects Releases Wiki Activity

Battery auto-detect and LPF for battery monitoring

This commit is contained in:
DarkVegetableMatter 2015-05-26 10:59:02 +01:00 committed by Dominic Clifton
parent 26ac6115e7
commit 942c89237e
10 changed files with 268 additions and 69 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

2
src/main/io/ledstrip.c
View File

@ -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()) {

4
src/main/io/serial_cli.c
View File

@ -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));

37
src/main/mw.c
View File

@ -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)) {
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
/* currentTime will rollover @ 70 minutes */
if ((currentTime - vbatLastServiced) >= VBATINTERVAL) {
vbatLastServiced = currentTime;
updateBattery();
}
}
if (feature(FEATURE_CURRENT_METER)) {
updateCurrentMeter(vbatCycleTime, &masterConfig.rxConfig, masterConfig.flight3DConfig.deadband3d_throttle);
}
vbatCycleTime = 0;
/* currentTime will rollover @ 70 minutes */
ibatTimeSinceLastServiced = (currentTime - ibatLastServiced);
if (ibatTimeSinceLastServiced >= IBATINTERVAL) {
ibatLastServiced = currentTime;
updateCurrentMeter((ibatTimeSinceLastServiced / 1000), &masterConfig.rxConfig, masterConfig.flight3DConfig.deadband3d_throttle);
}
}

131
src/main/sensors/battery.c
View File

@ -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,54 +61,107 @@ 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;
}
if (vbat <= batteryWarningVoltage) {
return BATTERY_WARNING;
}
return BATTERY_OK;
}
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;
/* 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;
}
/* 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;
}
}
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;
batteryState = BATTERY_NOTPRESENT;
batteryCellCount = 1;
batteryWarningVoltage = 0;
batteryCriticalVoltage = 0;
}
#define ADCVREF 3300 // in mV
int32_t currentSensorToCentiamps(uint16_t src)

11
src/main/sensors/battery.h
View File

@ -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);

5
src/main/telemetry/hott.c
View File

@ -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;
}

121
src/test/unit/battery_unittest.cc
View File

@ -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);
}
}

2
src/test/unit/ledstrip_unittest.cc
View File

@ -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;
}

12
src/test/unit/rc_controls_unittest.cc
View File

@ -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;
}
};

6
src/test/unit/telemetry_hott_unittest.cc
View File

@ -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;
}
}