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

halsim_ds_socket: Update tag parsing, and add rumble support (#1214)

Browse Source 
Outputs are now sent. Ensure only the proper number of outputs are actually sent though.

Also adds match time, and proper enable tags.
This commit is contained in:
Thad House
2018-07-26 01:30:29 -07:00
committed by Peter Johnson
parent d54c2665dc
commit 63c1f80d60
14 changed files with 542 additions and 343 deletions
Download Patch File Download Diff File Expand all files Collapse all files

1
hal/src/main/native/include/hal/DriverStation.h
View File

@@ -61,6 +61,7 @@ HAL_ENUM(HAL_MatchType) {
*/
#define HAL_kMaxJoystickAxes 12
#define HAL_kMaxJoystickPOVs 12
#define HAL_kMaxJoysticks 6
struct HAL_JoystickAxes {
int16_t count;

3
hal/src/main/native/include/mockdata/MockHooks.h
View File

@@ -9,9 +9,12 @@
#ifndef __FRC_ROBORIO__
#include "hal/Types.h"
extern "C" {
void HALSIM_WaitForProgramStart(void);
void HALSIM_SetProgramStarted(void);
HAL_Bool HALSIM_GetProgramStarted(void);
void HALSIM_RestartTiming(void);
} // extern "C"

1
hal/src/main/native/include/simulation/SimHooks.h
View File

@@ -15,6 +15,7 @@ namespace frc {
namespace sim {
void WaitForProgramStart() { HALSIM_WaitForProgramStart(); }
void SetProgramStarted() { HALSIM_SetProgramStarted(); }
bool GetProgramStarted() { return HALSIM_GetProgramStarted(); }
void RestartTiming() { HALSIM_RestartTiming(); }
} // namespace sim
} // namespace frc

3
hal/src/main/native/sim/MockHooks.cpp
View File

@@ -36,6 +36,7 @@ int64_t GetFPGATime() {
double GetFPGATimestamp() { return GetFPGATime() * 1.0e-6; }
void SetProgramStarted() { programStarted = true; }
bool GetProgramStarted() { return programStarted; }
} // namespace hal
using namespace hal;
@@ -52,5 +53,7 @@ void HALSIM_WaitForProgramStart(void) {
void HALSIM_SetProgramStarted(void) { SetProgramStarted(); }
HAL_Bool HALSIM_GetProgramStarted(void) { return GetProgramStarted(); }
void HALSIM_RestartTiming(void) { RestartTiming(); }
} // extern "C"

1
shared/plugins/setupBuild.gradle
View File

@@ -1,5 +1,6 @@
apply plugin: 'cpp'
apply plugin: 'edu.wpi.first.NativeUtils'
apply plugin: ExtraTasks
if (!project.hasProperty('onlyAthena')) {
ext.skipAthena = true

50
simulation/halsim_ds_socket/build.gradle
View File

@@ -5,4 +5,54 @@ ext {
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('onlyAthena')) {
"${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) {
lib project: ':hal', library: 'hal', linkage: 'shared'
lib project: ':wpiutil', library: 'wpiutil', linkage: 'shared'
lib library: pluginName, linkage: 'shared'
}
}
}
tasks.withType(RunTestExecutable) {
args "--gtest_output=xml:test_detail.xml"
outputs.dir outputDir
}

7
simulation/halsim_ds_socket/src/dev/native/cpp/main.cpp
View File

@@ -8,6 +8,7 @@
#include <thread>
#include <hal/HAL.h>
#include <wpi/Format.h>
#include <wpi/raw_ostream.h>
extern "C" int HALSIM_InitExtension(void);
@@ -15,4 +16,10 @@ 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));
}
}

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

