Revision 3:1b12fa9dc673, committed 2017-07-31

Comitter:

loopsva

Date:

Mon Jul 31 19:12:28 2017 +0000

Parent:

2:d52fb7ef7429

Commit message:

Working code. On hold for now. Moving to BQ34Z100-G1

Changed in this revision

--- a/bq27220.cpp	Wed Jun 21 19:56:02 2017 +0000
+++ b/bq27220.cpp	Mon Jul 31 19:12:28 2017 +0000
@@ -1,12 +1,22 @@
 #include    "mbed.h"
+#include    "rtos.h"
 #include    "bq27220.h"
 
+extern RawSerial pc;
+
 #define i2c_Buf dataSTR.i2c_Bufx
 
+
 BQ27220::BQ27220(PinName p_sda, PinName p_scl) :
     _i2c(p_sda, p_scl)
 {
-    _i2c.frequency(400000);
+    _i2c.frequency(100000);
+}
+
+BQ27220::BQ27220(PinName p_sda, PinName p_scl, int freq) :
+    _i2c(p_sda, p_scl)
+{
+    _i2c.frequency(freq);
 }
 
 void BQ27220::default_init(BQ27220_TypeDef& dataSTR)
@@ -19,14 +29,453 @@
     return(0);
 }
 
+uint16_t BQ27220::get_sub_cmmd(BQ27220_TypeDef& dataSTR, uint16_t cmmd)
+{   
+    i2c_Buf[0] = BQ_CNTL;
+    i2c_Buf[2] = cmmd >> 8;
+    i2c_Buf[1] = cmmd & 255;
+    _i2c.write(BQ27220_ADDR, i2c_Buf, 3, false);
+    Thread::wait(5); // needs large delay here
+    //pc.printf("sub-a: %02x %02x %02x %02x \r\n", i2c_Buf[0], i2c_Buf[1], i2c_Buf[2], i2c_Buf[3]);
+    int i = 0;
+    for(i = 0; i < 100; i++) {
+        wait_us(66);
+        _i2c.write(BQ27220_ADDR, i2c_Buf, 1, true);
+        _i2c.read(BQ27220_ADDR + 1, i2c_Buf, 4, false);
+        //pc.printf("sub-b: %02x %02x %02x %02x \r\n", i2c_Buf[0], i2c_Buf[1], i2c_Buf[2], i2c_Buf[3]);
+        if((i2c_Buf[0]== 0xa5) && (i2c_Buf[1] == 0xff)) break;
+    }
+    wait_us(66);
+    if(i > 98) pc.printf("sub-b: ERROR \r\n");
+    return (i2c_Buf[0] << 8) | i2c_Buf[1];
+}
+
+uint16_t BQ27220::get_sub_cmmd_s(BQ27220_TypeDef& dataSTR, uint16_t cmmd) 
+{   
+    i2c_Buf[0] = BQ_CNTL;
+    i2c_Buf[2] = cmmd >> 8;
+    i2c_Buf[1] = cmmd & 255;
+    //pc.printf("sub-a: %02x %02x %02x \r\n", i2c_Buf[0], i2c_Buf[1], i2c_Buf[2]);
+    _i2c.write(BQ27220_ADDR, i2c_Buf, 3, false);
+    wait_us(1066);
+    i2c_Buf[0] = BQ_MACDATA;
+    _i2c.write(BQ27220_ADDR, i2c_Buf, 1, true);
+    _i2c.read(BQ27220_ADDR + 1, i2c_Buf, 2, false);
+    wait_us(66);
+    //pc.printf("sub-b: %04x \r\n", (i2c_Buf[0] << 8) | i2c_Buf[1]);
+    return (i2c_Buf[0] << 8) | i2c_Buf[1];
+}
+
+uint16_t BQ27220::get_reg_2B(BQ27220_TypeDef& dataSTR, uint8_t reg)
+{   
+    i2c_Buf[0] = reg;
+    _i2c.write(BQ27220_ADDR, i2c_Buf, 1, true);
+    _i2c.read(BQ27220_ADDR + 1, i2c_Buf, 2, false);
+    //pc.printf("sub-b: %02x %02x %02x %02x \r\n", i2c_Buf[0], i2c_Buf[1], i2c_Buf[2], i2c_Buf[3]);
+    wait_us(66);
+    //Thread::wait(2);
+    return (i2c_Buf[1] << 8) | i2c_Buf[0];
+}
+
+void BQ27220::enter_cfg_update(BQ27220_TypeDef& dataSTR)
+{   
+    i2c_Buf[0] = BQ_CNTL;
+    i2c_Buf[1] = 0x90;
+    i2c_Buf[2] = 0x00;
+    _i2c.write(BQ27220_ADDR, i2c_Buf, 3, false);
+    wait_us(66);
+    Thread::wait(1500);
+}
+
+void BQ27220::exitCfgUpdateExit(BQ27220_TypeDef& dataSTR)
+{
+    i2c_Buf[0] = BQ_CNTL;
+    i2c_Buf[1] = BQ_EXIT_CFG_UPDATE & 255;
+    i2c_Buf[2] = BQ_EXIT_CFG_UPDATE >> 8;
+    pc.printf("exitCfg_cmmd: ->  ");
+    for(int i = 0; i < 3; i++) pc.printf("%02x ",i2c_Buf[i]);
+    pc.printf("\r\n");
+    _i2c.write(BQ27220_ADDR, i2c_Buf, 3, false);
+    wait_us(66);
+    Thread::wait(5);
+}
+
+void BQ27220::exitCfgUpdateReInit(BQ27220_TypeDef& dataSTR)
+{
+    i2c_Buf[0] = BQ_CNTL;
+    i2c_Buf[1] = BQ_EXIT_CFG_UPDATE_REINIT & 255;
+    i2c_Buf[2] = BQ_EXIT_CFG_UPDATE_REINIT >> 8;
+    pc.printf("exitInit_cmmd: ->  ");
+    for(int i = 0; i < 3; i++) pc.printf("%02x ",i2c_Buf[i]);
+    pc.printf("\r\n");
+    _i2c.write(BQ27220_ADDR, i2c_Buf, 3, false);
+    wait_us(66);
+    Thread::wait(5);
+}
+
+
+void BQ27220::reset(BQ27220_TypeDef& dataSTR)
+{
+    i2c_Buf[0] = BQ_CNTL;
+    i2c_Buf[1] = BQ_RESET & 255;
+    i2c_Buf[2] = BQ_RESET >> 8;
+    pc.printf("reset_cmmd: ->  ");
+    for(int i = 0; i < 3; i++) pc.printf("%02x ",i2c_Buf[i]);
+    pc.printf("\r\n");
+    _i2c.write(BQ27220_ADDR, i2c_Buf, 3, false);
+    wait_us(66);
+    Thread::wait(5);
+}
+    
+void BQ27220::useProfile_1(BQ27220_TypeDef& dataSTR)
+{   
+    i2c_Buf[0] = BQ_CNTL;
+    i2c_Buf[1] = BQ_SET_PROFILE_1 >> 8;
+    i2c_Buf[2] = BQ_SET_PROFILE_1 & 255;;
+    pc.printf("Profile_1_cmmd: ->  ");
+    for(int i = 0; i < 3; i++) pc.printf("%02x ",i2c_Buf[i]);
+    pc.printf("\r\n");
+    _i2c.write(BQ27220_ADDR, i2c_Buf, 3, false);
+    wait_us(66);
+    Thread::wait(200);
+}
+
+uint16_t BQ27220::get_cs_len(BQ27220_TypeDef& dataSTR, bool pf)
+{   
+    i2c_Buf[0] = BQ_MACDATASUM;
+    _i2c.write(BQ27220_ADDR, i2c_Buf, 1, true);
+    _i2c.read(BQ27220_ADDR + 1, i2c_Buf, 1, false);
+    wait_us(66);
+    //Thread::wait(5);
+    uint16_t csl = i2c_Buf[0];
+    
+    i2c_Buf[0] = BQ_MACDATALEN;
+    _i2c.write(BQ27220_ADDR, i2c_Buf, 1, true);
+    _i2c.read(BQ27220_ADDR + 1, i2c_Buf, 1, false);
+    wait_us(66);
+    //Thread::wait(5);
+    csl = (csl << 8) | i2c_Buf[0];
+    if(pf) pc.printf("get_cs: %02x\r\n", csl >> 8);
+    if(pf) pc.printf("get_ln: %02x\r\n", csl & 255);
+    return(csl);
+}
+
+uint8_t BQ27220::calc_checksum_rx(BQ27220_TypeDef& dataSTR, int length)
+{
+    uint8_t cs = 0;
+    //pc.printf("c_csum_rx_len: %02x -> ", length);
+    i2c_Buf[0] = BQ_SUB;
+    _i2c.write(BQ27220_ADDR, i2c_Buf, 1, true);
+    _i2c.read(BQ27220_ADDR + 1, i2c_Buf, 34, false);
+    wait_us(66);
+    Thread::wait(5);
+    for(int i = 0; i < length + 2; i++) {
+        cs += i2c_Buf[i];
+        //pc.printf("b: %02x cs: %02x  ", i2c_Buf[i], cs);
+    }
+    cs = 255 - cs;
+    //pc.printf("cs_rx:%02x \r\n", cs);
+    return (cs);
+}
+
+uint8_t BQ27220::calc_checksum_tx(BQ27220_TypeDef& dataSTR, int length)
+{
+    uint8_t cs = 0;
+    pc.printf("cs_tx_len: %02x ->    ", length);
+    for(int i = 0; i < length + 2; i++) {
+        cs += i2c_Buf[i + 1];
+        //pc.printf("i2c: %02x cs: %02x   ", i2c_Buf[i + 1], cs);
+        pc.printf("%02x ", i2c_Buf[i + 1]);
+    }
+    cs = 255 - cs;
+    pc.printf("\r\ncs_tx: %02x\r\n", cs);
+    return (cs);
+}
+
+uint32_t BQ27220::get_data_32(BQ27220_TypeDef& dataSTR, uint16_t sub_cmmd, int length)
+{   
+    i2c_Buf[0] = BQ_SUB;
+    i2c_Buf[2] = sub_cmmd >> 8;
+    i2c_Buf[1] = sub_cmmd & 255;
+    dataSTR.subReg = sub_cmmd;
+    //pc.printf("dat-32a: %02x %02x %02x \r\n", i2c_Buf[0], i2c_Buf[1], i2c_Buf[2]);
+    _i2c.write(BQ27220_ADDR, i2c_Buf, 3, false);
+    wait_us(3066);
+    //Thread::wait(3); //needs to be at least 2
+    
+    dataSTR.checksum = calc_checksum_rx(dataSTR, length);
+    
+    uint16_t cslen = get_cs_len(dataSTR, false);
+    dataSTR.macSumReg = cslen >> 8;
+    dataSTR.macLenReg = cslen & 255;
+    
+    i2c_Buf[0] = BQ_MACDATA;
+    _i2c.write(BQ27220_ADDR, i2c_Buf, 1, true);
+    _i2c.read(BQ27220_ADDR + 1, i2c_Buf, length, false);
+    wait_us(5066);
+    //Thread::wait(5); //seems to work down to 1
+    for(int i = 0; i < length; i++) {
+        dataSTR.macData[i] = dataSTR.i2c_Bufx[i];
+        pc.printf("%02x,", dataSTR.macData[i]);
+    }
+    pc.printf("\r\n");
+    //pc.printf(" mdl: %02x,  mdcs: %02x,  ccs: %02x\r\n", dataSTR.macLenReg, dataSTR.macSumReg, dataSTR.checksum);
+    return ((uint32_t)dataSTR.subReg); 
+}
+
+//#define BQ_SHORT          1
+
+void BQ27220::change_ram_1_2_4(BQ27220_TypeDef& dataSTR, uint16_t sub_cmmd, uint32_t value, int qty, bool pre)
+{   
+    if(pre) {
+#ifndef BQ_SHORT
+        pc.printf("ram124_a: %04x ->         ", sub_cmmd);
+        get_data_32(dataSTR, sub_cmmd, 32);
+#endif  
+    
+        if(qty == 1) {
+            dataSTR.macData[0] = value & 255;
+#ifdef BQ_SHORT
+            dataSTR.macData[1] = 0;
+            dataSTR.macData[2] = 0;
+            dataSTR.macData[3] = 0;
+#endif
+        } else
+        if(qty == 2) {
+            dataSTR.macData[0] = (value >> 8) & 255;
+            dataSTR.macData[1] = value & 255;
+#ifdef BQ_SHORT
+            dataSTR.macData[2] = 0;
+            dataSTR.macData[3] = 0;
+#endif
+        } else
+        if(qty == 4) {
+            dataSTR.macData[0] = (value >> 24) & 255;
+            dataSTR.macData[1] = (value >> 16) & 255;
+            dataSTR.macData[2] = (value >> 8) & 255;
+            dataSTR.macData[3] = value & 255;
+        } else {
+            pc.printf("ram124_q_error\r\n");
+            return;
+        }
+    }
+    
+    i2c_Buf[0] = BQ_SUB;
+    i2c_Buf[1] = sub_cmmd >> 8;
+    i2c_Buf[2] = sub_cmmd & 255;
+    if(pre) {
+        i2c_Buf[3] = dataSTR.macData[0];
+        i2c_Buf[4] = dataSTR.macData[1];
+        i2c_Buf[5] = dataSTR.macData[2];
+        i2c_Buf[6] = dataSTR.macData[3];
+    }
+    pc.printf("ram124_cmmd: ->  ");
+    int i = 0;
+#ifdef BQ_SHORT
+    for(i = 0; i < qty + 3; i++) pc.printf("%02x ",i2c_Buf[i]);
+    pc.printf("\r\n");
+    uint8_t x = calc_checksum_tx(dataSTR, qty);
+    _i2c.write(BQ27220_ADDR, i2c_Buf, qty + 3, false);
+#else
+    for(i = 0; i < 32; i++) i2c_Buf[i + 3] = dataSTR.macData[i];
+    for(i = 0; i < 35; i++) pc.printf("%02x ",i2c_Buf[i]);
+    pc.printf("\r\n");
+    uint8_t x = calc_checksum_tx(dataSTR, 32);
+    _i2c.write(BQ27220_ADDR, i2c_Buf, 35, false);
+#endif
+    wait_us(66);
+    //Thread::wait(5);
+    
+    i2c_Buf[0] = BQ_MACDATASUM;
+    i2c_Buf[1] = x;
+#ifndef BQ_SHORT
+    i2c_Buf[1] -= 0x20; //why is this???? !!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!
+    if((sub_cmmd >= 0x91e0) && (sub_cmmd < BQ_CONFIG_TAPER_CURR)) i2c_Buf[1]--;
+#endif
+    pc.printf("ram124_cs:   ->  ");
+    for(i = 0; i < 2; i++) pc.printf("%02x ",i2c_Buf[i]);
+    pc.printf("\r\n");
+    _i2c.write(BQ27220_ADDR, i2c_Buf, 2, false);
+    wait_us(66);
+    //Thread::wait(5);
+    
+    i2c_Buf[0] = BQ_MACDATALEN;
+#ifdef BQ_SHORT
+    i2c_Buf[1] = qty + 4;
+#else
+    i2c_Buf[1] = 36;
+#endif
+    pc.printf("ram124_len:  ->  ");
+    for(i = 0; i < 2; i++) pc.printf("%02x ",i2c_Buf[i]);
+    pc.printf("\r\n");
+    _i2c.write(BQ27220_ADDR, i2c_Buf, 2, false);
+    wait_us(5066);
+    //Thread::wait(200);
+    
+    get_cs_len(dataSTR, true);
+    //pc.printf("\r\n");
+    
+#ifndef BQ_SHORT
+    pc.printf("ram124_x: %04x ->         ", sub_cmmd);
+    get_data_32(dataSTR, sub_cmmd, 32);
+    pc.printf("\r\n");
+#endif
+}
+
+uint16_t BQ27220::get_16(BQ27220_TypeDef& dataSTR, uint16_t cmmd)
+{   
+    get_sub_cmmd_s(dataSTR, cmmd);
+    i2c_Buf[0] = BQ_MACDATA;
+    _i2c.write(BQ27220_ADDR, i2c_Buf, 1, true);
+    _i2c.read(BQ27220_ADDR + 1, i2c_Buf, 2, false);
+    wait_us(66);
+    return (i2c_Buf[0] << 8) | i2c_Buf[1]; 
+}
+
+uint8_t BQ27220::get_8(BQ27220_TypeDef& dataSTR, uint16_t cmmd)
+{   
+    //pc.printf("get_8: %04x\r\n", cmmd);
+    get_sub_cmmd_s(dataSTR, cmmd);
+    i2c_Buf[0] = BQ_MACDATA;
+    _i2c.write(BQ27220_ADDR, i2c_Buf, 1, true);
+    _i2c.read(BQ27220_ADDR + 1, i2c_Buf, 1, false);
+    wait_us(66);
+    return i2c_Buf[0]; 
+}
+
+void BQ27220::seal(BQ27220_TypeDef& dataSTR)
+{   
+    i2c_Buf[0] = BQ_CNTL;
+    i2c_Buf[1] = 0x30;
+    i2c_Buf[2] = 0x00;
+    _i2c.write(BQ27220_ADDR, i2c_Buf, 3, false);
+    wait_us(66);
+    wait_ms(5);
+    //Thread::wait(5);
+}
+
+void BQ27220::unseal(BQ27220_TypeDef& dataSTR)
+{   
+    i2c_Buf[0] = BQ_CNTL;
+    i2c_Buf[1] = 0x14;
+    i2c_Buf[2] = 0x04;
+    _i2c.write(BQ27220_ADDR, i2c_Buf, 3, false);
+    wait_us(66);
+    wait_ms(5);
+    //Thread::wait(5);
+    
+    i2c_Buf[0] = BQ_CNTL;
+    i2c_Buf[1] = 0x72;
+    i2c_Buf[2] = 0x36;
+    _i2c.write(BQ27220_ADDR, i2c_Buf, 3, false);
+    wait_ms(5);
+    //Thread::wait(5);
+}
+
+void BQ27220::full_access(BQ27220_TypeDef& dataSTR)
+{   
+    i2c_Buf[0] = BQ_CNTL;
+    i2c_Buf[1] = 0xff;
+    i2c_Buf[2] = 0xff;
+    _i2c.write(BQ27220_ADDR, i2c_Buf, 3, false);
+    wait_us(66);
+    wait_ms(5);
+    //Thread::wait(5);
+    
+    i2c_Buf[0] = BQ_CNTL;
+    i2c_Buf[1] = 0xff;
+    i2c_Buf[2] = 0xff;
+    _i2c.write(BQ27220_ADDR, i2c_Buf, 3, false);
+    wait_us(66);
+    wait_ms(5);
+    //Thread::wait(5);
+}
+
+uint32_t BQ27220::get_dev_id(BQ27220_TypeDef& dataSTR)
+{   
+    uint16_t dat = get_sub_cmmd(dataSTR, BQ_DEVICE_NUMBER);
+    //pc.printf("dat-idq: %04x \r\n", dat);
+    if(dat != 0xa5ff) return(dat);
+    i2c_Buf[0] = BQ_SUB;
+    _i2c.write(BQ27220_ADDR, i2c_Buf, 1, true);
+    _i2c.read(BQ27220_ADDR + 1, i2c_Buf, 4, false);
+    uint32_t id = (i2c_Buf[0] << 24) | (i2c_Buf[1] << 16) | (i2c_Buf[2] << 8) | i2c_Buf[3];
+    //pc.printf("dat-idq: %08x \r\n", id);
+    wait_us(66);
+    return(id) ; 
+}
+
+uint32_t BQ27220::get_fw_rev(BQ27220_TypeDef& dataSTR)
+{   
+    uint16_t dat = get_sub_cmmd(dataSTR, BQ_FW_VERSION);
+    //pc.printf("dat-fwq: %04x \r\n", dat);
+    if(dat != 0xa5ff) return(dat);
+    i2c_Buf[0] = BQ_SUB;
+    _i2c.write(BQ27220_ADDR, i2c_Buf, 1, true);
+    _i2c.read(BQ27220_ADDR + 1, i2c_Buf, 34, false);
+    wait_us(66);
+    return (i2c_Buf[0] << 24) | (i2c_Buf[1] << 16) | (i2c_Buf[2] << 8) | i2c_Buf[3]; 
+}
+
+uint32_t BQ27220::get_hw_rev(BQ27220_TypeDef& dataSTR)
+{   
+    uint16_t dat = get_sub_cmmd(dataSTR, BQ_HW_VERSION);
+    //pc.printf("dat-fwq: %04x \r\n", dat);
+    if(dat != 0xa5ff) return(dat);
+    i2c_Buf[0] = BQ_SUB;
+    _i2c.write(BQ27220_ADDR, i2c_Buf, 1, true);
+    _i2c.read(BQ27220_ADDR + 1, i2c_Buf, 34, false);
+    wait_us(66);
+    return (i2c_Buf[0] << 24) | (i2c_Buf[1] << 16) | (i2c_Buf[2] << 8) | i2c_Buf[3]; 
+}
+
+void BQ27220::set_ntc_as_sensor(BQ27220_TypeDef& dataSTR, bool ntc)
+{
+    uint16_t res = get_16(dataSTR, BQ_CONFIG_OP_CONFIG_A);
+        pc.printf("s_ntc: %04x ", res);
+        
+    if(!(ntc)) {
+        pc.printf(" N ");
+        res &= ~BQ_BIT_OCA_TEMPS;
+        res |=  BQ_BIT_OCA_BIE;
+
+    } else {
+        pc.printf(" L ");
+        res &= ~BQ_BIT_OCA_BIE;
+        res |=  BQ_BIT_OCA_TEMPS;
+    }
+    
+    pc.printf("new: %04x\r\n", res);
+    change_ram_1_2_4(dataSTR, BQ_CONFIG_OP_CONFIG_A - 0x20, (uint32_t)res, 2, true);
+}
+/*
+void BQ27220::set_reg(BQ27220_TypeDef& dataSTR, uint16_t reg, uint16_t da, int byt)
+{
+    uint16_t res = get_16(dataSTR, reg);
+    change_ram_1_2_4(dataSTR, reg, (uint32_t)da, byt, true);
+}
+*/
+/*
+uint16_t BQ27220::get_OS_reg(BQ27220_TypeDef& dataSTR)
+{
+    i2c_Buf[0] = BQ_OS;
+    _i2c.write(BQ27220_ADDR, i2c_Buf, 1, true);
+    _i2c.read(BQ27220_ADDR + 1, i2c_Buf, 2, false);
+    wait_us(66);
+    dataSTR.osReg = (i2c_Buf[1] << 8) | i2c_Buf[0];
+    return(dataSTR.osReg);
+}
+*/
+
 int BQ27220::read_registers(BQ27220_TypeDef& dataSTR)
 {
     i2c_Buf[0] = BQ_CNTL;
-    int result = _i2c.write((int)BQ27220_ADDR, i2c_Buf, 1, true);
+    int result = _i2c.write(BQ27220_ADDR, i2c_Buf, 1, true);
     if(result) return(result + 0x10);
-    _i2c.read((int)BQ27220_ADDR + 1, i2c_Buf, 32, false);
+    result = _i2c.read(BQ27220_ADDR + 1, i2c_Buf, 32, false);
     wait_us(66);
     if(result) return(result + 0x18);
+    //Thread::wait(1);
     
     dataSTR.cntlReg =    (i2c_Buf[BQ_CNTL    - BQ_CNTL + 1] << 8) | i2c_Buf[BQ_CNTL    - BQ_CNTL];
     dataSTR.arReg =      (i2c_Buf[BQ_AR      - BQ_CNTL + 1] << 8) | i2c_Buf[BQ_AR      - BQ_CNTL];
@@ -46,11 +495,12 @@
     dataSTR.mliReg =     (i2c_Buf[BQ_MLI     - BQ_CNTL + 1] << 8) | i2c_Buf[BQ_MLI     - BQ_CNTL];
     
     i2c_Buf[0] = BQ_MLTTE;
-    result = _i2c.write((int)BQ27220_ADDR, i2c_Buf, 1, true);
-    if(result) return(result + 0x20);
-    result =_i2c.read((int)BQ27220_ADDR + 1, i2c_Buf, 32, false);
+    result = _i2c.write(BQ27220_ADDR, i2c_Buf, 1, true);
+    //if(result) return(result + 0x20);
+    result =_i2c.read(BQ27220_ADDR + 1, i2c_Buf, 32, false);
     wait_us(66);
-    if(result) return(result + 0x28);
+    //Thread::wait(1);
+    //if(result) return(result + 0x28);
     
     dataSTR.mltteReg =   (i2c_Buf[BQ_MLTTE   -  BQ_MLTTE + 1] << 8) | i2c_Buf[BQ_MLTTE   -  BQ_MLTTE];
     dataSTR.rawccReg =   (i2c_Buf[BQ_RCC     -  BQ_MLTTE + 1] << 8) | i2c_Buf[BQ_RCC     -  BQ_MLTTE];
@@ -66,24 +516,27 @@
     dataSTR.btpcReg =    (i2c_Buf[BQ_BTPC    -  BQ_MLTTE + 1] << 8) | i2c_Buf[BQ_BTPC    -  BQ_MLTTE];
     dataSTR.osReg =      (i2c_Buf[BQ_OS      -  BQ_MLTTE + 1] << 8) | i2c_Buf[BQ_OS      -  BQ_MLTTE];
     dataSTR.dcReg =      (i2c_Buf[BQ_DC      -  BQ_MLTTE + 1] << 8) | i2c_Buf[BQ_DC      -  BQ_MLTTE];
+    dataSTR.subReg =     (i2c_Buf[BQ_SUB     -  BQ_MLTTE + 1] << 8) | i2c_Buf[BQ_SUB     -  BQ_MLTTE];
     
     i2c_Buf[0] = BQ_MACDATA;
-    result = _i2c.write((int)BQ27220_ADDR, i2c_Buf, 1, true);
-    if(result) return(result + 0x30);
-    result =_i2c.read((int)BQ27220_ADDR + 1, i2c_Buf, 32, false);
+    result = _i2c.write(BQ27220_ADDR, i2c_Buf, 1, true);
+    //if(result) return(result + 0x30);
+    result =_i2c.read(BQ27220_ADDR + 1, i2c_Buf, 32, false);
     wait_us(66);
-    if(result) return(result + 0x38);
+    //Thread::wait(1);
+    //if(result) return(result + 0x38);
     
     for(int i = 0; i < 32; i++) {
         dataSTR.macData[i] = i2c_Buf[i];
     }
     
     i2c_Buf[0] = BQ_MACDATASUM;
-    result = _i2c.write((int)BQ27220_ADDR, i2c_Buf, 1, true);
-    if(result) return(result + 0x40);
-    result =_i2c.read((int)BQ27220_ADDR + 1, i2c_Buf, 32, false);
+    result = _i2c.write(BQ27220_ADDR, i2c_Buf, 1, true);
+    //if(result) return(result + 0x40);
+    result =_i2c.read(BQ27220_ADDR + 1, i2c_Buf, 32, false);
     wait_us(66);
-    if(result) return(result + 0x48);
+    //Thread::wait(1);
+    //if(result) return(result + 0x48);
     
     dataSTR.macSumReg = (i2c_Buf[BQ_MACDATASUM  -  BQ_MACDATASUM + 1] << 8) | i2c_Buf[BQ_MACDATASUM  -  BQ_MACDATASUM];
     dataSTR.macLenReg = (i2c_Buf[BQ_MACDATALEN  -  BQ_MACDATASUM + 1] << 8) | i2c_Buf[BQ_MACDATALEN  -  BQ_MACDATASUM];
@@ -94,6 +547,3 @@
    
     return(0);
 }
