From 5df102d6f5d5c4e2c853413fd697d453735dec38 Mon Sep 17 00:00:00 2001 From: mikeller Date: Sun, 20 Nov 2016 18:56:18 +1300 Subject: [PATCH] Adjusted current meter setting ranges to allow for negative offset. Also added biquad filter for current and did some cleanup. --- src/main/io/serial_cli.c | 4 +- src/main/sensors/battery.c | 111 ++++++++++++++++++++++--------------- src/main/sensors/battery.h | 2 +- 3 files changed, 69 insertions(+), 48 deletions(-) diff --git a/src/main/io/serial_cli.c b/src/main/io/serial_cli.c index 526bcb6cf..9cda846bb 100755 --- a/src/main/io/serial_cli.c +++ b/src/main/io/serial_cli.c @@ -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 } }, diff --git a/src/main/sensors/battery.c b/src/main/sensors/battery.c index dee83f43f..2250460a3 100644 --- a/src/main/sensors/battery.c +++ b/src/main/sensors/battery.c @@ -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; } } diff --git a/src/main/sensors/battery.h b/src/main/sensors/battery.h index cac334f85..b2dafca27 100644 --- a/src/main/sensors/battery.h +++ b/src/main/sensors/battery.h @@ -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.