Diff: I2cPeripherals/I2cPeripherals.cpp

Revision:

4:4060309b9cc0

Parent:

3:27407c4984cf

Child:

7:16bf0085d914

--- a/I2cPeripherals/I2cPeripherals.cpp	Thu Feb 13 16:07:07 2014 +0000
+++ b/I2cPeripherals/I2cPeripherals.cpp	Tue Oct 14 08:15:03 2014 +0000
@@ -11,6 +11,9 @@
     Gyro_addr = 0;
     Accel_addr = 0;
     barometor_addr = 0;
+    ultrasonic_addr = 0;
+    ultrasonic_distance = 0;
+//pc.printf("start");
     find();
     start();
 
@@ -71,26 +74,28 @@
 
 void I2cPeripherals::find()
 {
+#define sensnum 11
     char tx[2];
     char rx[1];
-    SensorInf sens[10] = { 0xD0,0x68,0x00,      //ITG3200
+    SensorInf sens[sensnum] = { 0xD0,0x68,0x00,      //ITG3200   gyro
                            0xD2,0x68,0x00,
-                           0xD0,0x68,0x75,     //MPU6050
+                           0xD0,0x68,0x75,     //MPU6050    gyro/accel
                            0xD2,0x68,0x75,
-                           0xD4,0xD4,0x0F,    //L3GD20
+                           0xD4,0xD4,0x0F,    //L3GD20  gyro
                            0xD6,0xD4,0x0F,
-                           0xA6,0xE5,0x00,     //ADXL345
+                           0xA6,0xE5,0x00,     //ADXL345    accel
                            0x3A,0xE5,0x00,
-                           0xB8,0xBB,0x0F,     //LPS331
-                           0xBA,0xBB,0x0F
-                         };
+                           0xB8,0xBB,0x0F,     //LPS331 baro
+                           0xBA,0xBB,0x0F,
+                           0xE0,0x18,0x01       //SRF02  ultrasonic
+    };
 
-    for ( int num = 0; num < 10; num++ )    {
+    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
@@ -185,6 +190,14 @@
                 wait(1);
                 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;
         }
     }
 }
@@ -205,7 +218,7 @@
     return -1;
 }
 
-void I2cPeripherals::write_reg(char I2cAddr,char reg_addr,char* data,int len)
+void I2cPeripherals::write_reg(int I2cAddr,char reg_addr,char* data,int len)
 {
     char tx[17];
     if ( len >16 ) len = 16;
@@ -215,19 +228,45 @@
     _i2c.write(I2cAddr,tx,len+1);
 }
 
-void I2cPeripherals::read_reg(char I2cAddr,char reg_addr,char* data,int len)
+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)
@@ -238,7 +277,7 @@
     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)
@@ -276,7 +315,7 @@
     tx[0] = Accel_data | 0x80;
     if ( _i2c.write(Accel_addr,tx,1,true) != 0 )     {
         *x=*y=0;
-        *z=9.8;
+        *z=1;
         return false;
     }
     _i2c.read(Accel_addr,rx,6);
@@ -285,8 +324,10 @@
         case 0xD2:
 #ifdef MPU6050
             lsb = 0.000488;     //16g
-//            lsb = 0.000122;       //4g
-            *y = ( -(short(rx[0] << 8 | (uint8_t)rx[1])) ) * lsb;
+//            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
@@ -294,9 +335,10 @@
         case 0xA6:
         case 0x3A:
 #ifdef ADXL345
-            *y = ( -(short(rx[1] << 8 | (uint8_t)rx[0])) ) * 0.04;
-            *x = ( short(rx[3] << 8 | (uint8_t)rx[2]) ) * 0.04;
-            *z = ( short(rx[5] << 8 | (uint8_t)rx[4]) ) * 0.04;
+            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
     }
@@ -332,6 +374,40 @@
     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];
@@ -355,6 +431,8 @@
         return LCD_addr;
     case BARO_ADDR:
         return barometor_addr;
+    case ULTRASONIC_ADDR:
+        return ultrasonic_addr;
     }
     return 0;        
 }
@@ -364,3 +442,6 @@
 
 
 
+
+
+