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 uint8_t batteryCellCount; // Note: this can be 0 when no battery is detected or when the battery voltage sensor is missing or disabled.
  68 uint16_t batteryWarningVoltage;
  69 uint16_t batteryCriticalVoltage;
  70 static lowVoltageCutoff_t lowVoltageCutoff;
  71 //
  72 static currentMeter_t currentMeter;
  73 static voltageMeter_t voltageMeter;
  74
  75 static batteryState_e batteryState;
  76 static batteryState_e voltageState;
  77 static batteryState_e consumptionState;
  78
  79 #ifndef DEFAULT_CURRENT_METER_SOURCE
  80 #ifdef USE_VIRTUAL_CURRENT_METER
  81 #define DEFAULT_CURRENT_METER_SOURCE CURRENT_METER_VIRTUAL
  82 #else
  83 #ifdef USE_MSP_CURRENT_METER
  84 #define DEFAULT_CURRENT_METER_SOURCE CURRENT_METER_MSP
  85 #else
  86 #define DEFAULT_CURRENT_METER_SOURCE CURRENT_METER_NONE
  87 #endif
  88 #endif
  89 #endif
  90
  91 #ifndef DEFAULT_VOLTAGE_METER_SOURCE
  92 #define DEFAULT_VOLTAGE_METER_SOURCE VOLTAGE_METER_NONE
  93 #endif
  94
  95 PG_REGISTER_WITH_RESET_TEMPLATE(batteryConfig_t, batteryConfig, PG_BATTERY_CONFIG, 3);
  96
  97 PG_RESET_TEMPLATE(batteryConfig_t, batteryConfig,
  98     // voltage
  99     .vbatmaxcellvoltage = 430,
 100     .vbatmincellvoltage = 330,
 101     .vbatwarningcellvoltage = 350,
 102     .vbatnotpresentcellvoltage = 300, //A cell below 3 will be ignored
 103     .voltageMeterSource = DEFAULT_VOLTAGE_METER_SOURCE,
 104     .lvcPercentage = 100, //Off by default at 100%
 105
 106     // current
 107     .batteryCapacity = 0,
 108     .currentMeterSource = DEFAULT_CURRENT_METER_SOURCE,
 109
 110     // cells
 111     .forceBatteryCellCount = 0, //0 will be ignored
 112
 113     // warnings / alerts
 114     .useVBatAlerts = true,
 115     .useConsumptionAlerts = false,
 116     .consumptionWarningPercentage = 10,
 117     .vbathysteresis = 1,
 118
 119     .vbatfullcellvoltage = 410,
 120
 121     .vbatLpfPeriod = 30,
 122     .ibatLpfPeriod = 10,
 123     .vbatDurationForWarning = 0,
 124     .vbatDurationForCritical = 0,
 125 );
 126
 127 void batteryUpdateVoltage(timeUs_t currentTimeUs)
 128 {
 129     UNUSED(currentTimeUs);
 130
 131     switch (batteryConfig()->voltageMeterSource) {
 132 #ifdef USE_ESC_SENSOR
 133         case VOLTAGE_METER_ESC:
 134             if (featureIsEnabled(FEATURE_ESC_SENSOR)) {
 135                 voltageMeterESCRefresh();
 136                 voltageMeterESCReadCombined(&voltageMeter);
 137             }
 138             break;
 139 #endif
 140         case VOLTAGE_METER_ADC:
 141             voltageMeterADCRefresh();
 142             voltageMeterADCRead(VOLTAGE_SENSOR_ADC_VBAT, &voltageMeter);
 143             break;
 144
 145         default:
 146         case VOLTAGE_METER_NONE:
 147             voltageMeterReset(&voltageMeter);
 148             break;
 149     }
 150
 151     if (debugMode == DEBUG_BATTERY) {
 152         debug[0] = voltageMeter.unfiltered;
 153         debug[1] = voltageMeter.filtered;
 154     }
 155 }
 156
 157 static void updateBatteryBeeperAlert(void)
 158 {
 159     switch (getBatteryState()) {
 160         case BATTERY_WARNING:
 161             beeper(BEEPER_BAT_LOW);
 162
 163             break;
 164         case BATTERY_CRITICAL:
 165             beeper(BEEPER_BAT_CRIT_LOW);
 166
 167             break;
 168         case BATTERY_OK:
 169         case BATTERY_NOT_PRESENT:
 170         case BATTERY_INIT:
 171             break;
 172     }
 173 }
 174
 175 static bool isVoltageStable(void)
 176 {
 177     return ABS(voltageMeter.filtered - voltageMeter.unfiltered) <= VBAT_STABLE_MAX_DELTA;
 178 }
 179
 180 static bool isVoltageFromBat(void)
 181 {
 182     // We want to disable battery getting detected around USB voltage or 0V
 183
 184     return (voltageMeter.filtered >= batteryConfig()->vbatnotpresentcellvoltage  // Above ~0V
 185         && voltageMeter.filtered <= batteryConfig()->vbatmaxcellvoltage)  // 1s max cell voltage check
 186         || voltageMeter.filtered > batteryConfig()->vbatnotpresentcellvoltage * 2; // USB voltage - 2s or more check
 187 }
 188
 189 void batteryUpdatePresence(void)
 190 {
 191
 192
 193     if (
 194         (voltageState == BATTERY_NOT_PRESENT || voltageState == BATTERY_INIT) && isVoltageFromBat() && isVoltageStable()
 195     ) {
 196         // Battery has just been connected - calculate cells, warning voltages and reset state
 197
 198         consumptionState = voltageState = BATTERY_OK;
 199         if (batteryConfig()->forceBatteryCellCount != 0) {
 200             batteryCellCount = batteryConfig()->forceBatteryCellCount;
 201         } else {
 202             unsigned cells = (voltageMeter.filtered / batteryConfig()->vbatmaxcellvoltage) + 1;
 203             if (cells > MAX_AUTO_DETECT_CELL_COUNT) {
 204                 // something is wrong, we expect MAX_CELL_COUNT cells maximum (and autodetection will be problematic at 6+ cells)
 205                 cells = MAX_AUTO_DETECT_CELL_COUNT;
 206             }
 207             batteryCellCount = cells;
 208
 209             if (!ARMING_FLAG(ARMED)) {
 210                 changePidProfileFromCellCount(batteryCellCount);
 211             }
 212         }
 213         batteryWarningVoltage = batteryCellCount * batteryConfig()->vbatwarningcellvoltage;
 214         batteryCriticalVoltage = batteryCellCount * batteryConfig()->vbatmincellvoltage;
 215         lowVoltageCutoff.percentage = 100;
 216         lowVoltageCutoff.startTime = 0;
 217     } else if (
 218         voltageState != BATTERY_NOT_PRESENT && isVoltageStable() && !isVoltageFromBat()
 219     ) {
 220         /* battery has been disconnected - can take a while for filter cap to disharge so we use a threshold of batteryConfig()->vbatnotpresentcellvoltage */
 221
 222         consumptionState = voltageState = BATTERY_NOT_PRESENT;
 223
 224         batteryCellCount = 0;
 225         batteryWarningVoltage = 0;
 226         batteryCriticalVoltage = 0;
 227     }
 228     if (debugMode == DEBUG_BATTERY) {
 229         debug[2] = batteryCellCount;
 230         debug[3] = isVoltageStable();
 231     }
 232 }
 233
 234 static void batteryUpdateVoltageState(void)
 235 {
 236     // alerts are currently used by beeper, osd and other subsystems
 237     static uint32_t lastVoltageChangeMs;
 238     switch (voltageState) {
 239         case BATTERY_OK:
 240             if (voltageMeter.filtered <= (batteryWarningVoltage - batteryConfig()->vbathysteresis)) {
 241                 if (cmp32(millis(), lastVoltageChangeMs) >= batteryConfig()->vbatDurationForWarning * 100) {
 242                     voltageState = BATTERY_WARNING;
 243                 }
 244             } else {
 245                 lastVoltageChangeMs = millis();
 246             }
 247             break;
 248
 249         case BATTERY_WARNING:
 250             if (voltageMeter.filtered <= (batteryCriticalVoltage - batteryConfig()->vbathysteresis)) {
 251                 if (cmp32(millis(), lastVoltageChangeMs) >= batteryConfig()->vbatDurationForCritical * 100) {
 252                     voltageState = BATTERY_CRITICAL;
 253                 }
 254             } else {
 255                 if (voltageMeter.filtered > batteryWarningVoltage) {
 256                     voltageState = BATTERY_OK;
 257                 }
 258                 lastVoltageChangeMs = millis();
 259             }
 260             break;
 261
 262         case BATTERY_CRITICAL:
 263             if (voltageMeter.filtered > batteryCriticalVoltage) {
 264                 voltageState = BATTERY_WARNING;
 265                 lastVoltageChangeMs = millis();
 266             }
 267             break;
 268
 269         default:
 270             break;
 271     }
 272
 273 }
 274
 275 static void batteryUpdateLVC(timeUs_t currentTimeUs)
 276 {
 277     if (batteryConfig()->lvcPercentage < 100) {
 278         if (voltageState == BATTERY_CRITICAL && !lowVoltageCutoff.enabled) {
 279             lowVoltageCutoff.enabled = true;
 280             lowVoltageCutoff.startTime = currentTimeUs;
 281             lowVoltageCutoff.percentage = 100;
 282         }
 283         if (lowVoltageCutoff.enabled) {
 284             if (cmp32(currentTimeUs,lowVoltageCutoff.startTime) < LVC_AFFECT_TIME) {
 285                 lowVoltageCutoff.percentage = 100 - (cmp32(currentTimeUs,lowVoltageCutoff.startTime) * (100 - batteryConfig()->lvcPercentage) / LVC_AFFECT_TIME);
 286             }
 287             else {
 288                 lowVoltageCutoff.percentage = batteryConfig()->lvcPercentage;
 289             }
 290         }
 291     }
 292
 293 }
 294
 295 static void batteryUpdateConsumptionState(void)
 296 {
 297     if (batteryConfig()->useConsumptionAlerts && batteryConfig()->batteryCapacity > 0 && batteryCellCount > 0) {
 298         uint8_t batteryPercentageRemaining = calculateBatteryPercentageRemaining();
 299
 300         if (batteryPercentageRemaining == 0) {
 301             consumptionState = BATTERY_CRITICAL;
 302         } else if (batteryPercentageRemaining <= batteryConfig()->consumptionWarningPercentage) {
 303             consumptionState = BATTERY_WARNING;
 304         } else {
 305             consumptionState = BATTERY_OK;
 306         }
 307     }
 308 }
 309
 310 void batteryUpdateStates(timeUs_t currentTimeUs)
 311 {
 312     batteryUpdateVoltageState();
 313     batteryUpdateConsumptionState();
 314     batteryUpdateLVC(currentTimeUs);
 315     batteryState = MAX(voltageState, consumptionState);
 316 }
 317
 318 const lowVoltageCutoff_t *getLowVoltageCutoff(void)
 319 {
 320     return &lowVoltageCutoff;
 321 }
 322
 323 batteryState_e getBatteryState(void)
 324 {
 325     return batteryState;
 326 }
 327
 328 batteryState_e getVoltageState(void)
 329 {
 330     return voltageState;
 331 }
 332
 333 batteryState_e getConsumptionState(void)
 334 {
 335     return consumptionState;
 336 }
 337
 338 const char * const batteryStateStrings[] = {"OK", "WARNING", "CRITICAL", "NOT PRESENT", "INIT"};
 339
 340 const char * getBatteryStateString(void)
 341 {
 342     return batteryStateStrings[getBatteryState()];
 343 }
 344
 345 void batteryInit(void)
 346 {
 347     //
 348     // presence
 349     //
 350     batteryState = BATTERY_INIT;
 351     batteryCellCount = 0;
 352
 353     //
 354     // voltage
 355     //
 356     voltageState = BATTERY_INIT;
 357     batteryWarningVoltage = 0;
 358     batteryCriticalVoltage = 0;
 359     lowVoltageCutoff.enabled = false;
 360     lowVoltageCutoff.percentage = 100;
 361     lowVoltageCutoff.startTime = 0;
 362
 363     voltageMeterReset(&voltageMeter);
 364     switch (batteryConfig()->voltageMeterSource) {
 365         case VOLTAGE_METER_ESC:
 366 #ifdef USE_ESC_SENSOR
 367             voltageMeterESCInit();
 368 #endif
 369             break;
 370
 371         case VOLTAGE_METER_ADC:
 372             voltageMeterADCInit();
 373             break;
 374
 375         default:
 376             break;
 377     }
 378
 379     //
 380     // current
 381     //
 382     consumptionState = BATTERY_OK;
 383     currentMeterReset(&currentMeter);
 384     switch (batteryConfig()->currentMeterSource) {
 385         case CURRENT_METER_ADC:
 386             currentMeterADCInit();
 387             break;
 388
 389         case CURRENT_METER_VIRTUAL:
 390 #ifdef USE_VIRTUAL_CURRENT_METER
 391             currentMeterVirtualInit();
 392 #endif
 393             break;
 394
 395         case CURRENT_METER_ESC:
 396 #ifdef ESC_SENSOR
 397             currentMeterESCInit();
 398 #endif
 399             break;
 400         case CURRENT_METER_MSP:
 401 #ifdef USE_MSP_CURRENT_METER
 402             currentMeterMSPInit();
 403 #endif
 404             break;
 405
 406         default:
 407             break;
 408     }
 409
 410 }
 411
 412 void batteryUpdateCurrentMeter(timeUs_t currentTimeUs)
 413 {
 414     UNUSED(currentTimeUs);
 415     if (batteryCellCount == 0) {
 416         currentMeterReset(&currentMeter);
 417         return;
 418     }
 419
 420     static uint32_t ibatLastServiced = 0;
 421     const int32_t lastUpdateAt = cmp32(currentTimeUs, ibatLastServiced);
 422     ibatLastServiced = currentTimeUs;
 423
 424     switch (batteryConfig()->currentMeterSource) {
 425         case CURRENT_METER_ADC:
 426             currentMeterADCRefresh(lastUpdateAt);
 427             currentMeterADCRead(&currentMeter);
 428             break;
 429
 430         case CURRENT_METER_VIRTUAL: {
 431 #ifdef USE_VIRTUAL_CURRENT_METER
 432             throttleStatus_e throttleStatus = calculateThrottleStatus();
 433             bool throttleLowAndMotorStop = (throttleStatus == THROTTLE_LOW && featureIsEnabled(FEATURE_MOTOR_STOP));
 434             const int32_t throttleOffset = lrintf(mixerGetThrottle() * 1000);
 435
 436             currentMeterVirtualRefresh(lastUpdateAt, ARMING_FLAG(ARMED), throttleLowAndMotorStop, throttleOffset);
 437             currentMeterVirtualRead(&currentMeter);
 438 #endif
 439             break;
 440         }
 441
 442         case CURRENT_METER_ESC:
 443 #ifdef USE_ESC_SENSOR
 444             if (featureIsEnabled(FEATURE_ESC_SENSOR)) {
 445                 currentMeterESCRefresh(lastUpdateAt);
 446                 currentMeterESCReadCombined(&currentMeter);
 447             }
 448 #endif
 449             break;
 450         case CURRENT_METER_MSP:
 451 #ifdef USE_MSP_CURRENT_METER
 452             currentMeterMSPRefresh(currentTimeUs);
 453             currentMeterMSPRead(&currentMeter);
 454 #endif
 455             break;
 456
 457         default:
 458         case CURRENT_METER_NONE:
 459             currentMeterReset(&currentMeter);
 460             break;
 461     }
 462 }
 463
 464 float calculateVbatPidCompensation(void) {
 465     float batteryScaler =  1.0f;
 466     if (batteryConfig()->voltageMeterSource != VOLTAGE_METER_NONE && batteryCellCount > 0) {
 467         // Up to 33% PID gain. Should be fine for 4,2to 3,3 difference
 468         batteryScaler =  constrainf((( (float)batteryConfig()->vbatmaxcellvoltage * batteryCellCount ) / (float) voltageMeter.filtered), 1.0f, 1.33f);
 469     }
 470     return batteryScaler;
 471 }
 472
 473 uint8_t calculateBatteryPercentageRemaining(void)
 474 {
 475     uint8_t batteryPercentage = 0;
 476     if (batteryCellCount > 0) {
 477         uint16_t batteryCapacity = batteryConfig()->batteryCapacity;
 478
 479         if (batteryCapacity > 0) {
 480             batteryPercentage = constrain(((float)batteryCapacity - currentMeter.mAhDrawn) * 100 / batteryCapacity, 0, 100);
 481         } else {
 482             batteryPercentage = constrain((((uint32_t)voltageMeter.filtered - (batteryConfig()->vbatmincellvoltage * batteryCellCount)) * 100) / ((batteryConfig()->vbatmaxcellvoltage - batteryConfig()->vbatmincellvoltage) * batteryCellCount), 0, 100);
 483         }
 484     }
 485
 486     return batteryPercentage;
 487 }
 488
 489 void batteryUpdateAlarms(void)
 490 {
 491     // use the state to trigger beeper alerts
 492     if (batteryConfig()->useVBatAlerts) {
 493         updateBatteryBeeperAlert();
 494     }
 495 }
 496
 497 bool isBatteryVoltageConfigured(void)
 498 {
 499     return batteryConfig()->voltageMeterSource != VOLTAGE_METER_NONE;
 500 }
 501
 502 uint16_t getBatteryVoltage(void)
 503 {
 504     return voltageMeter.filtered;
 505 }
 506
 507 uint16_t getLegacyBatteryVoltage(void)
 508 {
 509     return (voltageMeter.filtered + 5) / 10;
 510 }
 511
 512 uint16_t getBatteryVoltageLatest(void)
 513 {
 514     return voltageMeter.unfiltered;
 515 }
 516
 517 uint8_t getBatteryCellCount(void)
 518 {
 519     return batteryCellCount;
 520 }
 521
 522 uint16_t getBatteryAverageCellVoltage(void)
 523 {
 524     return voltageMeter.filtered / batteryCellCount;
 525 }
 526
 527 bool isAmperageConfigured(void)
 528 {
 529     return batteryConfig()->currentMeterSource != CURRENT_METER_NONE;
 530 }
 531
 532 int32_t getAmperage(void) {
 533     return currentMeter.amperage;
 534 }
 535
 536 int32_t getAmperageLatest(void)
 537 {
 538     return currentMeter.amperageLatest;
 539 }
 540
 541 int32_t getMAhDrawn(void)
 542 {
 543     return currentMeter.mAhDrawn;
 544 }