Raynaud Gilles
VGR
Dependents: VITI_motor_angle_1 VITI_motor_angle_2 VITI_motor_angle_3
Diff: LSM9DS1.cpp
- Revision:
- 8:16e88babd42a
- Parent:
- 7:e6e3d320eb6c
- Child:
- 9:33258ebdb817
--- a/LSM9DS1.cpp Thu Jun 15 08:42:23 2017 +0000
+++ b/LSM9DS1.cpp Mon Jun 19 02:25:48 2017 +0000
@@ -7,7 +7,7 @@
mAddress = mAddr;
}
-uint16_t LSM9DS1::begin(gyro_scale gScl, accel_scale aScl, mag_scale mScl,
+bool LSM9DS1::begin(gyro_scale gScl, accel_scale aScl, mag_scale mScl,
gyro_odr gODR, accel_odr aODR, mag_odr mODR)
{
// Store the given scales in class variables. These scale variables
@@ -15,14 +15,14 @@
gScale = gScl;
aScale = aScl;
mScale = mScl;
-
+
// Once we have the scale values, we can calculate the resolution
// of each sensor. That's what these functions are for. One for each sensor
calcgRes(); // Calculate DPS / ADC tick, stored in gRes variable
calcmRes(); // Calculate Gs / ADC tick, stored in mRes variable
calcaRes(); // Calculate g / ADC tick, stored in aRes variable
-
-
+
+
// To verify communication, we can read from the WHO_AM_I register of
// each device. Store those in a variable so we can return them.
// The start of the addresses we want to read from
@@ -36,7 +36,7 @@
// Read in all the 8 bits of data
i2c.read(xgAddress, cmd+1, 1);
uint8_t xgTest = cmd[1]; // Read the accel/gyro WHO_AM_I
-
+
// Reset to the address of the mag who am i
cmd[0] = WHO_AM_I_M;
// Write the address we are going to read from and don't end the transaction
@@ -44,46 +44,55 @@
// Read in all the 8 bits of data
i2c.read(mAddress, cmd+1, 1);
uint8_t mTest = cmd[1]; // Read the mag WHO_AM_I
-
+
for(int ii = 0; ii < 3; ii++)
{
gBiasRaw[ii] = 0;
aBiasRaw[ii] = 0;
gBias[ii] = 0;
aBias[ii] = 0;
- autoCalib = false;
}
-
- // Gyro initialization stuff:
- initGyro(); // This will "turn on" the gyro. Setting up interrupts, etc.
- setGyroODR(gODR); // Set the gyro output data rate and bandwidth.
- setGyroScale(gScale); // Set the gyro range
-
+ autoCalib = false;
+
// Accelerometer initialization stuff:
initAccel(); // "Turn on" all axes of the accel. Set up interrupts, etc.
setAccelODR(aODR); // Set the accel data rate.
setAccelScale(aScale); // Set the accel range.
-
+
+ // Gyro initialization stuff:
+ initGyro(); // This will "turn on" the gyro. Setting up interrupts, etc.
+ setGyroODR(gODR); // Set the gyro output data rate and bandwidth.
+ setGyroScale(gScale); // Set the gyro range
+
// Magnetometer initialization stuff:
initMag(); // "Turn on" all axes of the mag. Set up interrupts, etc.
setMagODR(mODR); // Set the magnetometer output data rate.
setMagScale(mScale); // Set the magnetometer's range.
-
+
// Interrupt initialization stuff
initIntr();
-
+
// Once everything is initialized, return the WHO_AM_I registers we read:
- return (xgTest << 8) | mTest;
+ return ( ((xgTest << 8) | mTest) == (WHO_AM_I_AG_RSP << 8 | WHO_AM_I_M_RSP) );
}
void LSM9DS1::initGyro()
{
+ /*
char cmd[4] = {
CTRL_REG1_G,
gScale | G_ODR_119_BW_14,
0, // Default data out and int out
0 // Default power mode and high pass settings
};
+ */
+
+ char cmd[4] = {
+ CTRL_REG1_G,
+ gScale | G_ODR_119_BW_14,
+ 0x03, // Data pass througn HPF and LPF2, default int out
+ 0x80 // Low-power mode, disable high-pass filter, default cut-off frequency
+ };
// Write the data to the gyro control registers
i2c.write(xgAddress, cmd, 4);
@@ -103,7 +112,7 @@
}
void LSM9DS1::initMag()
-{
+{
char cmd[4] = {
CTRL_REG1_M,
0x10, // Default data rate, xy axes mode, and temp comp
@@ -116,14 +125,14 @@
}
void LSM9DS1::initIntr()
-{
+{
char cmd[2];
uint16_t thresholdG = 500;
uint8_t durationG = 10;
uint8_t thresholdX = 20;
uint8_t durationX = 1;
uint16_t thresholdM = 10000;
-
+
// 1. Configure the gyro interrupt generator
cmd[0] = INT_GEN_CFG_G;
cmd[1] = (1 << 5);
@@ -138,7 +147,7 @@
cmd[0] = INT_GEN_DUR_G;
cmd[1] = (durationG & 0x7F) | 0x80;
i2c.write(xgAddress, cmd, 2);
-
+
// 3. Configure accelerometer interrupt generator
cmd[0] = INT_GEN_CFG_XL;
cmd[1] = (1 << 1);
@@ -150,7 +159,7 @@
cmd[0] = INT_GEN_DUR_XL;
cmd[1] = (durationX & 0x7F);
i2c.write(xgAddress, cmd, 2);
-
+
// 5. Configure INT1 - assign it to gyro interrupt
cmd[0] = INT1_CTRL;
// cmd[1] = 0xC0;
@@ -160,7 +169,7 @@
// cmd[1] = 0x04;
cmd[1] = (1 << 2) | (1 << 5) | (1 << 4);
i2c.write(xgAddress, cmd, 2);
-
+
// Configure interrupt 2 to fire whenever new accelerometer
// or gyroscope data is available.
cmd[0] = INT2_CTRL;
@@ -169,12 +178,12 @@
cmd[0] = CTRL_REG8;
cmd[1] = (1 << 2) | (1 << 5) | (1 << 4);
i2c.write(xgAddress, cmd, 2);
-
+
// Configure magnetometer interrupt
cmd[0] = INT_CFG_M;
cmd[1] = (1 << 7) | (1 << 0);
i2c.write(xgAddress, cmd, 2);
-
+
// Configure magnetometer threshold
cmd[0] = INT_THS_H_M;
cmd[1] = uint8_t((thresholdM & 0x7F00) >> 8);
@@ -186,65 +195,75 @@
void LSM9DS1::calibration()
{
-
+
uint16_t samples = 0;
int32_t aBiasRawTemp[3] = {0, 0, 0};
int32_t gBiasRawTemp[3] = {0, 0, 0};
- /*
- // Turn on FIFO and set threshold to 32 samples
- enableXgFIFO(true);
- setXgFIFO( 1, 0x1F);
- while (samples < 0x1F)
- {
- samples = (i2c.read(FIFO_SRC) & 0x3F); // Read number of stored samples
- }
- for(int ii = 0; ii < samples ; ii++)
- { // Read the gyro data stored in the FIFO
- readGyro();
- gBiasRawTemp[0] += gx_raw;
- gBiasRawTemp[1] += gy_raw;
- gBiasRawTemp[2] += gz_raw;
- readAccel();
- aBiasRawTemp[0] += ax_raw;
- aBiasRawTemp[1] += ay_raw;
- aBiasRawTemp[2] += az_raw - (int16_t)(1./aRes); // Assumes sensor facing up!
- }
- for (int ii = 0; ii < 3; ii++)
- {
- gBias_raw[ii] = gBiasRawTemp[ii] / samples;
- gBias[ii] = gBias_raw[ii] * gRes;
- aBias_raw[ii] = aBiasRawTemp[ii] / samples;
- aBias[ii] = aBias_raw[ii] * aRes;
- }
-
-
-
- enableXgFIFO(false);
- setXgFIFO(0, 0x00);
- */
- while(samples < 300)
+ int32_t gDeltaBiasRaw[3] = {0, 0, 0};
+
+ // Turn off the autoCalib first to get the raw data
+ autoCalib = false;
+
+ while(samples < 200)
{
readGyro();
gBiasRawTemp[0] += gx_raw;
gBiasRawTemp[1] += gy_raw;
gBiasRawTemp[2] += gz_raw;
+ /*
readAccel();
aBiasRawTemp[0] += ax_raw;
aBiasRawTemp[1] += ay_raw;
aBiasRawTemp[2] += az_raw;
- wait_us(1000);
- samples++;
+ */
+ wait_ms(1); // 1 ms
+ samples++;
}
-
+
+ //
for(int ii = 0; ii < 3; ii++)
{
gBiasRaw[ii] = gBiasRawTemp[ii] / samples;
- aBiasRaw[ii] = aBiasRawTemp[ii] / samples;
-
+ // aBiasRaw[ii] = aBiasRawTemp[ii] / samples;
+ aBiasRaw[ii] = 0;
+
+ /*
gBias[ii] = gBiasRaw[ii] * gRes;
- aBias[ii] = aBiasRaw[ii] * aRes;
+ aBias[ii] = aBiasRaw[ii] * aRes;
+ */
+ gBiasRawTemp[ii] = 0;
}
-
+
+ // Re-calculate the bias
+ int bound_bias = 10;
+ int32_t samples_axis[3] = {0, 0, 0};
+ for (size_t i = 0; i < 500; ++i){
+ readGyro();
+ gDeltaBiasRaw[0] = gx_raw - gBiasRaw[0];
+ gDeltaBiasRaw[1] = gy_raw - gBiasRaw[1];
+ gDeltaBiasRaw[2] = gz_raw - gBiasRaw[2];
+ for (size_t k = 0; k < 3; ++k){
+ if (gDeltaBiasRaw[k] >= -bound_bias && gDeltaBiasRaw[k] <= bound_bias){
+ gBiasRawTemp[k] += gDeltaBiasRaw[k] + gBiasRaw[k];
+ samples_axis[k]++;
+ }
+ }
+ //
+ wait_ms(1); // 1 ms
+ }
+
+ //
+ for(int ii = 0; ii < 3; ii++)
+ {
+ gBiasRaw[ii] = gBiasRawTemp[ii] / samples_axis[ii];
+ // aBiasRaw[ii] = aBiasRawTemp[ii] / samples;
+ aBiasRaw[ii] = 0;
+
+ gBias[ii] = gBiasRaw[ii] * gRes;
+ aBias[ii] = aBiasRaw[ii] * aRes;
+ }
+
+ // Turn on the autoCalib
autoCalib = true;
}
@@ -265,20 +284,18 @@
ax_raw = data[0] | (data[1] << 8);
ay_raw = data[2] | (data[3] << 8);
az_raw = data[4] | (data[5] << 8);
- ax = ax_raw * aRes;
- ay = ay_raw * aRes;
- az = az_raw * aRes;
-
- if(autoCalib == true)
+
+ //
+ if(autoCalib)
{
ax_raw -= aBiasRaw[0];
ay_raw -= aBiasRaw[1];
az_raw -= aBiasRaw[2];
-
- ax = ax_raw * aRes;
- ay = ay_raw * aRes;
- az = az_raw * aRes;
}
+ //
+ ax = ax_raw * aRes;
+ ay = ay_raw * aRes;
+ az = az_raw * aRes;
}
void LSM9DS1::readMag()
@@ -327,7 +344,7 @@
// Read in all 8 bit registers containing the axes data
i2c.read(xgAddress, data, 2);
- // Temperature is a 12-bit signed integer
+ // Temperature is a 12-bit signed integer
temperature_raw = data[0] | (data[1] << 8);
temperature_c = (float)temperature_raw / 8.0 + 25;
@@ -352,21 +369,19 @@
gx_raw = data[0] | (data[1] << 8);
gy_raw = data[2] | (data[3] << 8);
gz_raw = data[4] | (data[5] << 8);
- gx = gx_raw * gRes;
- gy = gy_raw * gRes;
- gz = gz_raw * gRes;
-
- if(autoCalib == true)
+
+ //
+ if(autoCalib)
{
gx_raw -= gBiasRaw[0];
gy_raw -= gBiasRaw[1];
gz_raw -= gBiasRaw[2];
-
- gx = gx_raw * gRes;
- gy = gy_raw * gRes;
- gz = gz_raw * gRes;
}
-
+ //
+ gx = gx_raw * gRes;
+ gy = gy_raw * gRes;
+ gz = gz_raw * gRes;
+
}
void LSM9DS1::setGyroScale(gyro_scale gScl)
@@ -389,7 +404,7 @@
// Write the gyroscale out to the gyro
i2c.write(xgAddress, cmd, 2);
-
+
// We've updated the sensor, but we also need to update our class variables
// First update gScale:
gScale = gScl;
@@ -417,7 +432,7 @@
// Write the accelscale out to the accel
i2c.write(xgAddress, cmd, 2);
-
+
// We've updated the sensor, but we also need to update our class variables
// First update aScale:
aScale = aScl;
@@ -445,7 +460,7 @@
// Write the magscale out to the mag
i2c.write(mAddress, cmd, 2);
-
+
// We've updated the sensor, but we also need to update our class variables
// First update mScale:
mScale = mScl;
@@ -457,14 +472,15 @@
{
char cmd[2];
char cmdLow[2];
-
+
+ // Enable the low-power mode
if(gRate == G_ODR_15_BW_0 | gRate == G_ODR_60_BW_16 | gRate == G_ODR_119_BW_14 | gRate == G_ODR_119_BW_31) {
cmdLow[0] = CTRL_REG3_G;
- cmdLow[1] = 1;
-
+ cmdLow[1] = 1<<7; // LP_mode
+
i2c.write(xgAddress, cmdLow, 2);
}
-
+
// The start of the addresses we want to read from
cmd[0] = CTRL_REG1_G;
cmd[1] = 0;
@@ -534,13 +550,16 @@
switch (gScale)
{
case G_SCALE_245DPS:
- gRes = 245.0 / 32768.0;
+ // gRes = 245.0 / 32768.0;
+ gRes = 8.75*0.001; // deg./sec.
break;
case G_SCALE_500DPS:
- gRes = 500.0 / 32768.0;
+ // gRes = 500.0 / 32768.0;
+ gRes = 17.50*0.001; // deg./sec.
break;
case G_SCALE_2000DPS:
- gRes = 2000.0 / 32768.0;
+ // gRes = 2000.0 / 32768.0;
+ gRes = 70.0*0.001; // deg./sec.
break;
}
}
@@ -561,7 +580,8 @@
aRes = 8.0 / 32768.0;
break;
case A_SCALE_16G:
- aRes = 16.0 / 32768.0;
+ // aRes = 16.0 / 32768.0;
+ aRes = 0.732*0.001; // g (gravity)
break;
}
}
@@ -569,20 +589,24 @@
void LSM9DS1::calcmRes()
{
// Possible magnetometer scales (and their register bit settings) are:
- // 2 Gs (00), 4 Gs (01), 8 Gs (10) 12 Gs (11).
+ // 2 Gs (00), 4 Gs (01), 8 Gs (10) 12 Gs (11).
switch (mScale)
{
case M_SCALE_4GS:
- mRes = 4.0 / 32768.0;
+ // mRes = 4.0 / 32768.0;
+ mRes = 0.14*0.001; // gauss
break;
case M_SCALE_8GS:
- mRes = 8.0 / 32768.0;
+ // mRes = 8.0 / 32768.0;
+ mRes = 0.29*0.001; // gauss
break;
case M_SCALE_12GS:
- mRes = 12.0 / 32768.0;
+ // mRes = 12.0 / 32768.0;
+ mRes = 0.43*0.001; // gauss
break;
case M_SCALE_16GS:
- mRes = 16.0 / 32768.0;
+ // mRes = 16.0 / 32768.0;
+ mRes = 0.58*0.001; // gauss
break;
}
}
@@ -592,13 +616,13 @@
void LSM9DS1::enableXgFIFO(bool enable)
{
char cmd[2] = {CTRL_REG9, 0};
-
+
i2c.write(xgAddress, cmd, 1);
cmd[1] = i2c.read(CTRL_REG9);
-
+
if (enable) cmd[1] |= (1<<1);
else cmd[1] &= ~(1<<1);
-
+
i2c.write(xgAddress, cmd, 2);
}
@@ -611,4 +635,4 @@
cmd[1] = ((fifoMode & 0x7) << 5) | (threshold & 0x1F);
i2c.write(xgAddress, cmd, 2);
}
-*/
\ No newline at end of file
+*/