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 #include "flight/mixer_init.h"
  44
  45 #include "io/beeper.h"
  46
  47 #include "pg/pg.h"
  48 #include "pg/pg_ids.h"
  49
  50 #include "scheduler/scheduler.h"
  51 #ifdef USE_BATTERY_CONTINUE
  52 #include "pg/stats.h"
  53 #endif
  54
  55 #include "sensors/battery.h"
  56
  57 /**
  58  * terminology: meter vs sensors
  59  *
  60  * voltage and current sensors are used to collect data.
  61  * - e.g. voltage at an MCU ADC input pin, value from an ESC sensor.
  62  *   sensors require very specific configuration, such as resistor values.
  63  * voltage and current meters are used to process and expose data collected from sensors to the rest of the system.
  64  * - e.g. a meter exposes normalized, and often filtered, values from a sensor.
  65  *   meters require different or little configuration.
  66  *   meters also have different precision concerns, and may use different units to the sensors.
  67  *
  68  */
  69
  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     voltageStableUpdate(&voltageMeter);
 162
 163     DEBUG_SET(DEBUG_BATTERY, 0, voltageMeter.unfiltered);
 164     DEBUG_SET(DEBUG_BATTERY, 1, voltageMeter.displayFiltered);
 165     DEBUG_SET(DEBUG_BATTERY, 3, voltageMeter.voltageStableBits);
 166     DEBUG_SET(DEBUG_BATTERY, 4, voltageIsStable(&voltageMeter) ? 1 : 0);
 167     DEBUG_SET(DEBUG_BATTERY, 5, isVoltageFromBattery() ? 1 : 0);
 168     DEBUG_SET(DEBUG_BATTERY, 6, voltageMeter.voltageStablePrevFiltered);
 169     DEBUG_SET(DEBUG_BATTERY, 7, voltageState);
 170 }
 171
 172 static void updateBatteryBeeperAlert(void)
 173 {
 174     switch (getBatteryState()) {
 175         case BATTERY_WARNING:
 176             beeper(BEEPER_BAT_LOW);
 177
 178             break;
 179         case BATTERY_CRITICAL:
 180             beeper(BEEPER_BAT_CRIT_LOW);
 181
 182             break;
 183         case BATTERY_OK:
 184         case BATTERY_NOT_PRESENT:
 185         case BATTERY_INIT:
 186             break;
 187     }
 188 }
 189
 190 bool isVoltageFromBattery(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     if ((voltageState == BATTERY_NOT_PRESENT || voltageState == BATTERY_INIT)
 202         && isVoltageFromBattery()
 203         && voltageIsStable(&voltageMeter)) {
 204         // Battery has just been connected - calculate cells, warning voltages and reset state
 205
 206         consumptionState = voltageState = BATTERY_OK;
 207         if (batteryConfig()->forceBatteryCellCount != 0) {
 208             batteryCellCount = batteryConfig()->forceBatteryCellCount;
 209         } else {
 210             unsigned cells = (voltageMeter.displayFiltered / batteryConfig()->vbatmaxcellvoltage) + 1;
 211             if (cells > MAX_AUTO_DETECT_CELL_COUNT) {
 212                 // something is wrong, we expect MAX_CELL_COUNT cells maximum (and autodetection will be problematic at 6+ cells)
 213                 cells = MAX_AUTO_DETECT_CELL_COUNT;
 214             }
 215             batteryCellCount = cells;
 216
 217             if (!ARMING_FLAG(ARMED)) {
 218                 changePidProfileFromCellCount(batteryCellCount);
 219             }
 220         }
 221 #ifdef USE_RPM_LIMIT
 222         mixerResetRpmLimiter();
 223 #endif
 224         batteryWarningVoltage = batteryCellCount * batteryConfig()->vbatwarningcellvoltage;
 225         batteryCriticalVoltage = batteryCellCount * batteryConfig()->vbatmincellvoltage;
 226         batteryWarningHysteresisVoltage = (batteryWarningVoltage > batteryConfig()->vbathysteresis) ? batteryWarningVoltage - batteryConfig()->vbathysteresis : 0;
 227         batteryCriticalHysteresisVoltage = (batteryCriticalVoltage > batteryConfig()->vbathysteresis) ? batteryCriticalVoltage - batteryConfig()->vbathysteresis : 0;
 228         lowVoltageCutoff.percentage = 100;
 229         lowVoltageCutoff.startTime = 0;
 230     } else if (voltageState != BATTERY_NOT_PRESENT
 231                && voltageIsStable(&voltageMeter)
 232                && !isVoltageFromBattery()) {
 233         /* battery has been disconnected - can take a while for filter cap to disharge so we use a threshold of batteryConfig()->vbatnotpresentcellvoltage */
 234         consumptionState = voltageState = BATTERY_NOT_PRESENT;
 235
 236         batteryCellCount = 0;
 237         batteryWarningVoltage = 0;
 238         batteryCriticalVoltage = 0;
 239         batteryWarningHysteresisVoltage = 0;
 240         batteryCriticalHysteresisVoltage = 0;
 241         wattHoursDrawn = 0.0;
 242     }
 243 }
 244
 245 void batteryUpdateWhDrawn(void)
 246 {
 247     static int32_t mAhDrawnPrev = 0;
 248     const int32_t mAhDrawnCurrent = getMAhDrawn();
 249     wattHoursDrawn += voltageMeter.displayFiltered * (mAhDrawnCurrent - mAhDrawnPrev) / 100000.0f;
 250     mAhDrawnPrev = mAhDrawnCurrent;
 251 }
 252
 253 static void batteryUpdateVoltageState(void)
 254 {
 255     // alerts are currently used by beeper, osd and other subsystems
 256     static uint32_t lastVoltageChangeMs;
 257     switch (voltageState) {
 258         case BATTERY_OK:
 259             if (voltageMeter.displayFiltered <= batteryWarningHysteresisVoltage) {
 260                 if (cmp32(millis(), lastVoltageChangeMs) >= batteryConfig()->vbatDurationForWarning * 100) {
 261                     voltageState = BATTERY_WARNING;
 262                 }
 263             } else {
 264                 lastVoltageChangeMs = millis();
 265             }
 266             break;
 267
 268         case BATTERY_WARNING:
 269             if (voltageMeter.displayFiltered <= batteryCriticalHysteresisVoltage) {
 270                 if (cmp32(millis(), lastVoltageChangeMs) >= batteryConfig()->vbatDurationForCritical * 100) {
 271                     voltageState = BATTERY_CRITICAL;
 272                 }
 273             } else {
 274                 if (voltageMeter.displayFiltered > batteryWarningVoltage) {
 275                     voltageState = BATTERY_OK;
 276                 }
 277                 lastVoltageChangeMs = millis();
 278             }
 279             break;
 280
 281         case BATTERY_CRITICAL:
 282             if (voltageMeter.displayFiltered > batteryCriticalVoltage) {
 283                 voltageState = BATTERY_WARNING;
 284                 lastVoltageChangeMs = millis();
 285             }
 286             break;
 287
 288         default:
 289             break;
 290     }
 291
 292 }
 293
 294 static void batteryUpdateLVC(timeUs_t currentTimeUs)
 295 {
 296     if (batteryConfig()->lvcPercentage < 100) {
 297         if (voltageState == BATTERY_CRITICAL && !lowVoltageCutoff.enabled) {
 298             lowVoltageCutoff.enabled = true;
 299             lowVoltageCutoff.startTime = currentTimeUs;
 300             lowVoltageCutoff.percentage = 100;
 301         }
 302         if (lowVoltageCutoff.enabled) {
 303             if (cmp32(currentTimeUs,lowVoltageCutoff.startTime) < LVC_AFFECT_TIME) {
 304                 lowVoltageCutoff.percentage = 100 - (cmp32(currentTimeUs,lowVoltageCutoff.startTime) * (100 - batteryConfig()->lvcPercentage) / LVC_AFFECT_TIME);
 305             }
 306             else {
 307                 lowVoltageCutoff.percentage = batteryConfig()->lvcPercentage;
 308             }
 309         }
 310     }
 311
 312 }
 313
 314 static void batteryUpdateConsumptionState(void)
 315 {
 316     if (batteryConfig()->useConsumptionAlerts && batteryConfig()->batteryCapacity > 0 && batteryCellCount > 0) {
 317         uint8_t batteryPercentageRemaining = calculateBatteryPercentageRemaining();
 318
 319         if (batteryPercentageRemaining == 0) {
 320             consumptionState = BATTERY_CRITICAL;
 321         } else if (batteryPercentageRemaining <= batteryConfig()->consumptionWarningPercentage) {
 322             consumptionState = BATTERY_WARNING;
 323         } else {
 324             consumptionState = BATTERY_OK;
 325         }
 326     }
 327 }
 328
 329 void batteryUpdateStates(timeUs_t currentTimeUs)
 330 {
 331     batteryUpdateVoltageState();
 332     batteryUpdateConsumptionState();
 333     batteryUpdateLVC(currentTimeUs);
 334     batteryState = MAX(voltageState, consumptionState);
 335     batteryUpdateWhDrawn();
 336 }
 337
 338 const lowVoltageCutoff_t *getLowVoltageCutoff(void)
 339 {
 340     return &lowVoltageCutoff;
 341 }
 342
 343 batteryState_e getBatteryState(void)
 344 {
 345     return batteryState;
 346 }
 347
 348 batteryState_e getVoltageState(void)
 349 {
 350     return voltageState;
 351 }
 352
 353 batteryState_e getConsumptionState(void)
 354 {
 355     return consumptionState;
 356 }
 357
 358 const char * const batteryStateStrings[] = {"OK", "WARNING", "CRITICAL", "NOT PRESENT", "INIT"};
 359
 360 const char * getBatteryStateString(void)
 361 {
 362     return batteryStateStrings[getBatteryState()];
 363 }
 364
 365 void batteryInit(void)
 366 {
 367     //
 368     // presence
 369     //
 370     batteryState = BATTERY_INIT;
 371     batteryCellCount = 0;
 372
 373     //
 374     // Consumption
 375     //
 376     wattHoursDrawn = 0;
 377
 378     //
 379     // voltage
 380     //
 381     voltageState = BATTERY_INIT;
 382     batteryWarningVoltage = 0;
 383     batteryCriticalVoltage = 0;
 384     batteryWarningHysteresisVoltage = 0;
 385     batteryCriticalHysteresisVoltage = 0;
 386     lowVoltageCutoff.enabled = false;
 387     lowVoltageCutoff.percentage = 100;
 388     lowVoltageCutoff.startTime = 0;
 389
 390     voltageMeterReset(&voltageMeter);
 391
 392     voltageMeterGenericInit();
 393     switch (batteryConfig()->voltageMeterSource) {
 394         case VOLTAGE_METER_ESC:
 395 #ifdef USE_ESC_SENSOR
 396             voltageMeterESCInit();
 397 #endif
 398             break;
 399
 400         case VOLTAGE_METER_ADC:
 401             voltageMeterADCInit();
 402             break;
 403
 404         default:
 405             break;
 406     }
 407
 408     //
 409     // current
 410     //
 411     consumptionState = BATTERY_OK;
 412     currentMeterReset(&currentMeter);
 413     switch (batteryConfig()->currentMeterSource) {
 414         case CURRENT_METER_ADC:
 415             currentMeterADCInit();
 416             break;
 417
 418         case CURRENT_METER_VIRTUAL:
 419 #ifdef USE_VIRTUAL_CURRENT_METER
 420             currentMeterVirtualInit();
 421 #endif
 422             break;
 423
 424         case CURRENT_METER_ESC:
 425 #ifdef ESC_SENSOR
 426             currentMeterESCInit();
 427 #endif
 428             break;
 429         case CURRENT_METER_MSP:
 430 #ifdef USE_MSP_CURRENT_METER
 431             currentMeterMSPInit();
 432 #endif
 433             break;
 434
 435         default:
 436             break;
 437     }
 438 }
 439
 440 void batteryUpdateCurrentMeter(timeUs_t currentTimeUs)
 441 {
 442     if (batteryCellCount == 0) {
 443         currentMeterReset(&currentMeter);
 444         return;
 445     }
 446
 447     static uint32_t ibatLastServiced = 0;
 448     const int32_t lastUpdateAt = cmp32(currentTimeUs, ibatLastServiced);
 449     ibatLastServiced = currentTimeUs;
 450
 451     switch (batteryConfig()->currentMeterSource) {
 452         case CURRENT_METER_ADC:
 453             currentMeterADCRefresh(lastUpdateAt);
 454             currentMeterADCRead(&currentMeter);
 455             break;
 456
 457         case CURRENT_METER_VIRTUAL: {
 458 #ifdef USE_VIRTUAL_CURRENT_METER
 459             throttleStatus_e throttleStatus = calculateThrottleStatus();
 460             bool throttleLowAndMotorStop = (throttleStatus == THROTTLE_LOW && featureIsEnabled(FEATURE_MOTOR_STOP));
 461             const int32_t throttleOffset = lrintf(mixerGetThrottle() * 1000);
 462
 463             currentMeterVirtualRefresh(lastUpdateAt, ARMING_FLAG(ARMED), throttleLowAndMotorStop, throttleOffset);
 464             currentMeterVirtualRead(&currentMeter);
 465 #endif
 466             break;
 467         }
 468
 469         case CURRENT_METER_ESC:
 470 #ifdef USE_ESC_SENSOR
 471             if (featureIsEnabled(FEATURE_ESC_SENSOR)) {
 472                 currentMeterESCRefresh(lastUpdateAt);
 473                 currentMeterESCReadCombined(&currentMeter);
 474             }
 475 #endif
 476             break;
 477         case CURRENT_METER_MSP:
 478 #ifdef USE_MSP_CURRENT_METER
 479             currentMeterMSPRefresh(currentTimeUs);
 480             currentMeterMSPRead(&currentMeter);
 481 #endif
 482             break;
 483
 484         default:
 485         case CURRENT_METER_NONE:
 486             currentMeterReset(&currentMeter);
 487             break;
 488     }
 489 }
 490
 491 uint8_t calculateBatteryPercentageRemaining(void)
 492 {
 493     uint8_t batteryPercentage = 0;
 494     if (batteryCellCount > 0) {
 495         uint16_t batteryCapacity = batteryConfig()->batteryCapacity;
 496
 497         if (batteryCapacity > 0) {
 498             batteryPercentage = constrain(((float)batteryCapacity - currentMeter.mAhDrawn) * 100 / batteryCapacity, 0, 100);
 499         } else {
 500             batteryPercentage = constrain((((uint32_t)voltageMeter.displayFiltered - (batteryConfig()->vbatmincellvoltage * batteryCellCount)) * 100) / ((batteryConfig()->vbatmaxcellvoltage - batteryConfig()->vbatmincellvoltage) * batteryCellCount), 0, 100);
 501         }
 502     }
 503
 504     return batteryPercentage;
 505 }
 506
 507 void batteryUpdateAlarms(void)
 508 {
 509     // use the state to trigger beeper alerts
 510     if (batteryConfig()->useVBatAlerts) {
 511         updateBatteryBeeperAlert();
 512     }
 513 }
 514
 515 bool isBatteryVoltageConfigured(void)
 516 {
 517     return batteryConfig()->voltageMeterSource != VOLTAGE_METER_NONE;
 518 }
 519
 520 uint16_t getBatteryVoltage(void)
 521 {
 522     return voltageMeter.displayFiltered;
 523 }
 524
 525 uint16_t getLegacyBatteryVoltage(void)
 526 {
 527     return (voltageMeter.displayFiltered + 5) / 10;
 528 }
 529
 530 uint16_t getBatteryVoltageLatest(void)
 531 {
 532     return voltageMeter.unfiltered;
 533 }
 534
 535 uint8_t getBatteryCellCount(void)
 536 {
 537     return batteryCellCount;
 538 }
 539
 540 uint16_t getBatteryAverageCellVoltage(void)
 541 {
 542     return (batteryCellCount ? voltageMeter.displayFiltered / batteryCellCount : 0);
 543 }
 544
 545 #if defined(USE_BATTERY_VOLTAGE_SAG_COMPENSATION)
 546 uint16_t getBatterySagCellVoltage(void)
 547 {
 548     return (batteryCellCount ? voltageMeter.sagFiltered / batteryCellCount : 0);
 549 }
 550 #endif
 551
 552 bool isAmperageConfigured(void)
 553 {
 554     return batteryConfig()->currentMeterSource != CURRENT_METER_NONE;
 555 }
 556
 557 int32_t getAmperage(void)
 558 {
 559     return currentMeter.amperage;
 560 }
 561
 562 int32_t getAmperageLatest(void)
 563 {
 564     return currentMeter.amperageLatest;
 565 }
 566
 567 int32_t getMAhDrawn(void)
 568 {
 569 #ifdef USE_BATTERY_CONTINUE
 570     return currentMeter.mAhDrawn + currentMeter.mAhDrawnOffset;
 571 #else
 572     return currentMeter.mAhDrawn;
 573 #endif
 574 }
 575
 576 #ifdef USE_BATTERY_CONTINUE
 577 bool hasUsedMAh(void)
 578 {
 579     return batteryConfig()->isBatteryContinueEnabled
 580           && !(ARMING_FLAG(ARMED) || ARMING_FLAG(WAS_EVER_ARMED)) && (getBatteryState() == BATTERY_OK)
 581           && getBatteryAverageCellVoltage() < batteryConfig()->vbatfullcellvoltage
 582           && statsConfig()->stats_mah_used > 0;
 583 }
 584
 585 void setMAhDrawn(uint32_t mAhDrawn)
 586 {
 587     currentMeter.mAhDrawnOffset = mAhDrawn;
 588 }
 589 #endif
 590
 591 float getWhDrawn(void)
 592 {
 593     return wattHoursDrawn;
 594 }