Added quaternions checking to prevent negative sqrt argument in attit…

…udes calculation algorithm (#813) * Added quaternions checking to prevent negative sqrt argument * Quaternion normalize in case of its module more than 1 Co-authored-by: Petr Ledvina <[email protected]> * Quaternions checking code improvement * The math issue is resolved --------- Co-authored-by: Petr Ledvina <[email protected]>
betaflight · Feb 20, 2025 · 13ae3e7 · 13ae3e7
1 parent 6ff3484
commit 13ae3e7
Showing 1 changed file with 13 additions and 1 deletion.
diff --git a/src/flightlog.js b/src/flightlog.js
@@ -737,7 +737,19 @@ export function FlightLog(logData) {
               z: srcFrame[imuQuaternion[2]] / scaleFromFixedInt16,
               w: 1.0,
             };
-            q.w = Math.sqrt(1.0 - (q.x ** 2 + q.y ** 2 + q.z ** 2));
+
+            let m = q.x ** 2 + q.y ** 2 + q.z ** 2;
+            if (m < 1.0) {
+                // reconstruct .w of unit quaternion
+                q.w = Math.sqrt(1.0 - m);
+            } else {                
+                // normalize [0,x,y,z]
+                m = Math.sqrt(m);
+                q.x /= m;
+                q.y /= m;
+                q.z /= m;
+                q.w = 0;
+            }
             const xx = q.x ** 2,
                   xy = q.x * q.y,
                   xz = q.x * q.z,