normalize quaternion before converting to euler

tilak-io · Mar 5, 2024 · f67edac · f67edac
1 parent 27bc266
commit f67edac
Show file tree

Hide file tree

Showing 2 changed files with 6 additions and 3 deletions.
diff --git a/api/parsers/ulgparser.py b/api/parsers/ulgparser.py
@@ -14,8 +14,10 @@ def __init__(self):
         self.initEntities()
 
     def euler_from_quaternion(self, w, x, y, z):
-        angles = {}
+        norm = math.sqrt(w*w + x*x + y*y + z*z)
+        w, x, y, z = w / norm, x / norm, y / norm, z / norm
 
+        angles = {}
         #roll(x-axis rotation)
         sinr_cosp = 2 * (w * x + y * z)
         cosr_cosp = 1 - 2 * (x * x + y * y)
@@ -30,9 +32,10 @@ def euler_from_quaternion(self, w, x, y, z):
         siny_cosp = 2 * (w * z + x * y)
         cosy_cosp = 1 - 2 * (y * y + z * z)
         angles['yaw'] = math.atan2(siny_cosp, cosy_cosp)
-
         return angles
 
+
+
     def add_euler(self,datadict):
         if "vehicle_attitude" in datadict: 
             a=datadict['vehicle_attitude']

diff --git a/package.json b/package.json
@@ -1,6 +1,6 @@
 {
   "name": "tiplot",
-  "version": "1.2.3",
+  "version": "1.2.4",
   "private": true,
   "homepage": "./",
   "proxy": "http://localhost:5000",