gauge/ti/GaugeBQ27220.hpp

Go to the documentation of this file.

 
#pragma once
 
#include "../../SensorBuildOpt.h"
#if !SENSORLIB_EXCLUDE_GAUGE_BQ27220
 
#include "platform/comm/I2CDeviceWithHal.hpp"
#include "../GaugeBase.hpp"
 
// BQ27220 Unique device address
static constexpr uint8_t BQ27220_SLAVE_ADDRESS  = (0x55);
 
// *INDENT-OFF*
class FuelGaugeOperationStatus {
private:
    bool isConfigUpdateMode;
    bool isBtpThresholdExceeded;
    bool isCapacityAccumulationThrottled;
    bool isInitializationComplete;
    bool isDischargeCycleCompliant;
    bool isBatteryVoltageBelowEdv2;
    uint8_t securityAccessLevel;
    bool isCalibrationModeEnabled;
public:
    FuelGaugeOperationStatus(uint16_t value) {
        // parse CFGUPDATE bit
        isConfigUpdateMode = (value & 0x0400) != 0;
        // parse BTPINT bit
        isBtpThresholdExceeded = (value & 0x80) != 0;
        // parse SMTH bit
        isCapacityAccumulationThrottled = (value & 0x40) != 0;
        // parse INITCOMP bit
        isInitializationComplete = (value & 0x20) != 0;
        // parse VDQ bit
        isDischargeCycleCompliant = (value & 0x10) != 0;
        // parse EDV2 bit
        isBatteryVoltageBelowEdv2 = (value & 0x08) != 0;
        // parse SEC bit
        securityAccessLevel = (value >> 1) & 0x03;
        // parse CALMD bit
        isCalibrationModeEnabled = (value & 0x01) != 0;
    }
 
    // Gets whether the fuel gauge is in configuration update mode
    bool getIsConfigUpdateMode() const {
        return isConfigUpdateMode;
    }
 
    // Get whether the BTP threshold has been exceeded
    bool getIsBtpThresholdExceeded() const {
        return isBtpThresholdExceeded;
    }
 
    // Gets whether the remaining capacity accumulation is throttled
    bool getIsCapacityAccumulationThrottled() const {
        return isCapacityAccumulationThrottled;
    }
 
    // Get whether the fuel gauge initialization is completed
    bool getIsInitializationComplete() const {
        return isInitializationComplete;
    }
 
    // Get whether the current discharge cycle complies with FCC update requirements
    bool getIsDischargeCycleCompliant() const {
        return isDischargeCycleCompliant;
    }
 
    // Get whether the battery voltage is lower than the EDV2 threshold
    bool getIsBatteryVoltageBelowEdv2() const {
        return isBatteryVoltageBelowEdv2;
    }
 
    // Get the current security access level
    uint8_t getSecurityAccessLevel() const {
        return securityAccessLevel;
    }
 
    // Gets whether calibration mode is enabled
    bool getIsCalibrationModeEnabled() const {
        return isCalibrationModeEnabled;
    }
};
 
class BatteryStatus {
protected:
    struct {
        //* Full discharge detected. This flag is set and cleared based on the SOC
        //* Flag Config B option selected.
        bool fullDischargeDetected;
        //* OCV measurement update is complete. True when set
        bool ocvMeasurementUpdateComplete;
        //* Status bit indicating that an OCV read failed due to current.
        //* This bit can only be set if a battery is present after receiving an OCV_CMD(). True when set
        bool ocvReadFailedDueToCurrent;
        //* The device operates in SLEEP mode when set.
        //* This bit will be temporarily cleared during AD measurements in SLEEP mode.
        bool inSleepMode;
        //* Over-temperature is detected during charging. If Operation Config B [INT_OT] bit = 1,
        //* the SOC_INT pin toggles once when the [OTC] bit is set.
        bool overTemperatureDuringCharging;
        //* Over-temperature detected during discharge condition. True when set. If Operation Config B [INT_OT] bit = 1,
        //* the SOC_INT pin toggles once when the [OTD] bit is set.
        bool overTemperatureDuringDischarge;
        //* Full charge detected. This flag is set and cleared based on the SOC Flag Config A and SOC Flag Config B options selected.
        bool fullChargeDetected;
        //* Charge Inhibit: If set, indicates that charging should not begin because the Temperature() is outside the range
        //* [Charge Inhibit Temp Low, Charge Inhibit Temp High]. True when set
        bool chargeInhibited;
        //* Termination of charging alarm. This flag is set and cleared based on the selected SOC Flag Config A option.
        bool chargingTerminationAlarm;
        //* A good OCV measurement was made. True when set
        bool goodOcvMeasurement;
        //* Detects inserted battery. True when set.
        bool batteryInserted;
        //* Battery presence detected. True when set.
        bool batteryPresent;
        //* Termination discharge alarm. This flag is set and cleared according to the selected SOC Flag Config A option.
        bool dischargeTerminationAlarm;
        //* System shutdown bit indicating that the system should be shut down. True when set. If set, the SOC_INT pin toggles once.
        bool systemShutdownRequired;
        //* When set, the device is in DISCHARGE mode; when cleared, the device is in CHARGING or RELAXATION mode.
        bool inDischargeMode;
    } status;
public:
    BatteryStatus(uint16_t bitmaps){
        uint8_t hsb = (bitmaps >> 8) & 0xFF;
        uint8_t lsb = bitmaps & 0xFF;
        status.fullDischargeDetected = (hsb & sensorlib::_bv(7)) != 0;
        status.ocvMeasurementUpdateComplete = (hsb & sensorlib::_bv(6)) != 0;
        status.ocvReadFailedDueToCurrent = (hsb & sensorlib::_bv(5)) != 0;
        status.inSleepMode = (hsb & sensorlib::_bv(4)) != 0;
        status.overTemperatureDuringCharging = (hsb & sensorlib::_bv(3)) != 0;
        status.overTemperatureDuringDischarge = (hsb & sensorlib::_bv(2)) != 0;
        status.fullChargeDetected = (hsb & sensorlib::_bv(1)) != 0;
        status.chargeInhibited = (hsb & sensorlib::_bv(0)) != 0;
 
        // status.RSVD = (lsb & (1 << 7))!= 0;
        status.chargingTerminationAlarm = (lsb & sensorlib::_bv(6)) != 0;
        status.goodOcvMeasurement = (lsb & sensorlib::_bv(5)) != 0;
        status.batteryInserted = (lsb & sensorlib::_bv(4)) != 0;
        status.batteryPresent = (lsb & sensorlib::_bv(3)) != 0;
        status.dischargeTerminationAlarm = (lsb & sensorlib::_bv(2)) != 0;
        status.systemShutdownRequired = (lsb & sensorlib::_bv(1)) != 0;
        status.inDischargeMode = (lsb & sensorlib::_bv(0)) != 0;
    };
 
