The question says it all. I'm looking at
// Function which accumulates gyro and accelerometer data after device initialization. It calculates the average
// of the at-rest readings and then loads the resulting offsets into accelerometer and gyro bias registers.
void accelgyrocalLSM9DS1(float * dest1, float * dest2)
{
uint8_t data[6] = {0, 0, 0, 0, 0, 0};
int32_t gyro_bias[3] = {0, 0, 0}, accel_bias[3] = {0, 0, 0};
uint16_t samples, ii;
// enable the 3-axes of the gyroscope
writeByte(LSM9DS1XG_ADDRESS, LSM9DS1XG_CTRL_REG4, 0x38);
// configure the gyroscope
writeByte(LSM9DS1XG_ADDRESS, LSM9DS1XG_CTRL_REG1_G, Godr << 5 | Gscale << 3 | Gbw);
delay(200);
// enable the three axes of the accelerometer
writeByte(LSM9DS1XG_ADDRESS, LSM9DS1XG_CTRL_REG5_XL, 0x38);
// configure the accelerometer-specify bandwidth selection with Abw
writeByte(LSM9DS1XG_ADDRESS, LSM9DS1XG_CTRL_REG6_XL, Aodr << 5 | Ascale << 3 | 0x04 |Abw);
delay(200);
// enable block data update, allow auto-increment during multiple byte read
writeByte(LSM9DS1XG_ADDRESS, LSM9DS1XG_CTRL_REG8, 0x44);
// First get gyro bias
byte c = readByte(LSM9DS1XG_ADDRESS, LSM9DS1XG_CTRL_REG9);
writeByte(LSM9DS1XG_ADDRESS, LSM9DS1XG_CTRL_REG9, c | 0x02); // Enable gyro FIFO
delay(50); // Wait for change to take effect
writeByte(LSM9DS1XG_ADDRESS, LSM9DS1XG_FIFO_CTRL, 0x20 | 0x1F); // Enable gyro FIFO stream mode and set watermark at 32 samples
delay(1000); // delay 1000 milliseconds to collect FIFO samples
samples = (readByte(LSM9DS1XG_ADDRESS, LSM9DS1XG_FIFO_SRC) & 0x2F); // Read number of stored samples
for(ii = 0; ii < samples ; ii++) { // Read the gyro data stored in the FIFO
int16_t gyro_temp[3] = {0, 0, 0};
readBytes(LSM9DS1XG_ADDRESS, LSM9DS1XG_OUT_X_L_G, 6, &data[0]);
gyro_temp[0] = (int16_t) (((int16_t)data[1] << 8) | data[0]); // Form signed 16-bit integer for each sample in FIFO
gyro_temp[1] = (int16_t) (((int16_t)data[3] << 8) | data[2]);
gyro_temp[2] = (int16_t) (((int16_t)data[5] << 8) | data[4]);
gyro_bias[0] += (int32_t) gyro_temp[0]; // Sum individual signed 16-bit biases to get accumulated signed 32-bit biases
gyro_bias[1] += (int32_t) gyro_temp[1];
gyro_bias[2] += (int32_t) gyro_temp[2];
}
gyro_bias[0] /= samples; // average the data
gyro_bias[1] /= samples;
gyro_bias[2] /= samples;
dest1[0] = (float)gyro_bias[0]*gRes; // Properly scale the data to get deg/s
dest1[1] = (float)gyro_bias[1]*gRes;
dest1[2] = (float)gyro_bias[2]*gRes;
c = readByte(LSM9DS1XG_ADDRESS, LSM9DS1XG_CTRL_REG9);
writeByte(LSM9DS1XG_ADDRESS, LSM9DS1XG_CTRL_REG9, c & ~0x02); //Disable gyro FIFO
delay(50);
writeByte(LSM9DS1XG_ADDRESS, LSM9DS1XG_FIFO_CTRL, 0x00); // Enable gyro bypass mode
// now get the accelerometer bias
c = readByte(LSM9DS1XG_ADDRESS, LSM9DS1XG_CTRL_REG9);
writeByte(LSM9DS1XG_ADDRESS, LSM9DS1XG_CTRL_REG9, c | 0x02); // Enable accel FIFO
delay(50); // Wait for change to take effect
writeByte(LSM9DS1XG_ADDRESS, LSM9DS1XG_FIFO_CTRL, 0x20 | 0x1F); // Enable accel FIFO stream mode and set watermark at 32 samples
delay(1000); // delay 1000 milliseconds to collect FIFO samples
samples = (readByte(LSM9DS1XG_ADDRESS, LSM9DS1XG_FIFO_SRC) & 0x2F); // Read number of stored samples
for(ii = 0; ii < samples ; ii++) { // Read the accel data stored in the FIFO
int16_t accel_temp[3] = {0, 0, 0};
readBytes(LSM9DS1XG_ADDRESS, LSM9DS1XG_OUT_X_L_XL, 6, &data[0]);
accel_temp[0] = (int16_t) (((int16_t)data[1] << 8) | data[0]); // Form signed 16-bit integer for each sample in FIFO
accel_temp[1] = (int16_t) (((int16_t)data[3] << 8) | data[2]);
accel_temp[2] = (int16_t) (((int16_t)data[5] << 8) | data[4]);
accel_bias[0] += (int32_t) accel_temp[0]; // Sum individual signed 16-bit biases to get accumulated signed 32-bit biases
accel_bias[1] += (int32_t) accel_temp[1];
accel_bias[2] += (int32_t) accel_temp[2];
}
accel_bias[0] /= samples; // average the data
accel_bias[1] /= samples;
accel_bias[2] /= samples;
if(accel_bias[2] > 0L) {accel_bias[2] -= (int32_t) (1.0/aRes);} // Remove gravity from the z-axis accelerometer bias calculation
else {accel_bias[2] += (int32_t) (1.0/aRes);}
dest2[0] = (float)accel_bias[0]*aRes; // Properly scale the data to get g
dest2[1] = (float)accel_bias[1]*aRes;
dest2[2] = (float)accel_bias[2]*aRes;
c = readByte(LSM9DS1XG_ADDRESS, LSM9DS1XG_CTRL_REG9);
writeByte(LSM9DS1XG_ADDRESS, LSM9DS1XG_CTRL_REG9, c & ~0x02); //Disable accel FIFO
delay(50);
writeByte(LSM9DS1XG_ADDRESS, LSM9DS1XG_FIFO_CTRL, 0x00); // Enable accel bypass mode
}
and have many questions...
Just one, for example:
// First get gyro bias
byte c = readByte(LSM9DS1XG_ADDRESS, LSM9DS1XG_CTRL_REG9);
writeByte(LSM9DS1XG_ADDRESS, LSM9DS1XG_CTRL_REG9, c | 0x02); // Enable gyro FIFO
I can see that you're indeed, enabling the fifo, but why is it gyro? You just enabled above all axes for both G and XL
Then you do:
c = readByte(LSM9DS1XG_ADDRESS, LSM9DS1XG_CTRL_REG9);
writeByte(LSM9DS1XG_ADDRESS, LSM9DS1XG_CTRL_REG9, c & ~0x02); //Disable gyro FIFO
delay(50);
writeByte(LSM9DS1XG_ADDRESS, LSM9DS1XG_FIFO_CTRL, 0x00); // Enable gyro bypass mode
// now get the accelerometer bias
c = readByte(LSM9DS1XG_ADDRESS, LSM9DS1XG_CTRL_REG9);
writeByte(LSM9DS1XG_ADDRESS, LSM9DS1XG_CTRL_REG9, c | 0x02); // Enable accel FIFO
delay(50); // Wait for change to take effect
Sorry, I don't see where the difference is and how is it now XL and not G? What am I missing?
Thanks.
React
Typescript
Django
Laravel
Facebook
Microsoft
Google
Alibaba
D3
Tencent