Skip to content
New issue

Have a question about this project? Sign up for a free GitHub account to open an issue and contact its maintainers and the community.

Sign up for GitHub

By clicking “Sign up for GitHub”, you agree to our terms of service and privacy statement. We’ll occasionally send you account related emails.

Already on GitHub? Sign in to your account

Fix/imu grav dir #118

Merged
merged 2 commits into from
Feb 10, 2024
Merged

Fix/imu grav dir #118

Changes from all commits
Commits
Show all changes
2 commits
Select commit Hold shift + click to select a range
2ad1e89
Update IMUSensor.cs
Autumn60 Feb 9, 2024
bcc89bd
Merge pull request #117 from Autumn60/fix/imuGravDir
Autumn60 Feb 9, 2024
File filter

Filter by extension

Filter by extension
Conversations
Failed to load comments.
Loading
Jump to
Jump to file
Failed to load files.
Loading
Diff view
Diff view
44 changes: 31 additions & 13 deletions Assets/UnitySensors/Runtime/Scripts/Sensors/IMU/IMUSensor.cs
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -18,6 +18,12 @@ public class IMUSensor : UnitySensor
[SerializeField, ReadOnly]
private Vector3 _angularVelocity;

private Vector3 _position_tmp;
private Vector3 _velocity_tmp;
private Vector3 _acceleration_tmp;
private Quaternion _rotation_tmp;
private Vector3 _angularVelocity_tmp;

private Vector3 _position_last;
private Vector3 _velocity_last;
private Quaternion _rotation_last;
Expand All @@ -31,33 +37,45 @@ public class IMUSensor : UnitySensor
public Vector3 localVelocity { get => _transform.InverseTransformDirection(_velocity); }
public Vector3 localAcceleration { get => _transform.InverseTransformDirection(_acceleration.normalized) * _acceleration.magnitude; }

private Vector3 _gravity;
private Vector3 _gravityDirection;
private float _gravityMagnitude;
private float _time_last;

protected override void Init()
{
_transform = this.transform;
_gravity = Physics.gravity;
_gravityMagnitude = _gravity.magnitude;
_gravityDirection = Physics.gravity.normalized;
_gravityMagnitude = Physics.gravity.magnitude;
}

protected override void UpdateSensor()
private void FixedUpdate()
{
_position = _transform.position;
_rotation = _transform.rotation;
float dt = Time.fixedDeltaTime;

_velocity = (_position - _position_last) / dt;
_acceleration = (_velocity - _velocity_last) / dt;
_acceleration += _transform.InverseTransformDirection(_gravity).normalized * _gravityMagnitude;
_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;
rotation_delta.ToAngleAxis(out float angle, out Vector3 axis);
float angularSpeed = (angle * Mathf.Deg2Rad) / dt;
_angularVelocity = axis * angularSpeed;
_angularVelocity_tmp = axis * angularSpeed;

_position_last = _position_tmp;
_velocity_last = _velocity_tmp;
_rotation_last = _rotation_tmp;
}

protected override void UpdateSensor()
{
_position = _position_tmp;
_velocity = _velocity_tmp;
_acceleration = _acceleration_tmp;

_position_last = _position;
_velocity_last = _velocity;
_rotation_last = _rotation;
_rotation = _rotation_tmp;
_angularVelocity = _angularVelocity_tmp;

if (onSensorUpdated != null)
onSensorUpdated.Invoke();
Expand Down
Loading