src/main/sensors/battery.c

   1 /*
   2  * This file is part of Cleanflight.
   3  *
   4  * Cleanflight is free software: you can redistribute it and/or modify
   5  * it under the terms of the GNU General Public License as published by
   6  * the Free Software Foundation, either version 3 of the License, or
   7  * (at your option) any later version.
   8  *
   9  * Cleanflight is distributed in the hope that it will be useful,
  10  * but WITHOUT ANY WARRANTY; without even the implied warranty of
  11  * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
  12  * GNU General Public License for more details.
  13  *
  14  * You should have received a copy of the GNU General Public License
  15  * along with Cleanflight.  If not, see <http://www.gnu.org/licenses/>.
  16  */
  17
  18 #include <math.h>
  19
  20 #include "stdbool.h"
  21 #include "stdint.h"
  22 #include "stdlib.h"
  23
  24 #include "platform.h"
  25
  26 #include "build/debug.h"
  27
  28 #include "common/maths.h"
  29 #include "common/filter.h"
  30
  31 #include "config/config_reset.h"
  32 #include "config/parameter_group.h"
  33 #include "config/parameter_group_ids.h"
  34
  35 #include "drivers/adc.h"
  36 #include "drivers/time.h"
  37
  38 #include "fc/config.h"
  39 #include "fc/controlrate_profile.h"
  40 #include "fc/fc_core.h"
  41 #include "fc/runtime_config.h"
  42 #include "fc/stats.h"
  43 #include "fc/settings.h"
  44
  45 #include "flight/imu.h"
  46 #include "flight/mixer.h"
  47
  48 #include "navigation/navigation.h"
  49 #include "navigation/navigation_private.h"
  50
  51 #include "config/feature.h"
  52
  53 #include "sensors/battery.h"
  54 #include "sensors/esc_sensor.h"
  55
  56 #include "rx/rx.h"
  57
  58 #include "fc/rc_controls.h"
  59
  60 #include "io/beeper.h"
  61
  62 #if defined(USE_FAKE_BATT_SENSOR)
  63 #include "sensors/battery_sensor_fake.h"
  64 #endif
  65
  66 #define ADCVREF 3300                            // in mV (3300 = 3.3V)
  67
  68 #define VBATT_CELL_FULL_MAX_DIFF 10             // Max difference with cell max voltage for the battery to be considered full (10mV steps)
  69 #define VBATT_PRESENT_THRESHOLD 220             // Minimum voltage to consider battery present
  70 #define VBATT_STABLE_DELAY 40                   // Delay after connecting battery to begin monitoring
  71 #define VBATT_HYSTERESIS 10                     // Batt Hysteresis of +/-100mV for changing battery state
  72 #define VBATT_LPF_FREQ  1                       // Battery voltage filtering cutoff
  73 #define AMPERAGE_LPF_FREQ  1                    // Battery current filtering cutoff
  74 #define IMPEDANCE_STABLE_SAMPLE_COUNT_THRESH 10 // Minimum sample count to consider calculated power supply impedance as stable
  75
  76
  77 // Battery monitoring stuff
  78 static uint8_t batteryCellCount;                // cell count
  79 static uint16_t batteryFullVoltage;
  80 static uint16_t batteryWarningVoltage;
  81 static uint16_t batteryCriticalVoltage;
  82 static uint32_t batteryRemainingCapacity = 0;
  83 static bool batteryUseCapacityThresholds = false;
  84 static bool batteryFullWhenPluggedIn = false;
  85 static bool profileAutoswitchDisable = false;
  86
  87 static uint16_t vbat = 0;                       // battery voltage in 0.01V steps (filtered)
  88 static uint16_t powerSupplyImpedance = 0;       // calculated impedance in milliohm
  89 static uint16_t sagCompensatedVBat = 0;         // calculated no load vbat
  90 static bool powerSupplyImpedanceIsValid = false;
  91
  92 static int16_t amperage = 0;                    // amperage read by current sensor in centiampere (1/100th A)
  93 static int32_t power = 0;                       // power draw in cW (0.01W resolution)
  94 static int32_t mAhDrawn = 0;                    // milliampere hours drawn from the battery since start
  95 static int32_t mWhDrawn = 0;                    // energy (milliWatt hours) drawn from the battery since start
  96
  97 batteryState_e batteryState;
  98 const batteryProfile_t *currentBatteryProfile;
  99
 100 PG_REGISTER_ARRAY_WITH_RESET_FN(batteryProfile_t, MAX_BATTERY_PROFILE_COUNT, batteryProfiles, PG_BATTERY_PROFILES, 3);
 101
 102 void pgResetFn_batteryProfiles(batteryProfile_t *instance)
 103 {
 104     for (int i = 0; i < MAX_BATTERY_PROFILE_COUNT; i++) {
 105         RESET_CONFIG(batteryProfile_t, &instance[i],
 106 #ifdef USE_ADC
 107             .cells = SETTING_BAT_CELLS_DEFAULT,
 108
 109             .voltage = {
 110                 .cellDetect = SETTING_VBAT_CELL_DETECT_VOLTAGE_DEFAULT,
 111                 .cellMax = SETTING_VBAT_MAX_CELL_VOLTAGE_DEFAULT,
 112                 .cellMin = SETTING_VBAT_MIN_CELL_VOLTAGE_DEFAULT,
 113                 .cellWarning = SETTING_VBAT_WARNING_CELL_VOLTAGE_DEFAULT
 114             },
 115 #endif
 116
 117             .capacity = {
 118                 .value = SETTING_BATTERY_CAPACITY_DEFAULT,
 119                 .warning = SETTING_BATTERY_CAPACITY_WARNING_DEFAULT,
 120                 .critical = SETTING_BATTERY_CAPACITY_CRITICAL_DEFAULT,
 121             },
 122
 123             .controlRateProfile = 0,
 124
 125             .motor = {
 126                 .throttleIdle = SETTING_THROTTLE_IDLE_DEFAULT,
 127                 .throttleScale = SETTING_THROTTLE_SCALE_DEFAULT,
 128 #ifdef USE_DSHOT
 129                 .turtleModePowerFactor = SETTING_TURTLE_MODE_POWER_FACTOR_DEFAULT,
 130 #endif
 131             },
 132
 133             .failsafe_throttle = SETTING_FAILSAFE_THROTTLE_DEFAULT,                                 // default throttle off.
 134
 135             .nav = {
 136                 .mc = {
 137                     .hover_throttle = SETTING_NAV_MC_HOVER_THR_DEFAULT,
 138                 },
 139
 140                 .fw = {
 141                     .cruise_throttle = SETTING_NAV_FW_CRUISE_THR_DEFAULT,
 142                     .max_throttle = SETTING_NAV_FW_MAX_THR_DEFAULT,
 143                     .min_throttle = SETTING_NAV_FW_MIN_THR_DEFAULT,
 144                     .pitch_to_throttle = SETTING_NAV_FW_PITCH2THR_DEFAULT,                          // pwm units per degree of pitch (10pwm units ~ 1% throttle)
 145                     .launch_throttle = SETTING_NAV_FW_LAUNCH_THR_DEFAULT,
 146                     .launch_idle_throttle = SETTING_NAV_FW_LAUNCH_IDLE_THR_DEFAULT,                 // Motor idle or MOTOR_STOP
 147                 }
 148             },
 149
 150 #if defined(USE_POWER_LIMITS)
 151             .powerLimits = {
 152                 .continuousCurrent = SETTING_LIMIT_CONT_CURRENT_DEFAULT,                            // dA
 153                 .burstCurrent = SETTING_LIMIT_BURST_CURRENT_DEFAULT,                                // dA
 154                 .burstCurrentTime = SETTING_LIMIT_BURST_CURRENT_TIME_DEFAULT,                       // dS
 155                 .burstCurrentFalldownTime = SETTING_LIMIT_BURST_CURRENT_FALLDOWN_TIME_DEFAULT,      // dS
 156 #ifdef USE_ADC
 157                 .continuousPower = SETTING_LIMIT_CONT_POWER_DEFAULT,                                // dW
 158                 .burstPower = SETTING_LIMIT_BURST_POWER_DEFAULT,                                    // dW
 159                 .burstPowerTime = SETTING_LIMIT_BURST_POWER_TIME_DEFAULT,                           // dS
 160                 .burstPowerFalldownTime = SETTING_LIMIT_BURST_POWER_FALLDOWN_TIME_DEFAULT,          // dS
 161 #endif // USE_ADC
 162             }
 163 #endif // USE_POWER_LIMITS
 164
 165         );
 166     }
 167 }
 168
 169 PG_REGISTER_WITH_RESET_TEMPLATE(batteryMetersConfig_t, batteryMetersConfig, PG_BATTERY_METERS_CONFIG, 2);
 170
 171 PG_RESET_TEMPLATE(batteryMetersConfig_t, batteryMetersConfig,
 172
 173 #ifdef USE_ADC
 174     .voltage = {
 175         .type = SETTING_VBAT_METER_TYPE_DEFAULT,
 176         .scale = VBAT_SCALE_DEFAULT,
 177     },
 178 #endif
 179
 180     .current = {
 181         .type = SETTING_CURRENT_METER_TYPE_DEFAULT,
 182         .scale = CURRENT_METER_SCALE,
 183         .offset = CURRENT_METER_OFFSET
 184     },
 185
 186     .voltageSource = SETTING_BAT_VOLTAGE_SRC_DEFAULT,
 187
 188     .capacity_unit = SETTING_BATTERY_CAPACITY_UNIT_DEFAULT,
 189
 190     .cruise_power = SETTING_CRUISE_POWER_DEFAULT,
 191     .idle_power = SETTING_IDLE_POWER_DEFAULT,
 192     .rth_energy_margin = SETTING_RTH_ENERGY_MARGIN_DEFAULT,
 193
 194     .throttle_compensation_weight = SETTING_THR_COMP_WEIGHT_DEFAULT
 195
 196 );
 197
 198 void batteryInit(void)
 199 {
 200     batteryState = BATTERY_NOT_PRESENT;
 201     batteryCellCount = 0;
 202     batteryFullVoltage = 0;
 203     batteryWarningVoltage = 0;
 204     batteryCriticalVoltage = 0;
 205 }
 206
 207 #ifdef USE_ADC
 208 // profileDetect() profile sorting compare function
 209 static int profile_compare(profile_comp_t *a, profile_comp_t *b) {
 210     if (a->max_voltage < b->max_voltage)
 211         return -1;
 212     else if (a->max_voltage > b->max_voltage)
 213         return 1;
 214     else
 215         return 0;
 216 }
 217
 218 // Find profile matching plugged battery for profile_autoselect
 219 static int8_t profileDetect(void) {
 220     profile_comp_t profile_comp_array[MAX_BATTERY_PROFILE_COUNT];
 221
 222     // Prepare profile sort
 223     for (uint8_t i = 0; i < MAX_BATTERY_PROFILE_COUNT; ++i) {
 224         const batteryProfile_t *profile = batteryProfiles(i);
 225         profile_comp_array[i].profile_index = i;
 226         profile_comp_array[i].max_voltage = profile->cells * profile->voltage.cellDetect;
 227     }
 228
 229     // Sort profiles by max voltage
 230     qsort(profile_comp_array, MAX_BATTERY_PROFILE_COUNT, sizeof(*profile_comp_array), (int (*)(const void *, const void *))profile_compare);
 231
 232     // Return index of the first profile where vbat <= profile_max_voltage
 233     for (uint8_t i = 0; i < MAX_BATTERY_PROFILE_COUNT; ++i)
 234         if ((profile_comp_array[i].max_voltage > 0) && (vbat <= profile_comp_array[i].max_voltage))
 235             return profile_comp_array[i].profile_index;
 236
 237     // No matching profile found
 238     return -1;
 239 }
 240 #endif
 241
 242 void setBatteryProfile(uint8_t profileIndex)
 243 {
 244     if (profileIndex >= MAX_BATTERY_PROFILE_COUNT) {
 245         profileIndex = 0;
 246     }
 247     currentBatteryProfile = batteryProfiles(profileIndex);
 248     if ((currentBatteryProfile->controlRateProfile > 0) && (currentBatteryProfile->controlRateProfile < MAX_CONTROL_RATE_PROFILE_COUNT)) {
 249         setConfigProfile(currentBatteryProfile->controlRateProfile - 1);
 250     }
 251 }
 252
 253 void activateBatteryProfile(void)
 254 {
 255     static int8_t previous_battery_profile_index = -1;
 256     // Don't call batteryInit if the battery profile was not changed to prevent batteryCellCount to be reset while adjusting board alignment
 257     // causing the beeper to be silent when it is disabled while the board is connected through USB (beeper -ON_USB)
 258     if (systemConfig()->current_battery_profile_index != previous_battery_profile_index) {
 259         batteryInit();
 260         previous_battery_profile_index = systemConfig()->current_battery_profile_index;
 261     }
 262 }
 263
 264 #ifdef USE_ADC
 265 static void updateBatteryVoltage(timeUs_t timeDelta, bool justConnected)
 266 {
 267     static pt1Filter_t vbatFilterState;
 268
 269     switch (batteryMetersConfig()->voltage.type) {
 270         case VOLTAGE_SENSOR_ADC:
 271             {
 272                 vbat = getVBatSample();
 273                 break;
 274             }
 275 #if defined(USE_ESC_SENSOR)
 276         case VOLTAGE_SENSOR_ESC:
 277             {
 278                 escSensorData_t * escSensor = escSensorGetData();
 279                 if (escSensor && escSensor->dataAge <= ESC_DATA_MAX_AGE) {
 280                     vbat = escSensor->voltage;
 281                 }
 282                 else {
 283                     vbat = 0;
 284                 }
 285             }
 286             break;
 287 #endif
 288
 289 #if defined(USE_FAKE_BATT_SENSOR)
 290     case VOLTAGE_SENSOR_FAKE:
 291         vbat = fakeBattSensorGetVBat();
 292         break;
 293 #endif
 294     case VOLTAGE_SENSOR_NONE:
 295         default:
 296             vbat = 0;
 297             break;
 298     }
 299
 300 #ifdef USE_SIMULATOR
 301     if (ARMING_FLAG(SIMULATOR_MODE_HITL) && SIMULATOR_HAS_OPTION(HITL_SIMULATE_BATTERY)) {
 302         vbat = ((uint16_t)simulatorData.vbat)*10;
 303         return;
 304     }
 305 #endif
 306
 307     if (justConnected) {
 308         pt1FilterReset(&vbatFilterState, vbat);
 309     } else {
 310         vbat = pt1FilterApply4(&vbatFilterState, vbat, VBATT_LPF_FREQ, US2S(timeDelta));
 311     }
 312 }
 313
 314 batteryState_e checkBatteryVoltageState(void)
 315 {
 316     uint16_t stateVoltage = getBatteryVoltage();
 317     switch (batteryState)
 318     {
 319         case BATTERY_OK:
 320             if (stateVoltage <= (batteryWarningVoltage - VBATT_HYSTERESIS)) {
 321                 return BATTERY_WARNING;
 322             }
 323             break;
 324         case BATTERY_WARNING:
 325             if (stateVoltage <= (batteryCriticalVoltage - VBATT_HYSTERESIS)) {
 326                 return BATTERY_CRITICAL;
 327             } else if (stateVoltage > (batteryWarningVoltage + VBATT_HYSTERESIS)){
 328                 return BATTERY_OK;
 329             }
 330             break;
 331         case BATTERY_CRITICAL:
 332             if (stateVoltage > (batteryCriticalVoltage + VBATT_HYSTERESIS)) {
 333                 return BATTERY_WARNING;
 334             }
 335             break;
 336         default:
 337             break;
 338     }
 339
 340     return batteryState;
 341 }
 342
 343 static void checkBatteryCapacityState(void)
 344 {
 345     if (batteryRemainingCapacity == 0) {
 346         batteryState = BATTERY_CRITICAL;
 347     } else if (batteryRemainingCapacity <= currentBatteryProfile->capacity.warning - currentBatteryProfile->capacity.critical) {
 348         batteryState = BATTERY_WARNING;
 349     }
 350 }
 351
 352 void batteryUpdate(timeUs_t timeDelta)
 353 {
 354     /* battery has just been connected*/
 355     if (batteryState == BATTERY_NOT_PRESENT && vbat > VBATT_PRESENT_THRESHOLD) {
 356
 357         /* Actual battery state is calculated below, this is really BATTERY_PRESENT */
 358         batteryState = BATTERY_OK;
 359         /* wait for VBatt to stabilise then we can calc number of cells
 360         (using the filtered value takes a long time to ramp up)
 361         We only do this on the ground so don't care if we do block, not
 362         worse than original code anyway*/
 363         delay(VBATT_STABLE_DELAY);
 364         updateBatteryVoltage(timeDelta, true);
 365
 366         int8_t detectedProfileIndex = -1;
 367         if (feature(FEATURE_BAT_PROFILE_AUTOSWITCH) && (!profileAutoswitchDisable))
 368             detectedProfileIndex = profileDetect();
 369
 370         if (detectedProfileIndex != -1) {
 371             systemConfigMutable()->current_battery_profile_index = detectedProfileIndex;
 372             setBatteryProfile(detectedProfileIndex);
 373             batteryCellCount = currentBatteryProfile->cells;
 374         } else if (currentBatteryProfile->cells > 0)
 375             batteryCellCount = currentBatteryProfile->cells;
 376         else {
 377             batteryCellCount = (vbat / currentBatteryProfile->voltage.cellDetect) + 1;
 378             // Assume there are no 7S, 9S and 11S batteries so round up to 8S, 10S and 12S respectively
 379             if (batteryCellCount == 7 || batteryCellCount == 9 || batteryCellCount == 11) {
 380                 batteryCellCount += 1;
 381             }
 382             batteryCellCount = MIN(batteryCellCount, 12);
 383         }
 384
 385         batteryFullVoltage = batteryCellCount * currentBatteryProfile->voltage.cellMax;
 386         batteryWarningVoltage = batteryCellCount * currentBatteryProfile->voltage.cellWarning;
 387         batteryCriticalVoltage = batteryCellCount * currentBatteryProfile->voltage.cellMin;
 388
 389         batteryFullWhenPluggedIn = vbat >= (batteryFullVoltage - batteryCellCount * VBATT_CELL_FULL_MAX_DIFF);
 390         batteryUseCapacityThresholds = isAmperageConfigured() && batteryFullWhenPluggedIn && (currentBatteryProfile->capacity.value > 0) &&
 391                                            (currentBatteryProfile->capacity.warning > 0) && (currentBatteryProfile->capacity.critical > 0);
 392
 393     } else {
 394         updateBatteryVoltage(timeDelta, false);
 395
 396         /* battery has been disconnected - can take a while for filter cap to disharge so we use a threshold of VBATT_PRESENT_THRESHOLD */
 397         if (batteryState != BATTERY_NOT_PRESENT && vbat <= VBATT_PRESENT_THRESHOLD) {
 398             batteryState = BATTERY_NOT_PRESENT;
 399             batteryCellCount = 0;
 400             batteryWarningVoltage = 0;
 401             batteryCriticalVoltage = 0;
 402         }
 403     }
 404
 405     if (batteryState != BATTERY_NOT_PRESENT) {
 406
 407         if ((currentBatteryProfile->capacity.value > 0) && batteryFullWhenPluggedIn) {
 408             uint32_t capacityDiffBetweenFullAndEmpty = currentBatteryProfile->capacity.value - currentBatteryProfile->capacity.critical;
 409             int32_t drawn = (batteryMetersConfig()->capacity_unit == BAT_CAPACITY_UNIT_MWH ? mWhDrawn : mAhDrawn);
 410             batteryRemainingCapacity = (drawn > (int32_t)capacityDiffBetweenFullAndEmpty ? 0 : capacityDiffBetweenFullAndEmpty - drawn);
 411         }
 412
 413         if (batteryUseCapacityThresholds) {
 414             checkBatteryCapacityState();
 415         } else {
 416             batteryState = checkBatteryVoltageState();
 417         }
 418
 419         // handle beeper
 420         if (ARMING_FLAG(ARMED) || !ARMING_FLAG(WAS_EVER_ARMED) || failsafeIsActive())
 421             switch (batteryState) {
 422                 case BATTERY_WARNING:
 423                     beeper(BEEPER_BAT_LOW);
 424                     break;
 425                 case BATTERY_CRITICAL:
 426                     beeper(BEEPER_BAT_CRIT_LOW);
 427                     break;
 428                 default:
 429                     break;
 430             }
 431     }
 432 }
 433 #endif
 434
 435 batteryState_e getBatteryState(void)
 436 {
 437     return batteryState;
 438 }
 439
 440 bool batteryWasFullWhenPluggedIn(void)
 441 {
 442     return batteryFullWhenPluggedIn;
 443 }
 444
 445 bool batteryUsesCapacityThresholds(void)
 446 {
 447     return batteryUseCapacityThresholds;
 448 }
 449
 450 bool isBatteryVoltageConfigured(void)
 451 {
 452     return feature(FEATURE_VBAT);
 453 }
 454
 455 #ifdef USE_ADC
 456 uint16_t getVBatSample(void) {
 457     // calculate battery voltage based on ADC reading
 458     // result is Vbatt in 0.01V steps. 3.3V = ADC Vref, 0xFFF = 12bit adc, 1100 = 11:1 voltage divider (10k:1k)
 459     return (uint64_t)adcGetChannel(ADC_BATTERY) * batteryMetersConfig()->voltage.scale * ADCVREF / (0xFFF * 1000);
 460 }
 461 #endif
 462
 463 uint16_t getBatteryVoltage(void)
 464 {
 465     if (batteryMetersConfig()->voltageSource == BAT_VOLTAGE_SAG_COMP) {
 466         return sagCompensatedVBat;
 467     }
 468
 469     return vbat;
 470 }
 471
 472 uint16_t getBatteryRawVoltage(void)
 473 {
 474     return vbat;
 475 }
 476
 477 uint16_t getBatterySagCompensatedVoltage(void)
 478 {
 479     return sagCompensatedVBat;
 480 }
 481
 482 float calculateThrottleCompensationFactor(void)
 483 {
 484     return 1.0f + ((float)batteryFullVoltage / sagCompensatedVBat - 1.0f) * batteryMetersConfig()->throttle_compensation_weight;
 485 }
 486
 487 uint8_t getBatteryCellCount(void)
 488 {
 489     return batteryCellCount;
 490 }
 491
 492 uint16_t getBatteryAverageCellVoltage(void)
 493 {
 494     if (batteryCellCount > 0) {
 495         return getBatteryVoltage() / batteryCellCount;
 496     }
 497     return 0;
 498 }
 499
 500 uint16_t getBatteryRawAverageCellVoltage(void)
 501 {
 502     if (batteryCellCount > 0) {
 503         return vbat / batteryCellCount;
 504     }
 505     return 0;
 506 }
 507
 508 uint16_t getBatterySagCompensatedAverageCellVoltage(void)
 509 {
 510     if (batteryCellCount > 0) {
 511         return sagCompensatedVBat / batteryCellCount;
 512     }
 513     return 0;
 514 }
 515
 516 uint32_t getBatteryRemainingCapacity(void)
 517 {
 518     return batteryRemainingCapacity;
 519 }
 520
 521 bool isAmperageConfigured(void)
 522 {
 523     return feature(FEATURE_CURRENT_METER) && batteryMetersConfig()->current.type != CURRENT_SENSOR_NONE;
 524 }
 525
 526 int16_t getAmperage(void)
 527 {
 528     return amperage;
 529 }
 530
 531 int16_t getAmperageSample(void)
 532 {
 533     int32_t microvolts = ((uint32_t)adcGetChannel(ADC_CURRENT) * ADCVREF * 100) / 0xFFF * 10 - (int32_t)batteryMetersConfig()->current.offset * 100;
 534     return microvolts / batteryMetersConfig()->current.scale; // current in 0.01A steps
 535 }
 536
 537 int32_t getPower(void)
 538 {
 539     return power;
 540 }
 541
 542 int32_t getMAhDrawn(void)
 543 {
 544     return mAhDrawn;
 545 }
 546
 547 int32_t getMWhDrawn(void)
 548 {
 549     return mWhDrawn;
 550 }
 551
 552 void currentMeterUpdate(timeUs_t timeDelta)
 553 {
 554     static pt1Filter_t amperageFilterState;
 555     static int64_t mAhdrawnRaw = 0;
 556
 557     switch (batteryMetersConfig()->current.type) {
 558         case CURRENT_SENSOR_ADC:
 559             {
 560                 amperage = pt1FilterApply4(&amperageFilterState, getAmperageSample(), AMPERAGE_LPF_FREQ, US2S(timeDelta));
 561                 break;
 562             }
 563         case CURRENT_SENSOR_VIRTUAL:
 564             amperage = batteryMetersConfig()->current.offset;
 565             if (ARMING_FLAG(ARMED)) {
 566                 navigationFSMStateFlags_t stateFlags = navGetCurrentStateFlags();
 567                 bool allNav = navConfig()->general.flags.nav_overrides_motor_stop == NOMS_ALL_NAV && posControl.navState != NAV_STATE_IDLE;
 568                 bool autoNav = navConfig()->general.flags.nav_overrides_motor_stop == NOMS_AUTO_ONLY && (stateFlags & (NAV_AUTO_RTH | NAV_AUTO_WP));
 569                 int32_t throttleOffset;
 570
 571                 if (allNav || autoNav) {    // account for motors running in Nav modes with throttle low + motor stop
 572                     throttleOffset = (int32_t)rcCommand[THROTTLE] - 1000;
 573                 } else {
 574                     throttleOffset = (throttleStickIsLow() && ifMotorstopFeatureEnabled()) ? 0 : (int32_t)rcCommand[THROTTLE] - 1000;
 575                 }
 576                 int32_t throttleFactor = throttleOffset + (throttleOffset * throttleOffset / 50);
 577                 amperage += throttleFactor * batteryMetersConfig()->current.scale / 1000;
 578             }
 579             break;
 580 #if defined(USE_ESC_SENSOR)
 581         case CURRENT_SENSOR_ESC:
 582             {
 583                 escSensorData_t * escSensor = escSensorGetData();
 584                 if (escSensor && escSensor->dataAge <= ESC_DATA_MAX_AGE) {
 585                     amperage = pt1FilterApply4(&amperageFilterState, escSensor->current, AMPERAGE_LPF_FREQ, US2S(timeDelta));
 586                 }
 587                 else {
 588                     amperage = 0;
 589                 }
 590             }
 591             break;
 592 #endif
 593
 594 #if defined(USE_FAKE_BATT_SENSOR)
 595         case CURRENT_SENSOR_FAKE:
 596             amperage = fakeBattSensorGetAmerperage();
 597             break;
 598 #endif
 599         case CURRENT_SENSOR_NONE:
 600         default:
 601             amperage = 0;
 602             break;
 603     }
 604
 605     // Clamp amperage to positive values
 606     amperage = MAX(0, amperage);
 607
 608     // Work around int64 math compiler bug, don't change it unless the bug has been fixed !
 609     // should be: mAhdrawnRaw += (int64_t)amperage * timeDelta / 1000;
 610     mAhdrawnRaw += (int64_t)((int32_t)amperage * timeDelta) / 1000;
 611
 612     mAhDrawn = mAhdrawnRaw / (3600 * 100);
 613 }
 614
 615 void powerMeterUpdate(timeUs_t timeDelta)
 616 {
 617     static int64_t mWhDrawnRaw = 0;
 618     power = (int32_t)amperage * vbat / 100; // power unit is cW (0.01W resolution)
 619     int32_t heatLossesCompensatedPower_mW = (int32_t)amperage * vbat / 10 + sq((int64_t)amperage) * powerSupplyImpedance / 10000;
 620
 621     // Work around int64 math compiler bug, don't change it unless the bug has been fixed !
 622     // should be: mWhDrawnRaw += (int64_t)heatLossesCompensatedPower_mW * timeDelta / 10000;
 623     mWhDrawnRaw += (int64_t)((int64_t)heatLossesCompensatedPower_mW * timeDelta) / 10000;
 624
 625     mWhDrawn = mWhDrawnRaw / (3600 * 100);
 626 }
 627
 628 // calculate true power including heat losses in power supply (battery + wires)
 629 // power is in cW (0.01W)
 630 // batteryWarningVoltage is in cV (0.01V)
 631 int32_t heatLossesCompensatedPower(int32_t power)
 632 {
 633     return power + sq(power * 100 / batteryWarningVoltage) * powerSupplyImpedance / 100000;
 634 }
 635
 636 void sagCompensatedVBatUpdate(timeUs_t currentTime, timeUs_t timeDelta)
 637 {
 638     static timeUs_t recordTimestamp = 0;
 639     static int16_t amperageRecord;
 640     static uint16_t vbatRecord;
 641     static uint8_t impedanceSampleCount = 0;
 642     static pt1Filter_t impedanceFilterState;
 643     static pt1Filter_t sagCompVBatFilterState;
 644     static batteryState_e last_battery_state = BATTERY_NOT_PRESENT;
 645
 646     if ((batteryState != BATTERY_NOT_PRESENT) && (last_battery_state == BATTERY_NOT_PRESENT)) {
 647         pt1FilterReset(&sagCompVBatFilterState, vbat);
 648         pt1FilterReset(&impedanceFilterState, 0);
 649     }
 650
 651     if (batteryState == BATTERY_NOT_PRESENT) {
 652
 653         recordTimestamp = 0;
 654         impedanceSampleCount = 0;
 655         powerSupplyImpedance = 0;
 656         powerSupplyImpedanceIsValid = false;
 657         sagCompensatedVBat = vbat;
 658
 659     } else {
 660
 661         if (cmpTimeUs(currentTime, recordTimestamp) > MS2US(500))
 662             recordTimestamp = 0;
 663
 664         if (!recordTimestamp) {
 665             amperageRecord = amperage;
 666             vbatRecord = vbat;
 667             recordTimestamp = currentTime;
 668         } else if ((amperage - amperageRecord >= 200) && ((int16_t)vbatRecord - vbat >= 4)) {
 669
 670             uint16_t impedanceSample = (int32_t)(vbatRecord - vbat) * 1000 / (amperage - amperageRecord);
 671
 672             if (impedanceSampleCount <= IMPEDANCE_STABLE_SAMPLE_COUNT_THRESH) {
 673                 impedanceSampleCount += 1;
 674             }
 675
 676             if (impedanceFilterState.state) {
 677                 pt1FilterSetTimeConstant(&impedanceFilterState, impedanceSampleCount > IMPEDANCE_STABLE_SAMPLE_COUNT_THRESH ? 1.2 : 0.5);
 678                 pt1FilterApply3(&impedanceFilterState, impedanceSample, US2S(timeDelta));
 679             } else {
 680                 pt1FilterReset(&impedanceFilterState, impedanceSample);
 681             }
 682
 683             if (impedanceSampleCount > IMPEDANCE_STABLE_SAMPLE_COUNT_THRESH) {
 684                 powerSupplyImpedance = lrintf(pt1FilterGetLastOutput(&impedanceFilterState));
 685                 powerSupplyImpedanceIsValid = true;
 686             }
 687
 688         }
 689
 690         uint16_t sagCompensatedVBatSample = MIN(batteryFullVoltage, vbat + (int32_t)powerSupplyImpedance * amperage / 1000);
 691         pt1FilterSetTimeConstant(&sagCompVBatFilterState, sagCompensatedVBatSample < pt1FilterGetLastOutput(&sagCompVBatFilterState) ? 40 : 500);
 692         sagCompensatedVBat = lrintf(pt1FilterApply3(&sagCompVBatFilterState, sagCompensatedVBatSample, US2S(timeDelta)));
 693     }
 694
 695     DEBUG_SET(DEBUG_SAG_COMP_VOLTAGE, 0, powerSupplyImpedance);
 696     DEBUG_SET(DEBUG_SAG_COMP_VOLTAGE, 1, sagCompensatedVBat);
 697
 698     last_battery_state = batteryState;
 699 }
 700
 701 uint8_t calculateBatteryPercentage(void)
 702 {
 703     if (batteryState == BATTERY_NOT_PRESENT)
 704         return 0;
 705
 706     if (batteryFullWhenPluggedIn && isAmperageConfigured() && (currentBatteryProfile->capacity.value > 0) && (currentBatteryProfile->capacity.critical > 0)) {
 707         uint32_t capacityDiffBetweenFullAndEmpty = currentBatteryProfile->capacity.value - currentBatteryProfile->capacity.critical;
 708         return constrain(batteryRemainingCapacity * 100 / capacityDiffBetweenFullAndEmpty, 0, 100);
 709     } else
 710         return constrain((getBatteryVoltage() - batteryCriticalVoltage) * 100L / (batteryFullVoltage - batteryCriticalVoltage), 0, 100);
 711 }
 712
 713 void batteryDisableProfileAutoswitch(void) {
 714     profileAutoswitchDisable = true;
 715 }
 716
 717 bool isPowerSupplyImpedanceValid(void) {
 718     return powerSupplyImpedanceIsValid;
 719 }
 720
 721 uint16_t getPowerSupplyImpedance(void) {
 722     return powerSupplyImpedance;
 723 }
 724
 725 // returns cW (0.01W)
 726 int32_t calculateAveragePower(void) {
 727     return (int64_t)mWhDrawn * 360 / getFlightTime();
 728 }
 729
 730 // returns mWh / meter
 731 int32_t calculateAverageEfficiency(void) {
 732     return getFlyingEnergy() * 100 / getTotalTravelDistance();
 733 }