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

Merge pull request #1614 from mikeller/adjust_current_meter_settings 

Adjusted current meter setting ranges to allow for negative offset. Also added biquad filter for current and did some cleanup.
This commit is contained in:
Michael Keller 2016-11-25 20:13:55 +13:00 committed by GitHub
parent 75110a8299 5df102d6f5
commit 82e78bc249
3 changed files with 69 additions and 48 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

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

@ -789,8 +789,8 @@ const clivalue_t valueTable[] = {
{ "vbat_min_cell_voltage", VAR_UINT8 | MASTER_VALUE, &masterConfig.batteryConfig.vbatmincellvoltage, .config.minmax = { 10, 50 } },
{ "vbat_warning_cell_voltage", VAR_UINT8 | MASTER_VALUE, &masterConfig.batteryConfig.vbatwarningcellvoltage, .config.minmax = { 10, 50 } },
{ "vbat_hysteresis", VAR_UINT8 | MASTER_VALUE, &masterConfig.batteryConfig.vbathysteresis, .config.minmax = { 0, 250 } },
{ "current_meter_scale", VAR_INT16 | MASTER_VALUE, &masterConfig.batteryConfig.currentMeterScale, .config.minmax = { -10000, 10000 } },
{ "current_meter_offset", VAR_UINT16 | MASTER_VALUE, &masterConfig.batteryConfig.currentMeterOffset, .config.minmax = { 0, 3300 } },
{ "current_meter_scale", VAR_INT16 | MASTER_VALUE, &masterConfig.batteryConfig.currentMeterScale, .config.minmax = { -16000, 16000 } },
{ "current_meter_offset", VAR_INT16 | MASTER_VALUE, &masterConfig.batteryConfig.currentMeterOffset, .config.minmax = { -16000, 16000 } },
{ "multiwii_current_meter_output", VAR_UINT8 | MASTER_VALUE | MODE_LOOKUP, &masterConfig.batteryConfig.multiwiiCurrentMeterOutput, .config.lookup = { TABLE_OFF_ON } },
{ "current_meter_type", VAR_UINT8 | MASTER_VALUE | MODE_LOOKUP, &masterConfig.batteryConfig.currentMeterType, .config.lookup = { TABLE_CURRENT_SENSOR } },
{ "battery_notpresent_level", VAR_UINT8 | MASTER_VALUE, &masterConfig.batteryConfig.batterynotpresentlevel, .config.minmax = { 0, 200 } },

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

@ -44,7 +44,12 @@
#include "common/utils.h"
#define VBATT_LPF_FREQ 0.4f
#define VBAT_LPF_FREQ 0.4f
#define IBAT_LPF_FREQ 0.4f
#define VBAT_STABLE_MAX_DELTA 2
#define ADCVREF 3300 // in mV
// Battery monitoring stuff
uint8_t batteryCellCount;
@ -72,32 +77,35 @@ static void updateBatteryVoltage(void)
static biquadFilter_t vbatFilter;
static bool vbatFilterIsInitialised;
// store the battery voltage with some other recent battery voltage readings
uint16_t vbatSample;
if (!vbatFilterIsInitialised) {
biquadFilterInitLPF(&vbatFilter, VBAT_LPF_FREQ, 50000); //50HZ Update
vbatFilterIsInitialised = true;
}
#ifdef USE_ESC_TELEMETRY
if (batteryConfig->batteryMeterType == BATTERY_SENSOR_ESC && isEscTelemetryActive()) {
vbatSample = vbatLatest = getEscTelemetryVbat();
vbatLatest = getEscTelemetryVbat();
if (debugMode == DEBUG_BATTERY) {
debug[0] = -1;
}
vbat = biquadFilterApply(&vbatFilter, vbatLatest);
}
else
#endif
{
vbatSample = vbatLatest = batteryAdcToVoltage(adcGetChannel(ADC_BATTERY));
uint16_t vbatSample = adcGetChannel(ADC_BATTERY);
if (debugMode == DEBUG_BATTERY) {
debug[0] = vbatSample;
}
vbat = batteryAdcToVoltage(biquadFilterApply(&vbatFilter, vbatSample));
vbatLatest = batteryAdcToVoltage(vbatSample);
}
if (debugMode == DEBUG_BATTERY) debug[0] = vbatSample;
if (!vbatFilterIsInitialised) {
biquadFilterInitLPF(&vbatFilter, VBATT_LPF_FREQ, 50000); //50HZ Update
vbatFilterIsInitialised = true;
if (debugMode == DEBUG_BATTERY) {
debug[1] = vbat;
}
vbat = biquadFilterApply(&vbatFilter, vbatSample);
if (debugMode == DEBUG_BATTERY) debug[1] = vbat;
}
#define VBAT_STABLE_MAX_DELTA 2
void updateBattery(void)
{
uint16_t vbatPrevious = vbatLatest;
@ -125,11 +133,12 @@ void updateBattery(void)
batteryCriticalVoltage = 0;
}
if (debugMode == DEBUG_BATTERY) debug[2] = batteryState;
if (debugMode == DEBUG_BATTERY) debug[3] = batteryCellCount;
if (debugMode == DEBUG_BATTERY) {
debug[2] = batteryState;
debug[3] = batteryCellCount;
}
switch(batteryState)
{
switch(batteryState) {
case BATTERY_OK:
if (vbat <= (batteryWarningVoltage - batteryConfig->vbathysteresis)) {
batteryState = BATTERY_WARNING;
@ -180,7 +189,6 @@ void batteryInit(batteryConfig_t *initialBatteryConfig)
batteryCriticalVoltage = 0;
}
#define ADCVREF 3300 // in mV
static int32_t currentSensorToCentiamps(uint16_t src)
{
int32_t millivolts;
@ -191,50 +199,63 @@ static int32_t currentSensorToCentiamps(uint16_t src)
return (millivolts * 1000) / (int32_t)batteryConfig->currentMeterScale; // current in 0.01A steps
}
static void updateBatteryCurrent(void)
{
static biquadFilter_t ibatFilter;
static bool ibatFilterIsInitialised;
if (!ibatFilterIsInitialised) {
biquadFilterInitLPF(&ibatFilter, IBAT_LPF_FREQ, 50000); //50HZ Update
ibatFilterIsInitialised = true;
}
uint16_t ibatSample = adcGetChannel(ADC_CURRENT);
amperage = currentSensorToCentiamps(biquadFilterApply(&ibatFilter, ibatSample));
}
static void updateCurrentDrawn(int32_t lastUpdateAt)
{
int mAhDrawnRaw = (amperage * lastUpdateAt) / 1000;
mAhDrawn = mAhDrawn + mAhDrawnRaw / (3600 * 100);
}
void updateCurrentMeter(int32_t lastUpdateAt, rxConfig_t *rxConfig, uint16_t deadband3d_throttle)
{
#ifdef USE_ESC_TELEMETRY
UNUSED(lastUpdateAt);
#endif
static int64_t mAhdrawnRaw = 0;
static int32_t amperageRaw = 0;
int32_t throttleOffset = (int32_t)rcCommand[THROTTLE] - 1000;
int32_t throttleFactor = 0;
switch(batteryConfig->currentMeterType) {
case CURRENT_SENSOR_ADC:
amperageRaw -= amperageRaw / 8;
amperageRaw += adcGetChannel(ADC_CURRENT);
amperage = amperageLatest = currentSensorToCentiamps(amperageRaw / 8);
updateBatteryCurrent();
updateCurrentDrawn(lastUpdateAt);
break;
case CURRENT_SENSOR_VIRTUAL:
amperage = (int32_t)batteryConfig->currentMeterOffset;
if (ARMING_FLAG(ARMED)) {
throttleStatus_e throttleStatus = calculateThrottleStatus(rxConfig, deadband3d_throttle);
if (throttleStatus == THROTTLE_LOW && feature(FEATURE_MOTOR_STOP))
int throttleOffset = (int32_t)rcCommand[THROTTLE] - 1000;
if (throttleStatus == THROTTLE_LOW && feature(FEATURE_MOTOR_STOP)) {
throttleOffset = 0;
throttleFactor = throttleOffset + (throttleOffset * throttleOffset / 50);
}
int throttleFactor = throttleOffset + (throttleOffset * throttleOffset / 50);
amperage += throttleFactor * (int32_t)batteryConfig->currentMeterScale / 1000;
}
break;
case CURRENT_SENSOR_NONE:
amperage = 0;
updateCurrentDrawn(lastUpdateAt);
break;
case CURRENT_SENSOR_ESC:
#ifdef USE_ESC_TELEMETRY
if (batteryConfig->currentMeterType == CURRENT_SENSOR_ESC && isEscTelemetryActive()) {
if (isEscTelemetryActive()) {
amperage = getEscTelemetryCurrent();
mAhDrawn = getEscTelemetryConsumption();
}
#endif
break;
}
if (!feature(FEATURE_ESC_TELEMETRY)) {
mAhdrawnRaw += (amperage * lastUpdateAt) / 1000;
mAhDrawn = mAhdrawnRaw / (3600 * 100);
break;
#endif
case CURRENT_SENSOR_NONE:
amperage = 0;
break;
}
}

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

@ -53,7 +53,7 @@ typedef struct batteryConfig_s {
batterySensor_e batteryMeterType; // type of battery meter uses, either ADC or ESC
int16_t currentMeterScale; // scale the current sensor output voltage to milliamps. Value in 1/10th mV/A
uint16_t currentMeterOffset; // offset of the current sensor in millivolt steps
int16_t currentMeterOffset; // offset of the current sensor in millivolt steps
currentSensor_e currentMeterType; // type of current meter used, either ADC, Virtual or ESC
// FIXME this doesn't belong in here since it's a concern of MSP, not of the battery code.