Fix IMUSensor (#161)

Field-Robotics-Japan · May 29, 2024 · b1a5050 · b1a5050
1 parent dc59e69
commit b1a5050
Showing 1 changed file with 5 additions and 3 deletions.
diff --git a/Assets/UnitySensors/Runtime/Scripts/Sensors/IMU/IMUSensor.cs b/Assets/UnitySensors/Runtime/Scripts/Sensors/IMU/IMUSensor.cs
@@ -48,24 +48,26 @@ protected override void Init()
             _gravityMagnitude = Physics.gravity.magnitude;
         }
 
-        private void FixedUpdate()
+        protected override void Update()
         {
-            float dt = Time.fixedDeltaTime;
+            float dt = Time.deltaTime;
 
             _position_tmp = _transform.position;
             _velocity_tmp = (_position_tmp - _position_last) / dt;
             _acceleration_tmp = (_velocity_tmp - _velocity_last) / dt;
             _acceleration_tmp -= _transform.InverseTransformDirection(_gravityDirection) * _gravityMagnitude;
 
             _rotation_tmp = _transform.rotation;
-            Quaternion rotation_delta = Quaternion.Inverse(_rotation_last) * _rotation;
+            Quaternion rotation_delta = Quaternion.Inverse(_rotation_last) * _rotation_tmp;
             rotation_delta.ToAngleAxis(out float angle, out Vector3 axis);
             float angularSpeed = (angle * Mathf.Deg2Rad) / dt;
             _angularVelocity_tmp = axis * angularSpeed;
 
             _position_last = _position_tmp;
             _velocity_last = _velocity_tmp;
             _rotation_last = _rotation_tmp;
+
+            base.Update();
         }
 
         protected override void UpdateSensor()