File content as of revision 19:dfd1f76515c5:

#include "mbed.h"
#include "vt100.h"
#include "MSS.h"
#include "MSU.h"
#include "MMA8451Q.h"
#include "MMA8452Q.h"
#include "MAG3110.h"
#include "HIH6130.h"
#include "FXOS8700CQ.h"
#include "MAX44000.h"
#include "MAX44005.h"
#include "MAX44008.h"
#include "MPL3115A2.h"
#include "VEML6040.h"
#include "VEML6075.h"
#include "LM75B.h"
#include "FXAS21002.h"
#include "S11059.h"
#include "MAX30101.h"
#include "VCNL4100.h"
#include "AK9752.h"
#include "AK9753.h"
#include "AK09970N.h"

extern int test_loop ;
extern int interval ; /* wait interval ms */

extern vt100 *tty ;

i2c_sensor_type i2c_sensor[] = {
    {MSU_MMA8451Q_ADDRESS,   "MMA8451Q",   testMMA8451Q},
    {MSU_MMA8452Q_ADDRESS,   "MMA8452Q",   testMMA8452Q},
    {MSU_MAG3110_ADDRESS,    "MAG3110",    testMAG3110},
    {MSU_HIH6130_ADDRESS,    "HIH6130",    testHIH6130},
    {MSU_FXOS8700CQ_ADDRESS, "FXOS8700CQ", testFXOS8700CQ},
    {MSU_MAX44000_ADDRESS,   "MAX44000",   testMAX44000},
    {MSU_MAX44005_ADDRESS,   "MAX44005",   testMAX44005},
    {MSU_MAX44008_ADDRESS,   "MAX44008",   testMAX44008},
    {MSU_MAX30101_ADDRESS,   "MAX30101",   testMAX30101},
    {MSU_MPL3115A2_ADDRESS,  "MPL3115A2",  testMPL3115A2},
    {MSU_IS31SE5000_ADDRESS, "IS31SE5000", testIS31SE5000},
//    {MSU_VCNL4020_ADDRESS,   "VCNL4020",   testVCNL4020"},
    {MSU_VEML6040A_ADDRESS,  "VEML6040A",  testVEML6040A},
    {MSU_VEML6075_ADDRESS,   "VEML6075",   testVEML6075},
    {MSU_LM75B_ADDRESS,      "LM75B",      testLM75B},
    {MSU_FXAS21002_ADDRESS,  "FXAS21002",  testFXAS21002},
    {MSU_S11059_ADDRESS,     "S11059",     testS11059},
    {MSU_VCNL4100_I2C_ADDRESS, "VCNL4100", testVCNL4100},
    {MSU_AK9752_I2C_ADDRESS, "AK9752",     testAK9752},
    {MSU_AK9753_I2C_ADDRESS, "AK9753",     testAK9753},
    {MSU_AK09970N_I2C_ADDRESS, "AK09970N", testAK09970N},
    {0x00, 0, 0}
} ;

MAX30101 *max30101 = 0 ;
#ifndef FIFO_DEPTH
#define FIFO_DEPTH 32
#endif