    bool isFullDischargeDetected() const { 
        return status.fullDischargeDetected; 
    }
 
    bool isOcvMeasurementUpdateComplete() const {
         return status.ocvMeasurementUpdateComplete; 
    }
 
    bool isOcvReadFailedDueToCurrent() const {
         return status.ocvReadFailedDueToCurrent; 
    }
 
    bool isInSleepMode() const {
         return status.inSleepMode; 
    }
 
    bool isOverTemperatureDuringCharging() const {
         return status.overTemperatureDuringCharging; 
    }
 
    bool isOverTemperatureDuringDischarge() const {
         return status.overTemperatureDuringDischarge; 
    }
 
    bool isFullChargeDetected() const {
         return status.fullChargeDetected; 
    }
 
    bool isChargeInhibited() const {
         return status.chargeInhibited; 
    }
 
    bool isChargingTerminationAlarm() const {
         return status.chargingTerminationAlarm; 
    }
 
    bool isGoodOcvMeasurement() const {
         return status.goodOcvMeasurement; 
    }
 
    bool isBatteryInserted() const {
         return status.batteryInserted; 
    }
 
    bool isBatteryPresent() const {
         return status.batteryPresent; 
    }
 
    bool isDischargeTerminationAlarm() const {
         return status.dischargeTerminationAlarm; 
    }
 
    bool isSystemShutdownRequired() const {
         return status.systemShutdownRequired; 
    }
 
    bool isInDischargeMode() const {
         return status.inDischargeMode; 
    }
};
 
class OperationConfig {
private:
    //* OperationConfig *//
    uint16_t rawA;
    // High - byte bits
    bool temps;       // When set to 1, select an external thermistor for Temperature().
    bool reserved1;   // Reserved. Do not use.
    bool batg_pol;    // BATT_GD pin polarity. Low level is 0, high level is 1.
    bool batg_en;     // Enable BATT_GD function.
    bool reserved2;   // Reserved.
    bool sleep;       // If the working conditions allow, the fuel gauge can enter the SLEEP state. Set to true when set.
    bool slpwakechg;  // When Current > Sleep Current but not enough to trigger an event, accumulate the estimated charge from the sleep - wake state. Set to true when enabled.
    bool wrtemp;      // Write temperature. The temperature should be written by the host and used for fuel gauge monitoring. When not using an external thermistor and not using the internal temperature sensor, set it to false.
 
    // Low - byte bits
    bool bienable;    // When enabled, the fuel gauge can detect battery insertion. If disabled, the fuel gauge will rely on the host to use the BAT_INSERT or BAT_REMOVE command to set and clear the BatteryStatus(BATTPRES) bit. Set to true when set.
    bool reserved3;   // Reserved. Do not use.
    bool bl_pup_en;   // Battery insertion pin pull - up enable.
    int pfc_cfg;      // Pin Function Code (PFC) mode selection: PFC 0, 1, 2, or 3, selected by 00, 01, 10, or 11 respectively.
    bool wake_en;     // Wake - up enable.
    int wk_th1;       // Wake - up threshold 1.
    int wk_th0;       // Wake - up threshold 0.
 
    //* OperationConfigB *//
    uint16_t rawB;
    // High - byte bits
    // Reserved.
    // Reserved.
    // Reserved.
    // Reserved.
    bool defaultSeal; // Sealed during POR. 0 = No seal after POR (default setting), 1 = Seal after POR.
    bool nonRemovable; // Non - removable.
    // Reserved.
    // Reserved.
 
