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