void doDemo(void)
{
    int i ;
    float humidity = 0 ;
    float temperature = 0 ;
    uint8_t mode ;
    uint8_t txdata, rxdata ;
    uint16_t als ;
    uint8_t prox, overflow ;
    uint32_t data, alt, bar,  tmp ;
    uint16_t sample_time ;
    
    HIH6130 *hih6130 = new HIH6130(PIN_SDA, PIN_SCL, MSU_HIH6130_ADDRESS) ;
    MMA8452Q *mma8452q = new MMA8452Q(PIN_SDA, PIN_SCL, MSU_MMA8452Q_ADDRESS) ;
    MAX44000 *max44000 = new MAX44000(PIN_SDA, PIN_SCL, MSU_MAX44000_ADDRESS) ;
    rxdata = 0xF1 ;
    max44000->setRxConfig(rxdata) ;
    txdata = 0x0F ; /* light LED! */
    max44000->setTxConfig(txdata) ;
    mode = 0x30 ; // ALS & PROX mode
    max44000->setMainConfig(mode) ;

    MPL3115A2 *mpl3115a2 = new MPL3115A2(PIN_SDA, PIN_SCL, MSU_MPL3115A2_ADDRESS) ;
    tty->cls() ;
    tty->locate(10, 1) ;
    printf("=== Multi Sensor Demo (%s) ===\n", BOARD_NAME) ;
    tty->putStr(2, 3, "MMA8451Q") ;
    tty->putStr(2, 10, "MAX44000") ;
    tty->putStr(40, 3, "HIH6130") ;
    tty->putStr(40, 10, "MPL3115A2") ;
    for (i = 0 ; i < test_loop ; i++ ) {

    /* MMA8451Q */
        tty->locate(2, 5) ;
        printf("X:%.2f Y:%.2f Z:%.2f",
            mma8452q->getAccX(),
            mma8452q->getAccY(),
            mma8452q->getAccZ()) ;
            
        if ((i % 10) == 0) {
    /* HIH6130 */
        tty->locate(40, 5) ;
        hih6130->getValue(&humidity, &temperature) ;
        printf("Temp %.1f C", temperature) ;
        tty->locate(40, 7) ;
        printf("Humidity %.1f %%\n", humidity) ;
    /* MAX44000 */
        tty->locate(2, 12) ;
            max44000->getADC(&overflow, &als, &prox) ;
        printf("ALS[ %04X ]", als) ;
        if (overflow) {
            printf(" overflow ") ;
        } else {
            printf("          ") ;
        }
        tty->locate(2, 14) ;
        printf("PRX[ %02X ]\n", prox) ;
    /* MPL3115A2 */

        mpl3115a2->modeAlt() ;
        sample_time = mpl3115a2->OneShot() ;
        wait(((double)sample_time)/1000.0) ;
        alt = mpl3115a2->getAltitude() ;
        mpl3115a2->modeBar() ;
        sample_time = mpl3115a2->OneShot() ;
        wait(((double)sample_time)/1000.0) ;
        bar = mpl3115a2->getPressure() ;
        tmp = mpl3115a2->getTemperature() ;
        tty->locate(40, 11) ;
        printf("Altitude: %d",    (alt >> 16)&0xFFFF) ;
        tty->locate(40, 13) ;
        printf("Pressure: %d",    (bar >> 6)) ;
        tty->locate(40, 15) ;
        printf("Temperature: %d", (tmp >> 8) ) ;
        } 
    }
    txdata = 0x00 ; /* dim LED! */
    max44000->setTxConfig(txdata) ;
    tty->locate(1, 20) ;
    delete mpl3115a2 ;
    delete max44000 ;
    delete mma8452q ;
    delete hih6130 ;
}

void testLM75B(void)
{
    int8_t itemp = 0 ;
    float ftemp = 0.0 ;
    printf("test LM75B\n") ;
    LM75B *lm75b = new LM75B(PIN_SDA, PIN_SCL, MSU_LM75B_ADDRESS) ;
    for (int i = 0 ; i < test_loop ; i++ ) {
        itemp = lm75b->temp() ;
        lm75b->getTemp(&ftemp) ;
        printf("Temp = %d C degree,  %.3f C degree\n", itemp, ftemp) ;
        wait_ms(interval) ;
    }
    delete lm75b ;
}

void testFXAS21002(void)
{
    int16_t data[3] ;
    
    printf("test FXAS21002\n") ;
    FXAS21002 *fxas = new FXAS21002(PIN_SDA, PIN_SCL, MSU_FXAS21002_ADDRESS) ;
    fxas->activate(true) ;
    
    for (int i = 0 ; i < test_loop ; i++ ) {
        data[0] = fxas->getX() ;
        data[1] = fxas->getY() ;
        data[2] = fxas->getZ() ;
        printf("X[%6d], Y[%6d], Z[%6d]\n",
            data[0], data[1], data[2]) ;
        wait_ms(interval) ;
    }
    delete fxas ;
}
    
