Merge pull request #57 from Field-Robotics-Japan/fix/imuAccCalc

Fix/imu acc calc
Field-Robotics-Japan · Aug 9, 2023 · d1cb0a4 · d1cb0a4
2 parents e09f0ae + c831458
commit d1cb0a4
Show file tree

Hide file tree

Showing 2 changed files with 18 additions and 2 deletions.
diff --git a/Assets/UnitySensors/Runtime/Scripts/Sensors/IMU/IMUSensor.cs b/Assets/UnitySensors/Runtime/Scripts/Sensors/IMU/IMUSensor.cs
@@ -30,13 +30,19 @@ public class IMUSensor : Sensor
         public Vector3 angularVelocity { get => _angularVelocity; }
 
         public Vector3 localVelocity { get => _transform.InverseTransformDirection(_velocity); }
-        public Vector3 localAcceleration { get => _transform.InverseTransformDirection(_acceleration); }
+        public Vector3 localAcceleration { get => _transform.InverseTransformDirection(_acceleration.normalized) * _acceleration.magnitude; }
 
         private float _dt { get => base._frequency_inv; }
 
+        private Vector3 _gravity;
+        private float _gravityMagnitude;
+
         protected override void Init()
         {
             _transform = this.transform;
+
+            _gravity = Physics.gravity;
+            _gravityMagnitude = _gravity.magnitude;
         }
 
         protected override void UpdateSensor()
@@ -46,7 +52,7 @@ protected override void UpdateSensor()
 
             _velocity = (_position - _position_last) / _dt;
             _acceleration = (_velocity - _velocity_last) / _dt;
-            _acceleration += _transform.InverseTransformVector(Physics.gravity);
+            _acceleration += _transform.InverseTransformVector(_gravity).normalized * _gravityMagnitude;
 
             Quaternion rotation_delta = Quaternion.Inverse(_rotation_last) * _rotation;
             rotation_delta.ToAngleAxis(out float angle, out Vector3 axis);

diff --git a/Assets/UnitySensors/Samples/IMU/IMU.unity b/Assets/UnitySensors/Samples/IMU/IMU.unity
@@ -313,6 +313,16 @@ PrefabInstance:
       propertyPath: m_RootOrder
       value: 2
       objectReference: {fileID: 0}
+    - target: {fileID: 8550806465241961968, guid: 884f57c81164231419af9316aaca015d,
+        type: 3}
+      propertyPath: m_LocalScale.y
+      value: 1
+      objectReference: {fileID: 0}
+    - target: {fileID: 8550806465241961968, guid: 884f57c81164231419af9316aaca015d,
+        type: 3}
+      propertyPath: m_LocalScale.z
+      value: 1
+      objectReference: {fileID: 0}
     - target: {fileID: 8550806465241961968, guid: 884f57c81164231419af9316aaca015d,
         type: 3}
       propertyPath: m_LocalPosition.x