From f67edacf3b6d7022c7bd2f8f054afa6c7c6681a0 Mon Sep 17 00:00:00 2001 From: Hamza ZOGHMAR Date: Tue, 5 Mar 2024 16:05:23 +0100 Subject: [PATCH] normalize quaternion before converting to euler --- api/parsers/ulgparser.py | 7 +++++-- package.json | 2 +- 2 files changed, 6 insertions(+), 3 deletions(-) diff --git a/api/parsers/ulgparser.py b/api/parsers/ulgparser.py index 26fa5ea..fdefbf6 100644 --- 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 index 66efdff..8590487 100644 --- 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",