You are correct: the issue you are facing is related to the quantization of the half-precision float components of the quaternion stored in the FIFO.
The quaternion is made of four components: the scalar component qw = cos(angle/2), and the vector component qx qy qz = sin(angle/2) multiplied by the normalized direction of rotation [x,y,z]. Quaternion components satisfy the relation qw^2 + qx^2 + qy^2 + qz^2 = 1. Also, Q = [qw, qx, qy, qz] represents the same orientation as -Q. The SFLP output quaternion is chosen to have qw >= 0 always.
For efficiency, in LSM6DSV sensors the FIFO only stores 3 components: qx, qy, qz. The 4th component qw is recomputed as +sqrt(1 - qx^2 - qy^2 - qz^2) - newer generation sensors have been improved, see note below.
When any of the vector components, qx qy or qz, is near +/-1, the accuracy of qw is reduced as you have observed.
The workaround is to readout the quaternion components from the embedded advanced features registers before the next accelerometer and gyroscope data sample is taken; the readout must be performed when the computation of the embedded functions is completed, otherwise the embedded functions will not work as expected and the device must be rebooted.
- All 4 quaternion components are stored in page 3, from 0x4C to 0x53 (qw, qx, qy, qz).
- Wait for the rising edge of INT2_EMB_FUNC_ENDOP to be sure readout is completed after the computation of the embedded functions is completed
- Advanced features registers must be read one byte at a time (multiple-byte readout operation is not available)
Example application code is attached.
Newer generation sensors store all 4 quaternion components in the FIFO. Contact sales and marketing for more information.
