private void UpdateSensor(byte[] b)
{
// Note: sizeof(char) = 2 (Unicode)
if ((b[0] == UdpThread.packetPrefix) &&
(b[sizeof(byte) + 3 * sizeof(float)] == UdpThread.packetSuffix))
{
_sensor.timestamp_ms = _udp.ElapsedMilliseconds();
_sensor.steerAngle = BitConverter.ToSingle(b, sizeof(byte));
_sensor.steerRate = BitConverter.ToSingle(b,
sizeof(byte) + sizeof(float));
_sensor.wheelRate = BitConverter.ToSingle(b,
sizeof(byte) + 2 * sizeof(float));
}
}
public void SendTorque(double tau, State state)
{
_actuator.torque = tau;
_actuator.state = state;
_actuator.timestamp_ms = _udp.ElapsedMilliseconds();
// Note: sizeof(char) = 2 (Unicode)
int packetSize = 2 * sizeof(byte) + 2 * sizeof(float);
byte[] data = new byte[packetSize];
data[0] = UdpThread.packetPrefix;
data[packetSize - 1] = UdpThread.packetSuffix;
byte[] floatBytes =
BitConverter.GetBytes(Convert.ToSingle(_actuator.torque));
Buffer.BlockCopy(floatBytes, 0, data, sizeof(byte), sizeof(float));
floatBytes =
BitConverter.GetBytes(Convert.ToSingle(_actuator.state.lean));
Buffer.BlockCopy(floatBytes, 0, data, sizeof(byte) + sizeof(float),
sizeof(float));
_udp.TransmitData(data);
}
