File content as of revision 8:1db19b529b22:

#include "mbed.h"
#include "I2cPeripherals.h"
void wait(float);
//Serial pc(USBTX, USBRX);
I2cPeripherals::I2cPeripherals(PinName sda, PinName scl): _i2c(sda,scl)
{

    _i2c.frequency(400000);
//    LCD_contrast = 60;
    wait(0.5);
    LCD_addr = 0;
    Gyro_addr = 0;
    Accel_addr = 0;
    barometor_addr = 0;
    ultrasonic_addr = 0;
    ultrasonic_distance = 0;
//pc.printf("start");
    find();
    start(50);

}

void I2cPeripherals::start(int contrast)
{
    char tx[2];
//    find();             // I2C interface sensor detect
#ifdef ST7032
    LCD_addr = 0x7c;
    LCD_data = 0x40;
    tx[0] = 0x00;
    tx[1] = 0x38;
    if ( _i2c.write(LCD_addr,tx,2) == 0 )    {
        tx[1] = 0x39;
        _i2c.write(LCD_addr,tx,2);
        tx[1] = 0x14;       //1F
        _i2c.write(LCD_addr,tx,2);
        tx[1] = 0x70 | (contrast & 0x0F);
        _i2c.write(LCD_addr,tx,2);
        tx[1] = 0x5C | ((contrast >> 4) & 0x03);
        _i2c.write(LCD_addr,tx,2);
        tx[1] = 0x6C;
        _i2c.write(LCD_addr,tx,2);
        wait(0.3);
        tx[1] = 0x0C;
        _i2c.write(LCD_addr,tx,2); // display on
        tx[1] = 0x06;
        _i2c.write(LCD_addr,tx,2); 
 //       tx[0] = 0x40;
//        tx[1] = '*';
//        _i2c.write(LCD_addr,tx,2); // Display clear
        return;
    }
#endif
#ifdef ACM1602
    LCD_addr = 0xA0;
    LCD_data = 0x80;
    tx[0] = 0x00;
    tx[1] = 0x01;
    if ( _i2c.write(LCD_addr,tx,2) == 0 )  {
        wait(0.005);
        tx[1] = 0x38;
        _i2c.write(LCD_addr,tx,2);
        wait(0.005);
        tx[1] = 0x0F;
        _i2c.write(LCD_addr,tx,2);
        wait(0.005);
        tx[1] = 0x06;
        _i2c.write(LCD_addr,tx,2);
        wait(0.005);
        return;
    }
#endif
    LCD_addr = 0;
}

void I2cPeripherals::find()
{
#define sensnum 11
    char tx[2];
    char rx[1];
    SensorInf sens[sensnum] = { 0xD0,0x68,0x00,      //ITG3200   gyro
                           0xD2,0x68,0x00,
                           0xD0,0x68,0x75,     //MPU6050    gyro/accel
                           0xD2,0x68,0x75,
                           0xD4,0xD4,0x0F,    //L3GD20  gyro
                           0xD6,0xD4,0x0F,
                           0xA6,0xE5,0x00,     //ADXL345    accel
                           0x3A,0xE5,0x00,
                           0xB8,0xBB,0x0F,     //LPS331 baro
                           0xBA,0xBB,0x0F,
                           0xE0,0x18,0x01       //SRF02  ultrasonic
    };

    for ( int num = 0; num < sensnum; num++ )    {
        tx[0]= sens[num].whoaddr;
        if ( _i2c.write(sens[num].addr,tx,1,true) != 0 ) continue;
        _i2c.read (sens[num].addr,rx,1);
        if ( (rx[0]&0x7E) != (sens[num].who&0x7E) ) continue;
//pc.printf("who=%2x",rx[0]);
        switch ( num )   {
#ifdef ITG3200
            case 0:     //ITG3200
            case 1:
                Gyro_addr = sens[num].addr;
                Gyro_data = 0x1D;
                tx[0] = 0x16;
                tx[1] = 0x18;    // 0x1D
                _i2c.write(sens[num].addr,tx,2);
                tx[0] = 0x3E;
                tx[1] = 0x01;
                _i2c.write(sens[num].addr,tx,2);
                wait(0.001f);
                break;
#endif
#ifdef MPU6050
            case 2:     //MPU6050
            case 3:
                Gyro_addr = sens[num].addr;
                Gyro_data = 0x43;
                Accel_addr = sens[num].addr;
                Accel_data = 0x3B;
                tx[0] = 0x6B;
                tx[1] = 0x00;       // PWR on
                _i2c.write(sens[num].addr,tx,2);
                tx[0] = 0x1A;       // DLPF(Digitel low pass filter)
                tx[1] = 0x02;       // set 0 to 7
                _i2c.write(sens[num].addr,tx,2);
                wait(0.001);
                tx[0] = 0x1B;
                tx[1] = 0x18;       // +-2000deg
                _i2c.write(sens[num].addr,tx,2);
                wait(0.001);
                tx[0] = 0x1C;
                tx[1] = 0x18;       // 00:2g,08:4g,10:8g,18:+-16g
                _i2c.write(sens[num].addr,tx,2);
                wait(0.001);
                break;
#endif
#ifdef L3GD20
            case 4:     //L3GD20
            case 5:
                Gyro_addr = sens[num].addr;
                Gyro_data = 0x28;
                tx[0] = 0x20;
                tx[1] = 0xBF;               //rate 400Hz 0x8F-0xBF
                _i2c.write(sens[num].addr,tx,2);
                tx[0] = 0x21;
                tx[1] = 0x09;
                _i2c.write(sens[num].addr,tx,2);
                tx[0] = 0x23;
                tx[1] = 0xF0;
                _i2c.write(sens[num].addr,tx,2);
                tx[0] = 0x24;
                tx[1] = 0x10;
                _i2c.write(sens[num].addr,tx,2);
                break;
#endif
#ifdef ADXL345
            case 6:      //ADXL345
            case 7:
                Accel_addr = sens[num].addr;
                Accel_data = 0x32;
                tx[0] = 0x2D;
                tx[1] = 0x00;
                _i2c.write(sens[num].addr,tx,2);
                tx[0] = 0x31;
                tx[1] = 0x0B;       //full range   08:2g 09:4g 0A:8g 0B:16g
                _i2c.write(sens[num].addr,tx,2);
                tx[0] = 0x2C;
                tx[1] = 0x09;       //out rate  0xC:400Hz  0xD:800Hz
                _i2c.write(sens[num].addr,tx,2);
                tx[0] = 0x2D;
                tx[1] = 0x08;
                _i2c.write(sens[num].addr,tx,2);
                break;
#endif
#ifdef LPS331AP
            case 8:        //barometor
            case 9:
                barometor_addr = sens[num].addr;
                barometor_data = 0x2B;
                tx[0] = 0x10;    //RES_CNF
                tx[1] = 0x7A;
                _i2c.write(sens[num].addr,tx,2);
                tx[0] = 0x20;    // CTL_REG1
                tx[1] = 0xB0;    // power on
                _i2c.write(sens[num].addr,tx,2);
//            tx[0]= 0x21;    // CTL_REG2
//            tx[1]= 0x01;    // one shot start
//            _i2c.write(sens[num].addr,tx,2);
                wait(0.01);
                break;
#endif
            case 10:           //SFR02 ultrasonic
                ultrasonic_addr = sens[num].addr;
                ultrasonic_data = 0x02;
                tx[0] = 0x00;
                tx[1] = 0x52;
                _i2c.write(sens[num].addr,tx,2);
                wait(0.01);
                break;
        }
    }
}

