Added quaternions checking to prevent negative sqrt argument in attitudes 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]>

Author: demvlad

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,