src/lib/AnalogVbat/devAnalogVbat.cpp

   1 #include "devAnalogVbat.h"
   2
   3 #include <Arduino.h>
   4 #include "CRSF.h"
   5 #include "telemetry.h"
   6 #include "median.h"
   7 #include "logging.h"
   8
   9 // Sample 5x samples over 500ms (unless SlowUpdate)
  10 #define VBAT_SMOOTH_CNT         5
  11 #if defined(DEBUG_VBAT_ADC)
  12 #define VBAT_SAMPLE_INTERVAL    20U // faster updates in debug mode
  13 #else
  14 #define VBAT_SAMPLE_INTERVAL    100U
  15 #endif
  16
  17 typedef uint16_t vbatAnalogStorage_t;
  18 static MedianAvgFilter<vbatAnalogStorage_t, VBAT_SMOOTH_CNT>vbatSmooth;
  19 static uint8_t vbatUpdateScale;
  20
  21 #if defined(PLATFORM_ESP32)
  22 #include "esp_adc_cal.h"
  23 static esp_adc_cal_characteristics_t *vbatAdcUnitCharacterics;
  24 #endif
  25
  26 /* Shameful externs */
  27 extern Telemetry telemetry;
  28
  29 /**
  30  * @brief: Enable SlowUpdate mode to reduce the frequency Vbat telemetry is sent
  31  ***/
  32 void Vbat_enableSlowUpdate(bool enable)
  33 {
  34     vbatUpdateScale = enable ? 2 : 1;
  35 }
  36
  37 static bool initialize()
  38 {
  39     return GPIO_ANALOG_VBAT != UNDEF_PIN;
  40 }
  41
  42 static int start()
  43 {
  44     vbatUpdateScale = 1;
  45 #if defined(PLATFORM_ESP32)
  46     analogReadResolution(12);
  47
  48     int atten = hardware_int(HARDWARE_vbat_atten);
  49     if (atten != -1)
  50     {
  51         // if the configured value is higher than the max item (11dB, it indicates to use cal_characterize)
  52         bool useCal = atten > ADC_11db;
  53         if (useCal)
  54         {
  55             atten -= (ADC_11db + 1);
  56
  57             vbatAdcUnitCharacterics = new esp_adc_cal_characteristics_t();
  58             int8_t channel = digitalPinToAnalogChannel(GPIO_ANALOG_VBAT);
  59             adc_unit_t unit = (channel > (SOC_ADC_MAX_CHANNEL_NUM - 1)) ? ADC_UNIT_2 : ADC_UNIT_1;
  60             esp_adc_cal_characterize(unit, (adc_atten_t)atten, ADC_WIDTH_BIT_12, 3300, vbatAdcUnitCharacterics);
  61         }
  62         analogSetPinAttenuation(GPIO_ANALOG_VBAT, (adc_attenuation_t)atten);
  63     }
  64 #endif
  65
  66     return VBAT_SAMPLE_INTERVAL;
  67 }
  68
  69 static void reportVbat()
  70 {
  71     uint32_t adc = vbatSmooth.calc();
  72 #if defined(PLATFORM_ESP32) && !defined(DEBUG_VBAT_ADC)
  73     if (vbatAdcUnitCharacterics)
  74         adc = esp_adc_cal_raw_to_voltage(adc, vbatAdcUnitCharacterics);
  75 #endif
  76
  77     int32_t vbat;
  78     // For negative offsets, anything between abs(OFFSET) and 0 is considered 0
  79     if (ANALOG_VBAT_OFFSET < 0 && adc <= -ANALOG_VBAT_OFFSET)
  80         vbat = 0;
  81     else
  82         vbat = ((int32_t)adc - ANALOG_VBAT_OFFSET) * 100 / ANALOG_VBAT_SCALE;
  83
  84     CRSF_MK_FRAME_T(crsf_sensor_battery_t) crsfbatt = { 0 };
  85     // Values are MSB first (BigEndian)
  86     crsfbatt.p.voltage = htobe16((uint16_t)vbat);
  87     // No sensors for current, capacity, or remaining available
  88
  89     CRSF::SetHeaderAndCrc((uint8_t *)&crsfbatt, CRSF_FRAMETYPE_BATTERY_SENSOR, CRSF_FRAME_SIZE(sizeof(crsf_sensor_battery_t)), CRSF_ADDRESS_CRSF_TRANSMITTER);
  90     telemetry.AppendTelemetryPackage((uint8_t *)&crsfbatt);
  91 }
  92
  93 static int timeout()
  94 {
  95     if (telemetry.GetCrsfBatterySensorDetected())
  96     {
  97         return DURATION_NEVER;
  98     }
  99
 100     uint32_t adc = analogRead(GPIO_ANALOG_VBAT);
 101 #if defined(PLATFORM_ESP32) && defined(DEBUG_VBAT_ADC)
 102     // When doing DEBUG_VBAT_ADC, every value is adjusted (for logging)
 103     // in normal mode only the final value is adjusted to save CPU cycles
 104     if (vbatAdcUnitCharacterics)
 105         adc = esp_adc_cal_raw_to_voltage(adc, vbatAdcUnitCharacterics);
 106     DBGLN("$ADC,%u", adc);
 107 #endif
 108
 109     unsigned int idx = vbatSmooth.add(adc);
 110     if (idx == 0 && connectionState == connected)
 111         reportVbat();
 112
 113     return VBAT_SAMPLE_INTERVAL * vbatUpdateScale;
 114 }
 115
 116 device_t AnalogVbat_device = {
 117     .initialize = initialize,
 118     .start = start,
 119     .event = nullptr,
 120     .timeout = timeout,
 121     .subscribe = EVENT_NONE
 122 };