LSM6DSV80X FIFO quaternion output always shows X=0, but raw register read is correct

[AhilanAshwin]

I’m trying to read the 3-component “game rotation” quaternion out of the LSM6DSV80X’s FIFO, but every time I pop a FIFO record I get three half-precision floats [x,y,z] where x is always zero and y,z look plausible. When I read the same fusion engine registers directly (not via FIFO), I get a full four-component quaternion [w,x,y,z] where x is nonzero—as expected. Ive connected the STEVAL-MKI247A to my ESP 32 via SPI with the following configuration 

// Define SPI pins
#define CS_PIN 5    // Chip Select pin
#define SCK_PIN 18  // SPI Clock pin
#define INT_PIN 25  // Interrupt pin
#define MOSI_PIN 23 // Master Out Slave In pin
#define MISO_PIN 19 // Master In Slave Out pin

What ive done: 

• Enabled the SF-LP game rotation vector and timestamp in the embedded-function bank, then

• Configured FIFO streaming mode and set watermark = 1, and

• In loop() I parse the data from the FIFO queue and immediately compare it with the raw register read. 

1. How i enabled SF-LP FIFO batching:

fifo_sflp.game_rotation = 1;  
fifo_sflp.gravity       = 0;  
fifo_sflp.gbias         = 0;
lsm6dsv80x_fifo_sflp_batch_set(&dev_ctx, fifo_sflp);
lsm6dsv80x_fifo_mode_set(&dev_ctx, LSM6DSV80X_STREAM_MODE);

 
2. FIFO-read + tag check:

lsm6dsv80x_fifo_out_raw_get(&dev_ctx, &f_data);
if (f_data.tag == LSM6DSV80X_SFLP_GAME_ROTATION_VECTOR_TAG) {
  uint16_t hx = (f_data.data[1]<<8) | f_data.data[0];
  uint16_t hy = (f_data.data[3]<<8) | f_data.data[2];
  uint16_t hz = (f_data.data[5]<<8) | f_data.data[4];
  float x = npy_half_to_float(hx);
  float y = npy_half_to_float(hy);
  float z = npy_half_to_float(hz);
  Serial.printf("FIFO FLOATS : x=%.4f y=%.4f z=%.4f\n", x, y, z);
}

3. Read raw registers for comparison

uint16_t raw_q[4];
lsm6dsv80x_sflp_quaternion_raw_get(&dev_ctx, raw_q);
float w = lsm6dsv80x_from_quaternion_lsb_to_float(raw_q[0]);
float x = lsm6dsv80x_from_quaternion_lsb_to_float(raw_q[1]);
float y = lsm6dsv80x_from_quaternion_lsb_to_float(raw_q[2]);
float z = lsm6dsv80x_from_quaternion_lsb_to_float(raw_q[3]);
Serial.printf("RAW  FLOATS : w=%.4f x=%.4f y=%.4f z=%.4f\n", w, x, y, z);

4. Output:

FIFO FLOATS : x=0.0000 y=-0.1245 z=-0.9414  
RAW  FLOATS : w=0.2949 x=-0.1245 y=-0.9414 z=-0.0973

Any example on how to retrieve the sensor fusion output from via the fifo would be most appreciated! 

[casadeib · Accepted solution]

Hi @AhilanAshwin,

Did you try to have a look at the AN6281 and follow the information in section 8.6.3? 

You should fine the game rotation data format stored in FIFO. Hope this could help you to solve your issue.