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