Diff: Source/Adafruit_LSM303_U.cpp

Revision:

1:8c4f93e10af3

Parent:

0:772bf4786416

--- a/Source/Adafruit_LSM303_U.cpp	Sat Mar 21 12:33:05 2015 +0000
+++ b/Source/Adafruit_LSM303_U.cpp	Thu Oct 19 11:50:04 2017 +0000
@@ -21,6 +21,8 @@
 static float _lsm303Mag_Gauss_LSB_XY = 1100.0F;  // Varies with gain
 static float _lsm303Mag_Gauss_LSB_Z  = 980.0F;   // Varies with gain
 
+Serial serial( USBTX, USBRX);
+
 /***************************************************************************
  ACCELEROMETER
  ***************************************************************************/
@@ -62,15 +64,21 @@
 void Adafruit_LSM303_Accel_Unified::read()
 {
   // Read the accelerometer
+  // OUT_X_L_A: X-axis acceleration data. The value is expressed in 2’s complement
   i2c->writeByte(LSM303_ADDRESS_ACCEL, LSM303_REGISTER_ACCEL_OUT_X_L_A | 0x80);
   
   byte data[6];
   i2c->read(LSM303_ADDRESS_ACCEL, data, 6);
 
   // Shift values to create properly formed integer (low byte first)
-  _accelData.x = (int16_t)(data[0] | (data[1] << 8)) >> 4;
-  _accelData.y = (int16_t)(data[2] | (data[3] << 8)) >> 4;
-  _accelData.z = (int16_t)(data[4] | (data[5] << 8)) >> 4;
+  _accelData.x = (int16_t)((uint16_t)data[0] | ((uint16_t)data[1] << 8)) >> 4;
+  raw.x        = (int16_t)((uint16_t)data[0] | ((uint16_t)data[1] << 8)) >> 4;
+  
+  _accelData.y = (int16_t)((uint16_t)data[2] | ((uint16_t)data[3] << 8)) >> 4;
+  raw.y        = (int16_t)((uint16_t)data[2] | ((uint16_t)data[3] << 8)) >> 4;
+  
+  _accelData.z = (int16_t)((uint16_t)data[4] | ((uint16_t)data[5] << 8)) >> 4;
+  raw.z        = (int16_t)((uint16_t)data[4] | ((uint16_t)data[5] << 8)) >> 4;
 }
 
 /***************************************************************************
@@ -98,16 +106,20 @@
 bool Adafruit_LSM303_Accel_Unified::begin()
 {
   // Enable I2C
-  // Enable the accelerometer (100Hz)
-  write8(LSM303_ADDRESS_ACCEL, LSM303_REGISTER_ACCEL_CTRL_REG1_A, 0x57);
+  // Enable the accelerometer (200Hz)
+  write8(LSM303_ADDRESS_ACCEL, LSM303_REGISTER_ACCEL_CTRL_REG1_A, 0x67);
   
   // LSM303DLHC has no WHOAMI register so read CTRL_REG1_A back to check
   // if we are connected or not
   uint8_t reg1_a = read8(LSM303_ADDRESS_ACCEL, LSM303_REGISTER_ACCEL_CTRL_REG1_A);
-  if (reg1_a != 0x57)
+  serial.printf("LSM303_REGISTER_ACCEL_CTRL_REG1_A %d \r\n", reg1_a);
+  if (reg1_a != 0x67)
   {
     return false;
-  }  
+  }
+  
+  //400 Hz; normal power mode; X/Y/Z enabled
+  write8(LSM303_ADDRESS_ACCEL, LSM303_REGISTER_ACCEL_CTRL_REG1_A, 0b01110111);
   
   return true;
 }
@@ -198,13 +210,34 @@
   
   byte data[6];
   i2c->read(LSM303_ADDRESS_MAG, data, 6);
-  
+   
   
   // Shift values to create properly formed integer (low byte first)
-  _magData.x = (int16_t)(data[0] | ((int16_t)data[1] << 8));
-  _magData.y = (int16_t)(data[2] | ((int16_t)data[3] << 8));
-  _magData.z = (int16_t)(data[4] | ((int16_t)data[5] << 8));
+  // *0.5 da _magData.z in Sättigung geht
+  _magData.x = (int16_t)((uint16_t)data[1] | ((uint16_t)data[0] << 8));
+  raw.x      = (int16_t)((uint16_t)data[1] | ((uint16_t)data[0] << 8));
+  
+  _magData.z = (int16_t)((uint16_t)data[3] | ((uint16_t)data[2] << 8));
+  raw.z      = (int16_t)((uint16_t)data[3] | ((uint16_t)data[2] << 8));
+  
   
+//  uint32_t magZOverflow = ((uint16_t)data[4] | ((uint16_t)data[5] << 8));
+//  if( (magZOverflow > 30000) && (magZOverflow < 32767) )
+//  {
+//      _magData.z = (int16_t)33023;
+//      raw.z      = (int16_t)33023;
+//  }
+//  else
+//  {
+//       _magData.z = (int16_t)((uint16_t)data[4] | ((uint16_t)data[5] << 8));
+//       raw.z      = (int16_t)((uint16_t)data[4] | ((uint16_t)data[5] << 8));
+//  }
+  
+  _magData.y = (int16_t)((uint16_t)data[5] | ((uint16_t)data[4] << 8));
+  raw.y      = (int16_t)((uint16_t)data[5] | ((uint16_t)data[4] << 8));
+  
+  //serial.printf("%i\r\n", magZOverflow );
+    
   // ToDo: Calculate orientation
   _magData.orientation = 0.0;
 }
@@ -234,6 +267,7 @@
 /**************************************************************************/
 bool Adafruit_LSM303_Mag_Unified::begin()
 {
+  
   // Enable I2C
  
   
@@ -241,15 +275,36 @@
   write8(LSM303_ADDRESS_MAG, LSM303_REGISTER_MAG_MR_REG_M, 0x00);
 
   // LSM303DLHC has no WHOAMI register so read CRA_REG_M to check
-  // the default value (0b00010000/0x10)
+  write8(LSM303_ADDRESS_MAG, LSM303_REGISTER_MAG_CRA_REG_M, 0x10);  
   uint8_t reg1_a = read8(LSM303_ADDRESS_MAG, LSM303_REGISTER_MAG_CRA_REG_M);
+  serial.printf("LSM303_REGISTER_MAG_CRA_REG_M %d \r\n", reg1_a);
   if (reg1_a != 0x10)
   {
     return false;
   }
   
-  // Set the gain to a known level
-  setMagGain(LSM303_MAGGAIN_1_3);
+
+  
+  //Temp measurement off; 220 Hz data outputrate
+  this->write8(LSM303_ADDRESS_MAG, LSM303_REGISTER_MAG_CRA_REG_M, 0b00011100);
+  // Set the gain to +/- 1.9 Gauss;
+  this->write8(LSM303_ADDRESS_MAG, LSM303_REGISTER_MAG_CRB_REG_M, 0b01000000);
+    _magGain = LSM303_MAGGAIN_1_9;
+  //Continuous-conversion mode
+  this->write8(LSM303_ADDRESS_MAG, LSM303_REGISTER_MAG_MR_REG_M,  0b00000000);
+  
+
+  
+  
+  
+  uint8_t reg = read8(LSM303_ADDRESS_MAG, LSM303_REGISTER_MAG_CRA_REG_M);
+  serial.printf("CRA_REG_M register %u \r\n", reg);
+
+  reg = read8(LSM303_ADDRESS_MAG, LSM303_REGISTER_MAG_CRB_REG_M);
+  serial.printf("CRB_REG_M register %u \r\n", reg);
+  
+  reg = read8(LSM303_ADDRESS_MAG, LSM303_REGISTER_MAG_MR_REG_M);
+  serial.printf("MR_REG_M register %u \r\n", reg);
 
   return true;
 }
