src/main/sensors/battery.c

   1 /*
   2  * This file is part of Cleanflight and Betaflight.
   3  *
   4  * Cleanflight and Betaflight are free software. You can redistribute
   5  * this software and/or modify this software under the terms of the
   6  * GNU General Public License as published by the Free Software
   7  * Foundation, either version 3 of the License, or (at your option)
   8  * any later version.
   9  *
  10  * Cleanflight and Betaflight are distributed in the hope that they
  11  * will be useful, but WITHOUT ANY WARRANTY; without even the implied
  12  * warranty of MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.
  13  * See the GNU General Public License for more details.
  14  *
  15  * You should have received a copy of the GNU General Public License
  16  * along with this software.
  17  *
  18  * If not, see <http://www.gnu.org/licenses/>.
  19  */
  20
  21 #include "stdbool.h"
  22 #include "stdint.h"
  23 #include "math.h"
  24
  25 #include "platform.h"
  26
  27 #include "build/debug.h"
  28
  29 #include "common/filter.h"
  30 #include "common/maths.h"
  31 #include "common/utils.h"
  32
  33 #include "config/config.h"
  34 #include "config/feature.h"
  35
  36 #include "drivers/adc.h"
  37
  38 #include "fc/runtime_config.h"
  39 #include "fc/rc_controls.h"
  40
  41 #include "flight/mixer.h"
  42
  43 #include "io/beeper.h"
  44
  45 #include "pg/pg.h"
  46 #include "pg/pg_ids.h"
  47
  48 #include "sensors/battery.h"
  49
  50 /**
  51  * terminology: meter vs sensors
  52  *
  53  * voltage and current sensors are used to collect data.
  54  * - e.g. voltage at an MCU ADC input pin, value from an ESC sensor.
  55  *   sensors require very specific configuration, such as resistor values.
  56  * voltage and current meters are used to process and expose data collected from sensors to the rest of the system.
  57  * - e.g. a meter exposes normalized, and often filtered, values from a sensor.
  58  *   meters require different or little configuration.
  59  *   meters also have different precision concerns, and may use different units to the sensors.
  60  *
  61  */
  62
  63 #define VBAT_STABLE_MAX_DELTA 20
  64 #define LVC_AFFECT_TIME 10000000 //10 secs for the LVC to slowly kick in
  65
  66 // Battery monitoring stuff
  67 static uint8_t batteryCellCount; // Note: this can be 0 when no battery is detected or when the battery voltage sensor is missing or disabled.
  68 static uint16_t batteryWarningVoltage;
  69 static uint16_t batteryCriticalVoltage;
  70 static uint16_t batteryWarningHysteresisVoltage;
  71 static uint16_t batteryCriticalHysteresisVoltage;
  72 static lowVoltageCutoff_t lowVoltageCutoff;
  73 //
  74 static currentMeter_t currentMeter;
  75 static voltageMeter_t voltageMeter;
  76
  77 static batteryState_e batteryState;
  78 static batteryState_e voltageState;
  79 static batteryState_e consumptionState;
  80
  81 #ifndef DEFAULT_CURRENT_METER_SOURCE
  82 #ifdef USE_VIRTUAL_CURRENT_METER
  83 #define DEFAULT_CURRENT_METER_SOURCE CURRENT_METER_VIRTUAL
  84 #else
  85 #ifdef USE_MSP_CURRENT_METER
  86 #define DEFAULT_CURRENT_METER_SOURCE CURRENT_METER_MSP
  87 #else
  88 #define DEFAULT_CURRENT_METER_SOURCE CURRENT_METER_NONE
  89 #endif
  90 #endif
  91 #endif
  92
  93 #ifndef DEFAULT_VOLTAGE_METER_SOURCE
  94 #define DEFAULT_VOLTAGE_METER_SOURCE VOLTAGE_METER_NONE
  95 #endif
  96
  97 PG_REGISTER_WITH_RESET_TEMPLATE(batteryConfig_t, batteryConfig, PG_BATTERY_CONFIG, 3);
  98
  99 PG_RESET_TEMPLATE(batteryConfig_t, batteryConfig,
 100     // voltage
 101     .vbatmaxcellvoltage = VBAT_CELL_VOLTAGE_DEFAULT_MAX,
 102     .vbatmincellvoltage = VBAT_CELL_VOLTAGE_DEFAULT_MIN,
 103     .vbatwarningcellvoltage = 350,
 104     .vbatnotpresentcellvoltage = 300, //A cell below 3 will be ignored
 105     .voltageMeterSource = DEFAULT_VOLTAGE_METER_SOURCE,
 106     .lvcPercentage = 100, //Off by default at 100%
 107
 108     // current
 109     .batteryCapacity = 0,
 110     .currentMeterSource = DEFAULT_CURRENT_METER_SOURCE,
 111
 112     // cells
 113     .forceBatteryCellCount = 0, //0 will be ignored
 114
 115     // warnings / alerts
 116     .useVBatAlerts = true,
 117     .useConsumptionAlerts = false,
 118     .consumptionWarningPercentage = 10,
 119     .vbathysteresis = 1, // 0.01V
 120
 121     .vbatfullcellvoltage = 410,
 122
 123     .vbatDisplayLpfPeriod = 30,
 124     .vbatSagLpfPeriod = 2,
 125     .ibatLpfPeriod = 10,
 126     .vbatDurationForWarning = 0,
 127     .vbatDurationForCritical = 0,
 128 );
 129
 130 void batteryUpdateVoltage(timeUs_t currentTimeUs)
 131 {
 132     UNUSED(currentTimeUs);
 133
 134     switch (batteryConfig()->voltageMeterSource) {
 135 #ifdef USE_ESC_SENSOR
 136         case VOLTAGE_METER_ESC:
 137             if (featureIsEnabled(FEATURE_ESC_SENSOR)) {
 138                 voltageMeterESCRefresh();
 139                 voltageMeterESCReadCombined(&voltageMeter);
 140             }
 141             break;
 142 #endif
 143         case VOLTAGE_METER_ADC:
 144             voltageMeterADCRefresh();
 145             voltageMeterADCRead(VOLTAGE_SENSOR_ADC_VBAT, &voltageMeter);
 146             break;
 147
 148         default:
 149         case VOLTAGE_METER_NONE:
 150             voltageMeterReset(&voltageMeter);
 151             break;
 152     }
 153
 154     DEBUG_SET(DEBUG_BATTERY, 0, voltageMeter.unfiltered);
 155     DEBUG_SET(DEBUG_BATTERY, 1, voltageMeter.displayFiltered);
 156 }
 157
 158 static void updateBatteryBeeperAlert(void)
 159 {
 160     switch (getBatteryState()) {
 161         case BATTERY_WARNING:
 162             beeper(BEEPER_BAT_LOW);
 163
 164             break;
 165         case BATTERY_CRITICAL:
 166             beeper(BEEPER_BAT_CRIT_LOW);
 167
 168             break;
 169         case BATTERY_OK:
 170         case BATTERY_NOT_PRESENT:
 171         case BATTERY_INIT:
 172             break;
 173     }
 174 }
 175
 176 //TODO: make all of these independent of voltage filtering for display
 177
 178 static bool isVoltageStable(void)
 179 {
 180     return ABS(voltageMeter.displayFiltered - voltageMeter.unfiltered) <= VBAT_STABLE_MAX_DELTA;
 181 }
 182
 183 static bool isVoltageFromBat(void)
 184 {
 185     // We want to disable battery getting detected around USB voltage or 0V
 186
 187     return (voltageMeter.displayFiltered >= batteryConfig()->vbatnotpresentcellvoltage  // Above ~0V
 188         && voltageMeter.displayFiltered <= batteryConfig()->vbatmaxcellvoltage)  // 1s max cell voltage check
 189         || voltageMeter.displayFiltered > batteryConfig()->vbatnotpresentcellvoltage * 2; // USB voltage - 2s or more check
 190 }
 191
 192 void batteryUpdatePresence(void)
 193 {
 194
 195
 196     if ((voltageState == BATTERY_NOT_PRESENT || voltageState == BATTERY_INIT) && isVoltageFromBat() && isVoltageStable()) {
 197         // Battery has just been connected - calculate cells, warning voltages and reset state
 198
 199         consumptionState = voltageState = BATTERY_OK;
 200         if (batteryConfig()->forceBatteryCellCount != 0) {
 201             batteryCellCount = batteryConfig()->forceBatteryCellCount;
 202         } else {
 203             unsigned cells = (voltageMeter.displayFiltered / batteryConfig()->vbatmaxcellvoltage) + 1;
 204             if (cells > MAX_AUTO_DETECT_CELL_COUNT) {
 205                 // something is wrong, we expect MAX_CELL_COUNT cells maximum (and autodetection will be problematic at 6+ cells)
 206                 cells = MAX_AUTO_DETECT_CELL_COUNT;
 207             }
 208             batteryCellCount = cells;
 209
 210             if (!ARMING_FLAG(ARMED)) {
 211                 changePidProfileFromCellCount(batteryCellCount);
 212             }
 213         }
 214         batteryWarningVoltage = batteryCellCount * batteryConfig()->vbatwarningcellvoltage;
 215         batteryCriticalVoltage = batteryCellCount * batteryConfig()->vbatmincellvoltage;
 216         batteryWarningHysteresisVoltage = (batteryWarningVoltage > batteryConfig()->vbathysteresis) ? batteryWarningVoltage - batteryConfig()->vbathysteresis : 0;
 217         batteryCriticalHysteresisVoltage = (batteryCriticalVoltage > batteryConfig()->vbathysteresis) ? batteryCriticalVoltage - batteryConfig()->vbathysteresis : 0;
 218         lowVoltageCutoff.percentage = 100;
 219         lowVoltageCutoff.startTime = 0;
 220     } else if (voltageState != BATTERY_NOT_PRESENT && isVoltageStable() && !isVoltageFromBat()) {
 221         /* battery has been disconnected - can take a while for filter cap to disharge so we use a threshold of batteryConfig()->vbatnotpresentcellvoltage */
 222
 223         consumptionState = voltageState = BATTERY_NOT_PRESENT;
 224
 225         batteryCellCount = 0;
 226         batteryWarningVoltage = 0;
 227         batteryCriticalVoltage = 0;
 228         batteryWarningHysteresisVoltage = 0;
 229         batteryCriticalHysteresisVoltage = 0;
 230     }
 231 }
 232
 233 static void batteryUpdateVoltageState(void)
 234 {
 235     // alerts are currently used by beeper, osd and other subsystems
 236     static uint32_t lastVoltageChangeMs;
 237     switch (voltageState) {
 238         case BATTERY_OK:
 239             if (voltageMeter.displayFiltered <= batteryWarningHysteresisVoltage) {
 240                 if (cmp32(millis(), lastVoltageChangeMs) >= batteryConfig()->vbatDurationForWarning * 100) {
 241                     voltageState = BATTERY_WARNING;
 242                 }
 243             } else {
 244                 lastVoltageChangeMs = millis();
 245             }
 246             break;
 247
 248         case BATTERY_WARNING:
 249             if (voltageMeter.displayFiltered <= batteryCriticalHysteresisVoltage) {
 250                 if (cmp32(millis(), lastVoltageChangeMs) >= batteryConfig()->vbatDurationForCritical * 100) {
 251                     voltageState = BATTERY_CRITICAL;
 252                 }
 253             } else {
 254                 if (voltageMeter.displayFiltered > batteryWarningVoltage) {
 255                     voltageState = BATTERY_OK;
 256                 }
 257                 lastVoltageChangeMs = millis();
 258             }
 259             break;
 260
 261         case BATTERY_CRITICAL:
 262             if (voltageMeter.displayFiltered > batteryCriticalVoltage) {
 263                 voltageState = BATTERY_WARNING;
 264                 lastVoltageChangeMs = millis();
 265             }
 266             break;
 267
 268         default:
 269             break;
 270     }
 271
 272 }
 273
 274 static void batteryUpdateLVC(timeUs_t currentTimeUs)
 275 {
 276     if (batteryConfig()->lvcPercentage < 100) {
 277         if (voltageState == BATTERY_CRITICAL && !lowVoltageCutoff.enabled) {
 278             lowVoltageCutoff.enabled = true;
 279             lowVoltageCutoff.startTime = currentTimeUs;
 280             lowVoltageCutoff.percentage = 100;
 281         }
 282         if (lowVoltageCutoff.enabled) {
 283             if (cmp32(currentTimeUs,lowVoltageCutoff.startTime) < LVC_AFFECT_TIME) {
 284                 lowVoltageCutoff.percentage = 100 - (cmp32(currentTimeUs,lowVoltageCutoff.startTime) * (100 - batteryConfig()->lvcPercentage) / LVC_AFFECT_TIME);
 285             }
 286             else {
 287                 lowVoltageCutoff.percentage = batteryConfig()->lvcPercentage;
 288             }
 289         }
 290     }
 291
 292 }
 293
 294 static void batteryUpdateConsumptionState(void)
 295 {
 296     if (batteryConfig()->useConsumptionAlerts && batteryConfig()->batteryCapacity > 0 && batteryCellCount > 0) {
 297         uint8_t batteryPercentageRemaining = calculateBatteryPercentageRemaining();
 298
 299         if (batteryPercentageRemaining == 0) {
 300             consumptionState = BATTERY_CRITICAL;
 301         } else if (batteryPercentageRemaining <= batteryConfig()->consumptionWarningPercentage) {
 302             consumptionState = BATTERY_WARNING;
 303         } else {
 304             consumptionState = BATTERY_OK;
 305         }
 306     }
 307 }
 308
 309 void batteryUpdateStates(timeUs_t currentTimeUs)
 310 {
 311     batteryUpdateVoltageState();
 312     batteryUpdateConsumptionState();
 313     batteryUpdateLVC(currentTimeUs);
 314     batteryState = MAX(voltageState, consumptionState);
 315 }
 316
 317 const lowVoltageCutoff_t *getLowVoltageCutoff(void)
 318 {
 319     return &lowVoltageCutoff;
 320 }
 321
 322 batteryState_e getBatteryState(void)
 323 {
 324     return batteryState;
 325 }
 326
 327 batteryState_e getVoltageState(void)
 328 {
 329     return voltageState;
 330 }
 331
 332 batteryState_e getConsumptionState(void)
 333 {
 334     return consumptionState;
 335 }
 336
 337 const char * const batteryStateStrings[] = {"OK", "WARNING", "CRITICAL", "NOT PRESENT", "INIT"};
 338
 339 const char * getBatteryStateString(void)
 340 {
 341     return batteryStateStrings[getBatteryState()];
 342 }
 343
 344 void batteryInit(void)
 345 {
 346     //
 347     // presence
 348     //
 349     batteryState = BATTERY_INIT;
 350     batteryCellCount = 0;
 351
 352     //
 353     // voltage
 354     //
 355     voltageState = BATTERY_INIT;
 356     batteryWarningVoltage = 0;
 357     batteryCriticalVoltage = 0;
 358     batteryWarningHysteresisVoltage = 0;
 359     batteryCriticalHysteresisVoltage = 0;
 360     lowVoltageCutoff.enabled = false;
 361     lowVoltageCutoff.percentage = 100;
 362     lowVoltageCutoff.startTime = 0;
 363
 364     voltageMeterReset(&voltageMeter);
 365
 366     voltageMeterGenericInit();
 367     switch (batteryConfig()->voltageMeterSource) {
 368         case VOLTAGE_METER_ESC:
 369 #ifdef USE_ESC_SENSOR
 370             voltageMeterESCInit();
 371 #endif
 372             break;
 373
 374         case VOLTAGE_METER_ADC:
 375             voltageMeterADCInit();
 376             break;
 377
 378         default:
 379             break;
 380     }
 381
 382     //
 383     // current
 384     //
 385     consumptionState = BATTERY_OK;
 386     currentMeterReset(&currentMeter);
 387     switch (batteryConfig()->currentMeterSource) {
 388         case CURRENT_METER_ADC:
 389             currentMeterADCInit();
 390             break;
 391
 392         case CURRENT_METER_VIRTUAL:
 393 #ifdef USE_VIRTUAL_CURRENT_METER
 394             currentMeterVirtualInit();
 395 #endif
 396             break;
 397
 398         case CURRENT_METER_ESC:
 399 #ifdef ESC_SENSOR
 400             currentMeterESCInit();
 401 #endif
 402             break;
 403         case CURRENT_METER_MSP:
 404 #ifdef USE_MSP_CURRENT_METER
 405             currentMeterMSPInit();
 406 #endif
 407             break;
 408
 409         default:
 410             break;
 411     }
 412
 413 }
 414
 415 void batteryUpdateCurrentMeter(timeUs_t currentTimeUs)
 416 {
 417     UNUSED(currentTimeUs);
 418     if (batteryCellCount == 0) {
 419         currentMeterReset(&currentMeter);
 420         return;
 421     }
 422
 423     static uint32_t ibatLastServiced = 0;
 424     const int32_t lastUpdateAt = cmp32(currentTimeUs, ibatLastServiced);
 425     ibatLastServiced = currentTimeUs;
 426
 427     switch (batteryConfig()->currentMeterSource) {
 428         case CURRENT_METER_ADC:
 429             currentMeterADCRefresh(lastUpdateAt);
 430             currentMeterADCRead(&currentMeter);
 431             break;
 432
 433         case CURRENT_METER_VIRTUAL: {
 434 #ifdef USE_VIRTUAL_CURRENT_METER
 435             throttleStatus_e throttleStatus = calculateThrottleStatus();
 436             bool throttleLowAndMotorStop = (throttleStatus == THROTTLE_LOW && featureIsEnabled(FEATURE_MOTOR_STOP));
 437             const int32_t throttleOffset = lrintf(mixerGetThrottle() * 1000);
 438
 439             currentMeterVirtualRefresh(lastUpdateAt, ARMING_FLAG(ARMED), throttleLowAndMotorStop, throttleOffset);
 440             currentMeterVirtualRead(&currentMeter);
 441 #endif
 442             break;
 443         }
 444
 445         case CURRENT_METER_ESC:
 446 #ifdef USE_ESC_SENSOR
 447             if (featureIsEnabled(FEATURE_ESC_SENSOR)) {
 448                 currentMeterESCRefresh(lastUpdateAt);
 449                 currentMeterESCReadCombined(&currentMeter);
 450             }
 451 #endif
 452             break;
 453         case CURRENT_METER_MSP:
 454 #ifdef USE_MSP_CURRENT_METER
 455             currentMeterMSPRefresh(currentTimeUs);
 456             currentMeterMSPRead(&currentMeter);
 457 #endif
 458             break;
 459
 460         default:
 461         case CURRENT_METER_NONE:
 462             currentMeterReset(&currentMeter);
 463             break;
 464     }
 465 }
 466
 467 uint8_t calculateBatteryPercentageRemaining(void)
 468 {
 469     uint8_t batteryPercentage = 0;
 470     if (batteryCellCount > 0) {
 471         uint16_t batteryCapacity = batteryConfig()->batteryCapacity;
 472
 473         if (batteryCapacity > 0) {
 474             batteryPercentage = constrain(((float)batteryCapacity - currentMeter.mAhDrawn) * 100 / batteryCapacity, 0, 100);
 475         } else {
 476             batteryPercentage = constrain((((uint32_t)voltageMeter.displayFiltered - (batteryConfig()->vbatmincellvoltage * batteryCellCount)) * 100) / ((batteryConfig()->vbatmaxcellvoltage - batteryConfig()->vbatmincellvoltage) * batteryCellCount), 0, 100);
 477         }
 478     }
 479
 480     return batteryPercentage;
 481 }
 482
 483 void batteryUpdateAlarms(void)
 484 {
 485     // use the state to trigger beeper alerts
 486     if (batteryConfig()->useVBatAlerts) {
 487         updateBatteryBeeperAlert();
 488     }
 489 }
 490
 491 bool isBatteryVoltageConfigured(void)
 492 {
 493     return batteryConfig()->voltageMeterSource != VOLTAGE_METER_NONE;
 494 }
 495
 496 uint16_t getBatteryVoltage(void)
 497 {
 498     return voltageMeter.displayFiltered;
 499 }
 500
 501 uint16_t getLegacyBatteryVoltage(void)
 502 {
 503     return (voltageMeter.displayFiltered + 5) / 10;
 504 }
 505
 506 uint16_t getBatteryVoltageLatest(void)
 507 {
 508     return voltageMeter.unfiltered;
 509 }
 510
 511 uint8_t getBatteryCellCount(void)
 512 {
 513     return batteryCellCount;
 514 }
 515
 516 uint16_t getBatteryAverageCellVoltage(void)
 517 {
 518     return (batteryCellCount ? voltageMeter.displayFiltered / batteryCellCount : 0);
 519 }
 520
 521 #if defined(USE_BATTERY_VOLTAGE_SAG_COMPENSATION)
 522 uint16_t getBatterySagCellVoltage(void)
 523 {
 524     return (batteryCellCount ? voltageMeter.sagFiltered / batteryCellCount : 0);
 525 }
 526 #endif
 527
 528 bool isAmperageConfigured(void)
 529 {
 530     return batteryConfig()->currentMeterSource != CURRENT_METER_NONE;
 531 }
 532
 533 int32_t getAmperage(void) {
 534     return currentMeter.amperage;
 535 }
 536
 537 int32_t getAmperageLatest(void)
 538 {
 539     return currentMeter.amperageLatest;
 540 }
 541
 542 int32_t getMAhDrawn(void)
 543 {
 544     return currentMeter.mAhDrawn;
 545 }