void testMMA8451Q(void)   
{
    uint16_t ix, iy, iz ;
    float fx, fy, fz ;
    
    printf("test MMA8451Q\n") ;
    MMA8451Q *acc = new MMA8451Q(PIN_SDA, PIN_SCL, MSU_MMA8451Q_ADDRESS) ;
    for(int i = 0 ; i < test_loop ; i++) {
        ix = acc->getRawX() ;   
        iy = acc->getRawY() ;
        iz = acc->getRawZ() ;
        fx = acc->getAccX() ;
        fy = acc->getAccY() ;
        fz = acc->getAccZ() ;
        printf("X = %d [ %.2f ], Y = %d [ %.2f ], Z = %d [ %.2f ]\n",
            ix, fx, iy, fy, iz, fz ) ;
        wait_ms(interval) ;
    }
    delete acc ;
}

void testMMA8452Q(void)   
{
    float x, y, z ;
    printf("test MMA8452Q\n") ; 
    MMA8452Q *acc = new MMA8452Q(PIN_SDA, PIN_SCL, MSU_MMA8452Q_ADDRESS) ;
    for(int i = 0 ; i < test_loop ; i++) { 
         x = acc->getAccX() ;
         y = acc->getAccY() ;
         z = acc->getAccZ() ;
         printf("X[%.2f] Y[%.2f] Z[%.2f]\n",x, y, z) ;
        wait_ms(interval) ;
    }
    delete acc ;
}

void testMAG3110(void)    
{
    uint8_t status = 0 ;
    int16_t mX, mY, mZ ;
    float fx, fy, fz ;
    int x = 10 ;
    int y = 5 ;
    MAG3110 *mag = new MAG3110(PIN_SDA, PIN_SCL, MSU_MAG3110_ADDRESS) ;
    mag->activate() ;
    tty->cls() ;
    printf("test MAG3110\n") ; 
    for (int i = 0 ; i < test_loop ; i++) {
    wait_ms(interval) ;
    status = mag->getStatus() ;
    while((status & 0x80) == 0) { // wait for ZYXOW
       wait(0.01) ;
       status = mag->getStatus() ;
    }

    tty->locate(x+5, y) ;
    printf("--- MAG3110 ---") ;
    tty->locate(x, y+4) ;
    printf("Temperature: %02X",mag->getTemp()) ;
    tty->locate(x, y+5) ;
    printf("status: 0x%02X",mag->getStatus()) ;
    tty->locate(x, y+6) ;
    printf("ctrl reg1: 0x%02X ", mag->getCtrlReg1()) ;
    printf("reg2: 0x%02X", mag->getCtrlReg2()) ;
    mX = mag->getX() ; // here reset takes place!
    mY = mag->getY() ;
    mZ = mag->getZ() ;
    fx = 10000 * (float)mX / (float)0x7FFF ;
    fy = 10000 * (float)mY / (float)0x7FFF ;
    fz = 10000 * (float)mZ / (float)0x7FFF ;
    tty->locate(x, y+1) ;
    printf("X : ") ;
    if (fx >= 0) { printf(" ") ; }
    printf("%.2f uT : 0x%08X     ", fx, mX) ;

    tty->locate(x, y+2) ;
    printf("Y : ") ;
    if (fy >= 0) { printf(" ") ; }
    printf("%.2f uT : 0x%08X     ", fy, mY) ;
    
    tty->locate(x, y+3) ;
    printf("Z : ") ;
    if (fz >= 0) { printf(" ") ; }  
    printf("%.2f uT : 0x%08X     ", fz, mZ) ;
    }
    tty->locate(0, 20) ;
    mag->standby() ;
    delete mag ;
}

void testHIH6130(void)    
{       
    uint16_t result = 0 ;
    float humidity = 0 ;
    float temperature = 0 ;
    HIH6130 *hih = new HIH6130(PIN_SDA, PIN_SCL, MSU_HIH6130_ADDRESS) ;
    printf("test HIH6130\n") ; 
    for (int i = 0 ; i < test_loop ; i++) {
        result = hih->getValue(&humidity, &temperature) ;
        printf("Temp %.1f C   Humidity %.1f %%\n", temperature, humidity) ;
        wait_ms(interval) ;
    }
}

