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