//int I2cPeripherals::_putc(int value)
int I2cPeripherals::write_lcd(const char* value)
{
    if ( LCD_addr == 0 ) return -1;
    _i2c.start();
    _i2c.write(LCD_addr);
    _i2c.write(LCD_data);
    _i2c.write(LCD_addr,value,strlen(value));
    _i2c.stop();
    return 0;
}
/*
int I2cPeripherals::_getc()
{
    return -1;
}
*/
void I2cPeripherals::write_reg(int I2cAddr,char reg_addr,char* data,int len)
{
    char tx[17];
    if ( len >16 ) len = 16;
    tx[0] = reg_addr;
    if ( len > 1 ) tx[0] |= 0x80;
    for (int i=0; i<len; i++) tx[i+1] = data[i];
    _i2c.write(I2cAddr,tx,len+1);
}

void I2cPeripherals::read_reg(int I2cAddr,char reg_addr,char* data,int len)
{
    char tx[1];
    tx[0] = reg_addr | 0x80;
    _i2c.write(I2cAddr,tx,1,true);
    _i2c.read(I2cAddr,data,len);
}
int I2cPeripherals::write_EEPROM(short reg_addr,char* data,int len)
{
    char tx[3];
    int rc=0,i;
    int I2cAddr = I2C_EEPROM_ADDR;
    for (i=0; i<len; i++)  {
        tx[0] = reg_addr >> 8;
        tx[1] = reg_addr & 0xFF;
        tx[2] = data[i];
        rc = _i2c.write(I2cAddr,tx,3);
        if ( rc ) break;
        wait(0.01);
        reg_addr ++;
    }
    return rc;  
}

int I2cPeripherals::read_EEPROM(short reg_addr,char* data,int len)
{
    int I2cAddr = I2C_EEPROM_ADDR;
    int rc = _i2c.write(I2cAddr,(char*)& reg_addr,2,true);
    if ( rc ) return rc;
    rc = _i2c.read(I2cAddr,data,len);
    return rc;
}

void I2cPeripherals::cls()
{
    if ( LCD_addr == 0 ) return;
    char tx[2] = { 0x00, 0x01 };
    _i2c.write(LCD_addr,tx,2);
    wait(0.001);
}

void I2cPeripherals::locate(int clm,int row)
{
    char tx[2];

    if ( LCD_addr == 0 ) return;
    tx[0] = 0x00;
    tx[1] = 0x80 + (row * 0x40) + clm;
    _i2c.write(LCD_addr,tx,2);
    wait(0.001);
}

