public void Tick(Rigidbody body, float deltaTime, bool paused, float rpm, float maxRpm, int gearNumber)
{
var velocity = body.transform.InverseTransformDirection(body.velocity);
var rotation = body.transform.rotation;
var localRotation = body.transform.localRotation;
m_telemetry.state = paused ? (byte)FSMI_State.PAUSE : (byte)FSMI_State.NO_PAUSE;
m_telemetry.rpm = (uint)rpm;
m_telemetry.maxRpm = (uint)maxRpm;
m_telemetry.gearNumber = (sbyte)gearNumber;
m_telemetry.speed = velocity.magnitude * 3.6f; // km/h
m_telemetry.surgeSpeed = velocity.z;
m_telemetry.swaySpeed = velocity.x;
m_telemetry.heaveSpeed = velocity.y;
m_telemetry.roll = -Mathf.Deg2Rad * (localRotation.eulerAngles.z > 180 ? localRotation.eulerAngles.z - 360 : localRotation.eulerAngles.z);
m_telemetry.pitch = -Mathf.Deg2Rad * (localRotation.eulerAngles.x > 180 ? localRotation.eulerAngles.x - 360 : localRotation.eulerAngles.x);
m_telemetry.yaw = Mathf.Deg2Rad * (localRotation.eulerAngles.y > 180 ? localRotation.eulerAngles.y - 360 : localRotation.eulerAngles.y);
if (m_firstCall)
{
m_firstCall = false;
m_telemetry.surgeAcceleration = 0;
m_telemetry.swayAcceleration = 0;
m_telemetry.heaveAcceleration = 0;
m_telemetry.pitchSpeed = 0;
m_telemetry.rollSpeed = 0;
m_telemetry.yawSpeed = 0;
}
else
{
LowPassFilter(ref m_telemetry.surgeAcceleration, (m_telemetry.surgeSpeed - m_prevSurgeSpeed) / deltaTime, FSMI_VT_ACC_LOW_PASS_FACTOR);
LowPassFilter(ref m_telemetry.swayAcceleration, (m_telemetry.swaySpeed - m_prevSwaySpeed) / deltaTime, FSMI_VT_ACC_LOW_PASS_FACTOR);
LowPassFilter(ref m_telemetry.heaveAcceleration, (m_telemetry.heaveSpeed - m_prevHeaveSpeed) / deltaTime, FSMI_VT_ACC_LOW_PASS_FACTOR);
var deltaAngles = new Vector3(Mathf.DeltaAngle(body.transform.eulerAngles.x, m_prevAngles.x),
Mathf.DeltaAngle(body.transform.eulerAngles.y, m_prevAngles.y),
Mathf.DeltaAngle(body.transform.eulerAngles.z, m_prevAngles.z));
LowPassFilter(ref m_telemetry.rollSpeed, deltaAngles.z / deltaTime, FSMI_VT_ANGLES_SPEED_LOW_PASS_FACTOR);
LowPassFilter(ref m_telemetry.pitchSpeed, deltaAngles.x / deltaTime, FSMI_VT_ANGLES_SPEED_LOW_PASS_FACTOR);
LowPassFilter(ref m_telemetry.yawSpeed, deltaAngles.y / deltaTime, FSMI_VT_ANGLES_SPEED_LOW_PASS_FACTOR);
}
m_prevSurgeSpeed = m_telemetry.surgeSpeed;
m_prevSwaySpeed = m_telemetry.swaySpeed;
m_prevHeaveSpeed = m_telemetry.heaveSpeed;
m_prevAngles.x = body.transform.eulerAngles.x;
m_prevAngles.y = body.transform.eulerAngles.y;
m_prevAngles.z = body.transform.eulerAngles.z;
m_api.SendTelemetry(ref m_telemetry);
}
public void Update(float deltaTime)
{
if (null != m_telemetryObject)
{
m_telemetryObject.Update(deltaTime, ref m_telemetry);
m_api.SendTelemetry(ref m_telemetry);
}
if (null != m_positioningObject)
{
m_positioningObject.Update(deltaTime, ref m_matrix);
m_api.SendTopTableMatrixPhy(ref m_matrix);
}
if (null != m_platformInfoObject)
{
m_api.GetPlatformInfoEx(
ref m_platformInfo,
(uint)Marshal.SizeOf(typeof(FSMI_PlatformInfo)),
/* ? default ? */ 0
);
m_platformInfoObject.Update(deltaTime, ref m_platformInfo);
}
}
