diff --git a/src/main/fc/config.c b/src/main/fc/config.c index ad3c3a5b8..92cddade9 100755 --- a/src/main/fc/config.c +++ b/src/main/fc/config.c @@ -430,6 +430,9 @@ void resetBatteryConfig(batteryConfig_t *batteryConfig) batteryConfig->batteryCapacity = 0; batteryConfig->currentMeterType = CURRENT_SENSOR_ADC; batteryConfig->batterynotpresentlevel = 55; // VBAT below 5.5 V will be igonored + batteryConfig->useVBatAlerts = true; + batteryConfig->useConsumptionAlerts = false; + batteryConfig->consumptionWarningPercentage = 10; } #ifdef SWAP_SERIAL_PORT_0_AND_1_DEFAULTS diff --git a/src/main/io/serial_cli.c b/src/main/io/serial_cli.c index b7039530a..5ee05f6cd 100755 --- a/src/main/io/serial_cli.c +++ b/src/main/io/serial_cli.c @@ -798,6 +798,9 @@ const clivalue_t valueTable[] = { { "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 } }, + { "use_vbat_alerts", VAR_UINT8 | MASTER_VALUE | MODE_LOOKUP, &masterConfig.batteryConfig.useVBatAlerts, .config.lookup = { TABLE_OFF_ON } }, + { "use_consumption_alerts", VAR_UINT8 | MASTER_VALUE | MODE_LOOKUP, &masterConfig.batteryConfig.useConsumptionAlerts, .config.lookup = { TABLE_OFF_ON } }, + { "consumption_warning_percentage", VAR_UINT8 | MASTER_VALUE, &masterConfig.batteryConfig.consumptionWarningPercentage, .config.minmax = { 0, 100 } }, { "align_gyro", VAR_UINT8 | MASTER_VALUE | MODE_LOOKUP, &masterConfig.sensorAlignmentConfig.gyro_align, .config.lookup = { TABLE_ALIGNMENT } }, { "align_acc", VAR_UINT8 | MASTER_VALUE | MODE_LOOKUP, &masterConfig.sensorAlignmentConfig.acc_align, .config.lookup = { TABLE_ALIGNMENT } }, { "align_mag", VAR_UINT8 | MASTER_VALUE | MODE_LOOKUP, &masterConfig.sensorAlignmentConfig.mag_align, .config.lookup = { TABLE_ALIGNMENT } }, diff --git a/src/main/sensors/battery.c b/src/main/sensors/battery.c index 2250460a3..f1330bf41 100644 --- a/src/main/sensors/battery.c +++ b/src/main/sensors/battery.c @@ -58,12 +58,14 @@ uint16_t batteryCriticalVoltage; uint16_t vbat = 0; // battery voltage in 0.1V steps (filtered) uint16_t vbatLatest = 0; // most recent unsmoothed value -uint16_t amperageLatest = 0; // most recent value int32_t amperage = 0; // amperage read by current sensor in centiampere (1/100th A) +uint16_t amperageLatest = 0; // most recent value + int32_t mAhDrawn = 0; // milliampere hours drawn from the battery since start -static batteryState_e batteryState; +static batteryState_e vBatState; +static batteryState_e consumptionState; static uint16_t batteryAdcToVoltage(uint16_t src) { @@ -74,12 +76,12 @@ static uint16_t batteryAdcToVoltage(uint16_t src) static void updateBatteryVoltage(void) { - static biquadFilter_t vbatFilter; - static bool vbatFilterIsInitialised; + static biquadFilter_t vBatFilter; + static bool vBatFilterIsInitialised; - if (!vbatFilterIsInitialised) { - biquadFilterInitLPF(&vbatFilter, VBAT_LPF_FREQ, 50000); //50HZ Update - vbatFilterIsInitialised = true; + if (!vBatFilterIsInitialised) { + biquadFilterInitLPF(&vBatFilter, VBAT_LPF_FREQ, 50000); //50HZ Update + vBatFilterIsInitialised = true; } #ifdef USE_ESC_TELEMETRY @@ -88,17 +90,17 @@ static void updateBatteryVoltage(void) if (debugMode == DEBUG_BATTERY) { debug[0] = -1; } - vbat = biquadFilterApply(&vbatFilter, vbatLatest); + vbat = biquadFilterApply(&vBatFilter, vbatLatest); } else #endif { - uint16_t vbatSample = adcGetChannel(ADC_BATTERY); + uint16_t vBatSample = adcGetChannel(ADC_BATTERY); if (debugMode == DEBUG_BATTERY) { - debug[0] = vbatSample; + debug[0] = vBatSample; } - vbat = batteryAdcToVoltage(biquadFilterApply(&vbatFilter, vbatSample)); - vbatLatest = batteryAdcToVoltage(vbatSample); + vbat = batteryAdcToVoltage(biquadFilterApply(&vBatFilter, vBatSample)); + vbatLatest = batteryAdcToVoltage(vBatSample); } if (debugMode == DEBUG_BATTERY) { @@ -106,18 +108,35 @@ static void updateBatteryVoltage(void) } } +static void updateBatteryAlert(void) +{ + switch(getBatteryState()) { + case BATTERY_WARNING: + beeper(BEEPER_BAT_LOW); + + break; + case BATTERY_CRITICAL: + beeper(BEEPER_BAT_CRIT_LOW); + + break; + case BATTERY_OK: + case BATTERY_NOT_PRESENT: + break; + } +} + void updateBattery(void) { - uint16_t vbatPrevious = vbatLatest; + uint16_t vBatPrevious = vbatLatest; updateBatteryVoltage(); - uint16_t vbatMeasured = vbatLatest; + uint16_t vBatMeasured = vbatLatest; /* battery has just been connected*/ - if (batteryState == BATTERY_NOT_PRESENT && (ARMING_FLAG(ARMED) || (vbat > batteryConfig->batterynotpresentlevel && ABS(vbatMeasured - vbatPrevious) <= VBAT_STABLE_MAX_DELTA))) { + if (vBatState == BATTERY_NOT_PRESENT && (ARMING_FLAG(ARMED) || (vbat > batteryConfig->batterynotpresentlevel && ABS(vBatMeasured - vBatPrevious) <= VBAT_STABLE_MAX_DELTA))) { /* Actual battery state is calculated below, this is really BATTERY_PRESENT */ - batteryState = BATTERY_OK; + vBatState = BATTERY_OK; - unsigned cells = (vbatMeasured / batteryConfig->vbatmaxcellvoltage) + 1; + unsigned cells = (vBatMeasured / batteryConfig->vbatmaxcellvoltage) + 1; if (cells > 8) { // something is wrong, we expect 8 cells maximum (and autodetection will be problematic at 6+ cells) cells = 8; @@ -126,50 +145,55 @@ void updateBattery(void) 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 batteryConfig->batterynotpresentlevel */ - } else if (batteryState != BATTERY_NOT_PRESENT && !ARMING_FLAG(ARMED) && vbat <= batteryConfig->batterynotpresentlevel && ABS(vbatMeasured - vbatPrevious) <= VBAT_STABLE_MAX_DELTA) { - batteryState = BATTERY_NOT_PRESENT; + } else if (vBatState != BATTERY_NOT_PRESENT && !ARMING_FLAG(ARMED) && vbat <= batteryConfig->batterynotpresentlevel && ABS(vBatMeasured - vBatPrevious) <= VBAT_STABLE_MAX_DELTA) { + vBatState = BATTERY_NOT_PRESENT; batteryCellCount = 0; batteryWarningVoltage = 0; batteryCriticalVoltage = 0; } if (debugMode == DEBUG_BATTERY) { - debug[2] = batteryState; + debug[2] = vBatState; debug[3] = batteryCellCount; } - switch(batteryState) { - case BATTERY_OK: - if (vbat <= (batteryWarningVoltage - batteryConfig->vbathysteresis)) { - batteryState = BATTERY_WARNING; - beeper(BEEPER_BAT_LOW); - } - break; - case BATTERY_WARNING: - if (vbat <= (batteryCriticalVoltage - batteryConfig->vbathysteresis)) { - batteryState = BATTERY_CRITICAL; - beeper(BEEPER_BAT_CRIT_LOW); - } else if (vbat > batteryWarningVoltage) { - batteryState = BATTERY_OK; - } else { - beeper(BEEPER_BAT_LOW); - } - break; - case BATTERY_CRITICAL: - if (vbat > batteryCriticalVoltage) { - batteryState = BATTERY_WARNING; - beeper(BEEPER_BAT_LOW); - } else { - beeper(BEEPER_BAT_CRIT_LOW); - } - break; - case BATTERY_NOT_PRESENT: - break; + if (batteryConfig->useVBatAlerts) { + switch(vBatState) { + case BATTERY_OK: + if (vbat <= (batteryWarningVoltage - batteryConfig->vbathysteresis)) { + vBatState = BATTERY_WARNING; + } + + break; + case BATTERY_WARNING: + if (vbat <= (batteryCriticalVoltage - batteryConfig->vbathysteresis)) { + vBatState = BATTERY_CRITICAL; + } else if (vbat > batteryWarningVoltage) { + vBatState = BATTERY_OK; + } + + break; + case BATTERY_CRITICAL: + if (vbat > batteryCriticalVoltage) { + vBatState = BATTERY_WARNING; + } + + break; + case BATTERY_NOT_PRESENT: + break; + } + + updateBatteryAlert(); } } batteryState_e getBatteryState(void) { + batteryState_e batteryState = BATTERY_NOT_PRESENT; + if (vBatState != BATTERY_NOT_PRESENT) { + batteryState = MAX(vBatState, consumptionState); + } + return batteryState; } @@ -177,13 +201,14 @@ const char * const batteryStateStrings[] = {"OK", "WARNING", "CRITICAL", "NOT PR const char * getBatteryStateString(void) { - return batteryStateStrings[batteryState]; + return batteryStateStrings[getBatteryState()]; } void batteryInit(batteryConfig_t *initialBatteryConfig) { batteryConfig = initialBatteryConfig; - batteryState = BATTERY_NOT_PRESENT; + vBatState = BATTERY_NOT_PRESENT; + consumptionState = BATTERY_OK; batteryCellCount = 0; batteryWarningVoltage = 0; batteryCriticalVoltage = 0; @@ -201,22 +226,25 @@ static int32_t currentSensorToCentiamps(uint16_t src) static void updateBatteryCurrent(void) { - static biquadFilter_t ibatFilter; - static bool ibatFilterIsInitialised; + static biquadFilter_t iBatFilter; + static bool iBatFilterIsInitialised; - if (!ibatFilterIsInitialised) { - biquadFilterInitLPF(&ibatFilter, IBAT_LPF_FREQ, 50000); //50HZ Update - ibatFilterIsInitialised = true; + if (!iBatFilterIsInitialised) { + biquadFilterInitLPF(&iBatFilter, IBAT_LPF_FREQ, 50000); //50HZ Update + iBatFilterIsInitialised = true; } - uint16_t ibatSample = adcGetChannel(ADC_CURRENT); - amperage = currentSensorToCentiamps(biquadFilterApply(&ibatFilter, ibatSample)); + uint16_t iBatSample = adcGetChannel(ADC_CURRENT); + amperage = currentSensorToCentiamps(biquadFilterApply(&iBatFilter, iBatSample)); + amperageLatest = currentSensorToCentiamps(iBatSample); } static void updateCurrentDrawn(int32_t lastUpdateAt) { - int mAhDrawnRaw = (amperage * lastUpdateAt) / 1000; - mAhDrawn = mAhDrawn + mAhDrawnRaw / (3600 * 100); + static float mAhDrawnF = 0.0f; // used to get good enough resolution + + mAhDrawnF = mAhDrawnF + (amperage * lastUpdateAt / (100.0f * 1000 * 3600)); + mAhDrawn = mAhDrawnF; } void updateCurrentMeter(int32_t lastUpdateAt, rxConfig_t *rxConfig, uint16_t deadband3d_throttle) @@ -257,6 +285,28 @@ void updateCurrentMeter(int32_t lastUpdateAt, rxConfig_t *rxConfig, uint16_t dea break; } + + if (batteryConfig->useConsumptionAlerts) { + switch(consumptionState) { + case BATTERY_OK: + if (calculateBatteryCapacityRemainingPercentage() <= batteryConfig->consumptionWarningPercentage) { + consumptionState = BATTERY_WARNING; + } + + break; + case BATTERY_WARNING: + if (calculateBatteryCapacityRemainingPercentage() == 0) { + vBatState = BATTERY_CRITICAL; + } + + break; + case BATTERY_CRITICAL: + case BATTERY_NOT_PRESENT: + break; + } + + updateBatteryAlert(); + } } float calculateVbatPidCompensation(void) { diff --git a/src/main/sensors/battery.h b/src/main/sensors/battery.h index b2dafca27..872e01700 100644 --- a/src/main/sensors/battery.h +++ b/src/main/sensors/battery.h @@ -60,6 +60,9 @@ typedef struct batteryConfig_s { uint8_t multiwiiCurrentMeterOutput; // if set to 1 output the amperage in milliamp steps instead of 0.01A steps via msp uint16_t batteryCapacity; // mAh uint8_t batterynotpresentlevel; // Below this level battery is considered as not present + bool useVBatAlerts; // Issue alerts based on VBat readings + bool useConsumptionAlerts; // Issue alerts based on total power consumption + uint8_t consumptionWarningPercentage; // Percentage of remaining capacity that should trigger a battery warning } batteryConfig_t; typedef enum { diff --git a/src/main/target/NAZE/target.h b/src/main/target/NAZE/target.h index 82bc69ba7..184ea76ee 100644 --- a/src/main/target/NAZE/target.h +++ b/src/main/target/NAZE/target.h @@ -20,6 +20,8 @@ #define TARGET_CONFIG #define USE_HARDWARE_REVISION_DETECTION +#define CLI_MINIMAL_VERBOSITY + #define BOARD_HAS_VOLTAGE_DIVIDER #define LED0 PB3