void testFXOS8700CQ(void) 
{
    int16_t accData[3], mgnData[3] ;
    
    printf("test FXOS8700CQ\n") ; 
    FXOS8700CQ *fxos = new FXOS8700CQ(PIN_SDA, PIN_SCL, MSU_FXOS8700CQ_ADDRESS) ;
    fxos->init() ;
    
    for (int i = 0 ; i < test_loop ; i++) {
        fxos->readAccMgnData(accData, mgnData) ;
        printf("ACC X[%d], Y[%d], Z[%d],  MGN X[%d], Y[%d], Z[%d]\n",
            accData[0], accData[1], accData[2],
            mgnData[0], mgnData[1], mgnData[2]) ;
        wait_ms(interval) ;
    }
    delete fxos ;
}

void testMAX44000(void)   
{
  uint8_t mode ;
  uint8_t txdata, rxdata ;
  uint16_t als ;
  uint8_t prox, overflow ;
  int x = 10 ;
  int y = 5 ;
  
  tty->cls() ;
  printf("test MAX44000\n") ; 
  MAX44000 *max44000 = new MAX44000(PIN_SDA, PIN_SCL, MSU_MAX44000_ADDRESS) ; 
  
  rxdata = 0xF1 ;
  max44000->setRxConfig(rxdata) ;
  txdata = 0x0F ; /* light LED! */
  max44000->setTxConfig(txdata) ;
  mode = 0x30 ; // ALS & PROX mode
  max44000->setMainConfig(mode) ;
  
  for (int i = 0 ; i < test_loop ; i++ ) {
    max44000->getADC(&overflow, &als, &prox) ;
    printf("ALS[ %04X ]", als) ;
    if (overflow) {
        printf(" overflow ") ;
    } else {
        printf("          ") ;
    }
    printf("PRX[ %02X ]\n", prox) ;
    wait_ms(interval) ;
  }
  txdata = 0x00 ; /* dim LED! */
  max44000->setTxConfig(txdata) ;
  delete max44000 ;
}

void testMAX44005(void)   
{
    uint16_t amb_c, amb_r, amb_g, amb_b, amb_ir, amb_ircomp, amb_temp ;
    MAX44005  *max44005 = new MAX44005(PIN_SDA, PIN_SCL, MSU_MAX44005_ADDRESS) ;
    max44005->enableTEMP() ;
    max44005->setMode(0x04) ;
    printf("test MAX44005\n") ; 
    
    for (int i = 0 ; i < test_loop ; i++ ) {
        amb_c = max44005->getAMB_CLEAR() ;
        amb_r = max44005->getAMB_RED() ;
        amb_g = max44005->getAMB_GREEN() ;
        amb_b = max44005->getAMB_BLUE() ;
        amb_ir = max44005->getIR() ;
        amb_ircomp = max44005->getIRCOMP() ;
        amb_temp = max44005->getTEMP() ;
        printf("--- MAX44005 ---\n") ;
        printf("   C[ %04X ]\n", amb_c) ;
        printf("R[ %04X ] G[ %04X ] B[ %04X ]\n",amb_r, amb_g, amb_b) ;
        printf("IR[ %04X ] IRCOMP [ %04X ]\n",amb_ir, amb_ircomp) ;
        printf("   TEMP [ %04X ]\n",amb_temp) ;
        printf("\n") ;
        wait_ms(interval) ;
    }
    delete max44005 ;
}

#define MODE_CLEAR_RGB_IR     0x02
void testMAX44008(void)   
{
    uint16_t amb_c, amb_r, amb_g, amb_b, amb_ir, amb_ircomp, amb_temp ;
    
    MAX44008 *max44008 = new MAX44008(PIN_SDA, PIN_SCL, MSU_MAX44008_ADDRESS) ;
    max44008->setMode(MODE_CLEAR_RGB_IR) ;
        printf("test MAX44008\n") ; 
        
    for (int i = 0  ; i < test_loop ; i++ ) {
        amb_c = max44008->getAMB_CLEAR() ;
        amb_r = max44008->getAMB_RED() ;
        amb_g = max44008->getAMB_GREEN() ;
        amb_b = max44008->getAMB_BLUE() ;
        amb_ir = max44008->getIR() ;
        amb_ircomp = max44008->getIRCOMP() ;
        amb_temp = max44008->getTEMP() ;
        printf("--- MAX44008 ---\n") ;
        printf("   C[ %04X ]\n", amb_c) ;
        printf("R[ %04X ] G[ %04X ] B[ %04X ]\n",amb_r, amb_g, amb_b) ;
        printf("IR[ %04X ] IRCOMP [ %04X ]\n",amb_ir, amb_ircomp) ;
        printf("   TEMP [ %04X ]\n",amb_temp) ;
        printf("\n") ;
        wait_ms(interval) ;
    }
    delete max44008 ;
}