    // Low - byte bits
    bool intBrem;    // When the battery is removed and [BIEnable] = 1, GPIOUT pulses for 1ms. Enable when set.
    bool intBatL;    // Enable GPIOUT pin toggling when TDA is set.
    bool intState;   // Enable SOC_INT function to pulse the GPIOUT pin when the current direction changes.
    bool intOcv;     // Enable SOC_INT function to generate pulses based on the OCV command.
    // Reserved.
    bool intOt;      // Enable SOC_INT function to generate pulses based on over - temperature conditions and combined with BatteryStatus()[OTC or OTD].
    bool intPol;     // GPIOUT pin polarity control. Low level is 0, high level is 1.
    bool intFocv;    // If this bit is set, GPIOUT will pulse during the first measurement.
 
public:
    OperationConfig(uint32_t registerValue) {
 
        //* OperationConfig A *//
        rawA = (registerValue >> 16);
        // Parse high - byte bits
        temps = (rawA >> 7) & 1;
        reserved1 = (rawA >> 6) & 1;
        batg_pol = (rawA >> 5) & 1;
        batg_en = (rawA >> 4) & 1;
        reserved2 = (rawA >> 3) & 1;
        sleep = (rawA >> 2) & 1;
        slpwakechg = (rawA >> 1) & 1;
        wrtemp = rawA & 1;
 
        // Parse low - byte bits
        bienable = (rawA >> 15) & 1;
        reserved3 = (rawA >> 14) & 1;
        bl_pup_en = (rawA >> 13) & 1;
        pfc_cfg = (rawA >> 11) & 3;
        wake_en = (rawA >> 10) & 1;
        wk_th1 = (rawA >> 9) & 1;
        wk_th0 = (rawA >> 8) & 1;
 
        //* OperationConfig B *//
        rawB = (registerValue & 0xFFFF);
 
        // Parse high - byte bits
        defaultSeal = (rawB >> 3) & 1;
        nonRemovable = (rawB >> 2) & 1;
 
        // Parse low - byte bits
        intBrem = (rawB >> 15) & 1;
        intBatL = (rawB >> 14) & 1;
        intState = (rawB >> 13) & 1;
        intOcv = (rawB >> 12) & 1;
        intOt = (rawB >> 10) & 1;
        intPol = (rawB >> 9) & 1;
        intFocv = (rawB >> 8) & 1;
    }
 
    uint16_t getConfigA() const {
        return rawA;
    }
 
    uint16_t getConfigB() const {
        return rawB;
    }
 
    bool isTempsSet() const {
        return temps;
    }
 
    bool isBatgPolHigh() const {
        return batg_pol;
    }
 
    bool isBatgEnEnabled() const {
        return batg_en;
    }
 
    bool canEnterSleep() const {
        return sleep;
    }
 
    bool isSlpwakechgEnabled() const {
        return slpwakechg;
    }
 
    bool isWrtempEnabled() const {
        return wrtemp;
    }
 
    bool isBienableEnabled() const {
        return bienable;
    }
 
    bool isBlPupEnEnabled() const {
        return bl_pup_en;
    }
 
    int getPfcCfg() const {
        return pfc_cfg;
    }
 
    bool isWakeEnEnabled() const {
        return wake_en;
    }
 
    int getWkTh1() const {
        return wk_th1;
    }
 
    int getWkTh0() const {
        return wk_th0;
    }
 
    //* OperationConfig B *//
 
    bool isDefaultSealEnabled() const {
        return defaultSeal;
    }
 
    bool isNonRemovableSet() const {
        return nonRemovable;
    }
 
    bool isIntBremEnabled() const {
        return intBrem;
    }
 
    bool isIntBatLEnabled() const {
        return intBatL;
    }
 
    bool isIntStateEnabled() const {
        return intState;
    }
 
    bool isIntOcvEnabled() const {
        return intOcv;
    }
 
    bool isIntOtEnabled() const {
        return intOt;
    }
 
    bool isIntPolHigh() const {
        return intPol;
    }
 
    bool isIntFocvEnabled() const {
        return intFocv;
    }
};
// *INDENT-ON*
 