@@ -331,9 +386,9 @@
     }
     else
     {
-      s_com->print(_magData.x); s_com->print(" ");
-      s_com->print(_magData.y); s_com->print(" ");
-      s_com->print(_magData.z); s_com->println(" ");
+      serial.printf("%f \r\n",_magData.x);
+      serial.printf("%f \r\n",_magData.y);
+      serial.printf("%f \r\n",_magData.z);
       /* Check if the sensor is saturating or not */
       if ( (_magData.x >= 2040) | (_magData.x <= -2040) | 
            (_magData.y >= 2040) | (_magData.y <= -2040) | 
@@ -345,32 +400,32 @@
           case LSM303_MAGGAIN_5_6:
             setMagGain(LSM303_MAGGAIN_8_1);
             readingValid = false;
-            s_com->println("Changing range to +/- 8.1");
+            serial.printf("Changing range to +/- 8.1 Gauss");
             break;
           case LSM303_MAGGAIN_4_7:
             setMagGain(LSM303_MAGGAIN_5_6);
             readingValid = false;
-            s_com->println("Changing range to +/- 5.6");
+            serial.printf("Changing range to +/- 5.6 Gauss");
             break;
           case LSM303_MAGGAIN_4_0:
             setMagGain(LSM303_MAGGAIN_4_7);
             readingValid = false;
-            s_com->println("Changing range to +/- 4.7");
+            serial.printf("Changing range to +/- 4.7 Gauss");
             break;
           case LSM303_MAGGAIN_2_5:
             setMagGain(LSM303_MAGGAIN_4_0);
             readingValid = false;
-            s_com->println("Changing range to +/- 4.0");
+            serial.printf("Changing range to +/- 4.0 Gauss");
             break;
           case LSM303_MAGGAIN_1_9:
             setMagGain(LSM303_MAGGAIN_2_5);
             readingValid = false;
-            s_com->println("Changing range to +/- 2.5");
+            serial.printf("Changing range to +/- 2.5 Gauss");
             break;
           case LSM303_MAGGAIN_1_3:
             setMagGain(LSM303_MAGGAIN_1_9);
             readingValid = false;
-            s_com->println("Changing range to +/- 1.9");
+            serial.printf("Changing range to +/- 1.9");
             break;
           default:
             readingValid = true;