void testMPL3115A2(void)  
{
    uint32_t data, alt, bar,  tmp ;
    uint16_t sample_time ;
    MPL3115A2 *mpl = new MPL3115A2(PIN_SDA, PIN_SCL, MSU_MPL3115A2_ADDRESS) ;
    
    printf("test MPL3115A2\n") ; 
    for (int i = 0 ; i < test_loop ; i++) {
        mpl->modeAlt() ;
        sample_time = mpl->OneShot() ;
        wait(((double)sample_time)/1000.0) ;
        alt = mpl->getAltitude() ;
        mpl->modeBar() ;
        sample_time = mpl->OneShot() ;
        wait(((double)sample_time)/1000.0) ;
        bar = mpl->getPressure() ;
        tmp = mpl->getTemperature() ;
        printf("Altitude: %d, Pressure: %d, Temperature: %d\n",
            (alt >> 16)&0xFFFF,
            (bar >> 6),
            (tmp >> 8) ) ; 
        wait_ms(interval) ;
    }
    delete mpl ;
}

void testIS31SE5000(void) 
{
    printf("test ISI31SE5000\n") ; 
    printf("Sorry not implemented yet\n") ;
}

void testVEML6040A(void)  
{
    uint16_t uR, uG, uB, uW ;
    VEML6040 *veml = new VEML6040(PIN_SDA, PIN_SCL, MSU_VEML6040A_ADDRESS) ;

    for (int i = 0 ; i < test_loop ; i++ ) {
        veml->setCOLORConf(0x04) ; /* one time trigger, other wise write 0 */
        wait(0.1) ;
        veml->getRData(&uR) ; 
        veml->getGData(&uG) ;
        veml->getBData(&uB) ;
        veml->getWData(&uW) ;
        printf("VEML6040A R[%d], G[%d], B[%d], W[%d]\n", uR, uG, uB, uW) ;
        wait_ms(interval) ;
    }
    delete veml ;
}

void testVEML6075(void)  
{
    float uvi ;
    float uva, uvb, uva_cie, uvb_cie ;
    VEML6075 *veml = new VEML6075(PIN_SDA, PIN_SCL, MSU_VEML6075_ADDRESS) ;
    
    veml->setUVConf(0x00) ;
    for (int i = 0 ; i < test_loop ; i++ ) {
        uvi = veml->UVI() ;
        uva = veml->getUVA() ;
        uva_cie = veml->getUVA_CIE() ;
        uvb = veml->getUVB() ;
        uvb_cie = veml->getUVB_CIE() ;

        printf("VEML6075 UVI[%.4f] UVA[%.4f] UVA_CIE[%.4f] UVB[%.4f] UVB_CIE[%.4f]\n",
        uvi, uva, uva_cie, uvb, uvb_cie) ;
        wait_ms(interval) ;
    }
    delete veml ;
}

void testVCNL4100(void)
{
    uint8_t psData = 0x00 ;
    uint16_t alsData = 0x00 ;
    VCNL4100 *vcnl4100 = 0 ;
    
    vcnl4100 = new VCNL4100(PIN_SDA, PIN_SCL, MSU_VCNL4100_I2C_ADDRESS) ;
    
    printf("=== test VCNL4100 for %s (%s)\n", BOARD_NAME, __DATE__) ;
    printf("Proximity, Ambient Light\n") ;
    
    vcnl4100->setAlsConf(0x00) ;
    vcnl4100->setPsConf12(0x0000) ;
    vcnl4100->setSpo(0xA0) ;
    wait(0.1) ;
    
    for (int i = 0 ; i < test_loop ; i++ ) {
        psData = vcnl4100->getPsData() ;
        alsData = vcnl4100->getAlsData() ;
        printf("0x%02X, 0x%04X\n", psData, alsData) ;
        wait(1);
    }
    delete vcnl4100 ;
}


