Team2890/allwpilib
4
0
Fork 0
mirror of https://github.com/wpilibsuite/allwpilib synced 2026-06-19 00:41:43 +00:00
Code Issues Packages Projects Releases Wiki Activity

[hal] Use MrcLib to talk to DS (#8858)

Browse Source 
Using MrcLib on the robot is going to be the plan for the future, to
make things easier.

MrcLib is how sim is supported going forward. The desktop version of
mrclib can act as a robot server.

This is set up where the mrclib interface is in shared code. On robot,
that is the only backend used. On desktop, a default sim backend is
used. However, the sim plugin can switch that to the real robot backend,
so the robot code will exactly look like a real robot.
This commit is contained in:
Thad House
2026-06-06 12:15:17 -07:00
committed by GitHub
parent ddf9306264
commit 6e5171cd8f
47 changed files with 1754 additions and 1721 deletions
Download Patch File Download Diff File Expand all files Collapse all files

4
simulation/CMakeLists.txt
View File

@@ -1,7 +1,9 @@
if(WITH_GUI)
add_subdirectory(halsim_gui)
endif()
add_subdirectory(halsim_ds_socket)
# DS socket not supported in cmake
#add_subdirectory(halsim_ds_socket)
add_subdirectory(halsim_ws_core)
add_subdirectory(halsim_ws_client)
add_subdirectory(halsim_ws_server)

308
simulation/halsim_ds_socket/src/main/native/cpp/DSCommPacket.cpp
View File

@@ -1,308 +0,0 @@
// 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 "wpi/halsim/ds_socket/DSCommPacket.hpp"
#include <algorithm>
#include <cstring>
#include <span>
#include "wpi/hal/DashboardOpMode.hpp"
#include "wpi/hal/DriverStationTypes.h"
#include "wpi/hal/simulation/DriverStationData.h"
#include "wpi/hal/simulation/MockHooks.h"
using namespace halsim;
HAL_JoystickPOV DegreesToPOV(int degrees) {
switch (degrees) {
case 0:
return HAL_JOYSTICK_POV_UP;
case 45:
return HAL_JOYSTICK_POV_RIGHT_UP;
case 90:
return HAL_JOYSTICK_POV_RIGHT;
case 135:
return HAL_JOYSTICK_POV_RIGHT_DOWN;
case 180:
return HAL_JOYSTICK_POV_DOWN;
case 225:
return HAL_JOYSTICK_POV_LEFT_DOWN;
case 270:
return HAL_JOYSTICK_POV_LEFT;
case 315:
return HAL_JOYSTICK_POV_LEFT_UP;
case -1:
default:
return HAL_JOYSTICK_POV_CENTERED;
}
}
DSCommPacket::DSCommPacket() {
for (auto& i : m_joystick_packets) {
i.ResetTcp();
i.ResetUdp();
}
}
/*----------------------------------------------------------------------------
** The following methods help parse and hold information about the
** driver station and it's joysticks.
**--------------------------------------------------------------------------*/
void DSCommPacket::SetControl(uint8_t control, uint8_t request) {
HAL_RobotMode robotMode;
if ((control & kAutonomous) != 0) {
robotMode = HAL_ROBOT_MODE_AUTONOMOUS;
} else if ((control & kTest) != 0) {
robotMode = HAL_ROBOT_MODE_UTILITY;
} else {
robotMode = HAL_ROBOT_MODE_TELEOPERATED;
}
m_control_word = HAL_MakeControlWord(
wpi::hal::GetDashboardSelectedOpMode(robotMode), robotMode,
(control & kEnabled) != 0, (control & kEmergencyStop) != 0,
(control & kFMS_Attached) != 0, (request & kRequestNormalMask) != 0);
m_control_sent = control;
}
void DSCommPacket::SetAlliance(uint8_t station_code) {
m_alliance_station = static_cast<HAL_AllianceStationID>(station_code);
}
void DSCommPacket::ReadMatchtimeTag(std::span<const uint8_t> tagData) {
if (tagData.size() < 6) {
return;
}
uint32_t store = tagData[2] << 24;
store |= tagData[3] << 16;
store |= tagData[4] << 8;
store |= tagData[5];
static_assert(sizeof(uint32_t) == sizeof(float), "float must be 32 bits");
float matchTime = 0;
std::memcpy(&matchTime, &store, sizeof(float));
m_match_time = matchTime;
}
void DSCommPacket::ReadJoystickTag(std::span<const uint8_t> dataInput,
int index) {
DSCommJoystickPacket& stick = m_joystick_packets[index];
stick.ResetUdp();
if (dataInput.size() == 2) {
return;
}
dataInput = dataInput.subspan(2);
// Read axes
int axesLength = dataInput[0];
for (int i = 0; i < axesLength; i++) {
int8_t value = dataInput[1 + i];
if (value < 0) {
stick.axes.axes[i] = value / 128.0;
} else {
stick.axes.axes[i] = value / 127.0;
}
}
stick.axes.available = (1 << axesLength) - 1;
dataInput = dataInput.subspan(1 + axesLength);
// Read Buttons
int buttonCount = dataInput[0];
int numBytes = (buttonCount + 7) / 8;
stick.buttons.buttons = 0;
for (int i = 0; i < numBytes; i++) {
stick.buttons.buttons |= dataInput[numBytes - i] << (8 * (i));
}
if (buttonCount < 64) {
stick.buttons.available = (1ULL << buttonCount) - 1;
} else {
stick.buttons.available = (std::numeric_limits<uint64_t>::max)();
}
dataInput = dataInput.subspan(1 + numBytes);
int povsLength = dataInput[0];
for (int i = 0; i < povsLength * 2; i += 2) {
stick.povs.povs[i] =
DegreesToPOV((dataInput[1 + i] << 8) | dataInput[2 + i]);
}
stick.povs.available = (1 << povsLength) - 1;
return;
}
/*----------------------------------------------------------------------------
** Communication methods
**--------------------------------------------------------------------------*/
void DSCommPacket::DecodeTCP(std::span<const uint8_t> packet) {
// No header
while (!packet.empty()) {
int tagLength = packet[0] << 8 | packet[1];
auto tagPacket = packet.subspan(0, tagLength + 2);
if (tagLength == 0) {
return;
}
switch (packet[2]) {
case kJoystickNameTag:
ReadJoystickDescriptionTag(tagPacket);
break;
case kMatchInfoTag:
ReadNewMatchInfoTag(tagPacket);
break;
}
packet = packet.subspan(tagLength + 2);
}
}
void DSCommPacket::DecodeUDP(std::span<const uint8_t> packet) {
if (packet.size() < 6) {
return;
}
// Decode fixed header
m_hi = packet[0];
m_lo = packet[1];
// Comm Version is packet 2, ignore
SetControl(packet[3], packet[4]);
// DS sends values 0, 1, and 2 for Red, but kUnknown is 0, so the value needs
// to be offset by one
SetAlliance(packet[5] + 1);
// Return if packet finished
if (packet.size() == 6) {
return;
}
// Else, handle tagged data
packet = packet.subspan(6);
int joystickNum = 0;
// Loop to handle multiple tags
while (!packet.empty()) {
auto tagLength = packet[0];
auto tagPacket = packet.subspan(0, tagLength + 1);
switch (packet[1]) {
case kJoystickDataTag:
ReadJoystickTag(tagPacket, joystickNum);
joystickNum++;
break;
case kMatchTimeTag:
ReadMatchtimeTag(tagPacket);
break;
}
packet = packet.subspan(tagLength + 1);
}
}
void DSCommPacket::ReadNewMatchInfoTag(std::span<const uint8_t> data) {
// Size 2 bytes, tag 1 byte
if (data.size() <= 3) {
return;
}
int nameLength = std::min<size_t>(data[3], sizeof(matchInfo.eventName) - 1);
for (int i = 0; i < nameLength; i++) {
matchInfo.eventName[i] = data[4 + i];
}
matchInfo.eventName[nameLength] = '\0';
data = data.subspan(4 + nameLength);
if (data.size() < 4) {
return;
}
matchInfo.matchType = static_cast<HAL_MatchType>(
data[0]); // None, Practice, Qualification, Elimination, Test
matchInfo.matchNumber = (data[1] << 8) | data[2];
matchInfo.replayNumber = data[3];
HALSIM_SetMatchInfo(&matchInfo);
}
void DSCommPacket::ReadJoystickDescriptionTag(std::span<const uint8_t> data) {
if (data.size() < 3) {
return;
}
data = data.subspan(3);
int joystickNum = data[0];
DSCommJoystickPacket& packet = m_joystick_packets[joystickNum];
packet.ResetTcp();
packet.descriptor.isGamepad = data[1] != 0 ? 1 : 0;
packet.descriptor.gamepadType = data[2];
int nameLength =
std::min<size_t>(data[3], (sizeof(packet.descriptor.name) - 1));
for (int i = 0; i < nameLength; i++) {
packet.descriptor.name[i] = data[4 + i];
}
data = data.subspan(4 + nameLength);
packet.descriptor.name[nameLength] = '\0';
}
void DSCommPacket::SendJoysticks(void) {
for (int i = 0; i < HAL_MAX_JOYSTICKS; i++) {
DSCommJoystickPacket& packet = m_joystick_packets[i];
HALSIM_SetJoystickAxes(i, &packet.axes);
HALSIM_SetJoystickPOVs(i, &packet.povs);
HALSIM_SetJoystickButtons(i, &packet.buttons);
HALSIM_SetJoystickDescriptor(i, &packet.descriptor);
}
}
void DSCommPacket::SetupSendBuffer(wpi::net::raw_uv_ostream& buf) {
SetupSendHeader(buf);
}
void DSCommPacket::SetupSendHeader(wpi::net::raw_uv_ostream& buf) {
static constexpr uint8_t kCommVersion = 0x01;
// High low packet index, comm version
buf << m_hi << m_lo << kCommVersion;
// Control word and status check
buf << m_control_sent
<< static_cast<uint8_t>(HALSIM_GetProgramStarted() ? kRobotHasCode : 0);
// Battery voltage high and low
buf << static_cast<uint8_t>(12) << static_cast<uint8_t>(0);
// Request (Always 0)
buf << static_cast<uint8_t>(0);
}
void DSCommPacket::SendUDPToHALSim(void) {
SendJoysticks();
if (!HAL_ControlWord_IsEnabled(m_control_word)) {
m_match_time = -1;
}
HALSIM_SetDriverStationMatchTime(m_match_time);
HALSIM_SetDriverStationEnabled(HAL_ControlWord_IsEnabled(m_control_word));
HALSIM_SetDriverStationRobotMode(
HAL_ControlWord_GetRobotMode(m_control_word));
HALSIM_SetDriverStationEStop(HAL_ControlWord_IsEStopped(m_control_word));
HALSIM_SetDriverStationFmsAttached(
HAL_ControlWord_IsFMSAttached(m_control_word));
HALSIM_SetDriverStationDsAttached(
HAL_ControlWord_IsDSAttached(m_control_word));
HALSIM_SetDriverStationAllianceStationId(m_alliance_station);
HALSIM_NotifyDriverStationNewData();
}

283
simulation/halsim_ds_socket/src/main/native/cpp/main.cpp
View File

@@ -13,6 +13,7 @@
#include <sys/types.h>
#include <algorithm>
#include <atomic>
#include <cstdio>
#include <cstring>
@@ -20,169 +21,158 @@
#include <memory>
#include <string_view>
#include <fmt/format.h>
#include "mrclib/ApiVersion.h"
#include "mrclib/DsComms.hpp"
#include "mrclib/SimSystemServer.h"
#include "wpi/hal/DashboardOpMode.hpp"
#include "wpi/hal/DriverStation.h"
#include "wpi/hal/Extensions.h"
#include "wpi/halsim/ds_socket/DSCommPacket.hpp"
#include "wpi/net/EventLoopRunner.hpp"
#include "wpi/net/raw_uv_ostream.hpp"
#include "wpi/net/uv/Tcp.hpp"
#include "wpi/net/uv/Timer.hpp"
#include "wpi/net/uv/Udp.hpp"
#include "wpi/net/uv/util.hpp"
#include "wpi/util/print.hpp"
#include "wpi/hal/cpp/MrcLibDs.hpp"
#include "wpi/hal/simulation/DriverStationData.h"
#include "wpi/util/SafeThread.hpp"
#if defined(Win32) || defined(_WIN32)
#pragma comment(lib, "Ws2_32.lib")
#endif
static_assert(MRCLIB_MAX_AXES == HAL_MAX_JOYSTICK_AXES);
static_assert(MRCLIB_MAX_POVS == HAL_MAX_JOYSTICK_POVS);
static_assert(MRCLIB_MAX_JOYSTICKS == HAL_MAX_JOYSTICKS);
static_assert(MRCLIB_MAX_TOUCHPADS == HAL_MAX_JOYSTICK_TOUCHPADS);
static_assert(MRCLIB_MAX_TOUCHPAD_FINGERS == HAL_MAX_JOYSTICK_TOUCHPAD_FINGERS);
static_assert(MRCLIB_MAX_JOYSTICK_NAME_LENGTH ==
sizeof(HAL_JoystickDescriptor::name) - 1);
using namespace wpi::net::uv;
static std::unique_ptr<Buffer> singleByte;
static std::atomic<bool> gDSConnected = false;
static void WriteSimBackend();
namespace {
struct DataStore {
wpi::util::SmallVector<uint8_t, 128> m_frame;
size_t m_frameSize = (std::numeric_limits<size_t>::max)();
halsim::DSCommPacket* dsPacket;
class Thread : public wpi::util::SafeThread {
public:
Thread() {}
void Main() override;
};
void Thread::Main() {
wpi::util::Event event{false, false};
HAL_ProvideNewDataEventHandle(event.GetHandle());
while (m_active) {
bool timedOut = false;
wpi::util::WaitForObject(event.GetHandle(), 1.0, &timedOut);
WriteSimBackend();
}
HAL_RemoveNewDataEventHandle(event.GetHandle());
}
} // namespace
static SimpleBufferPool<4>& GetBufferPool() {
static SimpleBufferPool<4> bufferPool;
return bufferPool;
}
static std::atomic<bool> gDSConnected = true;
static void HandleTcpDataStream(Buffer& buf, size_t size, DataStore& store) {
std::string_view data{buf.base, size};
while (!data.empty()) {
if (store.m_frameSize == (std::numeric_limits<size_t>::max)()) {
if (store.m_frame.size() < 2u) {
size_t toCopy = (std::min)(2u - store.m_frame.size(), data.size());
store.m_frame.append(data.data(), data.data() + toCopy);
data.remove_prefix(toCopy);
if (store.m_frame.size() < 2u) {
return; // need more data
static void WriteSimBackend() {
MRC_Joysticks joysticks;
MRC_ControlData controlData;
int32_t status =
MRC_DsComms_GetControlDataWithJoysticks(&controlData, &joysticks);
if (status != 0) {
return;
}
HALSIM_SetDriverStationOpMode(controlData.currentOpMode);
HALSIM_SetDriverStationAllianceStationId(static_cast<HAL_AllianceStationID>(
mrclib::GetAlliance(controlData.controlFlags) + 1));
HALSIM_SetDriverStationMatchTime(controlData.matchTime);
HALSIM_SetDriverStationRobotMode(static_cast<HAL_RobotMode>(
mrclib::GetRobotMode(controlData.controlFlags)));
HALSIM_SetDriverStationDsAttached(
mrclib::GetDsConnected(controlData.controlFlags));
HALSIM_SetDriverStationFmsAttached(
mrclib::GetFmsConnected(controlData.controlFlags));
HALSIM_SetDriverStationEnabled(mrclib::GetEnabled(controlData.controlFlags));
HALSIM_SetDriverStationEStop(mrclib::GetEStop(controlData.controlFlags));
HAL_GameData gameData;
auto gameDataSize = controlData.gameDataLength;
if (gameDataSize > MRCLIB_MAX_GAMEDATA_LENGTH) {
gameDataSize = MRCLIB_MAX_GAMEDATA_LENGTH;
}
std::memcpy(gameData.gameData, controlData.gameData, gameDataSize);
gameData.gameData[gameDataSize] = '\0';
HALSIM_SetGameData(&gameData);
for (int i = 0; i < MRCLIB_MAX_JOYSTICKS; ++i) {
HAL_JoystickAxes axes{};
HAL_JoystickPOVs povs{};
HAL_JoystickButtons buttons{};
HAL_JoystickTouchpads touchpads{};
if (i < joysticks.count) {
auto& joystick = joysticks.joysticks[i];
axes.available = joystick.availableAxes;
for (size_t j = 0; j < MRCLIB_MAX_AXES; ++j) {
auto raw = joystick.axes[j];
axes.raw[j] = raw;
if (raw < 0) {
axes.axes[j] = raw / 32768.0f;
} else {
axes.axes[j] = raw / 32767.0f;
}
}
store.m_frameSize = (static_cast<uint16_t>(store.m_frame[0]) << 8) |
static_cast<uint16_t>(store.m_frame[1]);
}
if (store.m_frameSize != (std::numeric_limits<size_t>::max)()) {
size_t need = store.m_frameSize - (store.m_frame.size() - 2);
size_t toCopy = (std::min)(need, data.size());
store.m_frame.append(data.data(), data.data() + toCopy);
data.remove_prefix(toCopy);
need -= toCopy;
if (need == 0) {
auto ds = store.dsPacket;
ds->DecodeTCP(store.m_frame);
store.m_frame.clear();
store.m_frameSize = (std::numeric_limits<size_t>::max)();
povs.available = joystick.availablePovs;
for (size_t j = 0; j < MRCLIB_MAX_POVS; ++j) {
povs.povs[j] = static_cast<HAL_JoystickPOV>(joystick.povs[j]);
}
buttons.available = joystick.availableButtons;
buttons.buttons = joystick.buttons;
touchpads.count = joystick.touchpadCount;
for (size_t j = 0; j < MRCLIB_MAX_TOUCHPADS; ++j) {
touchpads.touchpads[j].count = joystick.touchpads[j].count;
for (size_t k = 0; k < MRCLIB_MAX_TOUCHPAD_FINGERS; ++k) {
auto& finger = joystick.touchpads[j].fingers[k];
touchpads.touchpads[j].fingers[k].down = finger.down ? 1 : 0;
touchpads.touchpads[j].fingers[k].x = finger.x / 65535.0f;
touchpads.touchpads[j].fingers[k].y = finger.y / 65535.0f;
}
}
}
HALSIM_SetJoystickAxes(i, &axes);
HALSIM_SetJoystickPOVs(i, &povs);
HALSIM_SetJoystickButtons(i, &buttons);
HALSIM_SetJoystickTouchpads(i, &touchpads);
}
}
static void SetupTcp(wpi::net::uv::Loop& loop) {
auto tcp = Tcp::Create(loop);
auto tcpWaitTimer = Timer::Create(loop);
MRC_JoystickDescriptors descriptors;
status = MRC_DsComms_GetJoystickDescriptors(&descriptors);
if (status == 0) {
for (int i = 0; i < MRCLIB_MAX_JOYSTICKS; ++i) {
HAL_JoystickDescriptor descriptor{};
auto recStore = std::make_shared<DataStore>();
recStore->dsPacket = loop.GetData<halsim::DSCommPacket>().get();
if (i < descriptors.count) {
auto& mrcDescriptor = descriptors.descriptors[i];
tcp->SetData(recStore);
descriptor.isGamepad = mrcDescriptor.isGamepad ? 1 : 0;
descriptor.gamepadType = mrcDescriptor.gamepadType;
descriptor.supportedOutputs = mrcDescriptor.supportedOutputs;
tcp->Bind("0.0.0.0", 1740);
tcp->Listen([t = tcp.get()] {
auto client = t->Accept();
gDSConnected = true;
wpi::hal::EnableDashboardOpMode();
client->data.connect([t](Buffer& buf, size_t len) {
HandleTcpDataStream(buf, len, *t->GetData<DataStore>());
});
client->StartRead();
client->end.connect([c = client.get()] {
c->Close();
gDSConnected = false;
});
});
}
static void SetupUdp(wpi::net::uv::Loop& loop) {
auto udp = wpi::net::uv::Udp::Create(loop);
udp->Bind("0.0.0.0", 1110);
// Simulation mode packet
auto simLoopTimer = Timer::Create(loop);
struct sockaddr_in simAddr;
NameToAddr("127.0.0.1", 1135, &simAddr);
simLoopTimer->timeout.connect([udpLocal = udp.get(), simAddr] {
udpLocal->Send(simAddr, {singleByte.get(), 1}, [](auto buf, Error err) {
if (err) {
wpi::util::print(stderr, "{}\n", err.str());
std::fflush(stderr);
auto nameLength = std::min<size_t>(mrcDescriptor.nameLength,
sizeof(descriptor.name) - 1);
std::memcpy(descriptor.name, mrcDescriptor.name, nameLength);
descriptor.name[nameLength] = '\0';
}
});
});
simLoopTimer->Start(Timer::Time{100}, Timer::Time{100});
// DS Timeout
int timeoutMs = 100;
if (auto envTimeout = std::getenv("DS_TIMEOUT_MS")) {
try {
timeoutMs = std::stoi(envTimeout);
} catch (const std::exception& e) {
wpi::util::print(stderr, "Error parsing DS_TIMEOUT_MS: {}\n", e.what());
HALSIM_SetJoystickDescriptor(i, &descriptor);
}
}
auto autoDisableTimer = Timer::Create(loop);
autoDisableTimer->timeout.connect([] { HALSIM_SetDriverStationEnabled(0); });
// UDP Receive then send
udp->received.connect(
[udpLocal = udp.get(), autoDisableTimer, timeoutMs](
Buffer& buf, size_t len, const sockaddr& recSock, unsigned int port) {
autoDisableTimer->Start(Timer::Time(timeoutMs));
auto ds = udpLocal->GetLoop()->GetData<halsim::DSCommPacket>();
ds->DecodeUDP({reinterpret_cast<uint8_t*>(buf.base), len});
struct sockaddr_in outAddr;
std::memcpy(&outAddr, &recSock, sizeof(sockaddr_in));
outAddr.sin_family = PF_INET;
outAddr.sin_port = htons(1150);
wpi::util::SmallVector<wpi::net::uv::Buffer, 4> sendBufs;
wpi::net::raw_uv_ostream stream{
sendBufs, [] { return GetBufferPool().Allocate(); }};
ds->SetupSendBuffer(stream);
udpLocal->Send(outAddr, sendBufs, [](auto bufs, Error err) {
GetBufferPool().Release(bufs);
if (err) {
wpi::util::print(stderr, "{}\n", err.str());
std::fflush(stderr);
}
});
ds->SendUDPToHALSim();
});
udp->StartRecv();
HALSIM_NotifyDriverStationNewData();
}
static void SetupEventLoop(wpi::net::uv::Loop& loop) {
auto loopData = std::make_shared<halsim::DSCommPacket>();
loop.SetData(loopData);
SetupUdp(loop);
SetupTcp(loop);
}
static std::unique_ptr<wpi::net::EventLoopRunner> eventLoopRunner;
void ThisIsAHackDontCallThis() {
eventLoopRunner = std::make_unique<wpi::net::EventLoopRunner>();
eventLoopRunner->ExecAsync(SetupEventLoop);
static void SetupBackend() {
static wpi::util::SafeThreadOwner<Thread> thread;
thread.Start();
}
/*----------------------------------------------------------------------------
@@ -206,7 +196,22 @@ int HALSIM_InitExtension(void) {
HAL_RegisterExtension("ds_socket", &gDSConnected);
singleByte = std::make_unique<Buffer>("0");
// Before initializing, we need to set up the fake system server
if (!MRC_CHECK_API_VERSION()) {
fmt::print(
stderr,
"Error: MRC API version mismatch. Restarting app and retrying...");
std::terminate();
}
MRC_SimSystemServer_Initialize();
wpi::hal::ForceDsInstance(wpi::hal::GetMrcLibDs());
WriteSimBackend();
SetupBackend();
std::puts("DriverStationSocket Initialized!");
return 0;

28
simulation/halsim_ds_socket/src/main/native/include/wpi/halsim/ds_socket/DSCommJoystickPacket.hpp
View File

@@ -1,28 +0,0 @@
// 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 <cstring>
#include "wpi/hal/DriverStationTypes.h"
namespace halsim {
struct DSCommJoystickPacket {
HAL_JoystickAxes axes;
HAL_JoystickButtons buttons;
HAL_JoystickPOVs povs;
HAL_JoystickDescriptor descriptor;
void ResetUdp() {
std::memset(&axes, 0, sizeof(axes));
std::memset(&buttons, 0, sizeof(buttons));
std::memset(&povs, 0, sizeof(povs));
}
void ResetTcp() { std::memset(&descriptor, 0, sizeof(descriptor)); }
};
} // namespace halsim

70
simulation/halsim_ds_socket/src/main/native/include/wpi/halsim/ds_socket/DSCommPacket.hpp
View File

@@ -1,70 +0,0 @@
// 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 <array>
#include <span>
#include "wpi/hal/simulation/DriverStationData.h"
#include "wpi/halsim/ds_socket/DSCommJoystickPacket.hpp"
#include "wpi/net/raw_uv_ostream.hpp"
class DSCommPacketTest;
namespace halsim {
class DSCommPacket {
friend class ::DSCommPacketTest;
public:
DSCommPacket(void);
void DecodeTCP(std::span<const uint8_t> packet);
void DecodeUDP(std::span<const uint8_t> packet);
void SendUDPToHALSim(void);
void SetupSendBuffer(wpi::net::raw_uv_ostream& buf);
/* TCP Tags */
static const uint8_t kGameDataTag = 0x0e;
static const uint8_t kJoystickNameTag = 0x02;
static const uint8_t kMatchInfoTag = 0x07;
/* UDP Tags*/
static const uint8_t kJoystickDataTag = 0x0c;
static const uint8_t kMatchTimeTag = 0x07;
/* Control word bits */
static const uint8_t kTest = 0x01;
static const uint8_t kEnabled = 0x04;
static const uint8_t kAutonomous = 0x02;
static const uint8_t kFMS_Attached = 0x08;
static const uint8_t kEmergencyStop = 0x80;
/* Control request bitmask */
static const uint8_t kRequestNormalMask = 0xF0;
/* Status bits */
static const uint8_t kRobotHasCode = 0x20;
private:
void SendJoysticks(void);
void SetControl(uint8_t control, uint8_t request);
void SetAlliance(uint8_t station_code);
void SetupSendHeader(wpi::net::raw_uv_ostream& buf);
void ReadMatchtimeTag(std::span<const uint8_t> tagData);
void ReadJoystickTag(std::span<const uint8_t> data, int index);
void ReadNewMatchInfoTag(std::span<const uint8_t> data);
void ReadJoystickDescriptionTag(std::span<const uint8_t> data);
uint8_t m_hi;
uint8_t m_lo;
uint8_t m_control_sent;
HAL_ControlWord m_control_word;
HAL_AllianceStationID m_alliance_station;
HAL_MatchInfo matchInfo;
std::array<DSCommJoystickPacket, HAL_MAX_JOYSTICKS> m_joystick_packets;
double m_match_time = -1;
};
} // namespace halsim

7
simulation/halsim_ds_socket/src/main/native/include/wpi/halsim/ds_socket/DsSocket.h Normal file
View File

@@ -0,0 +1,7 @@
// 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
// This file does nothing, is just here to make packaging happy.

129
simulation/halsim_ds_socket/src/test/native/cpp/DSCommPacketTest.cpp
View File

@@ -1,129 +0,0 @@
// 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 "wpi/halsim/ds_socket/DSCommPacket.hpp"
#include <gtest/gtest.h>
class DSCommPacketTest : public ::testing::Test {
public:
DSCommPacketTest() = default;
void SendJoysticks() { commPacket.SendJoysticks(); }
halsim::DSCommJoystickPacket& ReadJoystickTag(std::span<const uint8_t> data,
int index) {
commPacket.ReadJoystickTag(data, index);
return commPacket.m_joystick_packets[index];
}
halsim::DSCommJoystickPacket& ReadDescriptorTag(
std::span<const uint8_t> data) {
commPacket.ReadJoystickDescriptionTag(data);
return commPacket.m_joystick_packets[data[3]];
}
HAL_MatchInfo& ReadNewMatchInfoTag(std::span<const uint8_t> data) {
commPacket.ReadNewMatchInfoTag(data);
return commPacket.matchInfo;
}
protected:
halsim::DSCommPacket commPacket;
};
TEST_F(DSCommPacketTest, EmptyJoystickTag) {
for (int i = 0; i < HAL_MAX_JOYSTICKS; i++) {
uint8_t arr[2];
auto& data = ReadJoystickTag(arr, 0);
ASSERT_EQ(data.axes.available, 0);
ASSERT_EQ(data.povs.available, 0);
ASSERT_EQ(data.buttons.available, 0llu);
}
}
TEST_F(DSCommPacketTest, BlankJoystickTag) {
for (int i = 0; i < HAL_MAX_JOYSTICKS; i++) {
uint8_t arr[5];
arr[0] = 4;
arr[1] = 2;
arr[2] = 0;
arr[3] = 0;
arr[4] = 0;
auto& data = ReadJoystickTag(arr, 0);
ASSERT_EQ(data.axes.available, 0);
ASSERT_EQ(data.povs.available, 0);
ASSERT_EQ(data.buttons.available, 0llu);
}
}
TEST_F(DSCommPacketTest, MainJoystickTag) {
for (int i = 0; i < HAL_MAX_JOYSTICKS; i++) {
// Just random data
std::array<uint8_t, 12> _buttons{{0, 1, 0, 0, 1, 1, 0, 1, 0, 1, 0, 1}};
std::array<uint8_t, 2> _button_bytes{{0, 0}};
for (int btn = 0; btn < 8; btn++) {
_button_bytes[1] |= _buttons[btn] << btn;
}
for (int btn = 8; btn < 12; btn++) {
_button_bytes[0] |= _buttons[btn] << (btn - 8);
}
// 5 for base, 4 joystick, 12 buttons (2 bytes) 3 povs
uint8_t arr[5 + 4 + 2 + 6] = {// Size, Tag
16, 12,
// Axes
4, 0x9C, 0xCE, 0, 75,
// Buttons (LSB 0)
12, _button_bytes[0], _button_bytes[1],
// POVs
3, 0, 50, 0, 100, 0x0F, 0x00};
auto& data = ReadJoystickTag(arr, 0);
ASSERT_EQ(data.axes.available, 0xF);
ASSERT_EQ(data.povs.available, 0x7);
ASSERT_EQ(data.buttons.available, 0xFFFllu);
for (int btn = 0; btn < 12; btn++) {
ASSERT_EQ((data.buttons.buttons & (1llu << btn)) != 0, _buttons[btn] != 0)
<< "Button " << btn;
}
}
}
TEST_F(DSCommPacketTest, DescriptorTag) {
for (int i = 0; i < HAL_MAX_JOYSTICKS; i++) {
uint8_t arr[] = {// Size (2), tag
0, 0, 7,
// Joystick index, Is Xbox, Type
static_cast<uint8_t>(i), 1, 0,
// NameLen, Name (Not null terminated)
11, 'H', 'e', 'l', 'l', 'o', ' ', 'W', 'o', 'r', 'l', 'd',
// Axes count, Axes types
4, 1, 2, 3, 4,
// Button count, pov count,
12, 3};
arr[1] = sizeof(arr) - 2;
auto& data = ReadDescriptorTag(arr);
ASSERT_EQ(data.descriptor.isGamepad, 1);
ASSERT_EQ(data.descriptor.gamepadType, 0);
ASSERT_STREQ(data.descriptor.name, "Hello World");
}
}
TEST_F(DSCommPacketTest, MatchInfoTag) {
uint8_t arr[]{// Size (2), tag
0, 0, 8,
// Event Name Len, Event Name
4, 'W', 'C', 'B', 'C',
// Match type, Match num (2), replay num
2, 0, 18, 1};
arr[1] = sizeof(arr) - 2;
auto& matchInfo = ReadNewMatchInfoTag(arr);
ASSERT_STREQ(matchInfo.eventName, "WCBC");
ASSERT_EQ(matchInfo.matchType, HAL_MatchType::HAL_MATCH_TYPE_QUALIFICATION);
ASSERT_EQ(matchInfo.matchNumber, 18);
ASSERT_EQ(matchInfo.replayNumber, 1);
}

1
simulation/halsim_gui/BUILD.bazel
View File

@@ -155,6 +155,7 @@ package_default_cc_project(
PKG_CONFIG_DEPS = [
"//datalog:native/datalog/robotpy-native-datalog.pc",
"//datalog:robotpy-wpilog.generated_pkgcfg_files",
"//hal:native/wpihal/robotpy-native-mrclib.pc",
"//hal:native/wpihal/robotpy-native-wpihal.pc",
"//hal:robotpy-hal.generated_pkgcfg_files",
"//ntcore:native/ntcore/robotpy-native-ntcore.pc",