int I2cPeripherals::angular(float *x,float *y,float *z)
{
    char rx[6];
    char tx[1];
    float i = 0;
    tx[0] = Gyro_data | 0x80;
    if ( _i2c.write(Gyro_addr,tx,1,true) != 0 )     {
        *x=*y=*z=0;
        return Gyro_addr;
    }
    switch ( Gyro_addr )    {
        case 0xD0:
        case 0xD2:
            i = 0.06098;
            break;
        case 0xD4:
        case 0xD6:
            i = 0.06957;
    }
    _i2c.read(Gyro_addr,rx,6);
    *x = ( short(rx[0] << 8 | (uint8_t)rx[1]) ) * i;
    *y = ( short(rx[2] << 8 | (uint8_t)rx[3]) ) * i;
    *z = ( short(rx[4] << 8 | (uint8_t)rx[5]) ) * i;
    return true;
}

int  I2cPeripherals::Acceleration(float *x,float *y,float *z)
{
    //AXDL345 Data Read
    char rx[6];
    char tx[1];
    float lsb;
    tx[0] = Accel_data | 0x80;
    if ( _i2c.write(Accel_addr,tx,1,true) != 0 )     {
        *x=*y=0;
        *z=1;
        return false;
    }
    _i2c.read(Accel_addr,rx,6);
    switch ( Accel_addr )   {
        case 0xD0:
        case 0xD2:
#ifdef MPU6050
            lsb = 0.000488;     //16g
//            lsb = 0.000244;     //8g
//            lsb = 0.000122;     //4g
//            lsb = 0.000061;     //2g
            *y = ( -(short(rx[0] << 8 | (uint8_t)rx[1])) ) * lsb;//re
            *x = ( short(rx[2] << 8 | (uint8_t)rx[3]) ) * lsb;
            *z = ( short(rx[4] << 8 | (uint8_t)rx[5]) ) * lsb;
#endif
            break;
        case 0xA6:
        case 0x3A:
#ifdef ADXL345
            lsb = 0.004;
            *y = ( -(short(rx[1] << 8 | (uint8_t)rx[0])) ) * lsb;//re
            *x = ( short(rx[3] << 8 | (uint8_t)rx[2]) ) * lsb;
            *z = ( short(rx[5] << 8 | (uint8_t)rx[4]) ) * lsb;
            break;
#endif
    }
    return true;
}

int  I2cPeripherals::pressure()
{
    char tx[2];
    char rx[3];
    int press = 0;

    tx[0]= 0x28 | 0x80;
    if ( _i2c.write(barometor_addr,tx,1,true) != 0 )
        return 0;
    _i2c.read (barometor_addr,rx,3);
    press =int( rx[2]<<16 | rx[1]<<8 | rx[0] );
    return press;
}

int I2cPeripherals::temperature()
{
    char tx[1];
    char rx[2];
    int temp;

    tx[0]= 0x2B | 0x80;
    if ( _i2c.write(barometor_addr,tx,1,true) != 0 )
        return 0;
    _i2c.read (barometor_addr,rx,2);

    temp = short(rx[1]<<8 | rx[0]);
    return temp;
}

float I2cPeripherals::height_cm()
{
    return (float)ultrasonic(0x52)*0.0175f;
}
float I2cPeripherals::height_mm()
{
    return (float)ultrasonic(0x52)*0.175f;
}

float I2cPeripherals::height_us()
{
    return (float)ultrasonic(0x52)/1000000;
}

int I2cPeripherals::ultrasonic(char unit)
{
    char rx[2],tx[2];
    tx[0] = 0x00;
    if ( ultrasonic_addr == 0 ) return -1;
    _i2c.write(ultrasonic_addr,tx,1,true);
    _i2c.read (ultrasonic_addr,rx,1);
    if ( rx[0] != 0xFF )    {
        tx[0]= 0x02;
        _i2c.write(ultrasonic_addr,tx,1,true);
        _i2c.read (ultrasonic_addr,rx,2);
        ultrasonic_distance = short(rx[0] << 8 | (uint8_t)rx[1]);
        tx[0] = 0x00;
        tx[1] = unit;
        _i2c.write(ultrasonic_addr,tx,2);
    }            
    return ultrasonic_distance;

}

void I2cPeripherals::i2c_write(int i2caddr,char* tx,int len)
{
    char rx[1];
    rx[0] = 0;
    if ( i2caddr == 0 ) return;
     while( 1 )  {
        _i2c.write(i2caddr,tx,len);
        _i2c.write(i2caddr,tx,1,true);
        _i2c.read (i2caddr,rx,1);
        if ( tx[1] == rx[0] ) return;
    }
}

int I2cPeripherals::GetAddr(int type)   
{
    switch ( type ) {
    case GYRO_ADDR:
        return Gyro_addr;
    case ACCEL_ADDR:
        return Accel_addr;
    case LCD_ADDR:
        return LCD_addr;
    case BARO_ADDR:
        return barometor_addr;
    case ULTRASONIC_ADDR:
        return ultrasonic_addr;
    }
    return 0;        
}
;