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); } }