class GaugeBQ27220 : public GaugeBase, public I2CDeviceWithHal
{
private:
    typedef struct {
        uint16_t AtRate;                // mA        -   REG02 - 03
        uint16_t AtRateTimeToEmpty;     // Minute    -   REG04 - 05
        uint16_t Temperature;           // 0.1*K     -   REG06 - 07
        uint16_t Voltage;               // mV        -   REG08 - 09
        uint16_t BatteryStatus;         // bitmaps   -   REG0A - 0B
        int16_t  Current;               // mA        -   REG0C - 0D
 
        uint16_t RESERVE1;              // Rse       -   REG0E - 0F
 
        uint16_t RemainingCapacity;     // mAh       -   REG10 - 11
        uint16_t FullChargeCapacity;    // mAh       -   REG12 - 13
 
        uint16_t RESERVE2;              // Resv      -   REG14 - 15
 
        uint16_t TimeToEmpty;           // Minute    -   REG16 - 17
        uint16_t TimeToFull;            // Minute    -   REG18 - 19
        int16_t  StandbyCurrent;        // mA        -   REG1A - 1B
        uint16_t StandbyTimeToEmpty;    // Minute    -   REG1C - 1D
        int16_t  MaxLoadCurrent;        // mA        -   REG1E - 1F
        uint16_t MaxLoadTimeToEmpty;    // Minute    -   REG20 - 21
        uint16_t RawCoulombCount;       // Count     -   REG22 - 23
        int16_t  AveragePower;          // mW        -   REG24 - 25
 
        uint16_t RESERVE3;              // Resv      -   REG26 - 27
 
        uint16_t InternalTemperature;   // 0.1*K     -   REG28 - 29
        uint16_t CycleCount;            // Count     -   REG2A - 2B
        uint16_t StateOfCharge;         // %         -   REG2C - 2D
        uint16_t StateOfHealth;         // %         -   REG2E - 2F
        uint16_t ChargingVoltage;       // mV        -   REG30 - 31  - 65535 request max voltage
        uint16_t ChargingCurrent;       // mA        -   REG32 - 33  - 65535 request max current
        uint16_t BTPDischargeSet;       // Control   -   REG34 - 35
        uint16_t BTPChargeSet;          // Control   -   REG36 - 37
 
        uint16_t OperationStatus;       // Status    -   REG3A - 3B
        uint16_t DesignCapacity;        // mAh       -   REG3C - 3D
    } BatteryData;
 
public:
 
    enum Access {
        FULL_ACCESS = 1,
        UNBLOCK_ACCESS,
        SEALED_ACCESS,
    };
 
    GaugeBQ27220() : accessKey(0xFFFFFFFF) {}
 
    ~GaugeBQ27220() = default;
 
#if defined(ARDUINO)
    bool begin(TwoWire &wire, int sda = -1, int scl = -1)
    {
        return I2CDeviceWithHal::begin(wire, BQ27220_SLAVE_ADDRESS, sda, scl);
    }
 
#elif defined(ESP_PLATFORM)
 
#if defined(SENSORLIB_USE_I2C_LEGACY)
    bool begin(i2c_port_t port_num, int sda = -1, int scl = -1)
    {
        return I2CDeviceWithHal::begin(port_num, BQ27220_SLAVE_ADDRESS, sda, scl);
    }
#else
    bool begin(i2c_master_bus_handle_t handle)
    {
        return I2CDeviceWithHal::begin(handle, BQ27220_SLAVE_ADDRESS);
    }
#endif  //ESP_PLATFORM
#endif  //ARDUINO
 
    bool begin(SensorCommCustom::CustomCallback callback,
               SensorCommCustomHal::CustomHalCallback hal_callback)
    {
        return I2CDeviceWithHal::begin(callback, hal_callback, BQ27220_SLAVE_ADDRESS);
    }
 
 
    bool refresh() override
    {
        uint8_t buffer[REGISTER_COUNT];
        if (readRegBuff(START_REGISTER, buffer, REGISTER_COUNT) < 0) {
            return false;
        }
        uint8_t *ptr = (uint8_t *)&data;
        for (int i = 0; i < REGISTER_COUNT; i += 2) {
            uint16_t value = (buffer[i + 1] << 8) | buffer[i];
            *(uint16_t *)ptr = value;
            ptr += 2;
        }
 
        if (readRegBuff(0x3A, buffer, 4) < 0) {
            return false;
        }
        data.OperationStatus =  (buffer[1] << 8) | buffer[0];
        data.DesignCapacity =  (buffer[3] << 8) | buffer[2];
        return true;
    }
 
    // *INDENT-OFF*
    uint16_t getAtRate() const { return data.AtRate; }
 
    uint16_t getAtRateTimeToEmpty() const { return data.AtRateTimeToEmpty; }
 
    float getTemperature() override { return (data.Temperature * 0.1f) - 273.15f; }
 
    uint16_t getVoltage() override { return data.Voltage; }
 
    BatteryStatus getBatteryStatus() const { return BatteryStatus(data.BatteryStatus); }
 
    float getCurrent() override { return data.Current; }
 
    bool isCharging() override { return data.Current > 0; }
 
    bool isDischarging() override { return data.Current < 0; }
 
    uint16_t getRemainingCapacity() const { return data.RemainingCapacity; }
 
    uint16_t getFullChargeCapacity() const { return data.FullChargeCapacity; }
 
    uint16_t getTimeToEmpty() override { return data.TimeToEmpty; }
 
    uint16_t getTimeToFull() override { return data.TimeToFull; }
 
    int16_t  getStandbyCurrent() const { return data.StandbyCurrent; }
 
    uint16_t getStandbyTimeToEmpty() const { return data.StandbyTimeToEmpty; }
 
    int16_t  getMaxLoadCurrent() const { return data.MaxLoadCurrent; }
 
    uint16_t getMaxLoadTimeToEmpty() const { return data.MaxLoadTimeToEmpty; }
 
    uint16_t getRawCoulombCount() const { return data.RawCoulombCount; }
 
    int16_t  getAveragePower() const { return data.AveragePower; }
 
    float    getInternalTemperature() const { return (data.InternalTemperature * 0.1f) - 273.15f; }
 