-
-
-

--- a/bq27220.h	Wed Jun 21 19:56:02 2017 +0000
+++ b/bq27220.h	Mon Jul 31 19:12:28 2017 +0000
@@ -40,6 +40,7 @@
 
 #define BQ_OS                   0x3A
 #define BQ_DC                   0x3C
+#define BQ_SUB                  0x3E
 
 #define BQ_MACDATA              0x40
 
@@ -55,6 +56,7 @@
 #define BQ_CNTL_STAT            0x0000
 #define BQ_DEVICE_NUMBER        0x0001
 #define BQ_FW_VERSION           0x0002
+#define BQ_HW_VERSION           0x0003
 
 #define BQ_BOARD_OFFSET         0x0009
 #define BQ_CC_OFFSET            0x000A
@@ -76,7 +78,10 @@
 
 #define BQ_SET_SEALED           0x0030
 
-#define BQ_RESET                0x0040
+#define BQ_RESET                0x0041
+
+#define BQ_OP_STATUS            0x0054
+#define BQ_GAUGE_STATUS         0x0056
 
 #define BQ_EXIT_CAL             0x0080
 #define BQ_ENTER_CAL            0x0081
@@ -87,6 +92,79 @@
 
 #define BQ_RETURN_TO_ROM        0x0F00
 
