Diff: LSM9DS0.cpp

Revision:

1:2c34ccab5256

Parent:

0:0dbf7ee73651

Child:

2:48eb33afd0fa

--- a/LSM9DS0.cpp	Fri Feb 26 12:56:04 2016 +0000
+++ b/LSM9DS0.cpp	Sat Feb 27 09:03:06 2016 +0000
@@ -93,6 +93,10 @@
     setMagODR(mODR); // Set the magnetometer output data rate.
     setMagScale(mScale); // Set the magnetometer's range.
     
+    setGyroOffset(0,0,0);
+    setAccelOffset(0,0,0);
+    setMagOffset(0,0,0);
+    
     // Once everything is initialized, return the WHO_AM_I registers we read:
     return (xmTest << 8) | gTest;
 }
@@ -328,6 +332,70 @@
   xmWriteByte(FIFO_CTRL_REG, 0x00);       // Enable accelerometer bypass mode
 }
 
+//**********************
+//  Gyro section
+//**********************
+void LSM9DS0::readGyro()
+{
+    uint8_t temp[6]; // We'll read six bytes from the gyro into temp
+    gReadBytes(OUT_X_L_G, temp, 6); // Read 6 bytes, beginning at OUT_X_L_G
+    gx = (temp[1] << 8) | temp[0]; // Store x-axis values into gx
+    gy = (temp[3] << 8) | temp[2]; // Store y-axis values into gy
+    gz = (temp[5] << 8) | temp[4]; // Store z-axis values into gz
+}
+
+void LSM9DS0::setGyroOffset(int16_t _gx, int16_t _gy, int16_t _gz)
+{
+    gyroOffset[0] = _gx;
+    gyroOffset[1] = _gy;
+    gyroOffset[2] = _gz;
+}
+
+int16_t LSM9DS0::readRawGyroX( void )
+{
+    uint8_t temp[2];
+    gReadBytes(OUT_X_L_G, temp, 2);
+    gx = (temp[1] << 8) | temp[0];
+    return gx;
+}
+
+int16_t LSM9DS0::readRawGyroY( void )
+{
+    uint8_t temp[2];
+    gReadBytes(OUT_Y_L_G, temp, 2);
+    gy = (temp[1] << 8) | temp[0];
+    return gy;
+}
+
+int16_t LSM9DS0::readRawGyroZ( void )
+{
+    uint8_t temp[2];
+    gReadBytes(OUT_Z_L_G, temp, 2);
+    gz = (temp[1] << 8) | temp[0];
+    return gz;
+}
+
+float LSM9DS0::readFloatGyroX( void )
+{
+    float output = calcGyro(readRawGyroX() - gyroOffset[0]);
+    return output;
+}
+
+float LSM9DS0::readFloatGyroY( void )
+{
+    float output = calcGyro(readRawGyroY() - gyroOffset[1]);
+    return output;
+}
+
+float LSM9DS0::readFloatGyroZ( void )
+{
+    float output = calcGyro(readRawGyroZ() - gyroOffset[2]);
+    return output;
+}
+
+//**********************
+//  Accel section
+//**********************
 void LSM9DS0::readAccel()
 {
     uint8_t temp[6]; // We'll read six bytes from the accelerometer into temp   
@@ -337,6 +405,58 @@
     az = (temp[5] << 8) | temp[4]; // Store z-axis values into az
 }
 
+void LSM9DS0::setAccelOffset(int16_t _ax, int16_t _ay, int16_t _az)
+{
+    accelOffset[0] = _ax;
+    accelOffset[1] = _ay;
+    accelOffset[2] = _az;
+}
+
+int16_t LSM9DS0::readRawAccelX( void )
+{
+    uint8_t temp[2];
+    xmReadBytes(OUT_X_L_A, temp, 2);
+    ax = (temp[1] << 8) | temp[0];
+    return ax;
+}
+
+int16_t LSM9DS0::readRawAccelY( void )
+{
+    uint8_t temp[2];
+    xmReadBytes(OUT_Y_L_A, temp, 2);
+    ay = (temp[1] << 8) | temp[0];
+    return ay;
+}
+
+int16_t LSM9DS0::readRawAccelZ( void )
+{
+    uint8_t temp[2];
+    xmReadBytes(OUT_Z_L_A, temp, 2);
+    az = (temp[1] << 8) | temp[0];
+    return az;
+}
+
+float LSM9DS0::readFloatAccelX( void )
+{
+    float output = calcAccel(readRawAccelX() - accelOffset[0]);
+    return output;
+}
+
+float LSM9DS0::readFloatAccelY( void )
+{
+    float output = calcAccel(readRawAccelY() - accelOffset[1]);
+    return output;
+}
+
+float LSM9DS0::readFloatAccelZ( void )
+{
+    float output = calcAccel(readRawAccelZ() - accelOffset[2]);
+    return output;
+}
+
+//**********************
+//  Mag section
+//**********************
 void LSM9DS0::readMag()
 {
     uint8_t temp[6]; // We'll read six bytes from the mag into temp 
@@ -346,6 +466,58 @@
     mz = (temp[5] << 8) | temp[4]; // Store z-axis values into mz
 }
 
+void LSM9DS0::setMagOffset(int16_t _mx, int16_t _my, int16_t _mz)
+{
+    magOffset[0] = _mx;
+    magOffset[1] = _my;
+    magOffset[2] = _mz;
+}
+
+int16_t LSM9DS0::readRawMagX( void )
+{
+    uint8_t temp[2];
+    xmReadBytes(OUT_X_L_M, temp, 2);
+    mx = (temp[1] << 8) | temp[0];
+    return mx;
+}
+
+int16_t LSM9DS0::readRawMagY( void )
+{
+    uint8_t temp[2];
+    xmReadBytes(OUT_Y_L_M, temp, 2);
+    my = (temp[1] << 8) | temp[0];
+    return my;
+}
+
+int16_t LSM9DS0::readRawMagZ( void )
+{
+    uint8_t temp[2];
+    xmReadBytes(OUT_Z_L_M, temp, 2);
+    mz = (temp[1] << 8) | temp[0];
+    return mz;
+}
+
+float LSM9DS0::readFloatMagX( void )
+{
+    float output = calcMag(readRawMagX() - magOffset[0]);
+    return output;
+}
+
+float LSM9DS0::readFloatMagY( void )
+{
+    float output = calcMag(readRawMagY() - magOffset[1]);
+    return output;
+}
+
+float LSM9DS0::readFloatMagZ( void )
+{
+    float output = calcMag(readRawMagZ() - magOffset[2]);
+    return output;
+}
+
+//**********************
+//  Temp section
+//**********************
 void LSM9DS0::readTemp()
 {
     uint8_t temp[2]; // We'll read two bytes from the temperature sensor into temp  
@@ -353,15 +525,6 @@
     temperature = (((int16_t) temp[1] << 12) | temp[0] << 4 ) >> 4; // Temperature is a 12-bit signed integer
 }
 
-void LSM9DS0::readGyro()
-{
-    uint8_t temp[6]; // We'll read six bytes from the gyro into temp
-    gReadBytes(OUT_X_L_G, temp, 6); // Read 6 bytes, beginning at OUT_X_L_G
-    gx = (temp[1] << 8) | temp[0]; // Store x-axis values into gx
-    gy = (temp[3] << 8) | temp[2]; // Store y-axis values into gy
-    gz = (temp[5] << 8) | temp[4]; // Store z-axis values into gz
-}
-
 float LSM9DS0::calcGyro(int16_t gyro)
 {
     // Return the gyro raw reading times our pre-calculated DPS / (ADC tick):