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 = VBAT_CELL_VOLTAGE_DEFAULT_MAX,
 100     .vbatmincellvoltage = VBAT_CELL_VOLTAGE_DEFAULT_MIN,
 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     .vbatDisplayLpfPeriod = 30,
 122     .vbatSagLpfPeriod = 2,
 123     .ibatLpfPeriod = 10,
 124     .vbatDurationForWarning = 0,
 125     .vbatDurationForCritical = 0,
 126 );
 127
 128 void batteryUpdateVoltage(timeUs_t currentTimeUs)
 129 {
 130     UNUSED(currentTimeUs);
 131
 132     switch (batteryConfig()->voltageMeterSource) {
 133 #ifdef USE_ESC_SENSOR
 134         case VOLTAGE_METER_ESC:
 135             if (featureIsEnabled(FEATURE_ESC_SENSOR)) {
 136                 voltageMeterESCRefresh();
 137                 voltageMeterESCReadCombined(&voltageMeter);
 138             }
 139             break;
 140 #endif
 141         case VOLTAGE_METER_ADC:
 142             voltageMeterADCRefresh();
 143             voltageMeterADCRead(VOLTAGE_SENSOR_ADC_VBAT, &voltageMeter);
 144             break;
 145
 146         default:
 147         case VOLTAGE_METER_NONE:
 148             voltageMeterReset(&voltageMeter);
 149             break;
 150     }
 151
 152     DEBUG_SET(DEBUG_BATTERY, 0, voltageMeter.unfiltered);
 153     DEBUG_SET(DEBUG_BATTERY, 1, voltageMeter.displayFiltered);
 154 }
 155
 156 static void updateBatteryBeeperAlert(void)
 157 {
 158     switch (getBatteryState()) {
 159         case BATTERY_WARNING:
 160             beeper(BEEPER_BAT_LOW);
 161
 162             break;
 163         case BATTERY_CRITICAL:
 164             beeper(BEEPER_BAT_CRIT_LOW);
 165
 166             break;
 167         case BATTERY_OK:
 168         case BATTERY_NOT_PRESENT:
 169         case BATTERY_INIT:
 170             break;
 171     }
 172 }
 173
 174 //TODO: make all of these independent of voltage filtering for display
 175
 176 static bool isVoltageStable(void)
 177 {
 178     return ABS(voltageMeter.displayFiltered - voltageMeter.unfiltered) <= VBAT_STABLE_MAX_DELTA;
 179 }
 180
 181 static bool isVoltageFromBat(void)
 182 {
 183     // We want to disable battery getting detected around USB voltage or 0V
 184
 185     return (voltageMeter.displayFiltered >= batteryConfig()->vbatnotpresentcellvoltage  // Above ~0V
 186         && voltageMeter.displayFiltered <= batteryConfig()->vbatmaxcellvoltage)  // 1s max cell voltage check
 187         || voltageMeter.displayFiltered > batteryConfig()->vbatnotpresentcellvoltage * 2; // USB voltage - 2s or more check
 188 }
 189
 190 void batteryUpdatePresence(void)
 191 {
 192
 193
 194     if (
 195         (voltageState == BATTERY_NOT_PRESENT || voltageState == BATTERY_INIT) && isVoltageFromBat() && isVoltageStable()
 196     ) {
 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         lowVoltageCutoff.percentage = 100;
 217         lowVoltageCutoff.startTime = 0;
 218     } else if (
 219         voltageState != BATTERY_NOT_PRESENT && isVoltageStable() && !isVoltageFromBat()
 220     ) {
 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     }
 229 }
 230
 231 static void batteryUpdateVoltageState(void)
 232 {
 233     // alerts are currently used by beeper, osd and other subsystems
 234     static uint32_t lastVoltageChangeMs;
 235     switch (voltageState) {
 236         case BATTERY_OK:
 237             if (voltageMeter.displayFiltered <= (batteryWarningVoltage - batteryConfig()->vbathysteresis)) {
 238                 if (cmp32(millis(), lastVoltageChangeMs) >= batteryConfig()->vbatDurationForWarning * 100) {
 239                     voltageState = BATTERY_WARNING;
 240                 }
 241             } else {
 242                 lastVoltageChangeMs = millis();
 243             }
 244             break;
 245
 246         case BATTERY_WARNING:
 247             if (voltageMeter.displayFiltered <= (batteryCriticalVoltage - batteryConfig()->vbathysteresis)) {
 248                 if (cmp32(millis(), lastVoltageChangeMs) >= batteryConfig()->vbatDurationForCritical * 100) {
 249                     voltageState = BATTERY_CRITICAL;
 250                 }
 251             } else {
 252                 if (voltageMeter.displayFiltered > batteryWarningVoltage) {
 253                     voltageState = BATTERY_OK;
 254                 }
 255                 lastVoltageChangeMs = millis();
 256             }
 257             break;
 258
 259         case BATTERY_CRITICAL:
 260             if (voltageMeter.displayFiltered > batteryCriticalVoltage) {
 261                 voltageState = BATTERY_WARNING;
 262                 lastVoltageChangeMs = millis();
 263             }
 264             break;
 265
 266         default:
 267             break;
 268     }
 269
 270 }
 271
 272 static void batteryUpdateLVC(timeUs_t currentTimeUs)
 273 {
 274     if (batteryConfig()->lvcPercentage < 100) {
 275         if (voltageState == BATTERY_CRITICAL && !lowVoltageCutoff.enabled) {
 276             lowVoltageCutoff.enabled = true;
 277             lowVoltageCutoff.startTime = currentTimeUs;
 278             lowVoltageCutoff.percentage = 100;
 279         }
 280         if (lowVoltageCutoff.enabled) {
 281             if (cmp32(currentTimeUs,lowVoltageCutoff.startTime) < LVC_AFFECT_TIME) {
 282                 lowVoltageCutoff.percentage = 100 - (cmp32(currentTimeUs,lowVoltageCutoff.startTime) * (100 - batteryConfig()->lvcPercentage) / LVC_AFFECT_TIME);
 283             }
 284             else {
 285                 lowVoltageCutoff.percentage = batteryConfig()->lvcPercentage;
 286             }
 287         }
 288     }
 289
 290 }
 291
 292 static void batteryUpdateConsumptionState(void)
 293 {
 294     if (batteryConfig()->useConsumptionAlerts && batteryConfig()->batteryCapacity > 0 && batteryCellCount > 0) {
 295         uint8_t batteryPercentageRemaining = calculateBatteryPercentageRemaining();
 296
 297         if (batteryPercentageRemaining == 0) {
 298             consumptionState = BATTERY_CRITICAL;
 299         } else if (batteryPercentageRemaining <= batteryConfig()->consumptionWarningPercentage) {
 300             consumptionState = BATTERY_WARNING;
 301         } else {
 302             consumptionState = BATTERY_OK;
 303         }
 304     }
 305 }
 306
 307 void batteryUpdateStates(timeUs_t currentTimeUs)
 308 {
 309     batteryUpdateVoltageState();
 310     batteryUpdateConsumptionState();
 311     batteryUpdateLVC(currentTimeUs);
 312     batteryState = MAX(voltageState, consumptionState);
 313 }
 314
 315 const lowVoltageCutoff_t *getLowVoltageCutoff(void)
 316 {
 317     return &lowVoltageCutoff;
 318 }
 319
 320 batteryState_e getBatteryState(void)
 321 {
 322     return batteryState;
 323 }
 324
 325 batteryState_e getVoltageState(void)
 326 {
 327     return voltageState;
 328 }
 329
 330 batteryState_e getConsumptionState(void)
 331 {
 332     return consumptionState;
 333 }
 334
 335 const char * const batteryStateStrings[] = {"OK", "WARNING", "CRITICAL", "NOT PRESENT", "INIT"};
 336
 337 const char * getBatteryStateString(void)
 338 {
 339     return batteryStateStrings[getBatteryState()];
 340 }
 341
 342 void batteryInit(void)
 343 {
 344     //
 345     // presence
 346     //
 347     batteryState = BATTERY_INIT;
 348     batteryCellCount = 0;
 349
 350     //
 351     // voltage
 352     //
 353     voltageState = BATTERY_INIT;
 354     batteryWarningVoltage = 0;
 355     batteryCriticalVoltage = 0;
 356     lowVoltageCutoff.enabled = false;
 357     lowVoltageCutoff.percentage = 100;
 358     lowVoltageCutoff.startTime = 0;
 359
 360     voltageMeterReset(&voltageMeter);
 361
 362     voltageMeterGenericInit();
 363     switch (batteryConfig()->voltageMeterSource) {
 364         case VOLTAGE_METER_ESC:
 365 #ifdef USE_ESC_SENSOR
 366             voltageMeterESCInit();
 367 #endif
 368             break;
 369
 370         case VOLTAGE_METER_ADC:
 371             voltageMeterADCInit();
 372             break;
 373
 374         default:
 375             break;
 376     }
 377
 378     //
 379     // current
 380     //
 381     consumptionState = BATTERY_OK;
 382     currentMeterReset(&currentMeter);
 383     switch (batteryConfig()->currentMeterSource) {
 384         case CURRENT_METER_ADC:
 385             currentMeterADCInit();
 386             break;
 387
 388         case CURRENT_METER_VIRTUAL:
 389 #ifdef USE_VIRTUAL_CURRENT_METER
 390             currentMeterVirtualInit();
 391 #endif
 392             break;
 393
 394         case CURRENT_METER_ESC:
 395 #ifdef ESC_SENSOR
 396             currentMeterESCInit();
 397 #endif
 398             break;
 399         case CURRENT_METER_MSP:
 400 #ifdef USE_MSP_CURRENT_METER
 401             currentMeterMSPInit();
 402 #endif
 403             break;
 404
 405         default:
 406             break;
 407     }
 408
 409 }
 410
 411 void batteryUpdateCurrentMeter(timeUs_t currentTimeUs)
 412 {
 413     UNUSED(currentTimeUs);
 414     if (batteryCellCount == 0) {
 415         currentMeterReset(&currentMeter);
 416         return;
 417     }
 418
 419     static uint32_t ibatLastServiced = 0;
 420     const int32_t lastUpdateAt = cmp32(currentTimeUs, ibatLastServiced);
 421     ibatLastServiced = currentTimeUs;
 422
 423     switch (batteryConfig()->currentMeterSource) {
 424         case CURRENT_METER_ADC:
 425             currentMeterADCRefresh(lastUpdateAt);
 426             currentMeterADCRead(&currentMeter);
 427             break;
 428
 429         case CURRENT_METER_VIRTUAL: {
 430 #ifdef USE_VIRTUAL_CURRENT_METER
 431             throttleStatus_e throttleStatus = calculateThrottleStatus();
 432             bool throttleLowAndMotorStop = (throttleStatus == THROTTLE_LOW && featureIsEnabled(FEATURE_MOTOR_STOP));
 433             const int32_t throttleOffset = lrintf(mixerGetThrottle() * 1000);
 434
 435             currentMeterVirtualRefresh(lastUpdateAt, ARMING_FLAG(ARMED), throttleLowAndMotorStop, throttleOffset);
 436             currentMeterVirtualRead(&currentMeter);
 437 #endif
 438             break;
 439         }
 440
 441         case CURRENT_METER_ESC:
 442 #ifdef USE_ESC_SENSOR
 443             if (featureIsEnabled(FEATURE_ESC_SENSOR)) {
 444                 currentMeterESCRefresh(lastUpdateAt);
 445                 currentMeterESCReadCombined(&currentMeter);
 446             }
 447 #endif
 448             break;
 449         case CURRENT_METER_MSP:
 450 #ifdef USE_MSP_CURRENT_METER
 451             currentMeterMSPRefresh(currentTimeUs);
 452             currentMeterMSPRead(&currentMeter);
 453 #endif
 454             break;
 455
 456         default:
 457         case CURRENT_METER_NONE:
 458             currentMeterReset(&currentMeter);
 459             break;
 460     }
 461 }
 462
 463 float calculateVbatPidCompensation(void) {
 464     float batteryScaler =  1.0f;
 465     if (batteryConfig()->voltageMeterSource != VOLTAGE_METER_NONE && batteryCellCount > 0) {
 466         // Up to 33% PID gain. Should be fine for 4,2to 3,3 difference
 467         batteryScaler =  constrainf((( (float)batteryConfig()->vbatmaxcellvoltage * batteryCellCount ) / (float) voltageMeter.displayFiltered), 1.0f, 1.33f);
 468     }
 469     return batteryScaler;
 470 }
 471
 472 uint8_t calculateBatteryPercentageRemaining(void)
 473 {
 474     uint8_t batteryPercentage = 0;
 475     if (batteryCellCount > 0) {
 476         uint16_t batteryCapacity = batteryConfig()->batteryCapacity;
 477
 478         if (batteryCapacity > 0) {
 479             batteryPercentage = constrain(((float)batteryCapacity - currentMeter.mAhDrawn) * 100 / batteryCapacity, 0, 100);
 480         } else {
 481             batteryPercentage = constrain((((uint32_t)voltageMeter.displayFiltered - (batteryConfig()->vbatmincellvoltage * batteryCellCount)) * 100) / ((batteryConfig()->vbatmaxcellvoltage - batteryConfig()->vbatmincellvoltage) * batteryCellCount), 0, 100);
 482         }
 483     }
 484
 485     return batteryPercentage;
 486 }
 487
 488 void batteryUpdateAlarms(void)
 489 {
 490     // use the state to trigger beeper alerts
 491     if (batteryConfig()->useVBatAlerts) {
 492         updateBatteryBeeperAlert();
 493     }
 494 }
 495
 496 bool isBatteryVoltageConfigured(void)
 497 {
 498     return batteryConfig()->voltageMeterSource != VOLTAGE_METER_NONE;
 499 }
 500
 501 uint16_t getBatteryVoltage(void)
 502 {
 503     return voltageMeter.displayFiltered;
 504 }
 505
 506 uint16_t getLegacyBatteryVoltage(void)
 507 {
 508     return (voltageMeter.displayFiltered + 5) / 10;
 509 }
 510
 511 uint16_t getBatteryVoltageLatest(void)
 512 {
 513     return voltageMeter.unfiltered;
 514 }
 515
 516 uint8_t getBatteryCellCount(void)
 517 {
 518     return batteryCellCount;
 519 }
 520
 521 uint16_t getBatteryAverageCellVoltage(void)
 522 {
 523     return voltageMeter.displayFiltered / batteryCellCount;
 524 }
 525
 526 #if defined(USE_BATTERY_VOLTAGE_SAG_COMPENSATION)
 527 uint16_t getBatterySagCellVoltage(void)
 528 {
 529     return voltageMeter.sagFiltered / batteryCellCount;
 530 }
 531 #endif
 532
 533 bool isAmperageConfigured(void)
 534 {
 535     return batteryConfig()->currentMeterSource != CURRENT_METER_NONE;
 536 }
 537
 538 int32_t getAmperage(void) {
 539     return currentMeter.amperage;
 540 }
 541
 542 int32_t getAmperageLatest(void)
 543 {
 544     return currentMeter.amperageLatest;
 545 }
 546
 547 int32_t getMAhDrawn(void)
 548 {
 549     return currentMeter.mAhDrawn;
 550 }