    uint16_t getCycleCount() const { return data.CycleCount; }
 
    uint16_t getStateOfCharge() override { return data.StateOfCharge; }
 
    uint16_t getStateOfHealth() const { return data.StateOfHealth; }
 
    uint16_t getRequestChargingVoltage() const { return data.ChargingVoltage; }
 
    uint16_t getRequestChargingCurrent() const { return data.ChargingCurrent; }
 
    uint16_t getBTPDischargeSet() const { return data.BTPDischargeSet; }
 
    uint16_t getBTPChargeSet() const { return data.BTPChargeSet; }
 
    FuelGaugeOperationStatus getOperationStatus() const { return FuelGaugeOperationStatus(data.OperationStatus); }
 
    uint16_t getDesignCapacity() const { return data.DesignCapacity; }
 
    // *INDENT-ON*
 
    bool setNewCapacity(uint16_t newDesignCapacity, uint16_t newFullChargeCapacity)
    {
        return performConfigUpdate<bool>([this, newDesignCapacity, newFullChargeCapacity]() {
            // Set the design capacity
            if (!setCapacity(newDesignCapacity, sensorlib::_lowByte(BQ27220_ROM_DESIGN_CAPACITY),
                             sensorlib::_highByte(BQ27220_ROM_DESIGN_CAPACITY))) {
                SENSORLIB_LOG_E("Failed to set design capacity!");
                return false;
            }
            hal->delay(10);
 
            // Set full charge capacity
            if (!setCapacity(newFullChargeCapacity, sensorlib::_lowByte(BQ27220_ROM_FULL_CHARGE_CAPACITY),
                             sensorlib::_highByte(BQ27220_ROM_FULL_CHARGE_CAPACITY))) {
                SENSORLIB_LOG_E("Failed to set full charge capacity!");
                return false;
            }
            hal->delay(10);
 
            return true;
        });
    }
 
    OperationConfig getOperationConfig()
    {
        return performConfigUpdate<OperationConfig>([this]() {
            writeReg(BQ27220_REG_ROM_START, sensorlib::_lowByte(BQ27220_ROM_OPERATION_CONFIG_A));
            hal->delay(10);
            writeReg(BQ27220_REG_ROM_START + 1, sensorlib::_highByte(BQ27220_ROM_OPERATION_CONFIG_A));
            hal->delay(10);
            uint8_t buffer[4];
            readRegBuff(0x40, buffer, 4);
            uint16_t bitmapsA = (buffer[0] << 8) | buffer[1];
            uint16_t bitmapsB = (buffer[2] << 8) | buffer[3];
            OperationConfig value((bitmapsA << 16) | bitmapsB);
            return value;
        });
    }
 
    uint16_t getChipID() override
    {
        uint8_t buffer[2] = {0};
        if (sendSubCommand(BQ27220_SUB_CMD_DEVICE_NUMBER, true) < 0) {
            return 0;
        }
        if (this->getMACData(buffer, arraySize(buffer)) < 0) {
            return -1;
        }
        return static_cast<uint16_t>((buffer[1] << 8) | buffer[0]);
    }
 
    int getHardwareVersion()
    {
        uint8_t buffer[2];
        constexpr uint8_t value = 0x00;
        writeReg(BQ27220_SUB_CMD_HW_VERSION, value);
        if (this->getMACData(buffer, arraySize(buffer)) < 0) {
            return -1;
        }
        return static_cast<uint16_t>((buffer[1] << 8) | buffer[0]);
    }
 
 
    int getFirmwareVersion()
    {
        uint8_t buffer[4];
        if (sendSubCommand(BQ27220_SUB_CMD_FW_VERSION, true) < 0) {
            return -1;
        }
        if (this->getMACData(buffer, arraySize(buffer)) < 0) {
            return -1;
        }
        return  (buffer[3] << 24) | (buffer[2] << 16) | (buffer[1] << 8) | buffer[0];
    }
 
    void reset() override
    {
        sendSubCommand(BQ27220_SUB_CMD_RESET);
    }
 
    void setAccessKey(uint32_t key)
    {
        accessKey = key;
    }
 
private:
    FuelGaugeOperationStatus getOperationStatusNow()
    {
        return FuelGaugeOperationStatus(getHalfWord(BQ27220_REG_STA_OPERATION_STATUS));
    }
 
    // This read-write block will return the result data for the currently active subcommand.
    int getMACData(uint8_t *buffer, uint8_t request_len)
    {
        const uint8_t max_size = BQ27220_REG_MAC_BUFFER_END - BQ27220_REG_MAC_BUFFER_START + 1;
        if (request_len > max_size) {
            return -1;
        }
        if (!buffer) {
            return -1;
        }
        uint8_t reg = BQ27220_REG_MAC_BUFFER_START;
        return writeThenRead(&reg, 1, buffer, request_len);
    }
 
    // This read and write function returns the checksum of the current subcommand and data block.
    // Writing to this register provides the checksum required to execute a subcommand that requires data.
    uint16_t getMACDataSum()
    {
        uint8_t sum = 0x00;
        if (readRegBuff(BQ27220_REG_MAC_DATA_SUM, &sum, 1) < 0) {
            return 0;
        }
        return sum;
    }
 
