Skip to content

Commit

Permalink
Battery auto-detect and LPF for battery monitoring
Browse files Browse the repository at this point in the history
  • Loading branch information
@DarkVegetableMatter @hydra
DarkVegetableMatter authored and hydra committed Aug 4, 2015
1 parent 26ac611 commit 942c892
Show file tree
Hide file tree
Showing 10 changed files with 267 additions and 68 deletions.
2 changes: 1 addition & 1 deletion src/main/io/ledstrip.c
View file
Original file line number Diff line number Diff line change
Expand Up @@ -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()) {
Expand Down
4 changes: 2 additions & 2 deletions src/main/io/serial_cli.c
View file
Original file line number Diff line number Diff line change
Expand Up @@ -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));
Expand Down
43 changes: 20 additions & 23 deletions src/main/mw.c
View file
Original file line number Diff line number Diff line change
Expand Up @@ -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;
Expand Down Expand Up @@ -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;
Expand Down Expand Up @@ -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
}
if (feature(FEATURE_VBAT)) {
/* 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;
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);
}
}

Expand Down
129 changes: 95 additions & 34 deletions src/main/sensors/battery.c
View file
Original file line number Diff line number Diff line change
Expand Up @@ -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

Expand All @@ -46,60 +51,116 @@ 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
// result is Vbatt in 0.1V steps. 3.3V = ADC Vref, 0xFFF = 12bit adc, 110 = 11:1 voltage divider (10k:1k) * 10 for 0.1V
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;
}

void batteryInit(batteryConfig_t *initialBatteryConfig)
batteryState_e getBatteryState(void)
{
batteryConfig = initialBatteryConfig;
return batteryState;
}

uint32_t i;
const char * batteryStateStrings[] = {"OK", "WARNING", "CRITICAL", "NOT PRESENT"};

for (i = 0; i < BATTERY_SAMPLE_COUNT; i++) {
updateBatteryVoltage();
delay((32 / BATTERY_SAMPLE_COUNT) * 10);
}
const char * getBatteryStateString(void)
{
return batteryStateStrings[batteryState];
}

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;
void batteryInit(batteryConfig_t *initialBatteryConfig)
{
batteryConfig = initialBatteryConfig;
batteryState = BATTERY_NOTPRESENT;
batteryCellCount = 1;
batteryWarningVoltage = 0;
batteryCriticalVoltage = 0;
}

#define ADCVREF 3300 // in mV
Expand Down
11 changes: 7 additions & 4 deletions src/main/sensors/battery.h
View file
Original file line number Diff line number Diff line change
Expand Up @@ -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;
Expand All @@ -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);
Expand Down
5 changes: 4 additions & 1 deletion src/main/telemetry/hott.c
View file
Original file line number Diff line number Diff line change
Expand Up @@ -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;
}
Expand Down
Loading

0 comments on commit 942c892

Please sign in to comment.