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 "stdbool.h"
  19 #include "stdint.h"
  20
  21 #include "platform.h"
  22
  23 #include "common/maths.h"
  24
  25 #include "drivers/adc.h"
  26 #include "drivers/system.h"
  27
  28 #include "config/runtime_config.h"
  29 #include "config/config.h"
  30
  31 #include "sensors/battery.h"
  32
  33 #include "rx/rx.h"
  34
  35 #include "io/rc_controls.h"
  36 #include "flight/lowpass.h"
  37 #include "io/beeper.h"
  38
  39 #define VBATT_PRESENT_THRESHOLD_MV    10
  40 #define VBATT_LPF_FREQ  10
  41
  42 // Battery monitoring stuff
  43 uint8_t batteryCellCount = 3;       // cell count
  44 uint16_t batteryWarningVoltage;
  45 uint16_t batteryCriticalVoltage;
  46
  47 uint16_t vbat = 0;                   // battery voltage in 0.1V steps (filtered)
  48 uint16_t vbatLatestADC = 0;         // most recent unsmoothed raw reading from vbat ADC
  49 uint16_t amperageLatestADC = 0;     // most recent raw reading from current ADC
  50
  51 int32_t amperage = 0;               // amperage read by current sensor in centiampere (1/100th A)
  52 int32_t mAhDrawn = 0;               // milliampere hours drawn from the battery since start
  53
  54 batteryConfig_t *batteryConfig;
  55
  56 static batteryState_e batteryState;
  57 static lowpass_t lowpassFilter;
  58
  59 uint16_t batteryAdcToVoltage(uint16_t src)
  60 {
  61     // calculate battery voltage based on ADC reading
  62     // result is Vbatt in 0.1V steps. 3.3V = ADC Vref, 0xFFF = 12bit adc, 110 = 11:1 voltage divider (10k:1k) * 10 for 0.1V
  63     return ((((uint32_t)src * batteryConfig->vbatscale * 33 + (0xFFF * 5)) / (0xFFF * batteryConfig->vbatresdivval))/batteryConfig->vbatresdivmultiplier);
  64 }
  65
  66 static void updateBatteryVoltage(void)
  67 {
  68     uint16_t vbatSample;
  69     uint16_t vbatFiltered;
  70
  71     // store the battery voltage with some other recent battery voltage readings
  72     vbatSample = vbatLatestADC = adcGetChannel(ADC_BATTERY);
  73     vbatFiltered = (uint16_t)lowpassFixed(&lowpassFilter, vbatSample, VBATT_LPF_FREQ);
  74     vbat = batteryAdcToVoltage(vbatFiltered);
  75 }
  76
  77 #define VBATTERY_STABLE_DELAY 40
  78 /* Batt Hysteresis of +/-100mV */
  79 #define VBATT_HYSTERESIS 1
  80
  81 void updateBattery(void)
  82 {
  83     updateBatteryVoltage();
  84
  85     /* battery has just been connected*/
  86     if (batteryState == BATTERY_NOT_PRESENT && vbat > VBATT_PRESENT_THRESHOLD_MV)
  87     {
  88         /* Actual battery state is calculated below, this is really BATTERY_PRESENT */
  89         batteryState = BATTERY_OK;
  90         /* wait for VBatt to stabilise then we can calc number of cells
  91         (using the filtered value takes a long time to ramp up)
  92         We only do this on the ground so don't care if we do block, not
  93         worse than original code anyway*/
  94         delay(VBATTERY_STABLE_DELAY);
  95         updateBatteryVoltage();
  96
  97         unsigned cells = (batteryAdcToVoltage(vbatLatestADC) / batteryConfig->vbatmaxcellvoltage) + 1;
  98         if (cells > 8) {
  99             // something is wrong, we expect 8 cells maximum (and autodetection will be problematic at 6+ cells)
 100             cells = 8;
 101         }
 102         batteryCellCount = cells;
 103         batteryWarningVoltage = batteryCellCount * batteryConfig->vbatwarningcellvoltage;
 104         batteryCriticalVoltage = batteryCellCount * batteryConfig->vbatmincellvoltage;
 105     }
 106     /* battery has been disconnected - can take a while for filter cap to disharge so we use a threshold of VBATT_PRESENT_THRESHOLD_MV */
 107     else if (batteryState != BATTERY_NOT_PRESENT && vbat <= VBATT_PRESENT_THRESHOLD_MV)
 108     {
 109         batteryState = BATTERY_NOT_PRESENT;
 110         batteryCellCount = 0;
 111         batteryWarningVoltage = 0;
 112         batteryCriticalVoltage = 0;
 113     }
 114
 115     switch(batteryState)
 116     {
 117         case BATTERY_OK:
 118             if (vbat <= (batteryWarningVoltage - VBATT_HYSTERESIS)) {
 119                 batteryState = BATTERY_WARNING;
 120                 beeper(BEEPER_BAT_LOW);
 121             }
 122             break;
 123         case BATTERY_WARNING:
 124             if (vbat <= (batteryCriticalVoltage - VBATT_HYSTERESIS)) {
 125                 batteryState = BATTERY_CRITICAL;
 126                 beeper(BEEPER_BAT_CRIT_LOW);
 127             } else if (vbat > (batteryWarningVoltage + VBATT_HYSTERESIS)){
 128                 batteryState = BATTERY_OK;
 129             } else {
 130                 beeper(BEEPER_BAT_LOW);
 131             }
 132             break;
 133         case BATTERY_CRITICAL:
 134             if (vbat > (batteryCriticalVoltage + VBATT_HYSTERESIS)){
 135                 batteryState = BATTERY_WARNING;
 136                 beeper(BEEPER_BAT_LOW);
 137             } else {
 138                 beeper(BEEPER_BAT_CRIT_LOW);
 139             }
 140             break;
 141         case BATTERY_NOT_PRESENT:
 142             break;
 143     }
 144 }
 145
 146 batteryState_e getBatteryState(void)
 147 {
 148     return batteryState;
 149 }
 150
 151 const char * const batteryStateStrings[] = {"OK", "WARNING", "CRITICAL", "NOT PRESENT"};
 152
 153 const char * getBatteryStateString(void)
 154 {
 155     return batteryStateStrings[batteryState];
 156 }
 157
 158 void batteryInit(batteryConfig_t *initialBatteryConfig)
 159 {
 160     batteryConfig = initialBatteryConfig;
 161     batteryState = BATTERY_NOT_PRESENT;
 162     batteryCellCount = 1;
 163     batteryWarningVoltage = 0;
 164     batteryCriticalVoltage = 0;
 165 }
 166
 167 #define ADCVREF 3300   // in mV
 168 int32_t currentSensorToCentiamps(uint16_t src)
 169 {
 170     int32_t millivolts;
 171
 172     millivolts = ((uint32_t)src * ADCVREF) / 4096;
 173     millivolts -= batteryConfig->currentMeterOffset;
 174
 175     return (millivolts * 1000) / (int32_t)batteryConfig->currentMeterScale; // current in 0.01A steps
 176 }
 177
 178 void updateCurrentMeter(int32_t lastUpdateAt, rxConfig_t *rxConfig, uint16_t deadband3d_throttle)
 179 {
 180     static int32_t amperageRaw = 0;
 181     static int64_t mAhdrawnRaw = 0;
 182     int32_t throttleFactor = 0;
 183     int32_t throttleOffset = (int32_t)rcCommand[THROTTLE] - 1000;
 184
 185     switch(batteryConfig->currentMeterType) {
 186         case CURRENT_SENSOR_ADC:
 187             amperageRaw -= amperageRaw / 8;
 188             amperageRaw += (amperageLatestADC = adcGetChannel(ADC_CURRENT));
 189             amperage = currentSensorToCentiamps(amperageRaw / 8);
 190             break;
 191         case CURRENT_SENSOR_VIRTUAL:
 192             amperage = (int32_t)batteryConfig->currentMeterOffset;
 193             if (ARMING_FLAG(ARMED)) {
 194                 throttleStatus_e throttleStatus = calculateThrottleStatus(rxConfig, deadband3d_throttle);
 195                 if (throttleStatus == THROTTLE_LOW && feature(FEATURE_MOTOR_STOP))
 196                     throttleOffset = 0;
 197                 throttleFactor = throttleOffset + (throttleOffset * throttleOffset / 50);
 198                 amperage += throttleFactor * (int32_t)batteryConfig->currentMeterScale  / 1000;
 199             }
 200             break;
 201         case CURRENT_SENSOR_NONE:
 202             amperage = 0;
 203             break;
 204     }
 205
 206     mAhdrawnRaw += (amperage * lastUpdateAt) / 1000;
 207     mAhDrawn = mAhdrawnRaw / (3600 * 100);
 208 }
 209
 210 uint8_t calculateBatteryPercentage(void)
 211 {
 212     return (((uint32_t)vbat - (batteryConfig->vbatmincellvoltage * batteryCellCount)) * 100) / ((batteryConfig->vbatmaxcellvoltage - batteryConfig->vbatmincellvoltage) * batteryCellCount);
 213 }
 214
 215 uint8_t calculateBatteryCapacityRemainingPercentage(void)
 216 {
 217     uint16_t batteryCapacity = batteryConfig->batteryCapacity;
 218
 219     return constrain((batteryCapacity - constrain(mAhDrawn, 0, 0xFFFF)) * 100.0f / batteryCapacity , 0, 100);
 220 }