    // This read and write function returns the number of MACData()
    // bytes contained in MACDataSum() as part of the response.
    uint16_t getMACDataLen()
    {
        uint8_t length = 0x00;
        if (readRegBuff(BQ27220_REG_MAC_DATA_LEN, &length, 1) < 0) {
            return 0;
        }
        return length;
    }
 
    // Subcommands
    int sendSubCommand(uint16_t subCmd, bool waitConfirm = false)
    {
        uint8_t buffer[3];
        buffer[0] = 0x00;
        buffer[1] = sensorlib::_lowByte(subCmd);
        buffer[2] = sensorlib::_highByte(subCmd);
        if (writeBuff(buffer, arraySize(buffer)) < 0) {
            return -1;
        }
        if (!waitConfirm) {
            hal->delay(10);
            return 0;
        }
        constexpr uint8_t statusReg = 0x00;
        int waitCount = 20;
        hal->delay(10);
        while (waitCount--) {
            writeThenRead(&statusReg, 1, buffer, 2);
            uint16_t *value = reinterpret_cast<uint16_t *>(buffer);
            if (*value == 0xFFA5) {
                return 0;
            }
            hal->delay(100);
        }
        SENSORLIB_LOG_E("Subcommand failed!");
        return -1;
    }
 
    // Unlock Safe Mode
    int unsealDevice()
    {
        uint8_t cmd1[] = {0x00, 0x14, 0x04};
        if (writeBuff(cmd1, arraySize(cmd1)) < 0) {
            return -1;
        }
        hal->delay(10);
        uint8_t cmd2[] = {0x00, 0x72, 0x36};
        if (writeBuff(cmd2, arraySize(cmd2)) < 0) {
            return -1;
        }
        hal->delay(10);
        return 0;
    }
 
    // Full access key, 0xFFFFFFFF if not set
    int unsealFullAccess()
    {
        uint8_t buffer[3];
        buffer[0] = 0x00;
        buffer[1] = sensorlib::_lowByte((accessKey >> 24));
        buffer[2] = sensorlib::_lowByte((accessKey >> 16));
        if (writeBuff(buffer, arraySize(buffer)) < 0) {
            return -1;
        }
        hal->delay(10);
        buffer[1] = sensorlib::_lowByte((accessKey >> 8));
        buffer[2] = sensorlib::_lowByte((accessKey));
        if (writeBuff(buffer, arraySize(buffer)) < 0) {
            return -1;
        }
        hal->delay(10);
        return 0;
    }
 
    int exitSealMode()
    {
        return  sendSubCommand(BQ27220_SUB_CMD_SEALED);
    }
 
    template<typename ResultType, typename Func>
    ResultType performConfigUpdate(Func specificOperation)
    {
        bool isSealed = false;
 
        // Check access settings
        FuelGaugeOperationStatus status = getOperationStatusNow();
        uint8_t currAccesslevel = status.getSecurityAccessLevel();
        if (currAccesslevel == SEALED_ACCESS) {
            isSealed = true;
            unsealDevice();
        }
        if (currAccesslevel != FULL_ACCESS) {
            unsealFullAccess();
        }
 
        // Send ENTER_CFG_UPDATE command (0x0090)
        sendSubCommand(BQ27220_SUB_CMD_ENTER_CFG_UPDATE);
 
        // Confirm CFUPDATE mode by polling the OperationStatus() register until Bit 2 is set.
        bool isConfigUpdate = false;
        uint32_t timeout = hal->millis() + 1500UL;
        while (timeout > hal->millis()) {
            status = getOperationStatusNow();
            if (status.getIsConfigUpdateMode()) {
                isConfigUpdate = true;
                break;
            }
            hal->delay(100);
        }
        if (!isConfigUpdate) {
            SENSORLIB_LOG_E("The update mode has timed out. It may also be that the access key for full permissions is invalid!");
            if (std::is_same<ResultType, bool>::value) {
                return false;
            } else if (std::is_same<ResultType, OperationConfig>::value) {
                return { 0x00 };
            }
        }
 
        ResultType result = specificOperation();
 
        // Exit CFUPDATE mode by sending the EXIT_CFG_UPDATE_REINIT (0x0091) or EXIT_CFG_UPDATE (0x0092) command
        sendSubCommand(BQ27220_SUB_CMD_EXIT_CFG_UPDATE_REINIT);
        hal->delay(10);
 
        // Confirm that CFUPDATE mode has been exited by polling the OperationStatus() register until bit 2 is cleared
        timeout = hal->millis() + 3000UL;
        while (timeout > hal->millis()) {
            status = getOperationStatusNow();
            if (!status.getIsConfigUpdateMode()) {
                timeout = hal->millis() + 1000;
                break;
            }
            hal->delay(100);
        }
        if (hal->millis() > timeout) {
            SENSORLIB_LOG_E("Timed out waiting to exit update mode.");
            if (std::is_same<ResultType, bool>::value) {
                return false;
            } else if (std::is_same<ResultType, OperationConfig>::value) {
                return { 0x00 };
            }
        }
 
        // If the device was previously in SEALED state, return to SEALED mode by sending the Control(0x0030) subcommand
        if (isSealed) {
            SENSORLIB_LOG_D("Restore Safe Mode!");
            exitSealMode();
        }
 
        return result;
    }
 
 
    bool setCapacity(uint16_t newCapacity, uint8_t msbAccessValue, uint8_t lsbAccessValue)
    {
        constexpr uint8_t fixedDataLength = 0x06;
 
        // Write to access the MSB of Capacity
        writeReg(BQ27220_REG_ROM_START, msbAccessValue);
        hal->delay(10);
 
        // Write to access the LSB of Capacity
        writeReg(BQ27220_REG_ROM_START + 1, lsbAccessValue);
        hal->delay(10);
 
        // Write two Capacity bytes starting from 0x40
        uint8_t newCapacityMsb = sensorlib::_highByte(newCapacity);
        uint8_t newCapacityLsb = sensorlib::_lowByte(newCapacity);
        uint8_t capacityRaw[] = {newCapacityMsb, newCapacityLsb};
        writeRegBuff(BQ27220_REG_MAC_BUFFER_START, capacityRaw, 2);
 
        // Calculate new checksum
        uint8_t newChksum = 0xFF - ((msbAccessValue + lsbAccessValue + newCapacityMsb + newCapacityLsb) & 0xFF);
 
        // Write new checksum (0x60)
        writeReg(BQ27220_REG_MAC_DATA_SUM, newChksum);
 
        // Write the block length
        writeReg(BQ27220_REG_MAC_DATA_LEN, fixedDataLength);
 
        return true;
    }
 