+// Configuration parameters
+#define BQ_CONFIG_CC_GAIN       0x9184      //float
+
+#define BQ_CONFIG_CHG_INH_LO    0x91f5      //int16_t
+#define BQ_CONFIG_CHG_INH_HI    0x91f7      //int16_t
+#define BQ_CONFIG_CHG_INH_HYST  0x91f9      //int16_t
+#define BQ_CONFIG_CHG_CURR      0x91fb      //int16_t
+#define BQ_CONFIG_CHG_VOLT      0x91fd      //int16_t
+
+#define BQ_CONFIG_TAPER_CURR    0x9201      //int16_t
+#define BQ_CONFIG_OP_CONFIG_A   0x9206      //uint16_t
+#define BQ_CONFIG_OP_CONFIG_B   0x9208      //uint16_t
+#define BQ_CONFIG_SOC_DELTA     0x920b      //uint8_t
+#define BQ_CONFIG_CLK_CTRL      0x920c      //uint8_t
+#define BQ_CONFIG_IO_CONFIG     0x920d      //uint8_t
+#define BQ_CONFIG_INIT_DIS_SET  0x920e      //int16_t
+#define BQ_CONFIG_INIT_CHG_SET  0x9210      //int16_t
+#define BQ_CONFIG_DEVICE_TYPE   0x9212      //uint16_t
+
+#define BQ_CONFIG_SLEEP_CURR    0x9217      //int16_t
+#define BQ_CONFIG_BUS_LO_TIME   0x9219      //uint8_t
+#define BQ_CONFIG_OC_INH_T_LO   0x921a      //int16_t
+#define BQ_CONFIG_OC_INH_T_HI   0x921c      //int16_t
+#define BQ_CONFIG_SLEEP_V_TIME  0x921e      //uint8_t
+#define BQ_CONFIG_SLEEP_C_TIME  0x921f      //uint8_t
+
+#define BQ_CONFIG_DIS_DET_THRES 0x9228      //int16_t
+#define BQ_CONFIG_CHG_DET_THRES 0x922a      //int16_t
+#define BQ_CONFIG_QUIT_CURR     0x922c      //int16_t
+#define BQ_CONFIG_DIS_RELAX_TI  0x922e      //uint16_t
+
+#define BQ_CONFIG_CHG_RELAX_TI  0x9230      //uint8_t
+#define BQ_CONFIG_QUIT_RELAX_TI 0x9231      //uint8_t
+
+#define BQ_CONFIG_OT_CHG        0x9232      //int16_t
+#define BQ_CONFIG_OT_CHG_TIME   0x9234      //uint8_t
+#define BQ_CONFIG_OT_CGH_RECOV  0x9235      //int16_t
+#define BQ_CONFIG_OT_DSG        0x9237      //int16_t
+#define BQ_CONFIG_OT_DSG_TIME   0x9239      //uint8_t
+#define BQ_CONFIG_OT_DSG_RECOV  0x923a      //int16_t
+#define BQ_CONFIG_INIT_STBY     0x923c      //int8_t
+
+#define BQ_CONFIG_SYSD_S_VTHRS  0x9240      //int16_t
+#define BQ_CONFIG_SYSD_S_VTIME  0x9242      //uint8_t
+#define BQ_CONFIG_SYSD_C_VTHRS  0x9243      //int16_t
+
+#define BQ_GG_SMOOTHING_CONFIG  0x9271      //uint8_t
+#define BQ_CONFIG_FLAG_CONFIG_A 0x927f      //uint16_t
+#define BQ_CONFIG_FLAG_CONFIG_B 0x9281      //uint8_t
+
+#define BQ_CONFIG_BATTERY_ID    0x929a      //uint8_t
+
+// Gas Gauge parameters
+#define BQ_GG_CEDVp1_GAUGE_CONF 0x929b      //uint16_t
+#define BQ_GG_CEDVp1_FULLC_CAP  0x929d      //int16_t 15 bits
+#define BQ_GG_CEDVp1_DESIGN_CAP 0x929f      //int16_t 15 bits
+#define BQ_GG_CEDVp1_DESIGN_V   0x92a3      //int16_t 15 bits
+#define BQ_GG_CEDVp1_CHG_TERM_V 0x92a5      //int16_t
+#define BQ_GG_CEDVp1_EMF        0x92a7      //uint16_t
+#define BQ_GG_CEDVp1_C0         0x92a9      //uint16_t
+#define BQ_GG_CEDVp1_R0         0x92ab      //uint16_t
+#define BQ_GG_CEDVp1_T0         0x92ad      //uint16_t
+#define BQ_GG_CEDVp1_R1         0x92af      //uint16_t
+#define BQ_GG_CEDVp1_TC         0x92b1      //uint8_t
+#define BQ_GG_CEDVp1_C1         0x92b2      //uint8_t
+#define BQ_GG_CEDVp1_AGE_FACTOR 0x92b3      //uint8_t
+#define BQ_GG_CEDVp1_FIXED_EDV0 0x92b4      //int16_t
+#define BQ_GG_CEDVp1_HOLDT_EDV0 0x92b6      //uint8_t
+#define BQ_GG_CEDVp1_FIXED_EDV1 0x92b7      //int16_t
+#define BQ_GG_CEDVp1_HOLDT_EDV1 0x92b9      //uint8_t
+#define BQ_GG_CEDVp1_FIXED_EDV2 0x92ba      //int16_t
+#define BQ_GG_CEDVp1_HOLDT_EDV2 0x92bc      //uint8_t
+
 // CNTL_STAT register bit equates
 #define BQ_BIT_CS_CCA           0x0020
 #define BQ_BIT_CS_BCA           0x0010
