.. _program_listing_file_components_ESC_src_FuelGauge.cpp:

Program Listing for File FuelGauge.cpp
======================================

|exhale_lsh| :ref:`Return to documentation for file <file_components_ESC_src_FuelGauge.cpp>` (``components/ESC/src/FuelGauge.cpp``)

.. |exhale_lsh| unicode:: U+021B0 .. UPWARDS ARROW WITH TIP LEFTWARDS

.. code-block:: cpp

   
   #include "FuelGauge.h"
   
   #include <freertos/FreeRTOS.h>
   #include <freertos/task.h>
   
   #include <esp_err.h>
   #include <esp_log.h>
   #include <esp_system.h>
   
   #include <ErrUtil.h>
   
   #include "BQ27441_Constants.h"
   
   namespace esc {
   
   static const char* TAG = "ESC";
   
   static const i2c_port_t I2C_PORT = I2C_NUM_0;
   static const int        I2C_FREQUENCY = 100000;
   static const gpio_num_t I2C_SDA_PIN = GPIO_NUM_23;
   static const gpio_num_t I2C_SCL_PIN = GPIO_NUM_22;
   
   static const TickType_t I2C_TIMEOUT = 2000;
   
   template <typename T>
   static T constrain(T value, T min, T max)
   {
       if (value < min) { return min; }
       if (value > max) { return max; }
   
       return value;
   }
   
   FuelGauge::FuelGauge()
       : m_conf{},
         m_seal_again(false),
         m_manual_config(false)
   {
       m_conf.mode = I2C_MODE_MASTER;
       m_conf.sda_io_num = I2C_SDA_PIN;
       m_conf.sda_pullup_en = GPIO_PULLUP_DISABLE;
       m_conf.scl_io_num = I2C_SCL_PIN;
       m_conf.scl_pullup_en = GPIO_PULLUP_DISABLE;
       m_conf.master.clk_speed = I2C_FREQUENCY;
   }
   
   esp_err_t FuelGauge::temperature(TempMeasure type, std::uint16_t* temp)
   {
       switch (type)
       {
       case TempMeasure::Battery:
           ESP_ERR_TRY(readWord(TEMP, temp));
           break;
       case TempMeasure::Internal:
           ESP_ERR_TRY(readWord(INT_TEMP, temp));
           break;
       }
   
       return ESP_OK;
   }
   
   esp_err_t FuelGauge::voltage(std::uint16_t* voltage)
   {
       return readWord(VOLTAGE, voltage);
   }
   
   esp_err_t FuelGauge::avgCurrent(std::int16_t* avg_current)
   {
       return readWord(AVG_CURRENT, reinterpret_cast<std::uint16_t*>(avg_current));
   }
   
   esp_err_t FuelGauge::avgPower(std::int16_t* avg_power)
   {
       return readWord(AVG_POWER, reinterpret_cast<std::uint16_t*>(avg_power));
   }
   
   esp_err_t FuelGauge::soc(SocMeasure type, std::uint16_t* soc)
   {
       switch (type) {
       case SocMeasure::Filtered:
           return readWord(SOC, soc);
           break;
       case SocMeasure::Unfiltered:
           return readWord(SOC_UNFL, soc);
           break;
       default:
           return ESP_FAIL;
           break;
       }
   
       return ESP_OK;
   }
   
   esp_err_t FuelGauge::setCapacity(std::uint16_t capacity)
   {
       // Write to STATE subclass (82) of FuelGauge extended memory.
       // Offset 0x0A (10)
       // Design capacity is a 2-byte piece of data - MSB first
       std::uint8_t cap_msb = capacity >> 8;
       std::uint8_t cap_lsb = capacity & 0x00FF;
       std::uint8_t capacity_data[2] = {cap_msb, cap_lsb};
       return writeExtendedData(ID_STATE, 10, capacity_data, 2);
   }
   
   esp_err_t FuelGauge::status(std::uint16_t* result)
   {
       return readControlWord(STATUS, result);
   }
   
   esp_err_t FuelGauge::flags(std::uint16_t* result)
   {
       return readWord(FLAGS, result);
   }
   
   esp_err_t FuelGauge::GPOUTPolarity(bool* value)
   {
       if (value == nullptr) return ESP_FAIL;
   
       std::uint16_t opconfig = 0;
       ESP_ERR_TRY(opConfig(&opconfig));
   
       *value = (opconfig & OPCONFIG_GPIOPOL) == OPCONFIG_GPIOPOL;
   
       return ESP_OK;
   }
   
   esp_err_t FuelGauge::setGPOUTPolarity(bool active_high)
   {
       std::uint16_t old_opconfig = 0;
       ESP_ERR_TRY(opConfig(&old_opconfig));
   
       bool already_high = (active_high && (old_opconfig & OPCONFIG_GPIOPOL));
       bool already_low = (!active_high && !(old_opconfig & OPCONFIG_GPIOPOL));
   
       if (already_high || already_low) return ESP_OK;
   
       uint16_t new_opconfig = old_opconfig;
       if (active_high) {
           new_opconfig |= OPCONFIG_GPIOPOL;
       } else {
           new_opconfig &= ~(OPCONFIG_GPIOPOL);
       }
   
       ESP_ERR_TRY(writeOpConfig(new_opconfig));
   
       return ESP_OK;
   }
   
   esp_err_t FuelGauge::GPOUTFunction(bool* function)
   {
       if (function == nullptr) return ESP_FAIL;
   
       std::uint16_t opconfig = 0;
       ESP_ERR_TRY(opConfig(&opconfig));
   
       *function = ((opconfig & OPCONFIG_BATLOWEN) == OPCONFIG_BATLOWEN);
   
       return ESP_OK;
   }
   
   esp_err_t FuelGauge::setGPOUTFunction(bool function)
   {
       std::uint16_t old_opconfig = 0;
       ESP_ERR_TRY(opConfig(&old_opconfig));
   
       bool is_bat_low = (function && (old_opconfig & OPCONFIG_BATLOWEN));
       bool is_soc_int = (!function && !(old_opconfig & OPCONFIG_BATLOWEN));
       if (is_bat_low || is_soc_int) return ESP_OK;
   
       std::uint16_t new_opconfig = old_opconfig;
       if (function == BAT_LOW) {
           new_opconfig |= OPCONFIG_BATLOWEN;
       } else {
           new_opconfig &= ~(OPCONFIG_BATLOWEN);
       }
   
       ESP_ERR_TRY(writeOpConfig(new_opconfig));
   
       return ESP_OK;
   }
   
   esp_err_t FuelGauge::sociDelta(std::uint8_t* value)
   {
       if (value == nullptr) return ESP_FAIL;
   
       std::uint8_t soci = 0;
       ESP_ERR_TRY(readExtendedData(ID_STATE, 26, &soci));
   
       return ESP_OK;
   }
   
   esp_err_t FuelGauge::setSOCIDelta(std::uint8_t delta)
   {
       std::uint8_t soci = constrain<std::uint8_t>(delta, 0, 100);
       return writeExtendedData(ID_STATE, 26, &soci, 1);
   }
   
   esp_err_t FuelGauge::pulseGPOUT()
   {
       return executeControlWord(PULSE_SOC_INT);
   }
   
   esp_err_t FuelGauge::sealed(bool* is_sealed)
   {
       std::uint16_t stat = 0;
       ESP_ERR_TRY(status(&stat));
       *is_sealed = (stat & STATUS_SS) == STATUS_SS;
   
       return ESP_OK;
   }
   
   esp_err_t FuelGauge::seal(std::uint16_t* result)
   {
       return readControlWord(SEALED, result);
   }
   
   esp_err_t FuelGauge::unseal(std::uint16_t* result)
   {
       // To unseal the FuelGauge, write the key to the control
       // command. Then immediately write the same key to control again.
   
       ESP_ERR_TRY(readControlWord(UNSEAL_KEY, result));
   
       return readControlWord(UNSEAL_KEY, result);
   }
   
   esp_err_t FuelGauge::enterConfig(bool manual_config)
   {
       bool is_sealed = false;
       ESP_ERR_TRY(sealed(&is_sealed));
   
       if (is_sealed) {
           // Must be unsealed before making changes
           std::uint16_t res = 0;
           ESP_ERR_TRY(unseal(&res));
   
           m_seal_again = true;
   
           // Wait 2 s to get values updated.
           vTaskDelay(pdMS_TO_TICKS(2000));
       }
   
       ESP_ERR_TRY(executeControlWord(SET_CFG_UPDATE));
   
       std::int16_t timeout = I2C_TIMEOUT;
       while (timeout != 0) {
           vTaskDelay(1);
   
           std::uint16_t flags_res = 0;
           ESP_ERR_TRY(flags(&flags_res));
   
           if (flags_res & FLAG_CFGUPMODE) {
               // We are now in CFG update mode.
               m_manual_config = manual_config;
               return ESP_OK;
           }
   
           timeout -= 1;
       }
   
       // Timeout reached, no change in mode.
   
       return ESP_FAIL;
   }
   
   esp_err_t FuelGauge::exitConfig(bool resim)
   {
       // There are two methods for exiting config mode:
       //    1. Execute the EXIT_CFGUPDATE command
       //    2. Execute the SOFT_RESET command
       // EXIT_CFGUPDATE exits config mode _without_ an OCV (open-circuit voltage)
       // measurement, and without resimulating to update unfiltered-SoC and SoC.
       // If a new OCV measurement or resimulation is desired, SOFT_RESET or
       // EXIT_RESIM should be used to exit config mode.
       esp_err_t err = ESP_FAIL;
       if (resim) {
           err = softReset();
           if (err != ESP_OK) return err;
   
           std::int16_t timeout = I2C_TIMEOUT;
           while (timeout != 0) {
               vTaskDelay(1);
   
               std::uint16_t flags_res = 0;
               err = flags(&flags_res);
               if (err != ESP_OK) return err;
   
               if (!(flags_res & FLAG_CFGUPMODE)) {
                   if (m_seal_again) {
                       // Seal back up if we IC was sealed coming in
                       std::uint16_t seal_res = 0;
                       err = seal(&seal_res);
                       if (err != ESP_OK) return err;
   
                       // Wait 2 s to get values updated.
                       vTaskDelay(pdMS_TO_TICKS(2000));
                   }
   
                   m_manual_config = false;
                   return ESP_OK;
               }
   
               timeout -= 1;
           }
       } else {
           m_manual_config = false;
           err = executeControlWord(EXIT_CFGUPDATE);
           if (err != ESP_OK) return err;
           return ESP_OK;
       }
   
       return ESP_FAIL;
   }
   
   esp_err_t FuelGauge::opConfig(std::uint16_t* result)
   {
       return readWord(EXTENDED_OPCONFIG, result);
   }
   
   esp_err_t FuelGauge::writeOpConfig(std::uint16_t value)
   {
       std::uint8_t op_config_msb = value >> 8;
       std::uint8_t op_config_lsb = value & 0x00FF;
       std::uint8_t op_config_data[2] = {op_config_msb, op_config_lsb};
   
       // OpConfig register location: ID_REGISTERS id, offset 0
       return writeExtendedData(ID_REGISTERS, 0, op_config_data, 2);
   }
   
   esp_err_t FuelGauge::blockDataControl()
   {
       std::uint8_t enable_byte = 0x00;
       return i2cWriteBytes(EXTENDED_CONTROL, &enable_byte, 1);
   }
   
   esp_err_t FuelGauge::blockDataClass(std::uint8_t id)
   {
       return i2cWriteBytes(EXTENDED_DATACLASS, &id, 1);
   }
   
   esp_err_t FuelGauge::blockDataOffset(std::uint8_t offset)
   {
       return i2cWriteBytes(EXTENDED_DATABLOCK, &offset, 1);
   }
   
   esp_err_t FuelGauge::blockDataChecksum(std::uint8_t* csum)
   {
       if (csum == nullptr) return ESP_FAIL;
   
       std::uint8_t new_csum = 0;
       ESP_ERR_TRY(i2cReadBytes(EXTENDED_CHECKSUM, &new_csum, 1));
       *csum = new_csum;
   
       return ESP_OK;
   }
   
   esp_err_t FuelGauge::readBlockData(std::uint8_t offset, std::uint8_t* result)
   {
       if (result == nullptr) return ESP_FAIL;
   
       std::uint8_t ret = 0;
       std::uint8_t address = EXTENDED_BLOCKDATA + offset;
       ESP_ERR_TRY(i2cReadBytes(address, &ret, 1));
       *result = ret;
   
       return ESP_OK;
   }
   
   esp_err_t FuelGauge::writeBlockData(std::uint8_t offset, std::uint8_t data)
   {
       std::uint8_t address = EXTENDED_BLOCKDATA + offset;
       return i2cWriteBytes(address, &data, 1);
   }
   
   esp_err_t FuelGauge::computeBlockChecksum(std::uint8_t* checksum)
   {
       if (checksum == nullptr) return ESP_FAIL;
   
       std::uint8_t data[32] = {0};
       ESP_ERR_TRY(i2cReadBytes(EXTENDED_BLOCKDATA, data, 32));
   
       std::uint8_t ret = 0;
       for (int i = 0; i < 32; i++) {
           ret += data[i];
       }
       *checksum = 255 - ret;
   
       return ESP_OK;
   }
   
   esp_err_t FuelGauge::writeBlockChecksum(std::uint8_t csum)
   {
       return i2cWriteBytes(EXTENDED_CHECKSUM, &csum, 1);
   }
   
   esp_err_t FuelGauge::readExtendedData(std::uint8_t class_id,
                                         std::uint8_t offset,
                                         std::uint8_t* result)
   {
       if (!m_manual_config) ESP_ERR_TRY(enterConfig(false));
   
       // Enable block data memory control
       ESP_ERR_TRY(blockDataControl());
       // Write class ID using DataBlockClass()
       ESP_ERR_TRY(blockDataClass(class_id));
   
       // Write 32-bit block offset (usually 0)
       ESP_ERR_TRY(blockDataOffset(offset / 32));
   
       std::uint8_t new_csum = 0;
       // Compute checksum going in
       ESP_ERR_TRY(computeBlockChecksum(&new_csum));
   
       std::uint8_t old_csum = 0;
       ESP_ERR_TRY(blockDataChecksum(&old_csum));
   
       // Read from offset (limit to 0-31)
       ESP_ERR_TRY(readBlockData(offset % 32, result));
   
       if (!m_manual_config) ESP_ERR_TRY(exitConfig(false));
   
       return ESP_OK;
   }
   
   esp_err_t FuelGauge::writeExtendedData(std::uint8_t class_id,
                                          std::uint8_t offset,
                                          std::uint8_t* data,
                                          std::uint8_t len)
   {
       if (len == 0) return ESP_FAIL;
       if (len > 32) {
           ESP_LOGE(TAG, "max write length is 32");
           return ESP_FAIL;
       }
       if (data == nullptr) return ESP_FAIL;
   
       if (!m_manual_config) ESP_ERR_TRY(enterConfig(false));
   
       // Enable block data memory control
       ESP_ERR_TRY(blockDataControl());
   
       // Write class ID using DataBlockClass()
       ESP_ERR_TRY(blockDataClass(class_id));
   
       // Write 32-bit block offset (usually 0)
       ESP_ERR_TRY(blockDataOffset(offset / 32));
   
       vTaskDelay(pdMS_TO_TICKS(2000));
   
       // Compute checksum going in
       std::uint8_t computed_csum = 0;
       ESP_ERR_TRY(computeBlockChecksum(&computed_csum));
   
       std::uint8_t old_csum = 0;
       ESP_ERR_TRY(blockDataChecksum(&old_csum));
   
       // Write data bytes:
       for (int i = 0; i < len; i++) {
           // Write to offset, mod 32 if offset is greater than 32
           // The blockDataOffset above sets the 32-bit block
           ESP_ERR_TRY(writeBlockData((offset % 32) + i, data[i]));
       }
   
       vTaskDelay(pdMS_TO_TICKS(2000));
   
       // Write new checksum using BlockDataChecksum (0x60)
       std::uint8_t new_csum = 0;
       ESP_ERR_TRY(computeBlockChecksum(&new_csum));
   
       ESP_ERR_TRY(writeBlockChecksum(new_csum));
   
       vTaskDelay(pdMS_TO_TICKS(2000));
   
       if (!m_manual_config) ESP_ERR_TRY(exitConfig(false));
   
       return ESP_OK;
   }
   
   esp_err_t FuelGauge::softReset()
   {
       return executeControlWord(SOFT_RESET);
   }
   
   esp_err_t FuelGauge::readControlWord(std::uint16_t function,
                                        std::uint16_t* result)
   {
   
       if (result == nullptr) return ESP_FAIL;
   
       std::uint8_t sub_command_lsb = static_cast<std::uint8_t>(function & 0x00FF);
       std::uint8_t sub_command_msb = static_cast<std::uint8_t>(function >> 8);
       std::uint8_t command[2] = {sub_command_lsb, sub_command_msb};
       ESP_ERR_TRY(i2cWriteBytes(0, command, 2));
       std::uint8_t data[2] = {0};
       ESP_ERR_TRY(i2cReadBytes(0, data, 2));
   
       std::uint16_t d0 = static_cast<std::uint16_t>(data[1]) << 8;
       std::uint16_t d1 = static_cast<std::uint16_t>(data[0]);
       *result = (d1 | d0);
   
       return ESP_OK;
   }
   
   esp_err_t FuelGauge::executeControlWord(std::uint16_t function)
   {
       std::uint8_t sub_command_msb = (function >> 8);
       std::uint8_t sub_command_lsb = (function & 0x00FF);
       std::uint8_t command[2] = {sub_command_lsb, sub_command_msb};
   
       ESP_ERR_TRY(i2cWriteBytes(0, command, 2));
   
       return ESP_OK;
   }
   
   esp_err_t FuelGauge::readWord(std::uint8_t command, std::uint16_t* word)
   {
   
       if (word == nullptr) return ESP_FAIL;
   
       std::uint8_t data[2] = {0};
       ESP_ERR_TRY(i2cReadBytes(command, data, 2));
   
       std::uint16_t d0 = static_cast<std::uint16_t>(data[1]) << 8;
       std::uint16_t d1 = static_cast<std::uint16_t>(data[0]);
       *word = (d1 | d0);
   
       return ESP_OK;
   }
   
   esp_err_t FuelGauge::i2cWriteBytes(std::uint8_t sub_address,
                                      std::uint8_t* bytes,
                                      std::size_t count)
   {
       if (bytes == nullptr) return ESP_FAIL;
       if (count == 0) return ESP_FAIL;
   
       ESP_ERR_TRY(i2cInit());
   
       ESP_ERR_TRY(i2c_set_timeout(I2C_PORT, 14000));
   
       i2c_cmd_handle_t cmd = i2c_cmd_link_create();
   
       i2c_master_start(cmd);
   
       i2c_master_write_byte(cmd,
                             (FUELGAUGE_ADDRESS << 1) | I2C_MASTER_WRITE,
                             I2C_MASTER_ACK);
       i2c_master_write_byte(cmd, sub_address, I2C_MASTER_ACK);
   
       i2c_master_write(cmd, bytes, count, I2C_MASTER_ACK);
   
       i2c_master_stop(cmd);
   
       ESP_ERR_TRY(i2c_master_cmd_begin(I2C_PORT, cmd, I2C_TIMEOUT));
   
       i2c_cmd_link_delete(cmd);
   
       ESP_ERR_TRY(i2cDelete());
   
       return ESP_OK;
   }
   
   esp_err_t FuelGauge::i2cReadBytes(std::uint8_t sub_address,
                                     std::uint8_t* bytes,
                                     std::size_t count)
   {
       if (bytes == nullptr) return ESP_FAIL;
       if (count == 0) return ESP_FAIL;
   
       ESP_ERR_TRY(i2cInit());
   
       ESP_ERR_TRY(i2c_set_timeout(I2C_PORT, 14000));
   
       i2c_cmd_handle_t cmd = i2c_cmd_link_create();
   
       i2c_master_start(cmd);
       i2c_master_write_byte(cmd,
                             (FUELGAUGE_ADDRESS << 1) | I2C_MASTER_WRITE,
                             I2C_MASTER_ACK);
       i2c_master_write_byte(cmd, sub_address, I2C_MASTER_ACK);
   
       i2c_master_start(cmd);
       i2c_master_write_byte(cmd,
                             (FUELGAUGE_ADDRESS << 1) | I2C_MASTER_READ,
                             I2C_MASTER_ACK);
       i2c_master_read(cmd, bytes, count, I2C_MASTER_LAST_NACK);
       i2c_master_stop(cmd);
   
       ESP_ERR_TRY(i2c_master_cmd_begin(I2C_PORT, cmd, I2C_TIMEOUT));
   
       i2c_cmd_link_delete(cmd);
   
       ESP_ERR_TRY(i2cDelete());
   
       return ESP_OK;
   }
   
   esp_err_t FuelGauge::i2cInit()
   {
       ESP_ERR_TRY(i2c_param_config(I2C_PORT, &m_conf));
       ESP_ERR_TRY(i2c_driver_install(I2C_PORT, m_conf.mode, 0, 0, 0));
   
       return ESP_OK;
   }
   
   esp_err_t FuelGauge::i2cDelete()
   {
       ESP_ERR_TRY(i2c_driver_delete(I2C_PORT));
   
       return ESP_OK;
   }
   
   } // namespace esc