    int getHalfWord(uint8_t reg)
    {
        uint8_t buffer[2] = {0};
        if (writeThenRead(&reg, 1, buffer, arraySize(buffer)) < 0) {
            SENSORLIB_LOG_E("Read register %" PRIu32 " failed!", reg);
            return UINT16_MAX;
        }
        return (buffer[1] << 8) | buffer[0];
    }
 
    bool initImpl(uint8_t param) override
    {
        int chipID = getChipID();
        if (chipID != BQ27220_CHIP_ID) {
            SENSORLIB_LOG_E("Chip id not match : %" PRIu32, chipID);
            return false;
        }
 
        int sw = getFirmwareVersion();
        if (sw < 0) {
            SENSORLIB_LOG_E("Software version error!");
        } else {
            SENSORLIB_LOG_D("Software version 0x%" PRIu32, sw);
        }
        hal->delay(100);
        int hw = getHardwareVersion();
        if (hw < 0) {
            SENSORLIB_LOG_E("Hardware version error!");
        } else {
            SENSORLIB_LOG_D("Hardware version 0x%" PRIu32, hw);
        }
 
        return true;
    }
 
protected:
    uint32_t accessKey;
    BatteryData data;
    const uint8_t START_REGISTER  = 0x02;
    const uint8_t REGISTER_COUNT  = 54;
 
    //* Sub command
    static constexpr uint16_t BQ27220_SUB_CMD_CTRL_STATUS                = (0x0000);
    static constexpr uint16_t BQ27220_SUB_CMD_DEVICE_NUMBER              = (0x0001);
    static constexpr uint16_t BQ27220_SUB_CMD_FW_VERSION                 = (0x0002);
    static constexpr uint16_t BQ27220_SUB_CMD_HW_VERSION                 = (0x0003);
    static constexpr uint16_t BQ27220_SUB_CMD_BOARD_OFFSET               = (0x0009);
    static constexpr uint16_t BQ27220_SUB_CMD_CC_OFFSET                  = (0x000A);
    static constexpr uint16_t BQ27220_SUB_CMD_CC_OFFSET_SAVE             = (0x000B);
    static constexpr uint16_t BQ27220_SUB_CMD_OCV_CMD                    = (0x000C);
    static constexpr uint16_t BQ27220_SUB_CMD_BAT_INSERT                 = (0x000D);
    static constexpr uint16_t BQ27220_SUB_CMD_BAT_REMOVE                 = (0x000E);
    static constexpr uint16_t BQ27220_SUB_CMD_SET_SNOOZE                 = (0x0013);
    static constexpr uint16_t BQ27220_SUB_CMD_CLEAR_SNOOZE               = (0x0014);
    static constexpr uint16_t BQ27220_SUB_CMD_SET_PROFILE_1              = (0x0015);
    static constexpr uint16_t BQ27220_SUB_CMD_SET_PROFILE_2              = (0x0016);
    static constexpr uint16_t BQ27220_SUB_CMD_SET_PROFILE_3              = (0x0017);
    static constexpr uint16_t BQ27220_SUB_CMD_SET_PROFILE_4              = (0x0018);
    static constexpr uint16_t BQ27220_SUB_CMD_SET_PROFILE_5              = (0x0019);
    static constexpr uint16_t BQ27220_SUB_CMD_SET_PROFILE_6              = (0x001A);
    static constexpr uint16_t BQ27220_SUB_CMD_CAL_TOGGLE                 = (0x002D);
    static constexpr uint16_t BQ27220_SUB_CMD_SEALED                     = (0x0030);
    static constexpr uint16_t BQ27220_SUB_CMD_RESET                      = (0x0041);
    static constexpr uint16_t BQ27220_SUB_CMD_EXIT_CAL                   = (0x0080);
    static constexpr uint16_t BQ27220_SUB_CMD_ENTER_CAL                  = (0x0081);
    static constexpr uint16_t BQ27220_SUB_CMD_ENTER_CFG_UPDATE           = (0x0090);
    static constexpr uint16_t BQ27220_SUB_CMD_EXIT_CFG_UPDATE_REINIT     = (0x0091);
    static constexpr uint16_t BQ27220_SUB_CMD_EXIT_CFG_UPDATE            = (0x0092);
    static constexpr uint16_t BQ27220_SUB_CMD_RETURN_TO_ROM              = (0x0F00);
 