@@ -14,24 +14,24 @@
#include <thread>
#include <vector>
#include <FRCComm.h>
#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();
}
}
/*----------------------------------------------------------------------------
** The following methods help parse and hold information about the
** driver station and it's joysticks.
**--------------------------------------------------------------------------*/
void DSCommPacket::SetIndex(uint8_t hi, uint8_t lo) {
m_hi = hi;
m_lo = lo;
}
void DSCommPacket::GetIndex(uint8_t& hi, uint8_t& lo) {
hi = m_hi;
lo = m_lo;
}
void DSCommPacket::SetControl(uint8_t control, uint8_t request) {
std::memset(&m_control_word, 0, sizeof(m_control_word));
@@ -45,215 +45,263 @@ void DSCommPacket::SetControl(uint8_t control, uint8_t request) {
m_control_sent = control;
}
void DSCommPacket::GetControl(uint8_t& control) { control = m_control_sent; }
void DSCommPacket::GetStatus(uint8_t& status) { status = kRobotHasCode; }
void DSCommPacket::SetAlliance(uint8_t station_code) {
m_alliance_station = static_cast<enum AllianceStationID_t>(station_code);
m_alliance_station = static_cast<HAL_AllianceStationID>(station_code);
}
int DSCommPacket::AddDSCommJoystickPacket(uint8_t* data, int len) {
DSCommJoystickPacket stick;
if (len > 0) {
int axis_count = *data++;
len--;
if (axis_count > len) return -1;
len -= axis_count;
for (; axis_count > 0; axis_count--) {
stick.axes.push_back(*data++);
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];
float matchTime = *reinterpret_cast<float*>(&store);
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;
if (len > 2) {
stick.button_count = *data++;
stick.buttons = (*data++) << 8;
stick.buttons |= *data++;
len -= 3;
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[1 + 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];
}
if (len > 0) {
int pov_count = *data++;
len--;
if (pov_count * 2 > len) return -1;
len -= pov_count * 2;
for (; pov_count > 0; pov_count--) {
stick.povs.push_back((data[0] << 8) | data[1]);
data += 2;
}
}
stick.povs.count = povsLength;
m_joystick_packets.push_back(stick);
return len;
}
void DSCommPacket::GetControlWord(struct ControlWord_t& control_word) {
control_word = m_control_word;
}
void DSCommPacket::GetAllianceStation(
enum AllianceStationID_t& alliance_station) {
alliance_station = m_alliance_station;
return;
}
/*----------------------------------------------------------------------------
** Communication methods
**--------------------------------------------------------------------------*/
int DSCommPacket::DecodeTCP(uint8_t* packet, int len) {
if (len < 2) return 0;
if (packet[0] == 0 && packet[1] == 0) return 2;
int packet_len = packet[1];
if (packet_len + 2 > len) return 0;
int packet_type = static_cast<int>(packet[2]);
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);
Lock();
if (packet_type == kGameDataType) {
std::copy(
packet + 3,
packet + 3 +
std::min(static_cast<int>(sizeof(m_game_data)), packet_len - 1),
m_game_data);
} else if (packet_type == kJoystickNameType && len >= 7) {
int joystick = static_cast<int>(packet[3]);
if (joystick < kMaxJoysticks) {
m_joystick_types[joystick] = static_cast<int>(packet[5]);
int namelen = static_cast<int>(packet[6]);
m_joystick_names[joystick] =
std::string(reinterpret_cast<char*>(packet + 7), namelen);
if (tagLength == 0) {
return;
}
} else {
std::cerr << "TCP packet type " << packet_type << " unimplemented"
<< std::endl;
for (int i = 0; i < packet_len + 2; i++)
std::fprintf(stderr, "%02x ", packet[i]);
std::fprintf(stderr, "\n");
switch (packet[2]) {
case kJoystickNameTag:
ReadJoystickDescriptionTag(tagPacket);
break;
case kGameDataTag:
ReadGameSpecificMessageTag(tagPacket);
break;
case kMatchInfoTag:
ReadNewMatchInfoTag(tagPacket);
break;
}
packet = packet.slice(tagLength + 2);
}
Unlock();
return packet_len + 2;
}
void DSCommPacket::DecodeUDP(uint8_t* packet, int len) {
if (len < 3) return;
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]);
Lock();
m_joystick_packets.clear();
SetIndex(packet[0], packet[1]);
if (packet[2] != 0) {
if (len >= 6) {
SetControl(packet[3], packet[4]);
SetAlliance(packet[5]);
packet += 6;
len -= 6;
while (len > 0) {
int packet_len = *packet++;
if (packet_len > len) break;
if (*packet == kTagDsCommJoystick) {
if (AddDSCommJoystickPacket(packet + 1, packet_len - 1) < 0) break;
}
packet += packet_len;
len -= packet_len;
}
// 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);
}
m_udp_packets++;
Unlock();
}
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) {
unsigned int i;
for (i = 0; i < kMaxJoysticks; i++) {
struct HAL_JoystickAxes axes;
struct HAL_JoystickPOVs povs;
struct HAL_JoystickButtons buttons;
struct HAL_JoystickDescriptor descriptor;
int j;
std::memset(&axes, 0, sizeof(axes));
std::memset(&povs, 0, sizeof(povs));
std::memset(&buttons, 0, sizeof(buttons));
std::memset(&descriptor, 0, sizeof(descriptor));
if (i < m_joystick_packets.size()) {
axes.count = std::min(static_cast<int>(m_joystick_packets[i].axes.size()),
HAL_kMaxJoystickAxes);
for (j = 0; j < axes.count; j++) {
int8_t value = m_joystick_packets[i].axes[j];
if (value < 0) {
axes.axes[j] = value / 128.0;
} else {
axes.axes[j] = value / 127.0;
}
}
povs.count = std::min(static_cast<int>(m_joystick_packets[i].povs.size()),
HAL_kMaxJoystickPOVs);
for (j = 0; j < povs.count; j++)
povs.povs[j] = m_joystick_packets[i].povs[j];
buttons.count = m_joystick_packets[i].button_count;
buttons.buttons = m_joystick_packets[i].buttons;
descriptor.axisCount = axes.count;
descriptor.povCount = povs.count;
descriptor.buttonCount = buttons.count;
}
descriptor.type = m_joystick_types[i];
m_joystick_names[i].copy(descriptor.name, sizeof(descriptor.name) - 1, 0);
HALSIM_SetJoystickAxes(i, &axes);
HALSIM_SetJoystickPOVs(i, &povs);
HALSIM_SetJoystickButtons(i, &buttons);
HALSIM_SetJoystickDescriptor(i, &descriptor);
/* TODO(jwhite@codeweavers.com): If we want to support rumble, etc,
implement SetJoyStickOutputs, although that would be a callback */
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::SendTCPToHALSim(void) {
struct HAL_MatchInfo info;
Lock();
std::strncpy(info.eventName, "Simulation", sizeof(info.eventName));
info.matchType = HAL_MatchType::HAL_kMatchType_none;
info.matchNumber = 1;
info.replayNumber = 0;
std::copy(info.gameSpecificMessage,
info.gameSpecificMessage +
std::min(sizeof(info.gameSpecificMessage), sizeof(m_game_data)),
m_game_data);
HALSIM_SetMatchInfo(&info);
Unlock();
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) {
struct ControlWord_t control_word;
AllianceStationID_t alliance_station;
Lock();
GetControlWord(control_word);
GetAllianceStation(alliance_station);
auto now = std::chrono::high_resolution_clock::now();
if (m_udp_packets == 1) {
m_match_time = 0.0;
} else if (control_word.enabled) {
std::chrono::duration<double> delta = (now - m_packet_time);
m_match_time += delta.count();
}
m_packet_time = now;
SendJoysticks();
Unlock();
HALSIM_SetDriverStationMatchTime(m_match_time);
HALSIM_SetDriverStationEnabled(control_word.enabled);
HALSIM_SetDriverStationAutonomous(control_word.autonomous);
HALSIM_SetDriverStationTest(control_word.test);
HALSIM_SetDriverStationEStop(control_word.eStop);
HALSIM_SetDriverStationFmsAttached(control_word.fmsAttached);
HALSIM_SetDriverStationDsAttached(control_word.dsAttached);
HALSIM_SetDriverStationAllianceStationId(
static_cast<HAL_AllianceStationID>(alliance_station));
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();
}

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

@@ -23,6 +23,7 @@
#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>
@@ -38,16 +39,21 @@ static std::unique_ptr<Buffer> singleByte;
namespace {
struct DataStore {
wpi::SmallVector<char, 128> m_frame;
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_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);
@@ -57,7 +63,7 @@ static void HandleTcpDataStream(Buffer& buf, size_t size, DataStore& store) {
store.m_frameSize = (static_cast<uint16_t>(store.m_frame[0]) << 8) |
static_cast<uint16_t>(store.m_frame[1]);
}
if (store.m_frameSize != 0) {
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);
@@ -65,9 +71,7 @@ static void HandleTcpDataStream(Buffer& buf, size_t size, DataStore& store) {
need -= toCopy;
if (need == 0) {
auto ds = store.dsPacket;
ds->DecodeTCP(reinterpret_cast<uint8_t*>(store.m_frame.data()),
store.m_frame.size());
ds->SendTCPToHALSim();
ds->DecodeTCP(store.m_frame);
store.m_frame.clear();
store.m_frameSize = std::numeric_limits<size_t>::max();
}
@@ -80,6 +84,7 @@ static void SetupTcp(wpi::uv::Loop& loop) {
auto tcpWaitTimer = Timer::Create(loop);
auto recStore = std::make_shared<DataStore>();
recStore->dsPacket = loop.GetData<halsim::DSCommPacket>().get();
tcp->SetData(recStore);
@@ -87,31 +92,15 @@ static void SetupTcp(wpi::uv::Loop& loop) {
tcp->Listen([t = tcp.get()] {
auto client = t->Accept();
t->data.connect([t](Buffer& buf, size_t len) {
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(); });
});
}
/*----------------------------------------------------------------------------
** Send a reply packet back to the DS
**--------------------------------------------------------------------------*/
static void SetupReplyPacket(halsim::DSCommPacket* ds) {
static const uint8_t kTagGeneral = 0x01;
uint8_t* data = reinterpret_cast<uint8_t*>(ds->GetSendBuffer().base);
ds->GetIndex(data[0], data[1]);
data[2] = kTagGeneral;
ds->GetControl(data[3]);
ds->GetStatus(data[4]);
data[5] = 12; // Voltage upper
data[6] = 0; // Voltage lower
data[7] = 0; // Request
}
static void SetupUdp(wpi::uv::Loop& loop) {
auto udp = wpi::uv::Udp::Create(loop);
udp->Bind("0.0.0.0", 1110);
@@ -135,21 +124,25 @@ static void SetupUdp(wpi::uv::Loop& loop) {
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(reinterpret_cast<uint8_t*>(buf.base), len);
SetupReplyPacket(ds.get());
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);
udpLocal->Send(outAddr, wpi::ArrayRef<Buffer>{&ds->GetSendBuffer(), 1},
[](auto buf, Error err) {
if (err) {
wpi::errs() << err.str() << "\n";
wpi::errs().flush();
}
});
wpi::SmallVector<wpi::uv::Buffer, 4> sendBufs;
wpi::raw_uv_ostream stream{sendBufs, GetBufferPool()};
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();
});

21
simulation/halsim_ds_socket/src/main/native/include/DSCommJoystickPacket.h
View File

@@ -6,15 +6,26 @@
/*----------------------------------------------------------------------------*/
#pragma once
#include <vector>
#include <cstring>
#include <hal/DriverStation.h>
namespace halsim {
typedef struct {
std::vector<int16_t> axes;
uint8_t button_count;
uint32_t buttons;
std::vector<int16_t> povs;
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

86
simulation/halsim_ds_socket/src/main/native/include/DSCommPacket.h
View File

@@ -7,46 +7,35 @@
#pragma once
#include <chrono>
#include <mutex>
#include <string>
#include <thread>
#include <vector>
#include <array>
#include <DSCommJoystickPacket.h>
#include <FRCComm.h>
#include <mockdata/DriverStationData.h>
#include <wpi/uv/Buffer.h>
#include <wpi/ArrayRef.h>
#include <wpi/raw_uv_ostream.h>
class DSCommPacketTest;
namespace halsim {
class DSCommPacket {
public:
DSCommPacket(void) {
std::fill_n(m_joystick_types, kMaxJoysticks, -1);
sendDataBuffer = wpi::uv::Buffer::Allocate(8);
}
void Lock() { m_mutex.lock(); }
void Unlock() { m_mutex.unlock(); }
void SetIndex(uint8_t hi, uint8_t lo);
void GetIndex(uint8_t& hi, uint8_t& lo);
void SetControl(uint8_t control, uint8_t request);
void GetControl(uint8_t& control);
void GetStatus(uint8_t& status);
void SetAlliance(uint8_t station_code);
int AddDSCommJoystickPacket(uint8_t* data, int len);
void GetControlWord(struct ControlWord_t& control_word);
void GetAllianceStation(enum AllianceStationID_t& allianceStation);
int DecodeTCP(uint8_t* packet, int len);
void DecodeUDP(uint8_t* packet, int len);
void SendTCPToHALSim(void);
void SendUDPToHALSim(void);
void SendJoysticks(void);
wpi::uv::Buffer& GetSendBuffer(void) { return sendDataBuffer; }
friend class ::DSCommPacketTest;
/* TCP (FMS) types */
static const uint8_t kGameDataType = 0x0e;
static const uint8_t kJoystickNameType = 0x02;
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;
@@ -61,29 +50,26 @@ class DSCommPacket {
/* Status bits */
static const uint8_t kRobotHasCode = 0x20;
/* Joystick tag bits */
static const uint8_t kTagDsCommJoystick = 0x0c;
/* Joystick max count */
/* TODO(jwhite@codeweavers.com) This is a magic number in the HAL; fix it
* there */
static const uint8_t kMaxJoysticks = 6;
private:
uint8_t m_game_data[64];
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;
struct ControlWord_t m_control_word;
enum AllianceStationID_t m_alliance_station;
std::vector<DSCommJoystickPacket> m_joystick_packets;
std::string m_joystick_names[kMaxJoysticks];
int m_joystick_types[kMaxJoysticks];
std::mutex m_mutex;
int m_udp_packets;
std::chrono::high_resolution_clock::time_point m_packet_time;
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;
wpi::uv::Buffer sendDataBuffer;
};
} // namespace halsim

68
simulation/halsim_ds_socket/src/main/native/include/FRCComm.h
View File

@@ -1,68 +0,0 @@
/*----------------------------------------------------------------------------*/
/* Copyright (c) 2008-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. */
/*----------------------------------------------------------------------------*/
/*----------------------------------------------------------------------------
* The defines and enums in this file were copied from:
* ni-libraries/include/FRC_NetworkCommunication/FRCComm.h
* to avoid the complexities of trying to get gradle to
* reliably build against it.
*----------------------------------------------------------------------------*/
#ifndef WPILIB_SIMULATION_HALSIM_DS_SOCKET_SRC_MAIN_NATIVE_INCLUDE_FRCCOMM_H_
#define WPILIB_SIMULATION_HALSIM_DS_SOCKET_SRC_MAIN_NATIVE_INCLUDE_FRCCOMM_H_
#include <stdint.h>
#define ERR_FRCSystem_NetCommNotResponding -44049
#define ERR_FRCSystem_NoDSConnection -44018
enum AllianceStationID_t {
kAllianceStationID_red1,
kAllianceStationID_red2,
kAllianceStationID_red3,
kAllianceStationID_blue1,
kAllianceStationID_blue2,
kAllianceStationID_blue3,
};
enum MatchType_t {
kMatchType_none,
kMatchType_practice,
kMatchType_qualification,
kMatchType_elimination,
};
struct ControlWord_t {
#ifndef __vxworks
uint32_t enabled : 1;
uint32_t autonomous : 1;
uint32_t test : 1;
uint32_t eStop : 1;
uint32_t fmsAttached : 1;
uint32_t dsAttached : 1;
uint32_t control_reserved : 26;
#else
uint32_t control_reserved : 26;
uint32_t dsAttached : 1;
uint32_t fmsAttached : 1;
uint32_t eStop : 1;
uint32_t test : 1;
uint32_t autonomous : 1;
uint32_t enabled : 1;
#endif
};
struct JoystickAxes_t {
uint16_t count;
int16_t axes[1];
};
struct JoystickPOV_t {
uint16_t count;
int16_t povs[1];
};
#endif // WPILIB_SIMULATION_HALSIM_DS_SOCKET_SRC_MAIN_NATIVE_INCLUDE_FRCCOMM_H_

146
simulation/halsim_ds_socket/src/test/native/cpp/DSCommPacketTest.cpp Normal file
View File

@@ -0,0 +1,146 @@
/*----------------------------------------------------------------------------*/
/* 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. */
/*----------------------------------------------------------------------------*/
#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++) {
// 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
12, 0xFF, 0x0F,
// 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);
}
}
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');
}

17
simulation/halsim_ds_socket/src/test/native/cpp/main.cpp Normal file
View File

@@ -0,0 +1,17 @@
/*----------------------------------------------------------------------------*/
/* Copyright (c) 2015-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. */
/*----------------------------------------------------------------------------*/
#include <hal/HAL.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;
}