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 "scheduler/scheduler.h"
  49
  50 #include "sensors/battery.h"
  51
  52 /**
  53  * terminology: meter vs sensors
  54  *
  55  * voltage and current sensors are used to collect data.
  56  * - e.g. voltage at an MCU ADC input pin, value from an ESC sensor.
  57  *   sensors require very specific configuration, such as resistor values.
  58  * voltage and current meters are used to process and expose data collected from sensors to the rest of the system.
  59  * - e.g. a meter exposes normalized, and often filtered, values from a sensor.
  60  *   meters require different or little configuration.
  61  *   meters also have different precision concerns, and may use different units to the sensors.
  62  *
  63  */
  64
  65 #define VBAT_STABLE_MAX_DELTA 20
  66 #define LVC_AFFECT_TIME 10000000 //10 secs for the LVC to slowly kick in
  67
  68 // Battery monitoring stuff
  69 static uint8_t batteryCellCount; // Note: this can be 0 when no battery is detected or when the battery voltage sensor is missing or disabled.
  70 static uint16_t batteryWarningVoltage;
  71 static uint16_t batteryCriticalVoltage;
  72 static uint16_t batteryWarningHysteresisVoltage;
  73 static uint16_t batteryCriticalHysteresisVoltage;
  74 static lowVoltageCutoff_t lowVoltageCutoff;
  75 //
  76 static currentMeter_t currentMeter;
  77 static voltageMeter_t voltageMeter;
  78
  79 static batteryState_e batteryState;
  80 static batteryState_e voltageState;
  81 static batteryState_e consumptionState;
  82
  83 #ifndef DEFAULT_CURRENT_METER_SOURCE
  84 #ifdef USE_VIRTUAL_CURRENT_METER
  85 #define DEFAULT_CURRENT_METER_SOURCE CURRENT_METER_VIRTUAL
  86 #else
  87 #ifdef USE_MSP_CURRENT_METER
  88 #define DEFAULT_CURRENT_METER_SOURCE CURRENT_METER_MSP
  89 #else
  90 #define DEFAULT_CURRENT_METER_SOURCE CURRENT_METER_NONE
  91 #endif
  92 #endif
  93 #endif
  94
  95 #ifndef DEFAULT_VOLTAGE_METER_SOURCE
  96 #define DEFAULT_VOLTAGE_METER_SOURCE VOLTAGE_METER_NONE
  97 #endif
  98
  99 PG_REGISTER_WITH_RESET_TEMPLATE(batteryConfig_t, batteryConfig, PG_BATTERY_CONFIG, 3);
 100
 101 PG_RESET_TEMPLATE(batteryConfig_t, batteryConfig,
 102     // voltage
 103     .vbatmaxcellvoltage = VBAT_CELL_VOLTAGE_DEFAULT_MAX,
 104     .vbatmincellvoltage = VBAT_CELL_VOLTAGE_DEFAULT_MIN,
 105     .vbatwarningcellvoltage = 350,
 106     .vbatnotpresentcellvoltage = 300, //A cell below 3 will be ignored
 107     .voltageMeterSource = DEFAULT_VOLTAGE_METER_SOURCE,
 108     .lvcPercentage = 100, //Off by default at 100%
 109
 110     // current
 111     .batteryCapacity = 0,
 112     .currentMeterSource = DEFAULT_CURRENT_METER_SOURCE,
 113
 114     // cells
 115     .forceBatteryCellCount = 0, //0 will be ignored
 116
 117     // warnings / alerts
 118     .useVBatAlerts = true,
 119     .useConsumptionAlerts = false,
 120     .consumptionWarningPercentage = 10,
 121     .vbathysteresis = 1, // 0.01V
 122
 123     .vbatfullcellvoltage = 410,
 124
 125     .vbatDisplayLpfPeriod = 30,
 126     .vbatSagLpfPeriod = 2,
 127     .ibatLpfPeriod = 10,
 128     .vbatDurationForWarning = 0,
 129     .vbatDurationForCritical = 0,
 130 );
 131
 132 void batteryUpdateVoltage(timeUs_t currentTimeUs)
 133 {
 134     UNUSED(currentTimeUs);
 135
 136     switch (batteryConfig()->voltageMeterSource) {
 137 #ifdef USE_ESC_SENSOR
 138         case VOLTAGE_METER_ESC:
 139             if (featureIsEnabled(FEATURE_ESC_SENSOR)) {
 140                 voltageMeterESCRefresh();
 141                 voltageMeterESCReadCombined(&voltageMeter);
 142             }
 143             break;
 144 #endif
 145         case VOLTAGE_METER_ADC:
 146             voltageMeterADCRefresh();
 147             voltageMeterADCRead(VOLTAGE_SENSOR_ADC_VBAT, &voltageMeter);
 148             break;
 149
 150         default:
 151         case VOLTAGE_METER_NONE:
 152             voltageMeterReset(&voltageMeter);
 153             break;
 154     }
 155
 156     DEBUG_SET(DEBUG_BATTERY, 0, voltageMeter.unfiltered);
 157     DEBUG_SET(DEBUG_BATTERY, 1, voltageMeter.displayFiltered);
 158 }
 159
 160 static void updateBatteryBeeperAlert(void)
 161 {
 162     switch (getBatteryState()) {
 163         case BATTERY_WARNING:
 164             beeper(BEEPER_BAT_LOW);
 165
 166             break;
 167         case BATTERY_CRITICAL:
 168             beeper(BEEPER_BAT_CRIT_LOW);
 169
 170             break;
 171         case BATTERY_OK:
 172         case BATTERY_NOT_PRESENT:
 173         case BATTERY_INIT:
 174             break;
 175     }
 176 }
 177
 178 //TODO: make all of these independent of voltage filtering for display
 179
 180 static bool isVoltageStable(void)
 181 {
 182     return ABS(voltageMeter.displayFiltered - voltageMeter.unfiltered) <= VBAT_STABLE_MAX_DELTA;
 183 }
 184
 185 static bool isVoltageFromBat(void)
 186 {
 187     // We want to disable battery getting detected around USB voltage or 0V
 188
 189     return (voltageMeter.displayFiltered >= batteryConfig()->vbatnotpresentcellvoltage  // Above ~0V
 190         && voltageMeter.displayFiltered <= batteryConfig()->vbatmaxcellvoltage)  // 1s max cell voltage check
 191         || voltageMeter.displayFiltered > batteryConfig()->vbatnotpresentcellvoltage * 2; // USB voltage - 2s or more check
 192 }
 193
 194 void batteryUpdatePresence(void)
 195 {
 196
 197
 198     if ((voltageState == BATTERY_NOT_PRESENT || voltageState == BATTERY_INIT) && isVoltageFromBat() && isVoltageStable()) {
 199         // Battery has just been connected - calculate cells, warning voltages and reset state
 200
 201         consumptionState = voltageState = BATTERY_OK;
 202         if (batteryConfig()->forceBatteryCellCount != 0) {
 203             batteryCellCount = batteryConfig()->forceBatteryCellCount;
 204         } else {
 205             unsigned cells = (voltageMeter.displayFiltered / batteryConfig()->vbatmaxcellvoltage) + 1;
 206             if (cells > MAX_AUTO_DETECT_CELL_COUNT) {
 207                 // something is wrong, we expect MAX_CELL_COUNT cells maximum (and autodetection will be problematic at 6+ cells)
 208                 cells = MAX_AUTO_DETECT_CELL_COUNT;
 209             }
 210             batteryCellCount = cells;
 211
 212             if (!ARMING_FLAG(ARMED)) {
 213                 changePidProfileFromCellCount(batteryCellCount);
 214             }
 215         }
 216         batteryWarningVoltage = batteryCellCount * batteryConfig()->vbatwarningcellvoltage;
 217         batteryCriticalVoltage = batteryCellCount * batteryConfig()->vbatmincellvoltage;
 218         batteryWarningHysteresisVoltage = (batteryWarningVoltage > batteryConfig()->vbathysteresis) ? batteryWarningVoltage - batteryConfig()->vbathysteresis : 0;
 219         batteryCriticalHysteresisVoltage = (batteryCriticalVoltage > batteryConfig()->vbathysteresis) ? batteryCriticalVoltage - batteryConfig()->vbathysteresis : 0;
 220         lowVoltageCutoff.percentage = 100;
 221         lowVoltageCutoff.startTime = 0;
 222     } else if (voltageState != BATTERY_NOT_PRESENT && isVoltageStable() && !isVoltageFromBat()) {
 223         /* battery has been disconnected - can take a while for filter cap to disharge so we use a threshold of batteryConfig()->vbatnotpresentcellvoltage */
 224
 225         consumptionState = voltageState = BATTERY_NOT_PRESENT;
 226
 227         batteryCellCount = 0;
 228         batteryWarningVoltage = 0;
 229         batteryCriticalVoltage = 0;
 230         batteryWarningHysteresisVoltage = 0;
 231         batteryCriticalHysteresisVoltage = 0;
 232     }
 233 }
 234
 235 static void batteryUpdateVoltageState(void)
 236 {
 237     // alerts are currently used by beeper, osd and other subsystems
 238     static uint32_t lastVoltageChangeMs;
 239     switch (voltageState) {
 240         case BATTERY_OK:
 241             if (voltageMeter.displayFiltered <= batteryWarningHysteresisVoltage) {
 242                 if (cmp32(millis(), lastVoltageChangeMs) >= batteryConfig()->vbatDurationForWarning * 100) {
 243                     voltageState = BATTERY_WARNING;
 244                 }
 245             } else {
 246                 lastVoltageChangeMs = millis();
 247             }
 248             break;
 249
 250         case BATTERY_WARNING:
 251             if (voltageMeter.displayFiltered <= batteryCriticalHysteresisVoltage) {
 252                 if (cmp32(millis(), lastVoltageChangeMs) >= batteryConfig()->vbatDurationForCritical * 100) {
 253                     voltageState = BATTERY_CRITICAL;
 254                 }
 255             } else {
 256                 if (voltageMeter.displayFiltered > batteryWarningVoltage) {
 257                     voltageState = BATTERY_OK;
 258                 }
 259                 lastVoltageChangeMs = millis();
 260             }
 261             break;
 262
 263         case BATTERY_CRITICAL:
 264             if (voltageMeter.displayFiltered > batteryCriticalVoltage) {
 265                 voltageState = BATTERY_WARNING;
 266                 lastVoltageChangeMs = millis();
 267             }
 268             break;
 269
 270         default:
 271             break;
 272     }
 273
 274 }
 275
 276 static void batteryUpdateLVC(timeUs_t currentTimeUs)
 277 {
 278     if (batteryConfig()->lvcPercentage < 100) {
 279         if (voltageState == BATTERY_CRITICAL && !lowVoltageCutoff.enabled) {
 280             lowVoltageCutoff.enabled = true;
 281             lowVoltageCutoff.startTime = currentTimeUs;
 282             lowVoltageCutoff.percentage = 100;
 283         }
 284         if (lowVoltageCutoff.enabled) {
 285             if (cmp32(currentTimeUs,lowVoltageCutoff.startTime) < LVC_AFFECT_TIME) {
 286                 lowVoltageCutoff.percentage = 100 - (cmp32(currentTimeUs,lowVoltageCutoff.startTime) * (100 - batteryConfig()->lvcPercentage) / LVC_AFFECT_TIME);
 287             }
 288             else {
 289                 lowVoltageCutoff.percentage = batteryConfig()->lvcPercentage;
 290             }
 291         }
 292     }
 293
 294 }
 295
 296 static void batteryUpdateConsumptionState(void)
 297 {
 298     if (batteryConfig()->useConsumptionAlerts && batteryConfig()->batteryCapacity > 0 && batteryCellCount > 0) {
 299         uint8_t batteryPercentageRemaining = calculateBatteryPercentageRemaining();
 300
 301         if (batteryPercentageRemaining == 0) {
 302             consumptionState = BATTERY_CRITICAL;
 303         } else if (batteryPercentageRemaining <= batteryConfig()->consumptionWarningPercentage) {
 304             consumptionState = BATTERY_WARNING;
 305         } else {
 306             consumptionState = BATTERY_OK;
 307         }
 308     }
 309 }
 310
 311 void batteryUpdateStates(timeUs_t currentTimeUs)
 312 {
 313     batteryUpdateVoltageState();
 314     batteryUpdateConsumptionState();
 315     batteryUpdateLVC(currentTimeUs);
 316     batteryState = MAX(voltageState, consumptionState);
 317 }
 318
 319 const lowVoltageCutoff_t *getLowVoltageCutoff(void)
 320 {
 321     return &lowVoltageCutoff;
 322 }
 323
 324 batteryState_e getBatteryState(void)
 325 {
 326     return batteryState;
 327 }
 328
 329 batteryState_e getVoltageState(void)
 330 {
 331     return voltageState;
 332 }
 333
 334 batteryState_e getConsumptionState(void)
 335 {
 336     return consumptionState;
 337 }
 338
 339 const char * const batteryStateStrings[] = {"OK", "WARNING", "CRITICAL", "NOT PRESENT", "INIT"};
 340
 341 const char * getBatteryStateString(void)
 342 {
 343     return batteryStateStrings[getBatteryState()];
 344 }
 345
 346 void batteryInit(void)
 347 {
 348     //
 349     // presence
 350     //
 351     batteryState = BATTERY_INIT;
 352     batteryCellCount = 0;
 353
 354     //
 355     // voltage
 356     //
 357     voltageState = BATTERY_INIT;
 358     batteryWarningVoltage = 0;
 359     batteryCriticalVoltage = 0;
 360     batteryWarningHysteresisVoltage = 0;
 361     batteryCriticalHysteresisVoltage = 0;
 362     lowVoltageCutoff.enabled = false;
 363     lowVoltageCutoff.percentage = 100;
 364     lowVoltageCutoff.startTime = 0;
 365
 366     voltageMeterReset(&voltageMeter);
 367
 368     voltageMeterGenericInit();
 369     switch (batteryConfig()->voltageMeterSource) {
 370         case VOLTAGE_METER_ESC:
 371 #ifdef USE_ESC_SENSOR
 372             voltageMeterESCInit();
 373 #endif
 374             break;
 375
 376         case VOLTAGE_METER_ADC:
 377             voltageMeterADCInit();
 378             break;
 379
 380         default:
 381             break;
 382     }
 383
 384     //
 385     // current
 386     //
 387     consumptionState = BATTERY_OK;
 388     currentMeterReset(&currentMeter);
 389     switch (batteryConfig()->currentMeterSource) {
 390         case CURRENT_METER_ADC:
 391             currentMeterADCInit();
 392             break;
 393
 394         case CURRENT_METER_VIRTUAL:
 395 #ifdef USE_VIRTUAL_CURRENT_METER
 396             currentMeterVirtualInit();
 397 #endif
 398             break;
 399
 400         case CURRENT_METER_ESC:
 401 #ifdef ESC_SENSOR
 402             currentMeterESCInit();
 403 #endif
 404             break;
 405         case CURRENT_METER_MSP:
 406 #ifdef USE_MSP_CURRENT_METER
 407             currentMeterMSPInit();
 408 #endif
 409             break;
 410
 411         default:
 412             break;
 413     }
 414 }
 415
 416 void batteryUpdateCurrentMeter(timeUs_t currentTimeUs)
 417 {
 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 }