Diff: IMU.cpp

Revision:

8:dc48ce79ad59

Parent:

5:c3caf8b83e6b

Child:

10:605b0bfadc2e

--- a/IMU.cpp	Thu Jan 15 07:46:43 2015 +0000
+++ b/IMU.cpp	Wed Jan 21 16:30:33 2015 +0000
@@ -60,17 +60,31 @@
 // CTRL_REG4: set to 0x08 = 0000 1000 --> Full Scale = +/- 2G & high resolution --> Sensitivity = 0.001G/digit.
     address = LSM303_A_ADDR; 
     tx[0] = LSM303_A_CTRL_REG1;
-    tx[1] = 0x77; //                    --> 400 Hz Data rate speed - p.24/42 of datasheet
+    tx[1] = 0x77; //              --> 400 Hz Data rate speed - p.24/42 of datasheet     //dla minIMU9-v2 303DLHC 
+    //tx[1] = 0x87; //                --> 400 Hz Data rate speed - p.24/42 of datasheet     //dla altIMU10-v3 303D    
     ack |= _i2c.write(address, tx, 2);
     ack |= _i2c.write(address, tx, 1);
-    ack |= _i2c.read(address+1, &rx, 1); if (rx != 0x77) ack |= 1;    
+    ack |= _i2c.read(address+1, &rx, 1); if (rx != 0x77) ack |= 1;
+    
+    
+    /*   
+    //zmiana czulosci 303D
+    tx[0] = LSM303_A_CTRL_REG2;
+    tx[1] = 0x00;       //  --> +/- 16G
+    //tx[1] = 0x20;       //  --> +/- 16G + anti-alias 50Hz
+    ack |= _i2c.write(address, tx ,2);
+    ack |= _i2c.write(address, tx, 1);
+    ack |= _i2c.read(address+1, &rx, 1); if (rx != 0x18) ack |= 1;*/
+    
+    
     tx[0] = LSM303_A_CTRL_REG4;
     tx[1] = 0x08; // 0000 1000 enable high resolution mode + selects default 2G scale. p.26/42
     //tx[1] = 0x18; // 0001 1000 enable high resolution mode + selects 4G scale.
+    //tx[1]= 0x28;  //0011 1000 high resolution +-8G
     //tx[1]= 0x38;  //0011 1000 high resolution +-16G
     ack |= _i2c.write(address, tx ,2);
     ack |= _i2c.write(address, tx, 1);
-    ack |= _i2c.read(address+1, &rx, 1); if (rx != 0x18) ack |= 1;    
+    ack |= _i2c.read(address+1, &rx, 1); if (rx != 0x18) ack |= 1;  
 //
 // 1.c enable LSM303 magnetometer and set operational mode:
 // CRA_REG is reset from 0x10 to 0x14 = 00010100 -->  30 Hz data output rate.
@@ -107,7 +121,7 @@
 /* filter parameters: assume 400 Hz sampling rare and 2nd orcer Butterworth filter with fc = 5Hz. */
     pi = 3.1415926536;
     //A = tan(pi*5/400); a = 1 + sqrt(2.0)*A + A*A;
-    A = tan(pi*2/200); a = 1 + sqrt(2.0)*A + A*A;
+    A = tan(pi*5/200); a = 1 + sqrt(2.0)*A + A*A;
     FF[1] = 2*(A*A-1)/a;
     FF[2] = (1-sqrt(2.0)*A+A*A)/a;
     FF[0] = (1+FF[1]+FF[2])/4;
@@ -129,9 +143,12 @@
          W[0] = (int16_t) (D[1] << 8 | D[0]); 
          W[1] = (int16_t) (D[3] << 8 | D[2]); 
          W[2] = (int16_t) (D[5] << 8 | D[4]); 
-         *(d+0) = (float) 0.971*(W[0]-L3GD20_biasX)*L3GD20_SENSITIVITY_250DPS*L3GD20_DPS_TO_RADS;
-         *(d+1) = (float) 0.998*(W[1]-L3GD20_biasY)*L3GD20_SENSITIVITY_250DPS*L3GD20_DPS_TO_RADS;
-         *(d+2) = (float) 1.002*(W[2]-L3GD20_biasZ)*L3GD20_SENSITIVITY_250DPS*L3GD20_DPS_TO_RADS;
+         //*(d+0) = (float) 0.971*(W[0]-L3GD20_biasX)*L3GD20_SENSITIVITY_250DPS*L3GD20_DPS_TO_RADS;
+         //*(d+1) = (float) 0.998*(W[1]-L3GD20_biasY)*L3GD20_SENSITIVITY_250DPS*L3GD20_DPS_TO_RADS;
+         //*(d+2) = (float) 1.002*(W[2]-L3GD20_biasZ)*L3GD20_SENSITIVITY_250DPS*L3GD20_DPS_TO_RADS;
+         *(d+0) = (float) 0.971*(W[0])*L3GD20_SENSITIVITY_250DPS*L3GD20_DPS_TO_RADS;
+         *(d+1) = (float) 0.998*(W[1])*L3GD20_SENSITIVITY_250DPS*L3GD20_DPS_TO_RADS;
+         *(d+2) = (float) 1.002*(W[2])*L3GD20_SENSITIVITY_250DPS*L3GD20_DPS_TO_RADS;
          
 // Accelerometer data are stored as 12 bit readings, left justified per axis.
 // The data needs to be shifted 4 digits to the right! This is not general, only for the A measurement.
@@ -141,9 +158,16 @@
          W[0] = ((int16_t) (D[1] << 8 | D[0])) >> 4;
          W[1] = ((int16_t) (D[3] << 8 | D[2])) >> 4;
          W[2] = ((int16_t) (D[5] << 8 | D[4])) >> 4;
-         *(d+3) = (float) g_0*0.986*(W[0]+18)/1000;  // kalibracja - zmiana z 0.991 na 0.986 i z +34 na +18
-         *(d+4) = (float) g_0*0.99*(W[1]-4)/1000;   // kalibracja - zmiana z 0.970 na 0.99 i z +2 na -4
-         *(d+5) = (float) g_0*0.983*(W[2]+9)/1000;  // kalibracja - zmiana +28 na +9
+         //*(d+3) = (float) g_0*0.986*(W[0]+18)/1000;  // kalibracja - zmiana z 0.991 na 0.986 i z +34 na +18
+         //*(d+4) = (float) g_0*0.99*(W[1]-4)/1000;   // kalibracja - zmiana z 0.970 na 0.99 i z +2 na -4
+         //*(d+5) = (float) g_0*0.983*(W[2]+9)/1000;  // kalibracja - zmiana +28 na +9
+         
+         *(d+3) = (float) g_0*0.9675*(W[0]+34)/1000;  // kalibracja - MI
+         *(d+4) = (float) g_0*0.9665*(W[1]+2)/1000;   // kalibracja - MI
+         *(d+5) = (float) g_0*0.9646*(W[2]+73)/1000;  // kalibracja - MI
+         //*(d+3) = W[0];  // kalibracja - zmiana z 0.991 na 0.986 i z +34 na +18
+         //*(d+4) = W[1];   // kalibracja - zmiana z 0.970 na 0.99 i z +2 na -4
+         //*(d+5) = W[2]; // kalibracja - zmiana +28 na +9
 // GN = 001  
 // Magnetometer; are stored as 12 bit readings, right justified per axis.
          address = LSM303_M_ADDR;