void FixedUpdate()
{
if (MapOrigin == null || Bridge == null || Bridge.Status != Status.Connected)
{
return;
}
if (IsFirstFixedUpdate)
{
lock (MessageQueue)
{
MessageQueue.Clear();
}
IsFirstFixedUpdate = false;
}
var time = SimulatorManager.Instance.CurrentTime;
if (time < LastTimestamp)
{
return;
}
var location = MapOrigin.PositionToGpsLocation(transform.position, IgnoreMapOrigin);
var orientation = transform.rotation;
orientation.Set(-orientation.z, orientation.x, -orientation.y, orientation.w); // converting to right handed xyz
var data = new GpsData()
{
Name = Name,
Frame = Frame,
Time = SimulatorManager.Instance.CurrentTime,
Sequence = SendSequence++,
IgnoreMapOrigin = IgnoreMapOrigin,
Latitude = location.Latitude,
Longitude = location.Longitude,
Altitude = location.Altitude,
Northing = location.Northing,
Easting = location.Easting,
Orientation = orientation,
};
lock (MessageQueue)
{
MessageQueue.Enqueue(Tuple.Create(time, (Action)(() =>
{
if (Bridge != null && Bridge.Status == Status.Connected)
{
Publish(data);
}
})));
}
}
private void AddReferencePoint(MapOrigin origin)
{
var index = FindObjectsOfType <MapOriginReferencePoint>(true).Length + 1;
var p = new GameObject("ReferencePoint" + index).AddComponent <MapOriginReferencePoint>();
if (SceneView.lastActiveSceneView != null)
{
var camera = SceneView.lastActiveSceneView.camera.transform;
if (Physics.Raycast(new Ray(camera.position, camera.forward), out var hit))
{
p.transform.position = hit.point;
}
}
var gps = origin.PositionToGpsLocation(p.transform.position);
p.latitue = gps.Latitude;
p.longitude = gps.Longitude;
var mapHolder = FindObjectOfType <MapHolder>().transform;
var holder = mapHolder.Find("ReferencePoints");
if (holder == null)
{
holder = new GameObject("ReferencePoints").transform;
holder.parent = mapHolder;
}
p.transform.parent = holder;
Selection.activeGameObject = p.gameObject;
}
public void Init(MapOrigin origin)
{
this.origin = origin;
var gps = origin.PositionToGpsLocation(origin.transform.position);
latitude = Math.Round(gps.Latitude, 6);
longitude = Math.Round(gps.Longitude, 6);
minSize = new Vector2(250, 120);
maxSize = new Vector2(300, 120);
}
private void Awake()
{
MapOrigin = MapOrigin.Find();
GPSLocation = MapOrigin.PositionToGpsLocation(Vector3.zero);
}
public void Update()
{
if (MapOrigin == null)
{
return;
}
if (Time.time < NextSend)
{
return;
}
NextSend = Time.time + 1.0f / Frequency;
float speed = Dynamics.Speed;
MaxSpeed = Mathf.Max(MaxSpeed, speed);
var gps = MapOrigin.PositionToGpsLocation(transform.position);
var orientation = transform.rotation;
orientation.Set(-orientation.z, orientation.x, -orientation.y, orientation.w); // converting to right handed xyz
msg = new CanBusData()
{
Name = Name,
Frame = Frame,
Time = SimulatorManager.Instance.CurrentTime,
Sequence = SendSequence++,
Speed = speed,
Throttle = Dynamics.AccellInput > 0 ? Dynamics.AccellInput : 0,
Braking = Dynamics.AccellInput < 0 ? -Dynamics.AccellInput : 0,
Steering = Dynamics.SteerInput,
ParkingBrake = Dynamics.HandBrake,
HighBeamSignal = Actions.CurrentHeadLightState == HeadLightState.HIGH,
LowBeamSignal = Actions.CurrentHeadLightState == HeadLightState.LOW,
HazardLights = Actions.HazardLights,
FogLights = Actions.FogLights,
LeftTurnSignal = Actions.LeftTurnSignal,
RightTurnSignal = Actions.RightTurnSignal,
Wipers = false,
InReverse = Dynamics.Reverse,
Gear = Mathf.RoundToInt(Dynamics.CurrentGear),
EngineOn = Dynamics.CurrentIgnitionStatus == IgnitionStatus.On,
EngineRPM = Dynamics.CurrentRPM,
Latitude = gps.Latitude,
Longitude = gps.Longitude,
Altitude = gps.Altitude,
Orientation = orientation,
Velocity = Dynamics.Velocity,
};
if (Bridge != null && Bridge.Status == Status.Connected)
{
Publish(msg);
}
}
