Rename our allwpilib (which is now 2020) to not have 2019 in the name
Change-Id: I3c07f85ed32ab8b97db765a9b43f2a6ce7da964a
diff --git a/simulation/halsim_ds_socket/CMakeLists.txt b/simulation/halsim_ds_socket/CMakeLists.txt
new file mode 100644
index 0000000..3e2518c
--- /dev/null
+++ b/simulation/halsim_ds_socket/CMakeLists.txt
@@ -0,0 +1,16 @@
+project(halsim_ds_socket)
+
+include(CompileWarnings)
+
+file(GLOB halsim_ds_socket_src src/main/native/cpp/*.cpp)
+
+add_library(halsim_ds_socket MODULE ${halsim_ds_socket_src})
+wpilib_target_warnings(halsim_ds_socket)
+set_target_properties(halsim_ds_socket PROPERTIES DEBUG_POSTFIX "d")
+target_link_libraries(halsim_ds_socket PUBLIC hal)
+
+target_include_directories(halsim_ds_socket PRIVATE src/main/native/include)
+
+set_property(TARGET halsim_ds_socket PROPERTY FOLDER "libraries")
+
+install(TARGETS halsim_ds_socket EXPORT halsim_ds_socket DESTINATION "${main_lib_dest}")
diff --git a/simulation/halsim_ds_socket/build.gradle b/simulation/halsim_ds_socket/build.gradle
new file mode 100644
index 0000000..440e5d9
--- /dev/null
+++ b/simulation/halsim_ds_socket/build.gradle
@@ -0,0 +1,61 @@
+description = "A plugin that listens on a socket so that you can use the real Driver Station software to connect to the simulation"
+
+ext {
+ includeWpiutil = true
+ pluginName = 'halsim_ds_socket'
+}
+
+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 {
+ testSuites {
+ def comps = $.components
+ if (!project.hasProperty('onlylinuxathena') && !project.hasProperty('onlylinuxraspbian') && !project.hasProperty('onlylinuxaarch64bionic')) {
+ "${pluginName}Test"(GoogleTestTestSuiteSpec) {
+ for(NativeComponentSpec c : comps) {
+ if (c.name == pluginName) {
+ testing c
+ break
+ }
+ }
+ sources {
+ cpp {
+ source {
+ srcDirs 'src/test/native/cpp'
+ include '**/*.cpp'
+ }
+ exportedHeaders {
+ srcDirs 'src/test/native/include', 'src/main/native/cpp'
+ }
+ }
+ }
+ }
+ }
+ }
+ binaries {
+ withType(GoogleTestTestSuiteBinarySpec) {
+ project(':hal').addHalDependency(it, 'shared')
+ lib project: ':wpiutil', library: 'wpiutil', linkage: 'shared'
+ lib library: pluginName, linkage: 'shared'
+ if (it.targetPlatform.name == nativeUtils.wpi.platforms.roborio) {
+ nativeUtils.useRequiredLibrary(it, 'netcomm_shared', 'chipobject_shared', 'visa_shared', 'ni_runtime_shared')
+ }
+ }
+ }
+}
+
+tasks.withType(RunTestExecutable) {
+ args "--gtest_output=xml:test_detail.xml"
+ outputs.dir outputDir
+}
diff --git a/simulation/halsim_ds_socket/src/dev/native/cpp/main.cpp b/simulation/halsim_ds_socket/src/dev/native/cpp/main.cpp
new file mode 100644
index 0000000..2c1e83a
--- /dev/null
+++ b/simulation/halsim_ds_socket/src/dev/native/cpp/main.cpp
@@ -0,0 +1,26 @@
+/*----------------------------------------------------------------------------*/
+/* Copyright (c) 2018-2019 FIRST. All Rights Reserved. */
+/* Open Source Software - may be modified and shared by FRC teams. The code */
+/* must be accompanied by the FIRST BSD license file in the root directory of */
+/* the project. */
+/*----------------------------------------------------------------------------*/
+
+#include <thread>
+
+#include <hal/DriverStation.h>
+#include <hal/HALBase.h>
+#include <wpi/Format.h>
+#include <wpi/raw_ostream.h>
+
+extern "C" int HALSIM_InitExtension(void);
+
+int main() {
+ HAL_Initialize(500, 0);
+ HALSIM_InitExtension();
+
+ HAL_ObserveUserProgramStarting();
+
+ while (true) {
+ std::this_thread::sleep_for(std::chrono::milliseconds(100));
+ }
+}
diff --git a/simulation/halsim_ds_socket/src/main/native/cpp/DSCommPacket.cpp b/simulation/halsim_ds_socket/src/main/native/cpp/DSCommPacket.cpp
new file mode 100644
index 0000000..95f393b
--- /dev/null
+++ b/simulation/halsim_ds_socket/src/main/native/cpp/DSCommPacket.cpp
@@ -0,0 +1,312 @@
+/*----------------------------------------------------------------------------*/
+/* Copyright (c) 2018-2019 FIRST. All Rights Reserved. */
+/* Open Source Software - may be modified and shared by FRC teams. The code */
+/* must be accompanied by the FIRST BSD license file in the root directory of */
+/* the project. */
+/*----------------------------------------------------------------------------*/
+
+#include "DSCommPacket.h"
+
+#include <algorithm>
+#include <chrono>
+#include <cstring>
+#include <iostream>
+#include <thread>
+#include <vector>
+
+#include <mockdata/DriverStationData.h>
+#include <mockdata/MockHooks.h>
+#include <wpi/ArrayRef.h>
+#include <wpi/Format.h>
+
+using namespace halsim;
+
+DSCommPacket::DSCommPacket() {
+ for (auto& i : m_joystick_packets) {
+ i.ResetTcp();
+ i.ResetUdp();
+ }
+ matchInfo.gameSpecificMessageSize = 0;
+}
+
+/*----------------------------------------------------------------------------
+** 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) {
+ std::memset(&m_control_word, 0, sizeof(m_control_word));
+ m_control_word.enabled = (control & kEnabled) != 0;
+ m_control_word.autonomous = (control & kAutonomous) != 0;
+ m_control_word.test = (control & kTest) != 0;
+ m_control_word.eStop = (control & kEmergencyStop) != 0;
+ m_control_word.fmsAttached = (control & kFMS_Attached) != 0;
+ m_control_word.dsAttached = (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(wpi::ArrayRef<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(wpi::ArrayRef<uint8_t> dataInput,
+ int index) {
+ DSCommJoystickPacket& stick = m_joystick_packets[index];
+ stick.ResetUdp();
+
+ if (dataInput.size() == 2) {
+ return;
+ }
+
+ dataInput = dataInput.slice(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.count = axesLength;
+
+ dataInput = dataInput.slice(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));
+ }
+ stick.buttons.count = buttonCount;
+
+ dataInput = dataInput.slice(1 + numBytes);
+
+ int povsLength = dataInput[0];
+ for (int i = 0; i < povsLength * 2; i += 2) {
+ stick.povs.povs[i] = (dataInput[1 + i] << 8) | dataInput[2 + i];
+ }
+
+ stick.povs.count = povsLength;
+
+ return;
+}
+
+/*----------------------------------------------------------------------------
+** Communication methods
+**--------------------------------------------------------------------------*/
+void DSCommPacket::DecodeTCP(wpi::ArrayRef<uint8_t> packet) {
+ // No header
+ while (!packet.empty()) {
+ int tagLength = packet[0] << 8 | packet[1];
+ auto tagPacket = packet.slice(0, tagLength + 2);
+
+ if (tagLength == 0) {
+ return;
+ }
+
+ switch (packet[2]) {
+ case kJoystickNameTag:
+ ReadJoystickDescriptionTag(tagPacket);
+ break;
+ case kGameDataTag:
+ ReadGameSpecificMessageTag(tagPacket);
+ break;
+ case kMatchInfoTag:
+ ReadNewMatchInfoTag(tagPacket);
+ break;
+ }
+ packet = packet.slice(tagLength + 2);
+ }
+}
+
+void DSCommPacket::DecodeUDP(wpi::ArrayRef<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]);
+ SetAlliance(packet[5]);
+
+ // Return if packet finished
+ if (packet.size() == 6) return;
+
+ // Else, handle tagged data
+ packet = packet.slice(6);
+
+ int joystickNum = 0;
+
+ // Loop to handle multiple tags
+ while (!packet.empty()) {
+ auto tagLength = packet[0];
+ auto tagPacket = packet.slice(0, tagLength + 1);
+
+ switch (packet[1]) {
+ case kJoystickDataTag:
+ ReadJoystickTag(tagPacket, joystickNum);
+ joystickNum++;
+ break;
+ case kMatchTimeTag:
+ ReadMatchtimeTag(tagPacket);
+ break;
+ }
+ packet = packet.slice(tagLength + 1);
+ }
+}
+
+void DSCommPacket::ReadNewMatchInfoTag(wpi::ArrayRef<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.slice(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::ReadGameSpecificMessageTag(wpi::ArrayRef<uint8_t> data) {
+ // Size 2 bytes, tag 1 byte
+ if (data.size() <= 3) return;
+
+ int length = std::min<size_t>(((data[0] << 8) | data[1]) - 1,
+ sizeof(matchInfo.gameSpecificMessage));
+ for (int i = 0; i < length; i++) {
+ matchInfo.gameSpecificMessage[i] = data[3 + i];
+ }
+
+ matchInfo.gameSpecificMessageSize = length;
+
+ HALSIM_SetMatchInfo(&matchInfo);
+}
+void DSCommPacket::ReadJoystickDescriptionTag(wpi::ArrayRef<uint8_t> data) {
+ if (data.size() < 3) return;
+ data = data.slice(3);
+ int joystickNum = data[0];
+ DSCommJoystickPacket& packet = m_joystick_packets[joystickNum];
+ packet.ResetTcp();
+ packet.descriptor.isXbox = data[1] != 0 ? 1 : 0;
+ packet.descriptor.type = 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.slice(4 + nameLength);
+ packet.descriptor.name[nameLength] = '\0';
+ int axesCount = data[0];
+ packet.descriptor.axisCount = axesCount;
+ for (int i = 0; i < axesCount; i++) {
+ packet.descriptor.axisTypes[i] = data[1 + i];
+ }
+ data = data.slice(1 + axesCount);
+
+ packet.descriptor.buttonCount = data[0];
+ packet.descriptor.povCount = data[1];
+}
+
+void DSCommPacket::SendJoysticks(void) {
+ for (int i = 0; i < HAL_kMaxJoysticks; 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::raw_uv_ostream& buf) {
+ SetupSendHeader(buf);
+ SetupJoystickTag(buf);
+}
+
+void DSCommPacket::SetupSendHeader(wpi::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::SetupJoystickTag(wpi::raw_uv_ostream& buf) {
+ static constexpr uint8_t kHIDTag = 0x01;
+
+ // HID tags are sent 1 per device
+ int64_t outputs;
+ int32_t rightRumble;
+ int32_t leftRumble;
+ for (size_t i = 0; i < m_joystick_packets.size(); i++) {
+ // Length is 9, 1 tag and 8 data.
+ buf << static_cast<uint8_t>(9) << kHIDTag;
+ HALSIM_GetJoystickOutputs(i, &outputs, &leftRumble, &rightRumble);
+ auto op = static_cast<uint32_t>(outputs);
+ auto rr = static_cast<uint16_t>(rightRumble);
+ auto lr = static_cast<uint16_t>(leftRumble);
+ buf.write((op >> 24 & 0xFF));
+ buf.write((op >> 16 & 0xFF));
+ buf.write((op >> 8 & 0xFF));
+ buf.write((op & 0xFF));
+ buf.write((rr >> 8 & 0xFF));
+ buf.write((rr & 0xFF));
+ buf.write((lr >> 8 & 0xFF));
+ buf.write((lr & 0xFF));
+ }
+}
+
+void DSCommPacket::SendUDPToHALSim(void) {
+ SendJoysticks();
+
+ HALSIM_SetDriverStationMatchTime(m_match_time);
+ HALSIM_SetDriverStationEnabled(m_control_word.enabled);
+ HALSIM_SetDriverStationAutonomous(m_control_word.autonomous);
+ HALSIM_SetDriverStationTest(m_control_word.test);
+ HALSIM_SetDriverStationEStop(m_control_word.eStop);
+ HALSIM_SetDriverStationFmsAttached(m_control_word.fmsAttached);
+ HALSIM_SetDriverStationDsAttached(m_control_word.dsAttached);
+ HALSIM_SetDriverStationAllianceStationId(m_alliance_station);
+
+ HALSIM_NotifyDriverStationNewData();
+}
diff --git a/simulation/halsim_ds_socket/src/main/native/cpp/main.cpp b/simulation/halsim_ds_socket/src/main/native/cpp/main.cpp
new file mode 100644
index 0000000..7d683b3
--- /dev/null
+++ b/simulation/halsim_ds_socket/src/main/native/cpp/main.cpp
@@ -0,0 +1,192 @@
+/*----------------------------------------------------------------------------*/
+/* Copyright (c) 2018-2019 FIRST. All Rights Reserved. */
+/* Open Source Software - may be modified and shared by FRC teams. The code */
+/* must be accompanied by the FIRST BSD license file in the root directory of */
+/* the project. */
+/*----------------------------------------------------------------------------*/
+
+/*----------------------------------------------------------------------------
+** This extension reimplements enough of the FRC_Network layer to enable the
+** simulator to communicate with a driver station. That includes a
+** simple udp layer for communication.
+** The protocol does not appear to be well documented; this implementation
+** is based in part on the Toast ds_comms.cpp by Jaci and in part
+** by the protocol specification given by QDriverStation.
+**--------------------------------------------------------------------------*/
+
+#include <sys/types.h>
+
+#include <cstring>
+#include <iostream>
+
+#include <DSCommPacket.h>
+#include <wpi/EventLoopRunner.h>
+#include <wpi/StringRef.h>
+#include <wpi/raw_ostream.h>
+#include <wpi/raw_uv_ostream.h>
+#include <wpi/uv/Tcp.h>
+#include <wpi/uv/Timer.h>
+#include <wpi/uv/Udp.h>
+#include <wpi/uv/util.h>
+
+#if defined(Win32) || defined(_WIN32)
+#pragma comment(lib, "Ws2_32.lib")
+#endif
+
+using namespace wpi::uv;
+
+static std::unique_ptr<Buffer> singleByte;
+
+namespace {
+struct DataStore {
+ wpi::SmallVector<uint8_t, 128> m_frame;
+ size_t m_frameSize = (std::numeric_limits<size_t>::max)();
+ halsim::DSCommPacket* dsPacket;
+};
+} // namespace
+
+static SimpleBufferPool<4>& GetBufferPool() {
+ static SimpleBufferPool<4> bufferPool;
+ return bufferPool;
+}
+
+static void HandleTcpDataStream(Buffer& buf, size_t size, DataStore& store) {
+ wpi::StringRef 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.bytes_begin(), data.bytes_begin() + toCopy);
+ data = data.drop_front(toCopy);
+ if (store.m_frame.size() < 2u) return; // need more data
+ }
+ 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.bytes_begin(), data.bytes_begin() + toCopy);
+ data = data.drop_front(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)();
+ }
+ }
+ }
+}
+
+static void SetupTcp(wpi::uv::Loop& loop) {
+ auto tcp = Tcp::Create(loop);
+ auto tcpWaitTimer = Timer::Create(loop);
+
+ auto recStore = std::make_shared<DataStore>();
+ recStore->dsPacket = loop.GetData<halsim::DSCommPacket>().get();
+
+ tcp->SetData(recStore);
+
+ tcp->Bind("0.0.0.0", 1740);
+
+ tcp->Listen([t = tcp.get()] {
+ auto client = t->Accept();
+
+ 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(); });
+ });
+}
+
+static void SetupUdp(wpi::uv::Loop& loop) {
+ auto udp = wpi::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, wpi::ArrayRef<Buffer>{singleByte.get(), 1},
+ [](auto buf, Error err) {
+ if (err) {
+ wpi::errs() << err.str() << "\n";
+ wpi::errs().flush();
+ }
+ });
+ });
+ simLoopTimer->Start(Timer::Time{100}, Timer::Time{100});
+
+ // UDP Receive then send
+ udp->received.connect([udpLocal = udp.get()](Buffer& buf, size_t len,
+ const sockaddr& recSock,
+ unsigned int port) {
+ auto ds = udpLocal->GetLoop()->GetData<halsim::DSCommPacket>();
+ ds->DecodeUDP(
+ wpi::ArrayRef<uint8_t>{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::SmallVector<wpi::uv::Buffer, 4> sendBufs;
+ wpi::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::errs() << err.str() << "\n";
+ wpi::errs().flush();
+ }
+ });
+ ds->SendUDPToHALSim();
+ });
+
+ udp->StartRecv();
+}
+
+static void SetupEventLoop(wpi::uv::Loop& loop) {
+ auto loopData = std::make_shared<halsim::DSCommPacket>();
+ loop.SetData(loopData);
+ SetupUdp(loop);
+ SetupTcp(loop);
+}
+
+static std::unique_ptr<wpi::EventLoopRunner> eventLoopRunner;
+
+/*----------------------------------------------------------------------------
+** Main entry point. We will start listen threads going, processing
+** against our driver station packet
+**--------------------------------------------------------------------------*/
+extern "C" {
+#if defined(WIN32) || defined(_WIN32)
+__declspec(dllexport)
+#endif
+ int HALSIM_InitExtension(void) {
+ static bool once = false;
+
+ if (once) {
+ std::cerr << "Error: cannot invoke HALSIM_InitExtension twice."
+ << std::endl;
+ return -1;
+ }
+ once = true;
+
+ std::cout << "DriverStationSocket Initializing." << std::endl;
+
+ singleByte = std::make_unique<Buffer>("0");
+
+ eventLoopRunner = std::make_unique<wpi::EventLoopRunner>();
+
+ eventLoopRunner->ExecAsync(SetupEventLoop);
+
+ std::cout << "DriverStationSocket Initialized!" << std::endl;
+ return 0;
+}
+} // extern "C"
diff --git a/simulation/halsim_ds_socket/src/main/native/include/DSCommJoystickPacket.h b/simulation/halsim_ds_socket/src/main/native/include/DSCommJoystickPacket.h
new file mode 100644
index 0000000..6f7e0bf
--- /dev/null
+++ b/simulation/halsim_ds_socket/src/main/native/include/DSCommJoystickPacket.h
@@ -0,0 +1,31 @@
+/*----------------------------------------------------------------------------*/
+/* Copyright (c) 2018 FIRST. All Rights Reserved. */
+/* Open Source Software - may be modified and shared by FRC teams. The code */
+/* must be accompanied by the FIRST BSD license file in the root directory of */
+/* the project. */
+/*----------------------------------------------------------------------------*/
+
+#pragma once
+
+#include <cstring>
+
+#include <hal/DriverStationTypes.h>
+
+namespace halsim {
+
+typedef struct {
+ 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)); }
+} DSCommJoystickPacket;
+
+} // namespace halsim
diff --git a/simulation/halsim_ds_socket/src/main/native/include/DSCommPacket.h b/simulation/halsim_ds_socket/src/main/native/include/DSCommPacket.h
new file mode 100644
index 0000000..ebe36e4
--- /dev/null
+++ b/simulation/halsim_ds_socket/src/main/native/include/DSCommPacket.h
@@ -0,0 +1,75 @@
+/*----------------------------------------------------------------------------*/
+/* Copyright (c) 2018 FIRST. All Rights Reserved. */
+/* Open Source Software - may be modified and shared by FRC teams. The code */
+/* must be accompanied by the FIRST BSD license file in the root directory of */
+/* the project. */
+/*----------------------------------------------------------------------------*/
+
+#pragma once
+
+#include <array>
+
+#include <DSCommJoystickPacket.h>
+#include <mockdata/DriverStationData.h>
+#include <wpi/ArrayRef.h>
+#include <wpi/raw_uv_ostream.h>
+
+class DSCommPacketTest;
+
+namespace halsim {
+
+class DSCommPacket {
+ friend class ::DSCommPacketTest;
+
+ public:
+ DSCommPacket(void);
+ void DecodeTCP(wpi::ArrayRef<uint8_t> packet);
+ void DecodeUDP(wpi::ArrayRef<uint8_t> packet);
+ void SendUDPToHALSim(void);
+ void SetupSendBuffer(wpi::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::raw_uv_ostream& buf);
+ void SetupJoystickTag(wpi::raw_uv_ostream& buf);
+ void ReadMatchtimeTag(wpi::ArrayRef<uint8_t> tagData);
+ void ReadJoystickTag(wpi::ArrayRef<uint8_t> data, int index);
+ void ReadNewMatchInfoTag(wpi::ArrayRef<uint8_t> data);
+ void ReadGameSpecificMessageTag(wpi::ArrayRef<uint8_t> data);
+ void ReadJoystickDescriptionTag(wpi::ArrayRef<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_kMaxJoysticks> m_joystick_packets;
+ double m_match_time;
+};
+
+} // namespace halsim
diff --git a/simulation/halsim_ds_socket/src/test/native/cpp/DSCommPacketTest.cpp b/simulation/halsim_ds_socket/src/test/native/cpp/DSCommPacketTest.cpp
new file mode 100644
index 0000000..08caa2f
--- /dev/null
+++ b/simulation/halsim_ds_socket/src/test/native/cpp/DSCommPacketTest.cpp
@@ -0,0 +1,159 @@
+/*----------------------------------------------------------------------------*/
+/* Copyright (c) 2018-2019 FIRST. All Rights Reserved. */
+/* Open Source Software - may be modified and shared by FRC teams. The code */
+/* must be accompanied by the FIRST BSD license file in the root directory of */
+/* the project. */
+/*----------------------------------------------------------------------------*/
+
+#include "DSCommPacket.h"
+#include "gtest/gtest.h"
+
+class DSCommPacketTest : public ::testing::Test {
+ public:
+ DSCommPacketTest() {}
+
+ void SendJoysticks() { commPacket.SendJoysticks(); }
+
+ halsim::DSCommJoystickPacket& ReadJoystickTag(wpi::ArrayRef<uint8_t> data,
+ int index) {
+ commPacket.ReadJoystickTag(data, index);
+ return commPacket.m_joystick_packets[index];
+ }
+
+ halsim::DSCommJoystickPacket& ReadDescriptorTag(wpi::ArrayRef<uint8_t> data) {
+ commPacket.ReadJoystickDescriptionTag(data);
+ return commPacket.m_joystick_packets[data[3]];
+ }
+
+ HAL_MatchInfo& ReadNewMatchInfoTag(wpi::ArrayRef<uint8_t> data) {
+ commPacket.ReadNewMatchInfoTag(data);
+ return commPacket.matchInfo;
+ }
+
+ HAL_MatchInfo& ReadGameSpecificTag(wpi::ArrayRef<uint8_t> data) {
+ commPacket.ReadGameSpecificMessageTag(data);
+ return commPacket.matchInfo;
+ }
+
+ protected:
+ halsim::DSCommPacket commPacket;
+};
+
+TEST_F(DSCommPacketTest, EmptyJoystickTag) {
+ for (int i = 0; i < HAL_kMaxJoysticks; i++) {
+ uint8_t arr[2];
+ auto& data = ReadJoystickTag(arr, 0);
+ ASSERT_EQ(data.axes.count, 0);
+ ASSERT_EQ(data.povs.count, 0);
+ ASSERT_EQ(data.buttons.count, 0);
+ }
+}
+
+TEST_F(DSCommPacketTest, BlankJoystickTag) {
+ for (int i = 0; i < HAL_kMaxJoysticks; 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.count, 0);
+ ASSERT_EQ(data.povs.count, 0);
+ ASSERT_EQ(data.buttons.count, 0);
+ }
+}
+
+TEST_F(DSCommPacketTest, MainJoystickTag) {
+ for (int i = 0; i < HAL_kMaxJoysticks; 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.count, 4);
+ ASSERT_EQ(data.povs.count, 3);
+ ASSERT_EQ(data.buttons.count, 12);
+
+ for (int btn = 0; btn < 12; btn++) {
+ ASSERT_EQ((data.buttons.buttons & (1 << btn)) != 0, _buttons[btn] != 0)
+ << "Button " << btn;
+ }
+ }
+}
+
+TEST_F(DSCommPacketTest, DescriptorTag) {
+ for (int i = 0; i < HAL_kMaxJoysticks; 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.isXbox, 1);
+ ASSERT_EQ(data.descriptor.type, 0);
+ ASSERT_STREQ(data.descriptor.name, "Hello World");
+ ASSERT_EQ(data.descriptor.axisCount, 4);
+ for (int i = 0; i < 4; i++) {
+ ASSERT_EQ(data.descriptor.axisTypes[i], i + 1);
+ }
+ ASSERT_EQ(data.descriptor.buttonCount, 12);
+ ASSERT_EQ(data.descriptor.povCount, 3);
+ }
+}
+
+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_kMatchType_qualification);
+ ASSERT_EQ(matchInfo.matchNumber, 18);
+ ASSERT_EQ(matchInfo.replayNumber, 1);
+}
+
+TEST_F(DSCommPacketTest, GameDataTag) {
+ uint8_t arr[]{
+ // Size (2), tag
+ 0,
+ 0,
+ 17,
+ // Match data (length is taglength - 1)
+ 'W',
+ 'C',
+ 'B',
+ 'C',
+ };
+ arr[1] = sizeof(arr) - 2;
+ auto& matchInfo = ReadGameSpecificTag(arr);
+ ASSERT_EQ(matchInfo.gameSpecificMessageSize, 4);
+ ASSERT_EQ(matchInfo.gameSpecificMessage[0], 'W');
+ ASSERT_EQ(matchInfo.gameSpecificMessage[1], 'C');
+ ASSERT_EQ(matchInfo.gameSpecificMessage[2], 'B');
+ ASSERT_EQ(matchInfo.gameSpecificMessage[3], 'C');
+}
diff --git a/simulation/halsim_ds_socket/src/test/native/cpp/main.cpp b/simulation/halsim_ds_socket/src/test/native/cpp/main.cpp
new file mode 100644
index 0000000..c6b6c58
--- /dev/null
+++ b/simulation/halsim_ds_socket/src/test/native/cpp/main.cpp
@@ -0,0 +1,17 @@
+/*----------------------------------------------------------------------------*/
+/* Copyright (c) 2015-2019 FIRST. All Rights Reserved. */
+/* Open Source Software - may be modified and shared by FRC teams. The code */
+/* must be accompanied by the FIRST BSD license file in the root directory of */
+/* the project. */
+/*----------------------------------------------------------------------------*/
+
+#include <hal/HALBase.h>
+
+#include "gtest/gtest.h"
+
+int main(int argc, char** argv) {
+ HAL_Initialize(500, 0);
+ ::testing::InitGoogleTest(&argc, argv);
+ int ret = RUN_ALL_TESTS();
+ return ret;
+}