void testS11059(void) 
{
    uint16_t uR, uG, uB, uIR ;
    uint8_t ctrl ;
    S11059 *s11059 = new S11059(PIN_SDA, PIN_SCL, MSU_S11059_ADDRESS) ;
    
    for (int i = 0 ; i < test_loop ; i++ ) {
        ctrl = 0x89 ; /* ADC reset, High Gain, integration time 1.4ms */
        s11059->setControl(ctrl) ;
        ctrl = 0x09 ; /* Release ADC reset, High Gain, integration time 1.4ms */
        s11059->setControl(ctrl) ; /* start measure */
        wait(0.02) ;
        s11059->getRData(&uR) ;
        s11059->getGData(&uG) ;
        s11059->getBData(&uB) ;
        s11059->getIRData(&uIR) ;
        printf("S11059 R[%d], G[%d], B[%d], IR[%d]\n", uR, uG, uB, uIR) ;
        wait_ms(interval) ;
    }
    delete s11059 ;
}

void init_max30101(void)
{
    uint32_t slot_config = 0 ;
    uint8_t mode ;

    max30101 = new MAX30101(PIN_SDA, PIN_SCL, MAX30101_I2C_ADDRESS) ;
    max30101->reset() ;
    
    /* config for Mode Configuration (0x09) */

    mode = 0x07 ; /* Green, Red, and/or IR */
    // mode = 0x03 ; /* SpO2 mode */
    max30101->setMODE_CONFIG(mode) ;

    /* config for FIFO Configuration (0x08) */
    mode =
 //         (0x02  << 5) /* SMP_AVE = 2 -> avarage of 4 data */
          (0x05 << 5) /* SMP_AVE = 5 -> avarage of 32 data */
//        | (0x01  << 4)  /* FIFO_ROLLOVER_EN = 1 */
        | (0x00 << 4)  /* FIFO_ROLLOVER_EN = 0 */
        | (0x0C)        /* FIFO ALMOST FULL at 12 (0x0C) */
        ;
    max30101->setFIFO_CONFIG(mode) ;
    
    max30101->setLED1_PA(0x80) ;
    max30101->setLED2_PA(0x80) ;
//    max30101->setLED3_PA(0x80) ;
//    max30101->setPILOT_PA(0x80) ;
#if 0
    slot_config = 
          (0x02 << 24) /* SLOT2 LED2(IR) */
        | (0x01 << 16) /* SLOT1 LED1(RED) */
        | (0x00 << 8)  /* SLOT4 none */
        | (0x03)       /* SLOT3 LED3(GREEN) */
        ;
    max30101->setSLOT(slot_config) ;
#endif
    mode = 0x01 ;
    max30101->setPROX_INT_THR(mode) ;
}

void readSPO2_FIFO(void)
{
    uint32_t data[2] ;
    uint8_t fifo_wr_ptr, fifo_rd_ptr ;
    int num_available_samples = 0 ;
    int num_samples_to_read = 6 ;
    int i ;

    fifo_wr_ptr = max30101->getFIFO_WR_PTR() ;
    fifo_rd_ptr = max30101->getFIFO_RD_PTR() ;
    num_available_samples = (fifo_wr_ptr + FIFO_DEPTH - fifo_rd_ptr) % FIFO_DEPTH ;

    if (num_available_samples < num_samples_to_read) {
        num_samples_to_read = num_available_samples / 2 ;
    } else {
        num_samples_to_read = num_available_samples ;
    }
    printf("=== %d data %d to read===\n", 
    num_available_samples, num_samples_to_read) ;
    for (i = 0 ; i < num_samples_to_read ; i++ ) {
        data[0] = max30101->readFIFO() ; /* LED1 */
        data[1] = max30101->readFIFO() ; /* IR */
//        printf("LED1: 0x%05X, IR: 0x%05X\n", data[0], data[1]) ;
    }
//    max30101->setFIFO_WR_PTR(0) ;
//    max30101->setFIFO_RD_PTR(0) ;
}

