public void NewMessage(ControlCommands command)
{
header.timestamp_us = Timestamp.TimestampUs ();
header.protocol_version = CONTROL_PROTOCOL_VERSION;
header.command = command;
header.payload_length = 0;
}
private void UpdatePacketFrequencyStatistics()
{
//simple exponential smoothing of timings
ulong currentUs = Timestamp.TimestampUs();
float timeDiffMs = (currentUs - lastPacketTimeUs)/1000.0f;
avgPacketTimeMs += (timeDiffMs - avgPacketTimeMs) * Constants.ExponentialSmoothingAlpha;
lastPacketTimeUs = currentUs;
}
public void ReceiverThreadMain()
{
DATAGRAM datagram=new DATAGRAM();
int datagram_size = datagram.BinarySize();
EndPoint remote = new IPEndPoint(IPAddress.Any, 0);
byte[] data = new byte[datagram_size];
BinaryReader reader = new BinaryReader(new MemoryStream(data));
while (Run)
{
try
{
udpClient.Client.ReceiveFrom(data, ref remote);
reader.BaseStream.Position=0;
if(!(remote as IPEndPoint).Address.Equals(expectedRemote.Address))
{
Debug.Log("Ignoring packet from " + remote);
continue;
}
}
catch(SocketException)
{
//Debug.LogError(e.ToString());
break;
}
catch(ObjectDisposedException)
{ //socket was closed
//Debug.LogError(e.ToString());
Run = false;
break;
}
//gather server stats
if(lastPacketTimeUs==0) //first packet case
lastPacketTimeUs=Timestamp.TimestampUs();
else
UpdatePacketFrequencyStatistics();
//process datagram
datagram.FromBinary(reader);
onDatagram(datagram);
if (dumpWriter != null)
dumpWriter.Write(data);
}
reader.Close();
if(dumpWriter!=null)
dumpWriter.Close();
onDatagram = null;
}
public void DriveAhead(float distance_cm, float speed_cm_per_sec)
{
if (replay.ReplayAny())
{
print(name + " - ignoring drive command (replay)");
return;
}
StopBacktrack();
mode = DriveMode.Auto;
packet.timestamp_us = Timestamp.TimestampUs();
packet.command = DrivePacket.Commands.TO_POSITION_WITH_SPEED;
DistanceAndSpeedToEngineCountsAndSpeed (distance_cm, speed_cm_per_sec, out packet.param1, out packet.param2, out packet.param3, out packet.param4);
Send(packet);
timeSinceLastPacketMs = 0.0f;
}
private void PrepareBacktrackDump(string backtrackFilename)
{
FlushDump ();
FileStream filestream=File.Open(GetRecordFilename(), FileMode.Open, FileAccess.Read, FileShare.Write);
long packets = filestream.Length / packet.BinarySize ();
BinaryReader reader = new BinaryReader (filestream);
DrivePacket[] datagrams=new DrivePacket[packets];
for (int i = datagrams.Length - 1; i >= 0; --i)
{
datagrams [i] = new DrivePacket ();
datagrams [i].FromBinary (reader);
}
reader.Close ();
filestream.Close ();
BinaryWriter rewriter=new BinaryWriter(File.Open (backtrackFilename, FileMode.Create, FileAccess.Write));
ulong now = Timestamp.TimestampUs ();
ulong base_timestamp = datagrams[0].timestamp_us;
foreach (DrivePacket dp in datagrams)
{
dp.timestamp_us = now + (base_timestamp - dp.timestamp_us);
switch (dp.command)
{
case DrivePacket.Commands.SET_SPEED:
dp.param1 = (short) -dp.param1;
dp.param2 = (short) -dp.param2;
break;
case DrivePacket.Commands.TO_POSITION_WITH_SPEED:
dp.param3 = (short) -dp.param3;
dp.param4 = (short) -dp.param4;
break;
default:
break;
}
dp.ToBinary (rewriter);
}
rewriter.Close ();
}
void Update ()
{
if (replay.ReplayAny())
return;
timeSinceLastPacketMs += Time.deltaTime*1000.0f;
if (timeSinceLastPacketMs < packetDelayMs)
return;
if (IsManualInput ())
{
if (mode == DriveMode.Auto)
{
mode = DriveMode.Manual;
return;
}
if (mode == DriveMode.Backtrack)
StopReplay ();
}
if (mode == DriveMode.Backtrack)
{
if (!ReplayRunning)
mode = DriveMode.Manual;
else
return;
}
packet.timestamp_us = Timestamp.TimestampUs();
if(mode == DriveMode.Auto)
packet.command = DrivePacket.Commands.KEEPALIVE;
else if (mode == DriveMode.Manual)
{
packet.command = DrivePacket.Commands.SET_SPEED;
InputToEngineSpeeds (Input.GetAxis(input.horizontal), Input.GetAxis(input.vertical), (1.0f-input.accelerationPower) + input.accelerationPower *Input.GetAxis(input.acceleration), out packet.param1,out packet.param2);
}
Send(packet);
timeSinceLastPacketMs = 0.0f;
}
