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