void SPO2_isr(void)
{
    uint16_t flag, config ;
    float temp ;
    int temp_int, temp_frac ;

    flag = max30101->getIntStatus() ;
    config = max30101->getIntEnable() ;
//    printf("Int: 0x%04X Enable: 0x%04X\n", flag, config) ;
    if (flag & INT_ST_A_FULL) { /* A_FULL_EN */
//        printf("FIFO almost full!\n") ;
        readSPO2_FIFO() ;
    }
    if (flag & INT_ST_PPG_RGY) { /* New FIFO Data Ready */
        printf("New FIFO Data Ready\n") ;
    }
    if (flag & INT_ST_ALC_OVF) { /* Ambient Light Cancellaration Overflow */
        printf("Ambient Light Cancellaration Overflow\n") ;
    }
    if (flag & INT_ST_PROX_INT) {/* Proximity Threshold Triggered */
        printf("Proximity Threshold Triggered\n") ;
    }
    if (flag & INT_ST_PWR_RDY) {/* Power Ready Flag */
        printf("Power Ready!\n") ;
    }
    if (flag & INT_ST_DIE_TEMP_RDY) {/* Internal Temperature Ready Flag */
        printf("DIE Temperature Ready!\n") ;
        temp_int = max30101->getTEMP_INT() ;
        temp_frac = max30101->getTEMP_FRAC() ;
        temp = ((float)temp_int)+(((float)temp_frac)/16.0) ;
        printf("Temp: %.2f\n", temp) ;
    }
}

void initSPO2(void)
{
    uint8_t config_byte = 0 ;
    uint16_t slot_config = 0 ;
    
    /* config for SPO2 (0x0A) */
    config_byte =
          (0x01 << 5)  /* SPO2_ADC_RGE */
 //       | (0x05 << 3)  /* SPO2_SR 101 = 1000 samples/sec */
        | (0x00 << 3) /* SPO2_SR 000 = 50 samples/sec */
        | (0x00)       /* LED_PW 00 = 69us */
        ;
    max30101->setSPO2_CONFIG(config_byte) ;
    
    /* slot configuration */
    slot_config = 
          (0x02 << 12) /* SLOT2 LED2(IR) */
        | (0x01 << 8)  /* SLOT1 LED1(RED) */
        | (0x00 << 4)  /* SLOT4 none */
        | (0x00)       /* SLOT3 none */
        ;
    max30101->setSLOT(slot_config) ;
    
    /* Inititalize FIFO */
        /* config for FIFO Configuration (0x08) */
    config_byte =
          (0x02  << 5) /* SMP_AVE = 2 -> avarage of 4 data */
        | (0x01  << 4)  /* FIFO_ROLLOVER_EN = 1 */
//        | (0x0C)        /* FIFO ALMOST FULL at 12 (0x0C) */
        | (0x09)          /* FIFO ALMOST FULL at 9 */
        ;
    max30101->setFIFO_CONFIG(config_byte) ;
    

}

void testMAX30101(void)
{
    uint16_t int_config = 0 ;
    uint8_t flag = 0 ;
    float temp = 0.0 ;
    uint8_t id, rev ;
    InterruptIn *int0 ;
    MAX30101 *max30101 ;

    int0 = new InterruptIn(PIN_INT1) ;
        
    max30101 = new MAX30101(PIN_SDA, PIN_SCL, MSU_MAX30101_ADDRESS) ;

    init_max30101() ;
    id = max30101->getID() ;
    rev = max30101->getRev() ;
    printf("MAX30101 ID: 0x%02X, Rev: 0x%02X\n", id, rev) ;

    initSPO2() ;
    int0->fall(&SPO2_isr) ;
    max30101->setMODE_CONFIG(MODE_SPO2) ; 
    max30101->setIntEnable( INT_EN_A_FULL | INT_EN_DIE_TEMP_RDY ) ;
    int_config = max30101->getIntEnable() ;
    printf("Int enable: 0x%04X\n", int_config) ;
    flag = max30101->getIntStatus() ;

    for (int i = 0 ; i < test_loop ; i++ ) {
      max30101->setTEMP_EN() ; /* trigger temperature read */
        temp = max30101->getTEMP() ;
        flag = max30101->getIntStatus() ;
        printf("Temp = %.2f  interrupt flags = 0x%02X\n", temp, flag) ;
      wait(1) ;
    }
    
    delete max30101 ;
    delete int0 ;
}

