Have you ever run this code?

[sverdlovsk]

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.

[kriswiner]

Yes I wrote and have used this code and it works as intended. The LSM9DS1 has embedded within it two sensors, one is a magnetometer and one is a combination gyro/accelerometer. The gyro and accelerometer have a single FIFO buffer available that stores up to 32 16-bit gyro and accel data, so in principle, I could just run the FIFO once and then read the gyro and accel data to do the averaging. I do it twice here since I am copying another similar routine (from the LSM9DS0) where the gyro and accel are located in separate sensor devices embedded within one package and have separate FIFO buffers. It was just easier to keep the format of the coding even though the operations are redundant.

[sverdlovsk]

Well, how you are separating the data from the fifo then? How do you know who is who?

[kriswiner]

You read the data from the corresponding gyro or accel data registers, which are different. The normal data registers point to the FIFO buffer.

[juniskane]

Are you sure about the magnetometer orientation?

 // Sensors x, y, and z axes of the accelerometer and gyro are aligned. The magnetometer
 // the magnetometer z-axis (+ up) is aligned with the z-axis (+ up) of accelerometer and gyro, but the magnetometer
 // x-axis is aligned with the -y axis of the gyro and the magnetometer y axis is aligned with the -x axis of the gyro!
 // We have to make some allowance for this orientation mismatch in feeding the output to the  quaternion filter.
 ...
 MadgwickQuaternionUpdate(ax, ay, az, gx*PI/180.0f, gy*PI/180.0f, gz*PI/180.0f, -my, -mx, mz);
 // MahonyQuaternionUpdate(ax, ay, az, gx*PI/180.0f, gy*PI/180.0f, gz*PI/180.0f, -my, -mx, mz);

ST datasheet (DocID 025715 Rev 1) Fig 1. (p.10) seems to indicate only

 mx = -mx

is needed. Note that image has the "orientation dot" in different corner of the device package for the magnetometer!

Is there any reason you are not using the invSqrt(float x) from original Madgwick code to normalize quaternions? It seems to work for me on Cortex M3 just fine.

[kriswiner]

Yes, I think you are right, I didn't notice the orientation dot difference before. I'll change it but I have to wonder, why did ST change the dot orientation and, maybe it is a typo on their part?

Do you find it makes any difference?

I think I just use sqrt and then take the inverse separately. If you want to use the combined function I see no problem with doing so.

[juniskane]

I think I'll ask confirmation from ST. Odd for a datasheet to not use a common coordinate system within a single diagram. For some reason, I get significantly different X- and Y-biases during magnetometer calibration, even if I orient the sensor different way each time to compensate for any external fields. My sensor is on custom PCB so could be something in the HW. Negating just the sign of mx, instead of swapping and negating mx and my, seem to mostly remove systematic drift (i.e. my euler angles after filter/conversion did not converge for a stationary device), but I might have some other error source in my system as well.

[sverdlovsk]

I didn't see datasheets worse then ST very often...

[kriswiner]

I was going to ask ST for confirmation but I'll wait to see what answer they provide you juniskane. ST makes so many derivative products that they often just cut and paste to make their data sheets. But I have found them to be pretty responsive to questions, so I expect we will get a quick and straight answer from them on this issue. Thanks for the heads up!

[juniskane]

ST comfirmed that the image in datasheet, including the mixed orientation of the dot is correct.

I hope you don't mind that we have re-used some snippets of your code, of this repository, for our ThingSee One device: http://www.thingsee.com/

It has the new LSM9DS1 sensor, and we have plans to open-source at least the base layer of software. We'll buy you a beer, if meet at a same venue!

[kriswiner]

Hi, thanks for the update re: ST's figure. I will have to change a couple of things since I was looking at other ST diagrams and they also pull the rotated dot trick. Yes, the code is open-source meaning you may use it as you like. Acknowledgment and beer are gratefully accepted! I'll be at CES in January.

[dmamalis]

@kriswiner does this commit https://github.com/kriswiner/LSM9DS1/commit/b78206d10598aa88034d3fda7c1e1ccc11079c55 refer to the changes you are talking about?