    //* Standard commands
    static constexpr uint8_t BQ27220_REG_STA_AT_RATE                     = (0x02);
    static constexpr uint8_t BQ27220_REG_STA_AT_RATE_TIME_TO_EMPTY       = (0x04);
    static constexpr uint8_t BQ27220_REG_STA_NTC_TEMP                    = (0x06);
    static constexpr uint8_t BQ27220_REG_STA_BAT_VOLTAGE                 = (0x08);
    static constexpr uint8_t BQ27220_REG_STA_BAT_STATUS                  = (0x0A);
    static constexpr uint8_t BQ27220_REG_STA_CURRENT                     = (0x0C);
    static constexpr uint8_t BQ27220_REG_STA_REMAINING_CAPACITY          = (0x10);
    static constexpr uint8_t BQ27220_REG_STA_FULL_CHARGE_CAPACITY        = (0x12);
    static constexpr uint8_t BQ27220_REG_STA_TIME_TO_EMPTY               = (0x16);
    static constexpr uint8_t BQ27220_REG_STA_TIME_TO_FULL                = (0x18);
    static constexpr uint8_t BQ27220_REG_STA_STANDBY_CURRENT             = (0x1A);
    static constexpr uint8_t BQ27220_REG_STA_STANDBY_TIME_TO_EMPTY       = (0x1C);
    static constexpr uint8_t BQ27220_REG_STA_MAX_LOAD_CURRENT            = (0x1E);
    static constexpr uint8_t BQ27220_REG_STA_MAX_LOAD_TO_EMPTY           = (0x20);
    static constexpr uint8_t BQ27220_REG_STA_COULOMB_COUNT               = (0x22);
    static constexpr uint8_t BQ27220_REG_STA_AVG_POWER                   = (0x24);
    static constexpr uint8_t BQ27220_REG_STA_INTER_TEMP                  = (0x28);
    static constexpr uint8_t BQ27220_REG_STA_CYCLE_COUNT                 = (0x2A);
    static constexpr uint8_t BQ27220_REG_STA_STATE_OF_CHARGE             = (0x2C);
    static constexpr uint8_t BQ27220_REG_STA_STATE_OF_HEALTH             = (0x2E);
    static constexpr uint8_t BQ27220_REG_STA_CHARGING_VOLTAGE            = (0x30);
    static constexpr uint8_t BQ27220_REG_STA_CHARGING_CURRENT            = (0x32);
    static constexpr uint8_t BQ27220_REG_STA_BTP_DISC_SET                = (0x34);
    static constexpr uint8_t BQ27220_REG_STA_BTP_CHARGE_SET              = (0x36);
    static constexpr uint8_t BQ27220_REG_STA_OPERATION_STATUS            = (0x3A);
    static constexpr uint8_t BQ27220_REG_STA_DESIGN_CAPACITY             = (0x3C);
    static constexpr uint8_t BQ27220_REG_STA_DESIGN_CAPACITY_MSB         = (0x3E);
    static constexpr uint8_t BQ27220_REG_STA_DESIGN_CAPACITY_LSB         = (0x3F);
    static constexpr uint8_t BQ27220_REG_MAC_BUFFER_START                = (0x40);
    static constexpr uint8_t BQ27220_REG_MAC_BUFFER_END                  = (0x5F);
    static constexpr uint8_t BQ27220_REG_MAC_DATA_SUM                    = (0x60);
    static constexpr uint8_t BQ27220_REG_MAC_DATA_LEN                    = (0x61);
    static constexpr uint8_t BQ27220_REG_ANALOG_COUNT                    = (0x79);
    static constexpr uint8_t BQ27220_REG_RAW_CURRENT                     = (0x7A);
    static constexpr uint8_t BQ27220_REG_RAW_VOLTAGE                     = (0x7C);
    static constexpr uint8_t BQ27220_REG_ROM_START                       = (0x3E);
 
    static constexpr uint16_t  BQ27220_CHIP_ID                           = (0x0220);
 
    static constexpr uint16_t BQ27220_ROM_FULL_CHARGE_CAPACITY           = (0x929D);
    static constexpr uint16_t BQ27220_ROM_DESIGN_CAPACITY                = (0x929F);
    static constexpr uint16_t BQ27220_ROM_OPERATION_CONFIG_A             = (0x9206);
    static constexpr uint16_t BQ27220_ROM_OPERATION_CONFIG_B             = (0x9208);
};
 
 
#endif