void testAK9752(void) 
{
    int companyID, deviceID ;
    float temp, ir ;
    AK9752 *ak9752 = 0 ;
    ak9752 = new AK9752(PIN_SDA, PIN_SCL, MSU_AK9752_I2C_ADDRESS) ;
    companyID = ak9752->getCompanyCode() ;
    deviceID = ak9752->getDeviceID() ;
    printf("=== AK9752 ===\n") ;
    printf("Company Code = 0x%02X [should be 0x48]\n", companyID) ;
    printf("Device ID = 0x%02X [should be 0x14]\n", deviceID) ;
    printf("Temp  IR\n") ;
    for (int i = 0 ; i < test_loop ; i++ ) {
        ak9752->setCNTL2(0xFE) ; /* single shot */
        while(ak9752->dataReady() != true) {
            ;
        }
        ir = ak9752->getIR() ;
        temp = ak9752->getTMP() ;
        ak9752->dataOverRun() ; /* clear data ready */
        printf("%.2f %.2f\n", temp, ir) ;
        wait(0.1) ;
    }
    delete ak9752 ;
}

void testAK9753(void) 
{
    int companyID, deviceID ;
    float temp, ir[4] ;
    AK9753 *ak9753 = 0 ;
    ak9753 = new AK9753(PIN_SDA, PIN_SCL, MSU_AK9753_I2C_ADDRESS) ;
    ak9753->software_reset() ;
    companyID = ak9753->getCompanyCode() ;
    deviceID = ak9753->getDeviceID() ;
    printf("=== AK9753 ===\n") ;
    printf("Company Code = 0x%02X [should be 0x48]\n", companyID) ;
    printf("Device ID = 0x%02X [should be 0x13]\n", deviceID) ;
    printf("Temp  IR1  IR2  IR3  IR4\n") ;
    ak9753->setEINTEN(0x01) ; 
    for (int i = 0 ; i < test_loop ; i++ ) {
        ak9753->setECNTL1(0xAA) ; /* single shot */
        while(ak9753->dataReady() != true) {
            ;
        }
        temp = ak9753->getTMP() ;
        ir[0] = ak9753->getIR1() ;
        ir[1] = ak9753->getIR2() ;
        ir[2] = ak9753->getIR3() ;
        ir[3] = ak9753->getIR4() ;
        ak9753->dataOverRun() ; /* clear data ready */
        printf("%.2f  %.2f %.2f %.2f %.2f\n", 
            temp, ir[0], ir[1], ir[2], ir[3]) ;
        wait(0.2) ;
    }
    delete ak9753 ;
}

void testAK09970N(void) 
{
    uint16_t companyID, deviceID ;
    uint16_t status ;
    float  x, y, z ;
    int result ;

    AK09970N *ak09970n = 0 ;
    ak09970n = new AK09970N(PIN_SDA, PIN_SCL, MSU_AK09970N_I2C_ADDRESS) ;
    ak09970n->software_reset() ;
    ak09970n->getID(&companyID, &deviceID) ;
    printf("=== AK09970N ===\n") ;
    printf("Company ID = 0x%02X (expected 0x48)\n", companyID) ;
    printf("Device ID = 0x%02X (expected 0xC0)\n", deviceID) ;
    printf(" X        Y      Z\n") ;
//    status = ak09970n->getStatus() ;
//    printf("Status = 0x%04X\n", status) ;

    for (int i = 0 ; i < test_loop ; i++ ) {
//        result = ak09970n->setConfig(0x08) ; /* 10Hz continuous mode */
        result = ak09970n->singleShot() ;
        status = ak09970n->getStatus() ;
        while((status & 0x01) != 0x01) { /* wait for data ready */
            wait_ms(10) ;
            status = ak09970n->getStatus() ;
        }
        ak09970n->getX_Y_Z(&status, &x, &y, &z) ;
        printf("%2.4f %2.4f %2.4f\n", x, y, z) ;
       wait_ms(100) ;
    }
    delete ak09970n ;
}