LSM6DSV SFLP

[KWine]

I am reading quaternions from the SFLP and converting to yaw, pitch and roll (YPR). The results are remarkably stable and accurate! I noticed though when performing some simple testing with the sensor on a breadboard and the breadboard flat on a tabletop that the YPR values return NaN when the yaw is ~180 degrees. I need to investigate further the individual byte values from the FIFO under this condition but I am pretty sure this is due to the halffloat-to-float conversion. Are you aware of any such problem or have you seen this before, and is there a recommanded work around?

[KWine · Accepted solution]

OK, the problem is not in the half-to-float conversion, or not directly. The problem is that the sum of the squares of the three quaternions constructed from the SFLP can be greater than 1 such that the derived fourth quaternion element is negative. The solution is to normalize the three constructed components if the sum of their squares is > 1.

[KWine]

Just some additional information. After making sure the sum of the quaternion squares is always 1 or less I do not see any more NaN values. I compared the SFLP results with an simple open-source (Madgwick) sensor fusion algorithm. The LSM6DSV data provide essentially jitter-free results for both but the SFLP results are more accurate, properly showing 90-degree turns when rotating the breadboard by 90-degrees on the table top. I'd say the SFLP results are more accurate than I can hold the breadboard to the table edge. The Madgwick results are consistently low by about 10%; that is, they show turn angles of ~80 degrees for each turn. This could be cured with a fudge factor but there is no need to make use of the Madgwick algorithm on the MCU when the SFLP works so well...table-edge 90 turn test