@@ -94,20 +172,36 @@
 #define BQ_BIT_CS_BAT_ID2       0x0004
 #define BQ_BIT_CS_BAT_ID1       0x0002
 #define BQ_BIT_CS_BAT_ID0       0x0001
+#define BQ_BIT_CS_BAT_ID        (BQ_BIT_CS_BAT_ID2 | BQ_BIT_CS_BAT_ID1 | BQ_BIT_CS_BAT_ID0)
 
-//GaugingStatus register bit equates
+// Gauging Status register bit equates
 #define BQ_BIT_GS_VDQ           0x8000
 #define BQ_BIT_GS_EDV2          0x4000
 #define BQ_BIT_GS_EDV1          0x2000
+#define BQ_BIT_GS_RSVD1         0x1000
+#define BQ_BIT_GS_RSVD2         0x0800
 #define BQ_BIT_GS_FCCX          0x0400
+#define BQ_BIT_GS_RSVD3         0x0200
+#define BQ_BIT_GS_RSVD4         0x0100
 #define BQ_BIT_GS_CF            0x0080
 #define BQ_BIT_GS_DSG           0x0040
 #define BQ_BIT_GS_EDV           0x0020
+#define BQ_BIT_GS_RSVD5         0x0010
 #define BQ_BIT_GS_TC            0x0008
 #define BQ_BIT_GS_TD            0x0004
 #define BQ_BIT_GS_FC            0x0002
 #define BQ_BIT_GS_FD            0x0001
 
