Merge pull request #36 from RosProBridge/hotfix-imu-direction-and-scope-update

Imu direction and scope update

Author: prony5

Runtime/Tx/ImuTx.cs

@@ -9,7 +9,6 @@ namespace ProBridgeSenors.Tx
     [AddComponentMenu("ProBridge/Tx/sensor_msgs/Imu")]
     public class ImuTx : ProBridgeTxStamped<Imu>
     {
-        public Vector3 acceleration { get; private set; }
 
 
         [Header("Noise Parameters")]
@@ -24,7 +23,9 @@ public class ImuTx : ProBridgeTxStamped<Imu>
         private Vector3 _lastPosition;
         private Quaternion _lastRotation;
         private Vector3 _velocity;
-        private Vector3 _angularVelocity;
+        protected Vector3 _acceleration;
+        protected Vector3 _angularVelocity;
+        protected Quaternion _orientation;
         private bool _isGlobal;
         private Vector3 _gravityDirection;
         private float _gravityMagnitude;
@@ -48,15 +49,16 @@ void FixedUpdate()
             _lastPosition = transform.position;
             _lastRotation = transform.rotation;
 
-            acceleration = (_velocity - _lastVel) / Time.fixedDeltaTime - transform.InverseTransformDirection(_gravityDirection) * _gravityMagnitude;;
+            _acceleration = (_velocity - _lastVel) / Time.fixedDeltaTime - _gravityDirection * _gravityMagnitude;;
 
             if (!_isGlobal)
             {
-                acceleration = transform.InverseTransformDirection(acceleration);
+                _acceleration = transform.InverseTransformDirection(_acceleration);
                 _angularVelocity = transform.InverseTransformDirection(_angularVelocity);
             }
 
             _lastVel = _velocity;
+            _orientation = transform.rotation;
         }
         protected override ProBridge.ProBridge.Msg GetMsg(TimeSpan ts)
         {
@@ -82,8 +84,8 @@ protected override ProBridge.ProBridge.Msg GetMsg(TimeSpan ts)
             }            
 
             data.angular_velocity = _angularVelocity.ToRosAngular();
-            data.linear_acceleration = acceleration.ToRos();
-            data.orientation = transform.rotation.ToRos();
+            data.linear_acceleration = _acceleration.ToRos();
+            data.orientation = _orientation.ToRos();
 
             return base.GetMsg(ts);
         }