[sim] Add XRP-specific plugin (#5631)

Provides an implementation of a XRP-specific plugin that sends binary messages over UDP (to account for the less performant hardware on the XRP).

This plugin leverages the work already done for the WebSocket protocol and does a translation to/from JSON/binary.

settings.gradle

@@ -45,6 +45,7 @@ include 'simulation:halsim_gui'
 include 'simulation:halsim_ws_core'
 include 'simulation:halsim_ws_client'
 include 'simulation:halsim_ws_server'
+include 'simulation:halsim_xrp'
 include 'cameraserver'
 include 'cameraserver:multiCameraServer'
 include 'wpilibNewCommands'

simulation/CMakeLists.txt

@@ -5,3 +5,4 @@ add_subdirectory(halsim_ds_socket)
 add_subdirectory(halsim_ws_core)
 add_subdirectory(halsim_ws_client)
 add_subdirectory(halsim_ws_server)
+add_subdirectory(halsim_xrp)

simulation/halsim_ws_core/src/main/native/cpp/WSProvider_HAL.cpp

@@ -0,0 +1,59 @@
+// Copyright (c) FIRST and other WPILib contributors.
+// Open Source Software; you can modify and/or share it under the terms of
+// the WPILib BSD license file in the root directory of this project.
+
+#include "WSProvider_HAL.h"
+
+#include <algorithm>
+#include <atomic>
+#include <string_view>
+
+#include <hal/Extensions.h>
+#include <hal/HAL.h>
+#include <hal/Ports.h>
+#include <hal/simulation/MockHooks.h>
+#include <wpi/raw_ostream.h>
+
+namespace wpilibws {
+
+void HALSimWSProviderHAL::Initialize(WSRegisterFunc webRegisterFunc) {
+  CreateSingleProvider<HALSimWSProviderHAL>("HAL", webRegisterFunc);
+}
+
+HALSimWSProviderHAL::~HALSimWSProviderHAL() {
+  DoCancelCallbacks();
+}
+
+void HALSimWSProviderHAL::RegisterCallbacks() {
+  m_simPeriodicBeforeCbKey = HALSIM_RegisterSimPeriodicBeforeCallback(
+      [](void* param) {
+        static_cast<HALSimWSProviderHAL*>(param)->ProcessHalCallback(
+            {{">sim_periodic_before", true}});
+      },
+      this);
+
+  m_simPeriodicAfterCbKey = HALSIM_RegisterSimPeriodicAfterCallback(
+      [](void* param) {
+        static_cast<HALSimWSProviderHAL*>(param)->ProcessHalCallback(
+            {{">sim_periodic_after", true}});
+      },
+      this);
+}
+
+void HALSimWSProviderHAL::CancelCallbacks() {
+  DoCancelCallbacks();
+}
+
+void HALSimWSProviderHAL::DoCancelCallbacks() {
+  HALSIM_CancelSimPeriodicBeforeCallback(m_simPeriodicBeforeCbKey);
+  HALSIM_CancelSimPeriodicAfterCallback(m_simPeriodicAfterCbKey);
+
+  m_simPeriodicBeforeCbKey = 0;
+  m_simPeriodicAfterCbKey = 0;
+}
+
+void HALSimWSProviderHAL::OnNetValueChanged(const wpi::json& json) {
+  // no-op. This is all one way
+}
+
+}  // namespace wpilibws

simulation/halsim_ws_core/src/main/native/include/WSProvider_HAL.h

@@ -0,0 +1,32 @@
+// Copyright (c) FIRST and other WPILib contributors.
+// Open Source Software; you can modify and/or share it under the terms of
+// the WPILib BSD license file in the root directory of this project.
+
+#pragma once
+
+#include <memory>
+
+#include "WSHalProviders.h"
+
+namespace wpilibws {
+
+class HALSimWSProviderHAL : public HALSimWSHalProvider {
+ public:
+  static void Initialize(WSRegisterFunc webRegisterFunc);
+
+  using HALSimWSHalProvider::HALSimWSHalProvider;
+  ~HALSimWSProviderHAL() override;
+
+  void OnNetValueChanged(const wpi::json& json) override;
+
+ protected:
+  void RegisterCallbacks() override;
+  void CancelCallbacks() override;
+  void DoCancelCallbacks();
+
+ private:
+  int32_t m_simPeriodicBeforeCbKey = 0;
+  int32_t m_simPeriodicAfterCbKey = 0;
+};
+
+}  // namespace wpilibws

simulation/halsim_xrp/CMakeLists.txt

@@ -0,0 +1,16 @@
+project(halsim_xrp)
+
+include(CompileWarnings)
+
+file(GLOB halsim_xrp_src src/main/native/cpp/*.cpp)
+
+add_library(halsim_xrp SHARED ${halsim_xrp_src})
+wpilib_target_warnings(halsim_xrp)
+set_target_properties(halsim_xrp PROPERTIES DEBUG_POSTFIX "d")
+target_link_libraries(halsim_xrp PUBLIC hal halsim_ws_core)
+
+target_include_directories(halsim_xrp PRIVATE src/main/native/include)
+
+set_property(TARGET halsim_xrp PROPERTY FOLDER "libraries")
+
+install(TARGETS halsim_xrp EXPORT halsim_xrp DESTINATION "${main_lib_dest}")

simulation/halsim_xrp/README.md

@@ -0,0 +1,125 @@
+# HAL XRP Client
+
+This is an extension that provides a client version of the XRP protocol for transmitting robot hardware interface state to an XRP robot over UDP.
+
+## Configuration
+
+The XRP client has a number of configuration options available through environment variables.
+
+``HALSIMXRP_HOST``: The host to connect to.  Defaults to localhost.
+
+``HALSIMXRP_PORT``: The port number to connect to.  Defaults to 3540.
+
+## XRP Protocol
+
+The WPILib -> XRP protocol is binary-based to save on bandwidth due to hardware limitations of the XRP robot. The messages to/from the XRP follow a the format below:
+
+| 2 bytes             | 1 byte            | n bytes                             |
+|---------------------|-------------------|-------------------------------------|
+| _uint16_t_ sequence | _uint8_t_ control | [<Tagged Data>](#tagged-data) |
+
+### Control Byte
+
+The control byte is used to indicate the current `enabled` state of the WPILib robot code. When this is set to `1`, the robot is enabled, and when it is set to `0` it is disabled.
+
+Messages originating from the XRP have an unspecified value for the control byte.
+
+### Tagged Data
+
+The `Tagged Data` section can contain an arbitrary number of data blocks. Each block has the format below:
+
+| 1 byte         | 1 byte          | n bytes         |
+|----------------|-----------------|-----------------|
+| _uint8_t_ size | _uint8_t_ tagID | <payload> |
+
+The `size` byte encodes the size of the data block, _excluding_ itself. Thus the smallest block size is 2 bytes, with a size value of 1 (1 size byte, 1 tag byte, 0 payload bytes). Maximum size of the payload is 254 bytes.
+
+
+Utilizing tagged data blocks allows us to send multiple pieces of data in a single UDP packet. The tags currently implemented for the XRP are as follows:
+
+| Tag  | Description                   |
+|------|-------------------------------|
+| 0x12 | [XRPMotor](#xrpmotor)         |
+| 0x13 | [XRPServo](#xrpservo)         |
+| 0x14 | [DIO](#dio)                   |
+| 0x15 | [AnalogIn](#analogin)         |
+| 0x16 | [XRPGyro](#xrpgyro)           |
+| 0x17 | [BuiltInAccel](#builtinaccel) |
+| 0x18 | [Encoder](#encoder)           |
+
+
+#### XRPMotor
+
+| Order | Data Type | Description       |
+|-------|-----------|-------------------|
+| 0     | _uint8_t_ | ID                |
+| 1     | _float_   | Value [-1.0, 1.0] |
+
+IDs:
+| ID | Description |
+|----|-------------|
+| 0  | Left Motor  |
+| 1  | Right Motor |
+| 2  | Motor 3     |
+| 3  | Motor 4     |
+
+#### XRPServo
+
+| Order | Data Type | Description      |
+|-------|-----------|------------------|
+| 0     | _uint8_t_ | ID               |
+| 1     | _float_   | Value [0.0, 1.0] |
+
+IDs:
+| ID | Description |
+|----|-------------|
+| 4  | Servo 1     |
+| 5  | Servo 2     |
+
+#### DIO
+
+| Order | Data Type | Description        |
+|-------|-----------|--------------------|
+| 0     | _uint8_t_ | ID                 |
+| 1     | _uint8_t_ | Value (True/False) |
+
+#### AnalogIn
+
+| Order | Data Type | Description |
+|-------|-----------|-------------|
+| 0     | _uint8_t_ | ID          |
+| 1     | _float_   | Value       |
+
+#### XRPGyro
+
+| Order | Data Type | Description   |
+|-------|-----------|---------------|
+| 0     | _float_   | rate_x (dps)  |
+| 1     | _float_   | rate_y (dps)  |
+| 2     | _float_   | rate_z (dps)  |
+| 3     | _float_   | angle_x (deg) |
+| 4     | _float_   | angle_y (deg) |
+| 5     | _float_   | angle_z (deg) |
+
+#### BuiltInAccel
+
+| Order | Data Type | Description |
+|-------|-----------|-------------|
+| 0     | _float_   | accel_x (g) |
+| 1     | _float_   | accel_y (g) |
+| 2     | _float_   | accel_z (g) |
+
+#### Encoder
+
+| Order | Data Type | Description |
+|-------|-----------|-------------|
+| 0     | _uint8_t_ | ID          |
+| 1     | _int32_t_ | Value       |
+
+IDs:
+| ID | Description         |
+|----|---------------------|
+| 0  | Left Motor Encoder  |
+| 1  | Right Motor Encoder |
+| 2  | Motor 3 Encoder     |
+| 3  | Motor 4 Encoder     |

simulation/halsim_xrp/build.gradle

@@ -0,0 +1,35 @@
+if (!project.hasProperty('onlylinuxathena')) {
+
+    description = "XRP Extension"
+
+    ext {
+        includeWpiutil = true
+        pluginName = 'halsim_xrp'
+    }
+
+    apply plugin: 'google-test-test-suite'
+
+
+    ext {
+        staticGtestConfigs = [:]
+    }
+
+    staticGtestConfigs["${pluginName}Test"] = []
+    apply from: "${rootDir}/shared/googletest.gradle"
+
+    apply from: "${rootDir}/shared/plugins/setupBuild.gradle"
+
+    model {
+        binaries {
+            all {
+                if (it.targetPlatform.name == nativeUtils.wpi.platforms.roborio) {
+                    it.buildable = false
+                    return
+                }
+
+                lib project: ':wpinet', library: 'wpinet', linkage: 'shared'
+                lib project: ":simulation:halsim_ws_core", library: "halsim_ws_core", linkage: "static"
+            }
+        }
+    }
+}

simulation/halsim_xrp/src/dev/native/cpp/main.cpp

@@ -0,0 +1,5 @@
+// Copyright (c) FIRST and other WPILib contributors.
+// Open Source Software; you can modify and/or share it under the terms of
+// the WPILib BSD license file in the root directory of this project.
+
+int main() {}

simulation/halsim_xrp/src/main/native/cpp/HALSimXRP.cpp

@@ -0,0 +1,164 @@
+// Copyright (c) FIRST and other WPILib contributors.
+// Open Source Software; you can modify and/or share it under the terms of
+// the WPILib BSD license file in the root directory of this project.
+
+#include "HALSimXRP.h"
+
+#include <cstdio>
+
+#include <fmt/format.h>
+#include <wpi/Endian.h>
+#include <wpi/MathExtras.h>
+#include <wpi/SmallString.h>
+#include <wpinet/raw_uv_ostream.h>
+#include <wpinet/uv/util.h>
+
+namespace uv = wpi::uv;
+
+using namespace wpilibxrp;
+
+HALSimXRP::HALSimXRP(wpi::uv::Loop& loop,
+                     wpilibws::ProviderContainer& providers,
+                     wpilibws::HALSimWSProviderSimDevices& simDevicesProvider)
+    : m_loop(loop),
+      m_providers(providers),
+      m_simDevicesProvider(simDevicesProvider) {
+  m_loop.error.connect([](uv::Error err) {
+    fmt::print(stderr, "HALSim XRP Client libuv Error: {}\n", err.str());
+  });
+
+  m_udp_client = uv::Udp::Create(m_loop);
+  m_exec = UvExecFunc::Create(m_loop);
+  if (m_exec) {
+    m_exec->wakeup.connect([](auto func) { func(); });
+  }
+}
+
+bool HALSimXRP::Initialize() {
+  if (!m_udp_client || !m_exec) {
+    return false;
+  }
+
+  const char* host = std::getenv("HALSIMXRP_HOST");
+  if (host != nullptr) {
+    m_host = host;
+  } else {
+    m_host = "localhost";
+  }
+
+  const char* port = std::getenv("HALSIMXRP_PORT");
+  if (port != nullptr) {
+    try {
+      m_port = std::stoi(port);
+    } catch (const std::invalid_argument& err) {
+      fmt::print(stderr, "Error decoding HALSIMXRP_PORT ({})\n", err.what());
+      return false;
+    }
+  } else {
+    m_port = 3540;
+  }
+
+  wpilibxrp::WPILibUpdateFunc func = [&](const wpi::json& data) {
+    OnNetValueChanged(data);
+  };
+
+  m_xrp.SetWPILibUpdateFunc(func);
+
+  return true;
+}
+
+void HALSimXRP::Start() {
+  // struct sockaddr_in dest;
+  uv::NameToAddr(m_host, m_port, &m_dest);
+
+  m_udp_client->Connect(m_dest);
+
+  m_udp_client->received.connect(
+      [this, socket = m_udp_client.get()](auto data, size_t len, auto rinfo,
+                                          unsigned int flags) {
+        ParsePacket({reinterpret_cast<uint8_t*>(data.base), len});
+      });
+
+  m_udp_client->closed.connect([]() { fmt::print("Socket Closed\n"); });
+
+  // Fake the OnNetworkConnected call
+  auto hws = shared_from_this();
+  m_simDevicesProvider.OnNetworkConnected(hws);
+  m_providers.ForEach(
+      [hws](std::shared_ptr<wpilibws::HALSimWSBaseProvider> provider) {
+        provider->OnNetworkConnected(hws);
+      });
+
+  m_udp_client->StartRecv();
+
+  std::puts("HALSimXRP Initialized");
+}
+
+void HALSimXRP::ParsePacket(std::span<const uint8_t> packet) {
+  if (packet.size() < 3) {
+    return;
+  }
+
+  // Hand this off to the XRP object to deal with the messages
+  m_xrp.HandleXRPUpdate(packet);
+}
+
+void HALSimXRP::OnNetValueChanged(const wpi::json& msg) {
+  try {
+    auto& type = msg.at("type").get_ref<const std::string&>();
+    auto& device = msg.at("device").get_ref<const std::string&>();
+
+    wpi::SmallString<64> key;
+    key.append(type);
+    if (!device.empty()) {
+      key.append("/");
+      key.append(device);
+    }
+
+    auto provider = m_providers.Get(key.str());
+    if (provider) {
+      provider->OnNetValueChanged(msg.at("data"));
+    }
+  } catch (wpi::json::exception& e) {
+    fmt::print(stderr, "Error with incoming message: {}\n", e.what());
+  }
+}
+
+void HALSimXRP::OnSimValueChanged(const wpi::json& simData) {
+  // We'll use a signal from robot code to send all the data
+  if (simData["type"] == "HAL") {
+    auto halData = simData["data"];
+    if (halData.find(">sim_periodic_after") != halData.end()) {
+      SendStateToXRP();
+    }
+  } else {
+    m_xrp.HandleWPILibUpdate(simData);
+  }
+}
+
+uv::SimpleBufferPool<4>& HALSimXRP::GetBufferPool() {
+  static uv::SimpleBufferPool<4> bufferPool(128);
+  return bufferPool;
+}
+
+void HALSimXRP::SendStateToXRP() {
+  wpi::SmallVector<uv::Buffer, 4> sendBufs;
+  wpi::raw_uv_ostream stream{sendBufs, [&] {
+                               std::lock_guard lock(m_buffer_mutex);
+                               return GetBufferPool().Allocate();
+                             }};
+  m_xrp.SetupXRPSendBuffer(stream);
+
+  m_exec->Send([this, sendBufs]() mutable {
+    m_udp_client->Send(sendBufs, [&](auto bufs, uv::Error err) {
+      {
+        std::lock_guard lock(m_buffer_mutex);
+        GetBufferPool().Release(bufs);
+      }
+
+      if (err) {
+        // no-op
+      }
+    });
+  });
+}

simulation/halsim_xrp/src/main/native/cpp/HALSimXRPClient.cpp

@@ -0,0 +1,48 @@
+// Copyright (c) FIRST and other WPILib contributors.
+// Open Source Software; you can modify and/or share it under the terms of
+// the WPILib BSD license file in the root directory of this project.
+
+#include "HALSimXRPClient.h"
+
+#include <WSProviderContainer.h>
+#include <WSProvider_Analog.h>
+#include <WSProvider_BuiltInAccelerometer.h>
+#include <WSProvider_DIO.h>
+#include <WSProvider_DriverStation.h>
+#include <WSProvider_Encoder.h>
+#include <WSProvider_HAL.h>
+#include <WSProvider_SimDevice.h>
+#include <wpinet/EventLoopRunner.h>
+
+using namespace wpilibxrp;
+using namespace wpilibws;
+
+bool HALSimXRPClient::Initialize() {
+  bool result = true;
+  runner.ExecSync([&](wpi::uv::Loop& loop) {
+    simxrp = std::make_shared<HALSimXRP>(loop, providers, simDevices);
+
+    if (!simxrp->Initialize()) {
+      result = false;
+      return;
+    }
+
+    WSRegisterFunc registerFunc = [&](auto key, auto provider) {
+      providers.Add(key, provider);
+    };
+
+    // Minimized set of HAL providers
+    HALSimWSProviderAnalogIn::Initialize(registerFunc);
+    HALSimWSProviderBuiltInAccelerometer::Initialize(registerFunc);
+    HALSimWSProviderDIO::Initialize(registerFunc);
+    HALSimWSProviderDriverStation::Initialize(registerFunc);
+    HALSimWSProviderEncoder::Initialize(registerFunc);
+    HALSimWSProviderHAL::Initialize(registerFunc);
+
+    simDevices.Initialize(loop);
+
+    simxrp->Start();
+  });
+
+  return result;
+}

simulation/halsim_xrp/src/main/native/cpp/XRP.cpp

@@ -0,0 +1,373 @@
+// Copyright (c) FIRST and other WPILib contributors.
+// Open Source Software; you can modify and/or share it under the terms of
+// the WPILib BSD license file in the root directory of this project.
+
+#include "XRP.h"
+
+#include <fmt/format.h>
+#include <wpi/Endian.h>
+#include <wpi/MathExtras.h>
+
+using namespace wpilibxrp;
+
+XRP::XRP()
+    : m_gyro_name{"XRPGyro"}, m_wpilib_update_func([](const wpi::json&) {}) {
+  // Set up the inputs and outputs
+  m_motor_outputs.emplace(0, 0.0f);
+  m_motor_outputs.emplace(1, 0.0f);
+  m_motor_outputs.emplace(2, 0.0f);
+  m_motor_outputs.emplace(3, 0.0f);
+
+  m_servo_outputs.emplace(4, 0.5f);
+  m_servo_outputs.emplace(5, 0.5f);
+
+  m_encoder_inputs.emplace(1, 0);
+  m_encoder_inputs.emplace(2, 0);
+  m_encoder_inputs.emplace(0, 0);
+  m_encoder_inputs.emplace(3, 0);
+}
+
+void XRP::HandleWPILibUpdate(const wpi::json& data) {
+  if (data.count("type") == 0) {
+    return;
+  }
+
+  if (data["type"] == "DriverStation") {
+    HandleDriverStationSimValueChanged(data);
+  } else if (data["type"] == "XRPMotor") {
+    HandleMotorSimValueChanged(data);
+  } else if (data["type"] == "XRPServo") {
+    HandleServoSimValueChanged(data);
+  } else if (data["type"] == "DIO") {
+    HandleDIOSimValueChanged(data);
+  } else if (data["type"] == "Gyro") {
+    HandleGyroSimValueChanged(data);
+  } else if (data["type"] == "Encoder") {
+    HandleEncoderSimValueChanged(data);
+  }
+}
+
+void XRP::HandleXRPUpdate(std::span<const uint8_t> packet) {
+  uint16_t seq = (packet[0] << 8) + packet[1];
+
+  if (seq <= m_wpilib_bound_seq) {
+    // If the old sequence was within 3 or uint16_t max and the new
+    // sequence is < 3 - we've prob rolled over
+    if (!((0xFFFF - m_wpilib_bound_seq < 3) && seq < 3)) {
+      return;
+    }
+  }
+
+  m_wpilib_bound_seq = seq;
+
+  // Tagged data starts at byte 3
+  packet = packet.subspan(3);
+
+  // Loop to handle multiple tags
+  while (!packet.empty()) {
+    auto tagLength = packet[0];
+    auto tagPacket = packet.subspan(0, tagLength + 1);
+
+    // NOTE: tagPacket contains the size and tag bytes as well
+    // Verify that the packet is indeed the right size
+    if (tagPacket.size() != static_cast<size_t>(tagLength + 1)) {
+      break;
+    }
+
+    switch (packet[1]) {
+      case XRP_TAG_GYRO:
+        ReadGyroTag(tagPacket);
+        break;
+      case XRP_TAG_ACCEL:
+        ReadAccelTag(tagPacket);
+        break;
+      case XRP_TAG_DIO:
+        ReadDIOTag(tagPacket);
+        break;
+      case XRP_TAG_ENCODER:
+        ReadEncoderTag(tagPacket);
+        break;
+      case XRP_TAG_ANALOG:
+        ReadAnalogTag(tagPacket);
+        break;
+    }
+
+    packet = packet.subspan(tagLength + 1);
+  }
+}
+
+void XRP::SetupXRPSendBuffer(wpi::raw_uv_ostream& buf) {
+  SetupSendHeader(buf);
+  SetupMotorTag(buf);
+  SetupServoTag(buf);
+  SetupDigitalOutTag(buf);
+  m_xrp_bound_seq++;
+}
+
+// WPILib Sim Handlers
+void XRP::HandleDriverStationSimValueChanged(const wpi::json& data) {
+  auto dsData = data["data"];
+  if (dsData.find(">enabled") != dsData.end()) {
+    m_robot_enabled = dsData[">enabled"];
+  }
+}
+
+void XRP::HandleMotorSimValueChanged(const wpi::json& data) {
+  int deviceId = -1;
+  auto motorData = data["data"];
+
+  if (data["device"] == "motorL") {
+    deviceId = 0;
+  } else if (data["device"] == "motorR") {
+    deviceId = 1;
+  } else if (data["device"] == "motor3") {
+    deviceId = 2;
+  } else if (data["device"] == "motor4") {
+    deviceId = 3;
+  }
+
+  if (deviceId != -1 && motorData.find("<speed") != motorData.end()) {
+    m_motor_outputs[deviceId] = motorData["<speed"];
+  }
+}
+
+void XRP::HandleServoSimValueChanged(const wpi::json& data) {
+  int deviceId = -1;
+  auto servoData = data["data"];
+
+  if (data["device"] == "servo1") {
+    deviceId = 4;
+  } else if (data["device"] == "servo2") {
+    deviceId = 5;
+  }
+
+  if (deviceId != -1 && servoData.find("<position") != servoData.end()) {
+    m_servo_outputs[deviceId] = servoData["<position"];
+  }
+}
+
+void XRP::HandleDIOSimValueChanged(const wpi::json& data) {
+  int deviceId = -1;
+  auto dioData = data["data"];
+
+  try {
+    deviceId = std::stoi(data["device"].get<std::string>());
+  } catch (const std::invalid_argument&) {
+    deviceId = -1;
+  }
+
+  // Bail out early if device ID is invalid or if it's "spoken for"
+  if (deviceId == -1) {
+    return;
+  }
+
+  if (dioData.find("<init") != dioData.end() && dioData["<init"]) {
+    // All DIOs are initialized as inputs by default
+    m_digital_inputs.emplace(deviceId, false);
+  }
+
+  if (dioData.find("<input") != dioData.end() && dioData["<input"] == false) {
+    // We're registering an output device
+    // Remove from the digital inputs list (if present)
+    m_digital_inputs.erase(deviceId);
+    m_digital_outputs.emplace(deviceId, false);
+  }
+
+  if (dioData.find("<>value") != dioData.end() &&
+      m_digital_outputs.count(deviceId) > 0) {
+    m_digital_outputs[deviceId] = dioData["<>value"];
+  }
+}
+
+void XRP::HandleGyroSimValueChanged(const wpi::json& data) {
+  m_gyro_name = data["device"].get<std::string>();
+}
+
+void XRP::HandleEncoderSimValueChanged(const wpi::json& data) {
+  // We need to handle the various encoder cases
+  // 4/5 -> Encoder 0
+  // 6/7 -> Encoder 1
+  // 8/9 -> Encoder 2
+  // 10/11 -> Encoder 3
+  int deviceId = -1;
+  auto encData = data["data"];
+
+  try {
+    deviceId = std::stoi(data["device"].get<std::string>());
+  } catch (const std::invalid_argument&) {
+    deviceId = -1;
+  }
+
+  if (deviceId == -1) {
+    return;
+  }
+
+  if (encData.find("<init") != encData.end() && encData["<init"]) {
+    // The <channel_a and <channel_b values come with the init message
+    int chA = encData["<channel_a"];
+    int chB = encData["<channel_b"];
+
+    if ((chA == 4 && chB == 5) || (chA == 5 && chB == 4)) {
+      m_encoder_channel_map.emplace(0, deviceId);
+    } else if ((chA == 6 && chB == 7) || (chA == 7 && chB == 6)) {
+      m_encoder_channel_map.emplace(1, deviceId);
+    } else if ((chA == 8 && chB == 9) || (chA == 9 && chB == 8)) {
+      m_encoder_channel_map.emplace(2, deviceId);
+    } else if ((chA == 10 && chB == 11) || (chA == 11 && chB == 10)) {
+      m_encoder_channel_map.emplace(3, deviceId);
+    }
+  }
+}
+
+// ==================================
+// XRP Buffer Generation/Read Methods
+// ==================================
+
+void XRP::SetupSendHeader(wpi::raw_uv_ostream& buf) {
+  uint8_t pktSeq[2];
+  wpi::support::endian::write16be(pktSeq, m_xrp_bound_seq);
+
+  buf << pktSeq[0] << pktSeq[1]
+      << static_cast<uint8_t>(m_robot_enabled ? 1 : 0);
+}
+
+void XRP::SetupMotorTag(wpi::raw_uv_ostream& buf) {
+  uint8_t value[4];
+
+  for (auto motor : m_motor_outputs) {
+    // Motor payload is 6 bytes
+    buf << static_cast<uint8_t>(6)              // Size
+        << static_cast<uint8_t>(XRP_TAG_MOTOR)  // Tag
+        << static_cast<uint8_t>(motor.first);   // Channel
+
+    // Convert the value
+    wpi::support::endian::write32be(value, wpi::FloatToBits(motor.second));
+    buf << value[0] << value[1] << value[2] << value[3];
+  }
+}
+
+void XRP::SetupServoTag(wpi::raw_uv_ostream& buf) {
+  uint8_t value[4];
+
+  for (auto servo : m_servo_outputs) {
+    // Servo payload is 6 bytes
+    buf << static_cast<uint8_t>(6)              // Size
+        << static_cast<uint8_t>(XRP_TAG_SERVO)  // Tag
+        << static_cast<uint8_t>(servo.first);   // Channel
+
+    // Convert the value
+    wpi::support::endian::write32be(value, wpi::FloatToBits(servo.second));
+    buf << value[0] << value[1] << value[2] << value[3];
+  }
+}
+
+void XRP::SetupDigitalOutTag(wpi::raw_uv_ostream& buf) {
+  for (auto digitalOut : m_digital_outputs) {
+    // DIO payload is 3 bytes
+    buf << static_cast<uint8_t>(3)                           // Size
+        << static_cast<uint8_t>(XRP_TAG_DIO)                 // Tag
+        << static_cast<uint8_t>(digitalOut.first)            // Channel
+        << static_cast<uint8_t>(digitalOut.second ? 1 : 0);  // Value
+  }
+}
+
+void XRP::ReadGyroTag(std::span<const uint8_t> packet) {
+  if (packet.size() < 26) {
+    return;  // size(1) + tag(1) + 6x 4byte
+  }
+
+  packet = packet.subspan(2);  // Skip past the size and tag
+  float rate_x = wpi::BitsToFloat(wpi::support::endian::read32be(&packet[0]));
+  float rate_y = wpi::BitsToFloat(wpi::support::endian::read32be(&packet[4]));
+  float rate_z = wpi::BitsToFloat(wpi::support::endian::read32be(&packet[8]));
+  float angle_x = wpi::BitsToFloat(wpi::support::endian::read32be(&packet[12]));
+  float angle_y = wpi::BitsToFloat(wpi::support::endian::read32be(&packet[16]));
+  float angle_z = wpi::BitsToFloat(wpi::support::endian::read32be(&packet[20]));
+
+  // Make the json object
+  wpi::json gyroJson;
+  gyroJson["type"] = "Gyro";
+  gyroJson["device"] = m_gyro_name;
+  gyroJson["data"] = {{">rate_x", rate_x},   {">rate_y", rate_y},
+                      {">rate_z", rate_z},   {">angle_x", angle_x},
+                      {">angle_y", angle_y}, {">angle_z", angle_z}};
+
+  // Update WPILib
+  m_wpilib_update_func(gyroJson);
+}
+
+void XRP::ReadAccelTag(std::span<const uint8_t> packet) {
+  if (packet.size() < 14) {
+    return;  // size(1) + tag(1) + 3x 4 byte
+  }
+
+  packet = packet.subspan(2);  // Skip past the size and tag
+  float accel_x = wpi::BitsToFloat(wpi::support::endian::read32be(&packet[0]));
+  float accel_y = wpi::BitsToFloat(wpi::support::endian::read32be(&packet[4]));
+  float accel_z = wpi::BitsToFloat(wpi::support::endian::read32be(&packet[8]));
+
+  wpi::json accelJson;
+  accelJson["type"] = "Accel";
+  accelJson["device"] = "BuiltInAccel";
+  accelJson["data"] = {{">x", accel_x}, {">y", accel_y}, {">z", accel_z}};
+
+  // Update WPILib
+  m_wpilib_update_func(accelJson);
+}
+
+void XRP::ReadDIOTag(std::span<const uint8_t> packet) {
+  if (packet.size() < 4) {
+    return;  // size(1) + tag(1) + id(1) + value(1)
+  }
+
+  wpi::json dioJson;
+  dioJson["type"] = "DIO";
+  dioJson["device"] = std::to_string(packet[2]);
+  dioJson["data"] = {{"<>value", packet[3] == 1}};
+
+  m_wpilib_update_func(dioJson);
+}
+
+void XRP::ReadEncoderTag(std::span<const uint8_t> packet) {
+  if (packet.size() < 7) {
+    return;  // size(1) + tag(1) + id(1) + value(4)
+  }
+
+  uint8_t encoderId = packet[2];
+
+  packet = packet.subspan(3);  // Skip past the size and tag and ID
+  int32_t value =
+      static_cast<int32_t>(wpi::support::endian::read32be(&packet[0]));
+
+  // Look up the registered encoders
+  if (m_encoder_channel_map.count(encoderId) == 0) {
+    return;
+  }
+
+  uint8_t wpilibEncoderChannel = m_encoder_channel_map[encoderId];
+
+  wpi::json encJson;
+  encJson["type"] = "Encoder";
+  encJson["device"] = std::to_string(wpilibEncoderChannel);
+  encJson["data"] = {{">count", value}};
+
+  m_wpilib_update_func(encJson);
+}
+
+void XRP::ReadAnalogTag(std::span<const uint8_t> packet) {
+  if (packet.size() < 7) {
+    return;  // size(1) + tag(1) + id(1) + float
+  }
+
+  uint8_t analogId = packet[2];
+
+  packet = packet.subspan(3);
+  float voltage = wpi::BitsToFloat(wpi::support::endian::read32be(&packet[0]));
+
+  wpi::json analogJson;
+  analogJson["type"] = "AI";
+  analogJson["device"] = std::to_string(analogId);
+  analogJson["data"] = {{">voltage", voltage}};
+
+  m_wpilib_update_func(analogJson);
+}

simulation/halsim_xrp/src/main/native/cpp/main.cpp

@@ -0,0 +1,42 @@
+// Copyright (c) FIRST and other WPILib contributors.
+// Open Source Software; you can modify and/or share it under the terms of
+// the WPILib BSD license file in the root directory of this project.
+
+#include <cstdio>
+#include <memory>
+
+#include <hal/Extensions.h>
+
+#include "HALSimXRPClient.h"
+
+#if defined(Win32) || defined(_WIN32)
+#pragma comment(lib, "Ws2_32.lib")
+#endif
+
+using namespace wpilibxrp;
+
+static std::unique_ptr<HALSimXRPClient> gClient;
+
+/*--------------------------------------------------------------------------
+** Main Entry Point. Start up the listening threads
+**------------------------------------------------------------------------*/
+extern "C" {
+#if defined(WIN32) || defined(_WIN32)
+__declspec(dllexport)
+#endif
+
+    int HALSIM_InitExtension(void) {
+  std::puts("HALSim XRP Extension Initializing");
+
+  HAL_OnShutdown(nullptr, [](void*) { gClient.reset(); });
+
+  gClient = std::make_unique<HALSimXRPClient>();
+  if (!gClient->Initialize()) {
+    return -1;
+  }
+
+  std::puts("HALSim XRP Extention Initialized");
+  return 0;
+}
+
+}  // extern "C"

simulation/halsim_xrp/src/main/native/include/HALSimXRP.h

@@ -0,0 +1,75 @@
+// Copyright (c) FIRST and other WPILib contributors.
+// Open Source Software; you can modify and/or share it under the terms of
+// the WPILib BSD license file in the root directory of this project.
+
+#pragma once
+
+#include <functional>
+#include <memory>
+#include <span>
+#include <string>
+
+#include <HALSimBaseWebSocketConnection.h>
+#include <WSProviderContainer.h>
+#include <WSProvider_SimDevice.h>
+#include <wpinet/uv/Async.h>
+#include <wpinet/uv/Buffer.h>
+#include <wpinet/uv/Loop.h>
+#include <wpinet/uv/Timer.h>
+#include <wpinet/uv/Udp.h>
+
+#include "XRP.h"
+
+namespace wpi {
+class json;
+}  // namespace wpi
+
+namespace wpilibxrp {
+
+// This masquerades as a "WebSocket" so that we can reuse the
+// stuff in halsim_ws_core
+class HALSimXRP : public wpilibws::HALSimBaseWebSocketConnection,
+                  public std::enable_shared_from_this<HALSimXRP> {
+ public:
+  using LoopFunc = std::function<void(void)>;
+  using UvExecFunc = wpi::uv::Async<LoopFunc>;
+
+  HALSimXRP(wpi::uv::Loop& loop, wpilibws::ProviderContainer& providers,
+            wpilibws::HALSimWSProviderSimDevices& simDevicesProvider);
+  HALSimXRP(const HALSimXRP&) = delete;
+  HALSimXRP& operator=(const HALSimXRP&) = delete;
+
+  bool Initialize();
+  void Start();
+
+  void ParsePacket(std::span<const uint8_t> packet);
+  void OnNetValueChanged(const wpi::json& msg);
+  void OnSimValueChanged(const wpi::json& simData) override;
+
+  const std::string& GetTargetHost() const { return m_host; }
+  int GetTargetPort() const { return m_port; }
+  wpi::uv::Loop& GetLoop() { return m_loop; }
+
+  UvExecFunc& GetExec() { return *m_exec; }
+
+ private:
+  XRP m_xrp;
+
+  wpi::uv::Loop& m_loop;
+  std::shared_ptr<wpi::uv::Udp> m_udp_client;
+  std::shared_ptr<UvExecFunc> m_exec;
+
+  wpilibws::ProviderContainer& m_providers;
+  wpilibws::HALSimWSProviderSimDevices& m_simDevicesProvider;
+
+  std::string m_host;
+  int m_port;
+
+  void SendStateToXRP();
+  wpi::uv::SimpleBufferPool<4>& GetBufferPool();
+  std::mutex m_buffer_mutex;
+
+  struct sockaddr_in m_dest;
+};
+
+}  // namespace wpilibxrp

simulation/halsim_xrp/src/main/native/include/HALSimXRPClient.h

@@ -0,0 +1,31 @@
+// Copyright (c) FIRST and other WPILib contributors.
+// Open Source Software; you can modify and/or share it under the terms of
+// the WPILib BSD license file in the root directory of this project.
+
+#pragma once
+
+#include <memory>
+
+#include <WSProviderContainer.h>
+#include <WSProvider_SimDevice.h>
+#include <wpinet/EventLoopRunner.h>
+
+#include "HALSimXRP.h"
+
+namespace wpilibxrp {
+
+class HALSimXRPClient {
+ public:
+  HALSimXRPClient() = default;
+  HALSimXRPClient(const HALSimXRPClient&) = delete;
+  HALSimXRPClient& operator=(const HALSimXRPClient&) = delete;
+
+  bool Initialize();
+
+  wpilibws::ProviderContainer providers;
+  wpilibws::HALSimWSProviderSimDevices simDevices{providers};
+  wpi::EventLoopRunner runner;
+  std::shared_ptr<HALSimXRP> simxrp;
+};
+
+}  // namespace wpilibxrp

simulation/halsim_xrp/src/main/native/include/XRP.h

@@ -0,0 +1,88 @@
+// Copyright (c) FIRST and other WPILib contributors.
+// Open Source Software; you can modify and/or share it under the terms of
+// the WPILib BSD license file in the root directory of this project.
+
+#pragma once
+
+#include <functional>
+#include <map>
+#include <span>
+#include <string>
+
+#include <wpi/json.h>
+#include <wpinet/raw_uv_ostream.h>
+
+#define XRP_TAG_MOTOR 0x12
+#define XRP_TAG_SERVO 0x13
+#define XRP_TAG_DIO 0x14
+#define XRP_TAG_ANALOG 0x15
+#define XRP_TAG_GYRO 0x16
+#define XRP_TAG_ACCEL 0x17
+#define XRP_TAG_ENCODER 0x18
+
+namespace wpilibxrp {
+
+using WPILibUpdateFunc = std::function<void(const wpi::json&)>;
+
+class XRP {
+ public:
+  XRP();
+
+  void SetWPILibUpdateFunc(WPILibUpdateFunc func) {
+    m_wpilib_update_func = func;
+  }
+
+  void HandleWPILibUpdate(const wpi::json& data);
+  void HandleXRPUpdate(std::span<const uint8_t> packet);
+
+  void SetupXRPSendBuffer(wpi::raw_uv_ostream& buf);
+
+ private:
+  // To XRP Methods
+  void SetupSendHeader(wpi::raw_uv_ostream& buf);
+  void SetupMotorTag(wpi::raw_uv_ostream& buf);
+  void SetupServoTag(wpi::raw_uv_ostream& buf);
+  void SetupDigitalOutTag(wpi::raw_uv_ostream& buf);
+
+  // WPILib Sim Update Handlers
+  void HandleDriverStationSimValueChanged(const wpi::json& data);
+  void HandleMotorSimValueChanged(const wpi::json& data);
+  void HandleServoSimValueChanged(const wpi::json& data);
+  void HandleDIOSimValueChanged(const wpi::json& data);
+  void HandleGyroSimValueChanged(const wpi::json& data);
+  void HandleEncoderSimValueChanged(const wpi::json& data);
+
+  // XRP Packet Update Handlers
+  void ReadGyroTag(std::span<const uint8_t> packet);
+  void ReadAccelTag(std::span<const uint8_t> packet);
+  void ReadDIOTag(std::span<const uint8_t> packet);
+  void ReadEncoderTag(std::span<const uint8_t> packet);
+  void ReadAnalogTag(std::span<const uint8_t> packet);
+
+  // Robot State
+  std::map<uint8_t, bool> m_digital_outputs;
+  std::map<uint8_t, float> m_motor_outputs;
+  std::map<uint8_t, float> m_servo_outputs;
+
+  // Might not need these
+  std::map<uint8_t, bool> m_digital_inputs;
+  std::map<uint8_t, float> m_analog_inputs;
+  std::map<uint8_t, int32_t> m_encoder_inputs;
+
+  // We need a map from XRP encoder channels (0=left, 1=right etc)
+  // to WPILib device ID
+  // Key: XRP encoder number, Value: WPILib channel
+  // If no encoders are init-ed, this map is empty
+  std::map<uint8_t, uint8_t> m_encoder_channel_map;
+
+  uint16_t m_wpilib_bound_seq = 0;
+  uint16_t m_xrp_bound_seq = 0;
+
+  bool m_robot_enabled = false;
+
+  std::string m_gyro_name;
+
+  WPILibUpdateFunc m_wpilib_update_func;
+};
+
+}  // namespace wpilibxrp