+// CEDV Gauging Configuration register bit equates
+#define BQ_BIT_GC_SME0          0x1000
+#define BQ_BIT_GC_IGNORE_SD     0x0800
+#define BQ_BIT_GC_FC_FOR_VDQ    0x0400
+#define BQ_BIT_GC_FCC_LIMIT     0x0100
+#define BQ_BIT_GC_FIXED_EDV0    0x0020
+#define BQ_BIT_GC_SC            0x0010
+#define BQ_BIT_GC_EDV_CMP       0x0008
+#define BQ_BIT_GC_CSYNC         0x0002
+#define BQ_BIT_GC_CCT           0x0001
 
 // FLAGS register bit equates
 #define BQ_BIT_F_FD             0x8000
@@ -136,6 +230,74 @@
 #define BQ_BIT_OS_SEC1          0x0004
 #define BQ_BIT_OS_SEC0          0x0002
 #define BQ_BIT_OS_CALMD         0x0001
+
+// Operation Config Reg A bit equates
+#define BQ_BIT_OCA_TEMPS        0x8000
+#define BQ_BIT_OCA_RSVD1        0x4000
+#define BQ_BIT_OCA_BATG_POL     0x2000
+#define BQ_BIT_OCA_BATG_EN      0x1000
+#define BQ_BIT_OCA_RSVD2        0x0800
+#define BQ_BIT_OCA_SLEEP        0x0400
+#define BQ_BIT_OCA_SLPWAKECHG   0x0200
+#define BQ_BIT_OCA_WRTEMP       0x0100
+#define BQ_BIT_OCA_BIE          0x0080
+#define BQ_BIT_OCA_RSVD3        0x0040
+#define BQ_BIT_OCA_BI_PUP_EN    0x0020
+#define BQ_BIT_OCA_PFC_CFG1     0x0010
+#define BQ_BIT_OCA_PFC_CFG0     0x0008
+#define BQ_BIT_OCA_WAKE_EN      0x0004
+#define BQ_BIT_OCA_WK_TH1       0x0002
+#define BQ_BIT_OCA_WK_TH0       0x0001
+
+// Operation Config Reg B bit equates
+#define BQ_BIT_OCB_RSVD1        0x8000
+#define BQ_BIT_OCB_RSVD2        0x4000
+#define BQ_BIT_OCB_RSVD3        0x2000
+#define BQ_BIT_OCB_RSVD4        0x1000
+#define BQ_BIT_OCB_DEF_SEAL     0x0800
+#define BQ_BIT_OCB_NR           0x0400
+#define BQ_BIT_OCB_RSVD5        0x0200
+#define BQ_BIT_OCB_RSVD6        0x0100
+#define BQ_BIT_OCB_INT_BREM     0x0080
+#define BQ_BIT_OCB_INT_BATL     0x0040
+#define BQ_BIT_OCB_INT_STATE    0x0020
+#define BQ_BIT_OCB_INT_OCV      0x0010
+#define BQ_BIT_OCB_RSVD7        0x0008
+#define BQ_BIT_OCB_INT_OT       0x0004
+#define BQ_BIT_OCB_INT_POL      0x0002
+#define BQ_BIT_OCB_INT_FOCV     0x0001
+
+// SOC Flags Reg A bit equates
+#define BQ_BIT_SOCFA_TCSETVCT   0x0800
+#define BQ_BIT_SOCFA_FCSETVCT   0x0400
+#define BQ_BIT_SOCFA_TCCLEARRSOC 0x0080
+#define BQ_BIT_SOCFA_TCSETRSOC  0x0040
+#define BQ_BIT_SOCFA_TCCLEARV   0x0020
+#define BQ_BIT_SOCFA_TCSETV     0x0010
+#define BQ_BIT_SOCFA_TDCLEARRSOC 0x0008
+#define BQ_BIT_SOCFA_TDSETRSOC  0x0004
+#define BQ_BIT_SOCFA_TDCLEARV   0x0002
+#define BQ_BIT_SOCFA_TDSETV     0x0001
+
+// SOC Flags Reg B bit equates
+#define BQ_BIT_SOCFB_FCCLEARRSOC 0x0080
+#define BQ_BIT_SOCFB_FCSETRSOC  0x0040
+#define BQ_BIT_SOCFB_FCCLEARV   0x0020
+#define BQ_BIT_SOCFB_FCSETV     0x0010
+#define BQ_BIT_SOCFB_FDCLEARRSOC 0x0008
+#define BQ_BIT_SOCFB_FDSETRSOC  0x0004
+#define BQ_BIT_SOCFB_FDCCLEARV  0x0002
+#define BQ_BIT_SOCFB_FDSETV     0x0001
+
+// IO Config bit equates
+#define BQ_BIT_IOCFG_BtpIntPol  0x0002
+#define BQ_BIT_SOCFB_BTpIntEn   0x0001
+
+// Smoothing Config bit equates
+#define BQ_BIT_SMOC_SMOOTH_EOC_EN   0x0008
+#define BQ_BIT_SMOC_CMEXT       0x0004
+#define BQ_BIT_SMOC_VAVG        0x0002
+#define BQ_BIT_SMOC_SMEN        0x0001
     
 class BQ27220
 {
@@ -146,26 +308,26 @@
     **/
     typedef struct {
         uint16_t cntlReg;       /*!< CNTL register */
-        uint16_t arReg;         /*!< AR register */
+         int16_t arReg;         /*!< AR register */
         uint16_t artteReg;      /*!< ARTTE register */
         uint16_t tempReg;       /*!< TEMP register */
         uint16_t voltReg;       /*!< VOLT register */
         
         uint16_t flagsReg;      /*!< FLAGS register */
-        uint16_t currentReg;    /*!< CURRENT register */
+         int16_t currentReg;    /*!< CURRENT register */
         uint16_t rmReg;         /*!< RM register */
         uint16_t fccReg;        /*!< FCC register */
         uint16_t aiReg;         /*!< AI register */
         
         uint16_t tteReg;        /*!< TTE register */
         uint16_t ttfReg;        /*!< TTF register */
-        uint16_t siReg;         /*!< SI register */
+         int16_t siReg;         /*!< SI register */
         uint16_t stteReg;       /*!< STTE register */
-        uint16_t mliReg;        /*!< MLI register */
+         int16_t mliReg;        /*!< MLI register */
         
         uint16_t mltteReg;      /*!< MLTTE register */
         uint16_t rawccReg;      /*!< RCC register */
-        uint16_t apReg;         /*!< AP register */
+         int16_t apReg;         /*!< AP register */
         uint16_t intTempReg;    /*!< INTTEMP register */
         uint16_t cycReg;        /*!< CYC register */
         
@@ -178,17 +340,20 @@
         uint16_t btpcReg;       /*!< BTPC register */
         uint16_t osReg;         /*!< OS register */
         uint16_t dcReg;         /*!< DC register */
+        uint16_t subReg;        /*!< SUB command register */
         char macData[32];       /*!< MAC Data array */
-        uint16_t macSumReg;     /*!< MAC Data Sum register */
+        uint8_t macSumReg;      /*!< MAC Data Sum register */
         
-        uint16_t macLenReg;     /*!< MAC Data Len register */
+        uint8_t macLenReg;      /*!< MAC Data Len register */
         uint8_t anacReg;        /*!< Analog Count register */
         uint16_t rawcReg;       /*!< RAWC register */
         uint16_t rawvReg;       /*!< RAWV register */
         uint16_t rawtReg;       /*!< RAWT register */
+        
+        uint8_t checksum;       /*!< calculated checksum result */
 
-        int16_t shunt_res;      /*!< Shunt Resistor value * 1000, 20 = 0.020 ohm */
-        char i2c_Bufx[32];      /*!< i2c buffer */
+        int16_t shunt_res;      /*!< Shunt Resistor value / 1000 */
+        char i2c_Bufx[48];      /*!< i2c buffer */
     } BQ27220_TypeDef;
     
     /** Configure data pin
@@ -197,7 +362,7 @@
     **/
     //BQ27220(PinName p_sda, PinName p_scl, PinName p_pgrm);
     BQ27220(PinName p_sda, PinName p_scl);
-    
+    BQ27220(PinName p_sda, PinName p_scl, int freq);
     
     /** Write default values for CNTL register and shunt resistor * 1000
       * @param I2c pins
@@ -206,12 +371,87 @@
       */
     void default_init(BQ27220_TypeDef& dataSTR);
     
+    uint16_t get_OS_reg(BQ27220_TypeDef& dataSTR);
+    
     /** Read all bq registers and put them into the data structure
       * @param pointer to data structure
       * @return i2c error, 0 = no error
       */
     int read_registers(BQ27220_TypeDef& dataSTR);
     
+    /** Send sub-command and read data and/or result from sub-command
+      * @param pointer to data structure
+      * @return result and/or data
+      */
+    uint16_t get_sub_cmmd(BQ27220_TypeDef& dataSTR, uint16_t cmmd);
+    
+    /** Like above, without extra delays
+      * @param pointer to data structure
+      * @return result and/or data
+      */
+    uint16_t get_sub_cmmd_s(BQ27220_TypeDef& dataSTR, uint16_t cmmd);
+    
+    void change_cfg_OT_chg_time(BQ27220_TypeDef& dataSTR, uint8_t newtime);
+    void change_ram_1_2_4(BQ27220_TypeDef& dataSTR, uint16_t sub_cmmd, uint32_t value, int qty, bool pre);
+    void change_cfg_6_1(BQ27220_TypeDef& dataSTR);
+    uint16_t get_cs_len(BQ27220_TypeDef& dataSTR, bool pf);
+    void exitCfgUpdateReInit(BQ27220_TypeDef& dataSTR);
+    void exitCfgUpdateExit(BQ27220_TypeDef& dataSTR);
+    void set_reg(BQ27220_TypeDef& dataSTR, uint16_t reg, uint16_t da, int byt);
+    
+    uint16_t get_reg_2B(BQ27220_TypeDef& dataSTR, uint8_t reg);
+    
+    void unseal(BQ27220_TypeDef& dataSTR);
+    void full_access(BQ27220_TypeDef& dataSTR);
+    void enter_cfg_update(BQ27220_TypeDef& dataSTR);
+    void seal(BQ27220_TypeDef& dataSTR);
+    void useProfile_1(BQ27220_TypeDef& dataSTR);
+    void reset(BQ27220_TypeDef& dataSTR);
+    
+    /** Send sub-command to get device ID
+      * @param pointer to data structure
+      * @return sub-command + device id
+      */
+    uint32_t get_dev_id(BQ27220_TypeDef& dataSTR);
+    
+    /** Send sub-command to get firmware revision
+      * @param pointer to data structure
+      * @return revision
+      */
+    uint32_t get_fw_rev(BQ27220_TypeDef& dataSTR);
+    
+    /** Send sub-command to get firmware revision
+      * @param pointer to data structure
+      * @return revision
+      */
+    uint32_t get_hw_rev(BQ27220_TypeDef& dataSTR);
+    
+    uint8_t calc_checksum_rx(BQ27220_TypeDef& dataSTR, int length);
+    uint8_t calc_checksum_tx(BQ27220_TypeDef& dataSTR, int length);
+    
+    /** Send sub-command to get 32 bytes data
+      * @param pointer to data structure
+      * @param sub-command
+      * @return 32 bytes in macData
+      */
+    uint32_t get_data_32(BQ27220_TypeDef& dataSTR, uint16_t sub_cmmd, int length);
+    
+    /** Get signed 16 bit value
+      * @param pointer to data structure
+      * @param sub-command
+      * @return 16 bit signed value
+      */
+    uint16_t get_16(BQ27220_TypeDef& dataSTR, uint16_t cmmd);
+    
+    /** Get unsigned 8 bit value
+      * @param pointer to data structure
+      * @param sub-command
+      * @return 8 bit signed value
+      */
+    uint8_t get_8(BQ27220_TypeDef& dataSTR, uint16_t cmmd);
+    
+    void set_ntc_as_sensor(BQ27220_TypeDef& dataSTR, bool ntc);
+    
     /** Initialize SoC for a new battery
       * @param pointer to data structure
       * @return error, 0 = no error