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

2
glass/src/lib/native/cpp/other/FMS.cpp
View File

@@ -17,7 +17,7 @@ using namespace wpi::glass;
static const char* stations[] = {"Invalid", "Red 1", "Red 2", "Red 3",
"Blue 1", "Blue 2", "Blue 3"};
static const char* robotModes[] = {"Unknown", "Autonomous", "Teleoperated",
"Test"};
"Utility"};
void wpi::glass::DisplayFMS(FMSModel* model, bool editableDsAttached) {
if (!model->Exists() || model->IsReadOnly()) {

8
hal/BUILD.bazel
View File

@@ -116,6 +116,7 @@ wpilib_cc_library(
deps = [
":generated_mrc_cc_headers",
"//ntcore",
"//shared/bazel/thirdparty/mrclib",
"//wpinet",
"//wpiutil",
] + select({
@@ -238,6 +239,12 @@ generate_robotpy_native_wrapper_build_info(
pyproject_toml = "src/main/python/native-pyproject.toml",
)
# Alias to make robotpy gen simpler.
alias(
name = "shared/MrcLib",
actual = "//shared/bazel/thirdparty/mrclib:MrcLib",
)
define_native_wrapper(
name = "robotpy-native-wpihal",
)
@@ -245,6 +252,7 @@ define_native_wrapper(
PYBIND_PKGCFG_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",
"//ntcore:native/ntcore/robotpy-native-ntcore.pc",
"//ntcore:pyntcore.generated_pkgcfg_files",

23
hal/build.gradle
View File

@@ -1,12 +1,4 @@
ext {
addHalDependency = { binary, shared->
binary.lib project: ':hal', library: 'hal', linkage: shared
}
addHalJniDependency = { binary->
binary.lib project: ':hal', library: 'halJNIShared', linkage: 'shared'
}
nativeName = 'hal'
setBaseName = 'wpiHal'
devMain = 'org.wpilib.hardware.hal.DevMain'
@@ -60,6 +52,20 @@ ext {
apply from: "${rootDir}/shared/jni/setupBuild.gradle"
apply from: "${rootDir}/shared/libmrclib.gradle"
ext {
addHalDependency = { binary, shared->
nativeUtils.useRequiredLibrary(binary, 'mrclib')
binary.lib project: ':hal', library: 'hal', linkage: shared
}
addHalJniDependency = { binary->
nativeUtils.useRequiredLibrary(binary, 'mrclib')
binary.lib project: ':hal', library: 'halJNIShared', linkage: 'shared'
}
}
sourceSets.main.java.srcDir "${projectDir}/src/generated/main/java"
cppSourcesZip {
@@ -113,6 +119,7 @@ model {
if (!it.buildable || !(it instanceof NativeBinarySpec)) {
return
}
nativeUtils.useRequiredLibrary(it, 'mrclib')
if (it.component.name == "${nativeName}JNI") {
project(':ntcore').addNtcoreDependency(it, 'static')
lib project: ':wpinet', library: 'wpinet', linkage: 'static'

12
hal/robotpy_native_build_info.bzl generated
View File

@@ -19,7 +19,7 @@ def define_native_wrapper(name, pyproject_toml = None):
visibility = ["//visibility:public"],
)
libinit_files = ["native/wpihal/_init_robotpy_native_wpihal.py"]
libinit_files = ["native/wpihal/_init_robotpy_native_mrclib.py", "native/wpihal/_init_robotpy_native_wpihal.py"]
generate_native_files(
name = name,
@@ -29,7 +29,7 @@ def define_native_wrapper(name, pyproject_toml = None):
"//wpiutil:native/wpiutil/robotpy-native-wpiutil.pc",
],
libinit_files = libinit_files,
pc_files = ["native/wpihal/robotpy-native-wpihal.pc"],
pc_files = ["native/wpihal/robotpy-native-mrclib.pc", "native/wpihal/robotpy-native-wpihal.pc"],
)
copy_native_file(
@@ -38,6 +38,12 @@ def define_native_wrapper(name, pyproject_toml = None):
base_path = "native/wpihal/",
)
copy_native_file(
name = "MrcLib",
library = "shared/MrcLib",
base_path = "native/wpihal/",
)
robotpy_library(
name = name,
distribution = "robotpy-native-wpihal",
@@ -45,6 +51,7 @@ def define_native_wrapper(name, pyproject_toml = None):
data = [
name + ".pc_wrapper",
":wpiHal.copy_lib",
":MrcLib.copy_lib",
"{}.copy_headers".format(name),
],
deps = [
@@ -57,6 +64,7 @@ def define_native_wrapper(name, pyproject_toml = None):
strip_path_prefixes = ["hal"],
entry_points = {
"pkg_config": [
"mrclib = native.mrclib",
"wpihal = native.wpihal",
],
},

13
hal/src/main/java/org/wpilib/hardware/hal/DriverStationJNI.java
View File

@@ -283,7 +283,6 @@ public class DriverStationJNI extends JNIWrapper {
*
* @param isError true for error, false for warning
* @param errorCode the error code
* @param isLVCode true for a LV error code, false for a standard error code
* @param details the details of the error
* @param location the file location of the error
* @param callStack the callstack of the error
@@ -294,7 +293,6 @@ public class DriverStationJNI extends JNIWrapper {
public static native int sendError(
boolean isError,
int errorCode,
boolean isLVCode,
String details,
String location,
String callStack,
@@ -308,6 +306,17 @@ public class DriverStationJNI extends JNIWrapper {
*/
public static native int sendConsoleLine(String line);
/**
* Sends a program crash to the driver station.
*
* @param details the details of the crash
* @param location the file location of the crash
* @param callStack the callstack of the crash
* @return the error code, or 0 for success
* @see "HAL_SendProgramCrash"
*/
public static native int sendProgramCrash(String details, String location, String callStack);
/**
* Refresh the DS control word.
*

302
hal/src/main/native/cpp/FIRSTDriverStation.cpp Normal file
View File

@@ -0,0 +1,302 @@
// 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 <atomic>
#include <cstdio>
#include <cstring>
#include <functional>
#include <string>
#include <fmt/format.h>
#include "wpi/hal/DashboardOpMode.hpp"
#include "wpi/hal/cpp/MrcLibDs.hpp"
#include "wpi/hal/monotonic_clock.hpp"
#include "wpi/util/Synchronization.hpp"
#include "wpi/util/mutex.hpp"
static wpi::hal::MrcLibDs* mrcLibDs;
wpi::hal::MrcLibDs::~MrcLibDs() = default;
static void DefaultPrintErrorImpl(const struct WPI_String* line) {
if (line && line->str) {
std::fwrite(line->str, line->len, 1, stderr);
}
}
static std::atomic<void (*)(const struct WPI_String* line)> gPrintErrorImpl{
DefaultPrintErrorImpl};
namespace wpi::hal {
int32_t DefaultSendErrorImpl(bool isError, int32_t errorCode,
const struct WPI_String* details,
const struct WPI_String* location,
const struct WPI_String* callStack, bool printMsg,
wpi::hal::BackendPrintFunction& backendWriteFunc) {
auto detailsView = wpi::util::to_string_view(details);
auto locationView = wpi::util::to_string_view(location);
auto callStackView = wpi::util::to_string_view(callStack);
// Avoid flooding console by keeping track of previous 5 error
// messages and only printing again if they're longer than 1 second old.
static wpi::util::mutex msgMutex;
static constexpr int KEEP_MSGS = 5;
std::scoped_lock lock(msgMutex);
static std::string prevMsg[KEEP_MSGS];
static std::chrono::time_point<monotonic_clock> prevMsgTime[KEEP_MSGS];
static bool initialized = false;
if (!initialized) {
for (int i = 0; i < KEEP_MSGS; i++) {
prevMsgTime[i] = monotonic_clock::now() - std::chrono::seconds(2);
}
initialized = true;
}
auto curTime = monotonic_clock::now();
int i;
for (i = 0; i < KEEP_MSGS; ++i) {
if (prevMsg[i] == detailsView) {
break;
}
}
int retval = 0;
if (i == KEEP_MSGS || (curTime - prevMsgTime[i]) >= std::chrono::seconds(1)) {
if (backendWriteFunc) {
retval = backendWriteFunc(isError, errorCode, details, location,
callStack, &printMsg);
}
if (printMsg) {
fmt::memory_buffer buf;
if (!locationView.empty() && locationView[0] != '\0') {
fmt::format_to(fmt::appender{buf},
"{} at {}: ", isError ? "Error" : "Warning",
locationView);
}
fmt::format_to(fmt::appender{buf}, "{}\n", detailsView);
if (!callStackView.empty() && callStackView[0] != '\0') {
fmt::format_to(fmt::appender{buf}, "{}\n", callStackView);
}
auto printError = gPrintErrorImpl.load();
struct WPI_String line = {buf.data(), buf.size()};
printError(&line);
}
if (i == KEEP_MSGS) {
// replace the oldest one
i = 0;
auto first = prevMsgTime[0];
for (int j = 1; j < KEEP_MSGS; ++j) {
if (prevMsgTime[j] < first) {
first = prevMsgTime[j];
i = j;
}
}
prevMsg[i] = detailsView;
}
prevMsgTime[i] = curTime;
}
return retval;
}
} // namespace wpi::hal
extern "C" {
void HAL_SetPrintErrorImpl(void (*func)(const struct WPI_String* line)) {
gPrintErrorImpl.store(func ? func : DefaultPrintErrorImpl);
}
int32_t HAL_SendError(HAL_Bool isError, int32_t errorCode,
const struct WPI_String* details,
const struct WPI_String* location,
const struct WPI_String* callStack, HAL_Bool printMsg) {
return mrcLibDs->sendError(isError, errorCode, details, location, callStack,
printMsg);
}
int32_t HAL_SendConsoleLine(const struct WPI_String* line) {
return mrcLibDs->sendConsoleLine(line);
}
int32_t HAL_SendProgramCrash(const struct WPI_String* details,
const struct WPI_String* location,
const struct WPI_String* callStack) {
return mrcLibDs->sendProgramCrash(details, location, callStack);
}
int32_t HAL_GetControlWord(HAL_ControlWord* controlWord) {
return mrcLibDs->getControlWord(controlWord);
}
int32_t HAL_GetUncachedControlWord(HAL_ControlWord* controlWord) {
return mrcLibDs->getUncachedControlWord(controlWord);
}
int32_t HAL_SetOpModeOptions(const struct HAL_OpModeOption* options,
int32_t count) {
return mrcLibDs->setOpModeOptions(options, count);
}
int32_t HAL_GetJoystickAxes(int32_t joystickNum, HAL_JoystickAxes* axes) {
return mrcLibDs->getJoystickAxes(joystickNum, axes);
}
int32_t HAL_GetJoystickPOVs(int32_t joystickNum, HAL_JoystickPOVs* povs) {
return mrcLibDs->getJoystickPOVs(joystickNum, povs);
}
int32_t HAL_GetJoystickButtons(int32_t joystickNum,
HAL_JoystickButtons* buttons) {
return mrcLibDs->getJoystickButtons(joystickNum, buttons);
}
int32_t HAL_GetJoystickTouchpads(int32_t joystickNum,
HAL_JoystickTouchpads* touchpads) {
return mrcLibDs->getJoystickTouchpads(joystickNum, touchpads);
}
void HAL_GetAllJoystickData(int32_t joystickNum, HAL_JoystickAxes* axes,
HAL_JoystickPOVs* povs,
HAL_JoystickButtons* buttons,
HAL_JoystickTouchpads* touchpads) {
mrcLibDs->getAllJoystickData(joystickNum, axes, povs, buttons, touchpads);
}
int32_t HAL_GetJoystickDescriptor(int32_t joystickNum,
HAL_JoystickDescriptor* desc) {
return mrcLibDs->getJoystickDescriptor(joystickNum, desc);
}
int32_t HAL_GetMatchInfo(HAL_MatchInfo* info) {
return mrcLibDs->getMatchInfo(info);
}
HAL_AllianceStationID HAL_GetAllianceStation(int32_t* status) {
HAL_AllianceStationID allianceStation;
*status = mrcLibDs->getAllianceStation(&allianceStation);
return allianceStation;
}
HAL_Bool HAL_GetJoystickIsGamepad(int32_t joystickNum) {
HAL_JoystickDescriptor joystickDesc;
int32_t status = mrcLibDs->getJoystickDescriptor(joystickNum, &joystickDesc);
if (status == 0) {
return joystickDesc.isGamepad != 0;
}
return false;
}
int32_t HAL_GetJoystickGamepadType(int32_t joystickNum) {
HAL_JoystickDescriptor joystickDesc;
int32_t status = mrcLibDs->getJoystickDescriptor(joystickNum, &joystickDesc);
if (status == 0) {
return joystickDesc.gamepadType;
}
return -1;
}
int32_t HAL_GetGameData(HAL_GameData* gameData) {
return mrcLibDs->getGameData(gameData);
}
int32_t HAL_GetJoystickSupportedOutputs(int32_t joystickNum) {
HAL_JoystickDescriptor joystickDesc;
int32_t status = mrcLibDs->getJoystickDescriptor(joystickNum, &joystickDesc);
if (status == 0) {
return joystickDesc.supportedOutputs;
}
return -1;
}
void HAL_GetJoystickName(struct WPI_String* name, int32_t joystickNum) {
HAL_JoystickDescriptor joystickDesc;
const char* cName = joystickDesc.name;
if (mrcLibDs->getJoystickDescriptor(joystickNum, &joystickDesc) == 0) {
cName = joystickDesc.name;
} else {
cName = "";
}
auto len = std::strlen(cName);
auto write = WPI_AllocateString(name, len);
std::memcpy(write, cName, len);
}
int32_t HAL_SetJoystickRumble(int32_t joystickNum, int32_t leftRumble,
int32_t rightRumble, int32_t leftTriggerRumble,
int32_t rightTriggerRumble) {
return mrcLibDs->setJoystickRumble(joystickNum, leftRumble, rightRumble,
leftTriggerRumble, rightTriggerRumble);
}
int32_t HAL_SetJoystickLeds(int32_t joystickNum, int32_t leds) {
return mrcLibDs->setJoystickLeds(joystickNum, leds);
}
double HAL_GetMatchTime(int32_t* status) {
double matchTime;
*status = mrcLibDs->getMatchTime(&matchTime);
if (*status != 0) {
return -1.0;
}
return matchTime;
}
void HAL_ObserveUserProgramStarting(void) {
mrcLibDs->observeUserProgramStarting();
}
void HAL_ObserveUserProgram(HAL_ControlWord word) {
mrcLibDs->observeUserProgram(word);
}
HAL_Bool HAL_RefreshDSData(void) {
bool wasRefreshed;
if (mrcLibDs->refreshDSData(&wasRefreshed) < 0) {
return false;
}
return wasRefreshed;
}
void HAL_ProvideNewDataEventHandle(WPI_EventHandle handle) {
mrcLibDs->provideNewDataEventHandle(handle);
}
void HAL_RemoveNewDataEventHandle(WPI_EventHandle handle) {
mrcLibDs->removeNewDataEventHandle(handle);
}
HAL_Bool HAL_GetOutputsEnabled(void) {
bool outputsEnabled;
if (mrcLibDs->getOutputsEnabled(&outputsEnabled) < 0) {
return false;
}
return outputsEnabled;
}
HAL_Bool HAL_GetSystemTimeValid(int32_t* status) {
bool systemTimeValid;
*status = mrcLibDs->getSystemTimeValid(&systemTimeValid);
return systemTimeValid;
}
} // extern "C"
namespace wpi::hal {
void InitializeDriverStation() {
InitializeDashboardOpMode();
mrcLibDs = wpi::hal::GetDefaultDriverStationImpl();
}
void ForceDsInstance(MrcLibDs* ds) {
mrcLibDs = ds;
}
void WaitForInitialPacket() {
wpi::util::Event waitForInitEvent;
mrcLibDs->provideNewDataEventHandle(waitForInitEvent.GetHandle());
bool timed_out = false;
wpi::util::WaitForObject(waitForInitEvent.GetHandle(), 0.1, &timed_out);
// Don't care what the result is, just want to give it a chance.
mrcLibDs->removeNewDataEventHandle(waitForInitEvent.GetHandle());
}
} // namespace wpi::hal

40
hal/src/main/native/cpp/jni/DriverStationJNI.cpp
View File

@@ -341,20 +341,23 @@ Java_org_wpilib_hardware_hal_DriverStationJNI_getGameData
/*
* Class: org_wpilib_hardware_hal_DriverStationJNI
* Method: sendError
* Signature: (ZIZLjava/lang/String;Ljava/lang/String;Ljava/lang/String;Z)I
* Signature: (ZILjava/lang/String;Ljava/lang/String;Ljava/lang/String;Z)I
*/
JNIEXPORT jint JNICALL
Java_org_wpilib_hardware_hal_DriverStationJNI_sendError
(JNIEnv* env, jclass, jboolean isError, jint errorCode, jboolean isLVCode,
jstring details, jstring location, jstring callStack, jboolean printMsg)
(JNIEnv* env, jclass, jboolean isError, jint errorCode, jstring details,
jstring location, jstring callStack, jboolean printMsg)
{
JStringRef detailsStr{env, details};
JStringRef locationStr{env, location};
JStringRef callStackStr{env, callStack};
jint returnValue =
HAL_SendError(isError, errorCode, isLVCode, detailsStr.c_str(),
locationStr.c_str(), callStackStr.c_str(), printMsg);
WPI_String detailsWpiStr = wpi::util::make_string(detailsStr);
WPI_String locationWpiStr = wpi::util::make_string(locationStr);
WPI_String callStackWpiStr = wpi::util::make_string(callStackStr);
jint returnValue = HAL_SendError(isError, errorCode, &detailsWpiStr,
&locationWpiStr, &callStackWpiStr, printMsg);
return returnValue;
}
@@ -369,7 +372,30 @@ Java_org_wpilib_hardware_hal_DriverStationJNI_sendConsoleLine
{
JStringRef lineStr{env, line};
jint returnValue = HAL_SendConsoleLine(lineStr.c_str());
WPI_String lineWpiStr = wpi::util::make_string(lineStr);
jint returnValue = HAL_SendConsoleLine(&lineWpiStr);
return returnValue;
}
/*
* Class: org_wpilib_hardware_hal_DriverStationJNI
* Method: sendProgramCrash
* Signature: (Ljava/lang/String;Ljava/lang/String;Ljava/lang/String;)I
*/
JNIEXPORT jint JNICALL
Java_org_wpilib_hardware_hal_DriverStationJNI_sendProgramCrash
(JNIEnv* env, jclass, jstring details, jstring location, jstring callStack)
{
JStringRef detailsStr{env, details};
JStringRef locationStr{env, location};
JStringRef callStackStr{env, callStack};
WPI_String detailsWpiStr = wpi::util::make_string(detailsStr);
WPI_String locationWpiStr = wpi::util::make_string(locationStr);
WPI_String callStackWpiStr = wpi::util::make_string(callStackStr);
jint returnValue =
HAL_SendProgramCrash(&detailsWpiStr, &locationWpiStr, &callStackWpiStr);
return returnValue;
}

7
hal/src/main/native/cpp/jni/HALUtil.cpp
View File

@@ -113,8 +113,11 @@ void ReportError(JNIEnv* env, int32_t status, bool doThrow) {
// Make a copy of message for safety, calling back into the HAL might
// invalidate the string.
std::string lastMessage{message};
HAL_SendError(1, status, 0, lastMessage.c_str(), func.c_str(),
stack.c_str(), 1);
WPI_String details = wpi::util::make_string(lastMessage);
WPI_String location = wpi::util::make_string(func);
WPI_String callStack = wpi::util::make_string(stack);
HAL_SendError(1, status, &details, &location, &callStack, 1);
}
}

7
hal/src/main/native/cpp/jni/simulation/DriverStationDataJNI.cpp
View File

@@ -658,8 +658,9 @@ Java_org_wpilib_hardware_hal_simulation_DriverStationDataJNI_setSendError
HALSIM_SetSendError(nullptr);
} else {
HALSIM_SetSendError([](HAL_Bool isError, int32_t errorCode,
HAL_Bool isLVCode, const char* details,
const char* location, const char* callStack,
const struct WPI_String* details,
const struct WPI_String* location,
const struct WPI_String* callStack,
HAL_Bool printMsg) { return 0; });
}
}
@@ -676,7 +677,7 @@ Java_org_wpilib_hardware_hal_simulation_DriverStationDataJNI_setSendConsoleLine
if (shouldSend) {
HALSIM_SetSendConsoleLine(nullptr);
} else {
HALSIM_SetSendConsoleLine([](const char* line) { return 0; });
HALSIM_SetSendConsoleLine([](const struct WPI_String* line) { return 0; });
}
}

670
hal/src/main/native/cpp/mrclib/MrcLibDs.cpp Normal file
View File

@@ -0,0 +1,670 @@
// 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/hal/cpp/MrcLibDs.hpp"
#include <algorithm>
#include <cstring>
#include <exception>
#include <span>
#include <utility>
#include <vector>
#include <fmt/format.h>
#include "mrclib/ApiVersion.h"
#include "mrclib/Console.h"
#include "mrclib/DsComms.h"
#include "mrclib/DsComms.hpp"
#include "mrclib/DsCommsControl.h"
#include "mrclib/MrcString.hpp"
#include "wpi/hal/DashboardOpMode.hpp"
#include "wpi/hal/Errors.h"
#include "wpi/util/EventVector.hpp"
#include "wpi/util/mutex.hpp"
using namespace wpi::hal;
namespace {
struct JoystickDataCache {
JoystickDataCache() { std::memset(this, 0, sizeof(*this)); }
void Update(const MRC_ControlData& controlData,
const MRC_Joysticks& joystickData);
HAL_JoystickAxes axes[HAL_MAX_JOYSTICKS];
HAL_JoystickPOVs povs[HAL_MAX_JOYSTICKS];
HAL_JoystickButtons buttons[HAL_MAX_JOYSTICKS];
HAL_JoystickTouchpads touchpads[HAL_MAX_JOYSTICKS];
HAL_AllianceStationID allianceStation;
float matchTime;
HAL_ControlWord controlWord;
HAL_GameData gameData;
};
static_assert(std::is_standard_layout_v<JoystickDataCache>);
struct TcpCache {
TcpCache() { std::memset(this, 0, sizeof(*this)); }
void Update();
void CloneTo(TcpCache* other) { std::memcpy(other, this, sizeof(*this)); }
HAL_MatchInfo matchInfo;
HAL_JoystickDescriptor descriptors[HAL_MAX_JOYSTICKS];
};
static_assert(std::is_standard_layout_v<TcpCache>);
} // namespace
void JoystickDataCache::Update(const MRC_ControlData& controlData,
const MRC_Joysticks& joystickData) {
matchTime = controlData.matchTime;
uint32_t allianceInt = mrclib::GetAlliance(controlData.controlFlags);
allianceInt += 1;
allianceStation = static_cast<HAL_AllianceStationID>(allianceInt);
auto gameDataSize = controlData.gameDataLength;
if (gameDataSize > MRCLIB_MAX_GAMEDATA_LENGTH) {
gameDataSize = MRCLIB_MAX_GAMEDATA_LENGTH;
}
std::memcpy(this->gameData.gameData, controlData.gameData, gameDataSize);
this->gameData.gameData[gameDataSize] = '\0';
if (mrclib::GetSupportsOpModes(controlData.controlFlags)) {
controlWord =
HAL_MakeControlWord(controlData.currentOpMode,
static_cast<HAL_RobotMode>(
mrclib::GetRobotMode(controlData.controlFlags)),
mrclib::GetEnabled(controlData.controlFlags),
mrclib::GetEStop(controlData.controlFlags),
mrclib::GetFmsConnected(controlData.controlFlags),
mrclib::GetDsConnected(controlData.controlFlags));
} else {
wpi::hal::EnableDashboardOpMode();
auto robotMode = static_cast<HAL_RobotMode>(
mrclib::GetRobotMode(controlData.controlFlags));
controlWord =
HAL_MakeControlWord(wpi::hal::GetDashboardSelectedOpMode(robotMode),
static_cast<HAL_RobotMode>(
mrclib::GetRobotMode(controlData.controlFlags)),
mrclib::GetEnabled(controlData.controlFlags),
mrclib::GetEStop(controlData.controlFlags),
mrclib::GetFmsConnected(controlData.controlFlags),
mrclib::GetDsConnected(controlData.controlFlags));
}
for (size_t count = 0; count < joystickData.count; count++) {
auto& newStick = joystickData.joysticks[count];
axes[count].available = newStick.availableAxes;
for (size_t i = 0; i < MRCLIB_MAX_AXES; i++) {
auto raw = newStick.axes[i];
axes[count].raw[i] = raw;
int16_t axisValue = raw;
if (axisValue < 0) {
axes[count].axes[i] = axisValue / 32768.0f;
} else {
axes[count].axes[i] = axisValue / 32767.0f;
}
}
povs[count].available = newStick.availablePovs;
for (size_t i = 0; i < MRCLIB_MAX_POVS; i++) {
povs[count].povs[i] = static_cast<HAL_JoystickPOV>(newStick.povs[i]);
}
buttons[count].available = newStick.availableButtons;
buttons[count].buttons = newStick.buttons;
touchpads[count].count = newStick.touchpadCount;
for (size_t i = 0; i < MRCLIB_MAX_TOUCHPADS; i++) {
touchpads[count].touchpads[i].count = newStick.touchpads[i].count;
for (size_t j = 0; j < MRCLIB_MAX_TOUCHPAD_FINGERS; j++) {
auto& finger = newStick.touchpads[i].fingers[j];
touchpads[count].touchpads[i].fingers[j].down = finger.down ? 1 : 0;
touchpads[count].touchpads[i].fingers[j].x = finger.x / 65535.0f;
touchpads[count].touchpads[i].fingers[j].y = finger.y / 65535.0f;
}
}
}
// Mark remaining sticks as unavailable
for (size_t i = joystickData.count; i < HAL_MAX_JOYSTICKS; i++) {
axes[i].available = 0;
povs[i].available = 0;
buttons[i].available = 0;
touchpads[i].count = 0;
}
}
void TcpCache::Update() {
MRC_MatchInfo newMatchInfo;
std::memset(&newMatchInfo, 0, sizeof(newMatchInfo));
MRC_DsComms_GetMatchInfo(&newMatchInfo);
matchInfo.matchNumber = newMatchInfo.matchNumber;
matchInfo.matchType = static_cast<HAL_MatchType>(newMatchInfo.matchType);
matchInfo.replayNumber = newMatchInfo.replayNumber;
auto eventNameSize = newMatchInfo.eventNameLength;
if (eventNameSize > MRCLIB_MAX_EVENTNAME_LENGTH) {
eventNameSize = MRCLIB_MAX_EVENTNAME_LENGTH;
}
std::memcpy(matchInfo.eventName, newMatchInfo.eventName, eventNameSize);
matchInfo.eventName[eventNameSize] = '\0';
MRC_JoystickDescriptors descriptorsMsg;
std::memset(&descriptorsMsg, 0, sizeof(descriptorsMsg));
MRC_DsComms_GetJoystickDescriptors(&descriptorsMsg);
size_t descriptorCount = descriptorsMsg.count;
for (size_t count = 0; count < descriptorCount; count++) {
const auto& newDesc = descriptorsMsg.descriptors[count];
auto& desc = descriptors[count];
desc.isGamepad = newDesc.isGamepad ? 1 : 0;
desc.supportedOutputs = newDesc.supportedOutputs;
desc.gamepadType = newDesc.gamepadType;
auto joystickNameLen = newDesc.nameLength;
std::memcpy(desc.name, newDesc.name, joystickNameLen);
desc.name[joystickNameLen] = '\0';
}
for (size_t i = descriptorCount; i < HAL_MAX_JOYSTICKS; i++) {
std::memset(&descriptors[i], 0, sizeof(descriptors[i]));
}
}
class MrcLibDsImpl : public MrcLibDs {
public:
MrcLibDsImpl();
int32_t sendError(bool isError, int32_t errorCode,
const struct WPI_String* details,
const struct WPI_String* location,
const struct WPI_String* callStack, bool printMsg) override;
int32_t sendConsoleLine(const struct WPI_String* line) override;
int32_t sendProgramCrash(const struct WPI_String* details,
const struct WPI_String* location,
const struct WPI_String* callStack) override;
int32_t getControlWord(HAL_ControlWord* controlWord) override;
int32_t getUncachedControlWord(HAL_ControlWord* controlWord) override;
int32_t setOpModeOptions(const struct HAL_OpModeOption* options,
int32_t count) override;
int32_t getAllianceStation(HAL_AllianceStationID* allianceStation) override;
int32_t getJoystickAxes(int32_t joystickNum, HAL_JoystickAxes* axes) override;
int32_t getJoystickPOVs(int32_t joystickNum, HAL_JoystickPOVs* povs) override;
int32_t getJoystickButtons(int32_t joystickNum,
HAL_JoystickButtons* buttons) override;
int32_t getJoystickTouchpads(int32_t joystickNum,
HAL_JoystickTouchpads* touchpads) override;
int32_t getAllJoystickData(int32_t joystickNum, HAL_JoystickAxes* axes,
HAL_JoystickPOVs* povs,
HAL_JoystickButtons* buttons,
HAL_JoystickTouchpads* touchpads) override;
int32_t getJoystickDescriptor(int32_t joystickNum,
HAL_JoystickDescriptor* desc) override;
int32_t getGameData(HAL_GameData* gameData) override;
int32_t setJoystickRumble(int32_t joystickNum, int32_t leftRumble,
int32_t rightRumble, int32_t leftTriggerRumble,
int32_t rightTriggerRumble) override;
int32_t setJoystickLeds(int32_t joystickNum, int32_t leds) override;
int32_t getMatchTime(double* matchTime) override;
int32_t getMatchInfo(HAL_MatchInfo* info) override;
int32_t getOutputsEnabled(bool* outputsEnabled) override;
int32_t refreshDSData(bool* wasRefreshed) override;
void provideNewDataEventHandle(WPI_EventHandle handle) override;
void removeNewDataEventHandle(WPI_EventHandle handle) override;
int32_t observeUserProgramStarting() override;
int32_t observeUserProgram(HAL_ControlWord controlWord) override;
int32_t getSystemTimeValid(bool* systemTimeValid) override;
wpi::util::EventVector newDataEvents;
private:
int32_t BackendPrintFunctionImpl(bool isError, int32_t errorCode,
const struct WPI_String* details,
const struct WPI_String* location,
const struct WPI_String* callStack,
bool* forcePrintMsg);
HAL_ControlWord newestControlWord{};
JoystickDataCache caches[2];
JoystickDataCache* currentRead = &caches[0];
JoystickDataCache* cacheToUpdate = &caches[1];
BackendPrintFunction backendPrintFunc;
wpi::util::mutex cacheMutex;
TcpCache tcpCaches[2];
TcpCache* tcpCurrentRead = &tcpCaches[0];
TcpCache* tcpCacheToUpdate = &tcpCaches[1];
wpi::util::mutex tcpCacheMutex;
wpi::util::mutex joystickOutputMutexes[MRCLIB_MAX_JOYSTICKS];
MRC_JoystickOutputs joystickOutputs[MRCLIB_MAX_JOYSTICKS];
};
static MrcLibDsImpl* staticImpl;
static void MRC_CALLCONV newDataCallback() {
if (staticImpl) {
staticImpl->newDataEvents.Wakeup();
}
}
namespace wpi::hal {
MrcLibDs* GetMrcLibDs() {
static MrcLibDsImpl impl;
return &impl;
}
} // namespace wpi::hal
static_assert(sizeof(int32_t) >= sizeof(int),
"WPILIB_NetworkComm status variable is larger than 32 bits");
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_GAMEDATA_LENGTH == sizeof(HAL_GameData::gameData) - 1);
static_assert(MRCLIB_MAX_EVENTNAME_LENGTH ==
sizeof(HAL_MatchInfo::eventName) - 1);
static_assert(MRCLIB_MAX_JOYSTICK_NAME_LENGTH ==
sizeof(HAL_JoystickDescriptor::name) - 1);
MrcLibDsImpl::MrcLibDsImpl() {
// Initialize joystick outputs to 0
for (size_t i = 0; i < MRCLIB_MAX_JOYSTICKS; i++) {
std::memset(&joystickOutputs[i], 0, sizeof(joystickOutputs[i]));
}
if (!MRC_CHECK_API_VERSION()) {
fmt::print(
stderr,
"Error: MRC API version mismatch. Restarting app and retrying...");
std::terminate();
}
MRC_DsComms_Initialize();
MRC_DsCommsControl_Initialize();
MRC_Console_Initialize();
// Wait for 10 seconds for the system server to be ready.
if (!MRC_DsComms_WaitForSystemServer(10000)) {
fmt::print(stderr,
"Error: Waiting for server ready failed. Restarting app and "
"retrying...");
std::terminate();
}
backendPrintFunc =
[this](bool isError, int32_t errorCode, const struct WPI_String* details,
const struct WPI_String* location,
const struct WPI_String* callStack, bool* forcePrintMsg) {
return BackendPrintFunctionImpl(isError, errorCode, details, location,
callStack, forcePrintMsg);
};
newestControlWord.value = 0;
staticImpl = this;
MRC_DsComms_SetNewDataCallback(newDataCallback);
MRC_DsCommsControl_SetHasUserCode(true);
}
static MRC_String WPIStringToMRCString(const struct WPI_String* wpiStr) {
MRC_String mrcStr;
if (wpiStr) {
mrcStr.str = wpiStr->str;
mrcStr.len = wpiStr->len;
} else {
mrcStr.str = nullptr;
mrcStr.len = 0;
}
return mrcStr;
}
int32_t MrcLibDsImpl::BackendPrintFunctionImpl(
bool isError, int32_t errorCode, const struct WPI_String* details,
const struct WPI_String* location, const struct WPI_String* callStack,
bool* forcePrintMsg) {
// Don't touch forcePrintMsg, it's a sim thing.
MRC_String mrcDetails = WPIStringToMRCString(details);
MRC_String mrcLocation = WPIStringToMRCString(location);
MRC_String mrcCallStack = WPIStringToMRCString(callStack);
return MRC_Console_WriteError(isError, errorCode, &mrcDetails, &mrcLocation,
&mrcCallStack);
}
int32_t MrcLibDsImpl::sendError(bool isError, int32_t errorCode,
const struct WPI_String* details,
const struct WPI_String* location,
const struct WPI_String* callStack,
bool printMsg) {
return DefaultSendErrorImpl(isError, errorCode, details, location, callStack,
printMsg, backendPrintFunc);
}
int32_t MrcLibDsImpl::sendConsoleLine(const struct WPI_String* line) {
MRC_String mrcLine = WPIStringToMRCString(line);
return MRC_Console_WriteLine(&mrcLine);
}
int32_t MrcLibDsImpl::sendProgramCrash(const struct WPI_String* details,
const struct WPI_String* location,
const struct WPI_String* callStack) {
MRC_String mrcDetails = WPIStringToMRCString(details);
MRC_String mrcLocation = WPIStringToMRCString(location);
MRC_String mrcCallStack = WPIStringToMRCString(callStack);
return MRC_Console_WriteProgramCrash(&mrcDetails, &mrcLocation,
&mrcCallStack);
}
int32_t MrcLibDsImpl::getControlWord(HAL_ControlWord* controlWord) {
std::scoped_lock lock{cacheMutex};
*controlWord = newestControlWord;
return 0;
}
int32_t MrcLibDsImpl::getUncachedControlWord(HAL_ControlWord* controlWord) {
MRC_ControlData data;
int32_t status = MRC_DsComms_GetControlData(&data);
bool dataValid = (status == 0);
if (dataValid && mrclib::GetDsConnected(data.controlFlags)) {
if (mrclib::GetSupportsOpModes(data.controlFlags)) {
*controlWord = HAL_MakeControlWord(
data.currentOpMode,
static_cast<HAL_RobotMode>(mrclib::GetRobotMode(data.controlFlags)),
mrclib::GetEnabled(data.controlFlags),
mrclib::GetEStop(data.controlFlags),
mrclib::GetFmsConnected(data.controlFlags),
mrclib::GetDsConnected(data.controlFlags));
} else {
wpi::hal::EnableDashboardOpMode();
auto robotMode =
static_cast<HAL_RobotMode>(mrclib::GetRobotMode(data.controlFlags));
*controlWord = HAL_MakeControlWord(
wpi::hal::GetDashboardSelectedOpMode(robotMode),
static_cast<HAL_RobotMode>(mrclib::GetRobotMode(data.controlFlags)),
mrclib::GetEnabled(data.controlFlags),
mrclib::GetEStop(data.controlFlags),
mrclib::GetFmsConnected(data.controlFlags),
mrclib::GetDsConnected(data.controlFlags));
}
} else {
// DS disconnected. Clear the control word
controlWord->value = 0;
}
return 0;
}
int32_t MrcLibDsImpl::setOpModeOptions(const struct HAL_OpModeOption* options,
int32_t count) {
if (count < 0 || count > 1000 || (count != 0 && !options)) {
return HAL_PARAMETER_OUT_OF_RANGE;
}
std::vector<MRC_OpMode> newOptions;
newOptions.reserve(count);
if (count != 0) {
for (auto&& option : std::span{options, options + count}) {
if (option.id == 0) {
continue;
}
auto& newValue = newOptions.emplace_back();
newValue.hash = option.id;
auto nameLen =
std::min(static_cast<size_t>(option.name.len),
static_cast<size_t>(MRCLIB_MAX_OPMODE_NAME_LENGTH));
std::memcpy(newValue.name, option.name.str, nameLen);
newValue.nameLength = static_cast<uint8_t>(nameLen);
auto groupLen =
std::min(static_cast<size_t>(option.group.len),
static_cast<size_t>(MRCLIB_MAX_OPMODE_GROUP_LENGTH));
std::memcpy(newValue.group, option.group.str, groupLen);
newValue.groupLength = static_cast<uint8_t>(groupLen);
auto descLen =
std::min(static_cast<size_t>(option.description.len),
static_cast<size_t>(MRCLIB_MAX_OPMODE_DESCRIPTION_LENGTH));
std::memcpy(newValue.description, option.description.str, descLen);
newValue.descriptionLength = static_cast<uint8_t>(descLen);
newValue.textColor = option.textColor;
newValue.backgroundColor = option.backgroundColor;
}
}
int32_t status =
MRC_DsCommsControl_SetOpModeOptions(newOptions.data(), newOptions.size());
if (count == 0) {
wpi::hal::SetDashboardOpModeOptions({});
} else {
wpi::hal::SetDashboardOpModeOptions({options, options + count});
}
return status;
}
int32_t MrcLibDsImpl::getAllianceStation(
HAL_AllianceStationID* allianceStation) {
MRC_ControlData data;
int32_t status = MRC_DsComms_GetControlData(&data);
if (status == 0) {
*allianceStation = static_cast<HAL_AllianceStationID>(
mrclib::GetAlliance(data.controlFlags) + 1);
} else {
*allianceStation = HAL_ALLIANCE_STATION_UNKNOWN;
}
return status;
}
#define CHECK_JOYSTICK_NUMBER(stickNum) \
if ((stickNum) < 0 || (stickNum) >= HAL_MAX_JOYSTICKS) \
return HAL_PARAMETER_OUT_OF_RANGE
int32_t MrcLibDsImpl::getJoystickAxes(int32_t joystickNum,
HAL_JoystickAxes* axes) {
CHECK_JOYSTICK_NUMBER(joystickNum);
std::scoped_lock lock{cacheMutex};
*axes = currentRead->axes[joystickNum];
return 0;
}
int32_t MrcLibDsImpl::getJoystickPOVs(int32_t joystickNum,
HAL_JoystickPOVs* povs) {
CHECK_JOYSTICK_NUMBER(joystickNum);
std::scoped_lock lock{cacheMutex};
*povs = currentRead->povs[joystickNum];
return 0;
}
int32_t MrcLibDsImpl::getJoystickButtons(int32_t joystickNum,
HAL_JoystickButtons* buttons) {
CHECK_JOYSTICK_NUMBER(joystickNum);
std::scoped_lock lock{cacheMutex};
*buttons = currentRead->buttons[joystickNum];
return 0;
}
int32_t MrcLibDsImpl::getJoystickTouchpads(int32_t joystickNum,
HAL_JoystickTouchpads* touchpads) {
CHECK_JOYSTICK_NUMBER(joystickNum);
std::scoped_lock lock{cacheMutex};
*touchpads = currentRead->touchpads[joystickNum];
return 0;
}
int32_t MrcLibDsImpl::getAllJoystickData(int32_t joystickNum,
HAL_JoystickAxes* axes,
HAL_JoystickPOVs* povs,
HAL_JoystickButtons* buttons,
HAL_JoystickTouchpads* touchpads) {
CHECK_JOYSTICK_NUMBER(joystickNum);
std::scoped_lock lock{cacheMutex};
*axes = currentRead->axes[joystickNum];
*povs = currentRead->povs[joystickNum];
*buttons = currentRead->buttons[joystickNum];
*touchpads = currentRead->touchpads[joystickNum];
return 0;
}
int32_t MrcLibDsImpl::getJoystickDescriptor(int32_t joystickNum,
HAL_JoystickDescriptor* desc) {
CHECK_JOYSTICK_NUMBER(joystickNum);
std::scoped_lock lock{tcpCacheMutex};
*desc = tcpCurrentRead->descriptors[joystickNum];
return 0;
}
int32_t MrcLibDsImpl::getGameData(HAL_GameData* gameData) {
std::scoped_lock lock{cacheMutex};
*gameData = currentRead->gameData;
return 0;
}
int32_t MrcLibDsImpl::setJoystickRumble(int32_t joystickNum, int32_t leftRumble,
int32_t rightRumble,
int32_t leftTriggerRumble,
int32_t rightTriggerRumble) {
CHECK_JOYSTICK_NUMBER(joystickNum);
std::scoped_lock lock{joystickOutputMutexes[joystickNum]};
joystickOutputs[joystickNum].leftRumble =
std::clamp<int32_t>(leftRumble, 0, UINT16_MAX);
joystickOutputs[joystickNum].rightRumble =
std::clamp<int32_t>(rightRumble, 0, UINT16_MAX);
joystickOutputs[joystickNum].leftTriggerRumble =
std::clamp<int32_t>(leftTriggerRumble, 0, UINT16_MAX);
joystickOutputs[joystickNum].rightTriggerRumble =
std::clamp<int32_t>(rightTriggerRumble, 0, UINT16_MAX);
return MRC_DsCommsControl_SetJoystickOutputs(joystickNum,
&joystickOutputs[joystickNum]);
}
int32_t MrcLibDsImpl::setJoystickLeds(int32_t joystickNum, int32_t leds) {
CHECK_JOYSTICK_NUMBER(joystickNum);
std::scoped_lock lock{joystickOutputMutexes[joystickNum]};
joystickOutputs[joystickNum].r = (leds >> 16) & 0xFF;
joystickOutputs[joystickNum].g = (leds >> 8) & 0xFF;
joystickOutputs[joystickNum].b = leds & 0xFF;
return MRC_DsCommsControl_SetJoystickOutputs(joystickNum,
&joystickOutputs[joystickNum]);
}
int32_t MrcLibDsImpl::getMatchTime(double* matchTime) {
std::scoped_lock lock{cacheMutex};
*matchTime = currentRead->matchTime;
return 0;
}
int32_t MrcLibDsImpl::getMatchInfo(HAL_MatchInfo* info) {
std::scoped_lock lock{tcpCacheMutex};
*info = tcpCurrentRead->matchInfo;
return 0;
}
int32_t MrcLibDsImpl::observeUserProgramStarting() {
return MRC_DsCommsControl_SetHasUserCodeReady(true);
}
int32_t MrcLibDsImpl::observeUserProgram(HAL_ControlWord controlWord) {
return MRC_DsCommsControl_SetOpModeTrace(controlWord.value &
(HAL_CONTROLWORD_OPMODE_HASH_MASK |
HAL_CONTROLWORD_ROBOT_MODE_MASK |
HAL_CONTROLWORD_ENABLED_MASK));
}
int32_t MrcLibDsImpl::refreshDSData(bool* wasRefreshed) {
MRC_Joysticks joystickData;
MRC_ControlData controlData;
int32_t status =
MRC_DsComms_GetControlDataWithJoysticks(&controlData, &joystickData);
bool updatedData = false;
if (status == 0 && mrclib::GetDsConnected(controlData.controlFlags)) {
// Update the cache
cacheToUpdate->Update(controlData, joystickData);
updatedData = true;
} else {
// DS disconnected. Clear the control word
cacheToUpdate->controlWord.value = 0;
}
{
std::scoped_lock lock{cacheMutex};
std::swap(cacheToUpdate, currentRead);
newestControlWord = currentRead->controlWord;
}
tcpCacheToUpdate->Update();
{
std::scoped_lock tcpLock(tcpCacheMutex);
std::swap(tcpCacheToUpdate, tcpCurrentRead);
}
*wasRefreshed = updatedData;
return status;
}
int32_t MrcLibDsImpl::getOutputsEnabled(bool* outputsEnabled) {
MRC_ControlData data;
int32_t status = MRC_DsComms_GetControlData(&data);
if (status == 0) {
*outputsEnabled =
mrclib::GetWatchdogActive(data.controlFlags) ? true : false;
} else {
*outputsEnabled = false;
}
return status;
}
int32_t MrcLibDsImpl::getSystemTimeValid(bool* systemTimeValid) {
MRC_Bool valid;
int32_t status = MRC_DsComms_GetSystemTimeValid(&valid);
if (status == 0) {
*systemTimeValid = valid ? true : false;
} else {
*systemTimeValid = false;
}
return status;
}
void MrcLibDsImpl::provideNewDataEventHandle(WPI_EventHandle handle) {
newDataEvents.Add(handle);
}
void MrcLibDsImpl::removeNewDataEventHandle(WPI_EventHandle handle) {
newDataEvents.Remove(handle);
}

26
hal/src/main/native/include/wpi/hal/DriverStation.h
View File

@@ -27,7 +27,6 @@ extern "C" {
*
* @param isError true for error, false for warning
* @param errorCode the error code
* @param isLVCode true for a LV error code, false for a standard error code
* @param details the details of the error
* @param location the file location of the error
* @param callStack the callstack of the error
@@ -35,24 +34,37 @@ extern "C" {
* DS
* @return the error code, or 0 for success
*/
int32_t HAL_SendError(HAL_Bool isError, int32_t errorCode, HAL_Bool isLVCode,
const char* details, const char* location,
const char* callStack, HAL_Bool printMsg);
int32_t HAL_SendError(HAL_Bool isError, int32_t errorCode,
const struct WPI_String* details,
const struct WPI_String* location,
const struct WPI_String* callStack, HAL_Bool printMsg);
/**
* Set the print function used by HAL_SendError
*
* @param func Function called by HAL_SendError when stderr is printed
*/
void HAL_SetPrintErrorImpl(void (*func)(const char* line, size_t size));
void HAL_SetPrintErrorImpl(void (*func)(const struct WPI_String* line));
/**
* Sends a line to the driver station console.
*
* @param line the line to send (null terminated)
* @param line the line to send
* @return the error code, or 0 for success
*/
int32_t HAL_SendConsoleLine(const char* line);
int32_t HAL_SendConsoleLine(const struct WPI_String* line);
/**
* Sends a program crash message to the driver station.
*
* @param details the details of the crash
* @param location the file location of the crash
* @param callStack the callstack of the crash
* @return the error code, or 0 for success
*/
int32_t HAL_SendProgramCrash(const struct WPI_String* details,
const struct WPI_String* location,
const struct WPI_String* callStack);
/**
* Gets the current control word of the driver station.

26
hal/src/main/native/include/wpi/hal/DriverStation.hpp
View File

@@ -6,8 +6,34 @@
#include "wpi/hal/DriverStation.h"
#include "wpi/hal/DriverStationTypes.hpp"
#include "wpi/util/string.hpp"
namespace wpi::hal {
inline int32_t SendError(bool isError, int32_t errorCode,
std::string_view details, std::string_view location,
std::string_view callStack, bool printMsg) {
WPI_String detailsWpiStr = wpi::util::make_string(details);
WPI_String locationWpiStr = wpi::util::make_string(location);
WPI_String callStackWpiStr = wpi::util::make_string(callStack);
return HAL_SendError(isError, errorCode, &detailsWpiStr, &locationWpiStr,
&callStackWpiStr, printMsg);
}
inline int32_t SendConsoleLine(std::string_view line) {
WPI_String lineWpiStr = wpi::util::make_string(line);
return HAL_SendConsoleLine(&lineWpiStr);
}
inline int32_t SendProgramCrash(std::string_view details,
std::string_view location,
std::string_view callStack) {
WPI_String detailsWpiStr = wpi::util::make_string(details);
WPI_String locationWpiStr = wpi::util::make_string(location);
WPI_String callStackWpiStr = wpi::util::make_string(callStack);
return HAL_SendProgramCrash(&detailsWpiStr, &locationWpiStr,
&callStackWpiStr);
}
inline ControlWord GetControlWord() {
HAL_ControlWord word;
HAL_GetControlWord(&word);

2
hal/src/main/native/include/wpi/hal/DriverStationTypes.h
View File

@@ -164,7 +164,7 @@ struct HAL_JoystickDescriptor {
typedef struct HAL_JoystickDescriptor HAL_JoystickDescriptor;
struct HAL_MatchInfo {
char eventName[64];
char eventName[65];
HAL_MatchType matchType;
uint16_t matchNumber;
uint8_t replayNumber;

105
hal/src/main/native/include/wpi/hal/cpp/MrcLibDs.hpp Normal file
View File

@@ -0,0 +1,105 @@
// 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 <stdint.h>
#include <functional>
#include "wpi/hal/DriverStationTypes.h"
#include "wpi/util/Synchronization.h"
#include "wpi/util/string.hpp"
namespace wpi::hal {
using BackendPrintFunction = std::function<int32_t(
bool isError, int32_t errorCode, const struct WPI_String* details,
const struct WPI_String* location, const struct WPI_String* callStack,
bool* forcePrintMsg)>;
int32_t DefaultSendErrorImpl(bool isError, int32_t errorCode,
const struct WPI_String* details,
const struct WPI_String* location,
const struct WPI_String* callStack, bool printMsg,
wpi::hal::BackendPrintFunction& backendWriteFunc);
class MrcLibDs {
public:
virtual ~MrcLibDs() = 0;
virtual int32_t sendError(bool isError, int32_t errorCode,
const struct WPI_String* details,
const struct WPI_String* location,
const struct WPI_String* callStack,
bool printMsg) = 0;
virtual int32_t sendConsoleLine(const struct WPI_String* line) = 0;
virtual int32_t sendProgramCrash(const struct WPI_String* details,
const struct WPI_String* location,
const struct WPI_String* callStack) = 0;
virtual int32_t getControlWord(HAL_ControlWord* controlWord) = 0;
virtual int32_t getUncachedControlWord(HAL_ControlWord* controlWord) = 0;
virtual int32_t setOpModeOptions(const struct HAL_OpModeOption* options,
int32_t count) = 0;
virtual int32_t getAllianceStation(
HAL_AllianceStationID* allianceStation) = 0;
virtual int32_t getJoystickAxes(int32_t joystickNum,
HAL_JoystickAxes* axes) = 0;
virtual int32_t getJoystickPOVs(int32_t joystickNum,
HAL_JoystickPOVs* povs) = 0;
virtual int32_t getJoystickButtons(int32_t joystickNum,
HAL_JoystickButtons* buttons) = 0;
virtual int32_t getJoystickTouchpads(int32_t joystickNum,
HAL_JoystickTouchpads* touchpads) = 0;
virtual int32_t getAllJoystickData(int32_t joystickNum,
HAL_JoystickAxes* axes,
HAL_JoystickPOVs* povs,
HAL_JoystickButtons* buttons,
HAL_JoystickTouchpads* touchpads) = 0;
virtual int32_t getJoystickDescriptor(int32_t joystickNum,
HAL_JoystickDescriptor* desc) = 0;
virtual int32_t getGameData(HAL_GameData* gameData) = 0;
virtual int32_t setJoystickRumble(int32_t joystickNum, int32_t leftRumble,
int32_t rightRumble,
int32_t leftTriggerRumble,
int32_t rightTriggerRumble) = 0;
virtual int32_t setJoystickLeds(int32_t joystickNum, int32_t leds) = 0;
virtual int32_t getMatchTime(double* matchTime) = 0;
virtual int32_t getMatchInfo(HAL_MatchInfo* info) = 0;
virtual int32_t getOutputsEnabled(bool* outputsEnabled) = 0;
virtual int32_t refreshDSData(bool* wasRefreshed) = 0;
virtual void provideNewDataEventHandle(WPI_EventHandle handle) = 0;
virtual void removeNewDataEventHandle(WPI_EventHandle handle) = 0;
virtual int32_t observeUserProgramStarting() = 0;
virtual int32_t observeUserProgram(HAL_ControlWord controlWord) = 0;
virtual int32_t getSystemTimeValid(bool* systemTimeValid) = 0;
};
MrcLibDs* GetMrcLibDs();
MrcLibDs* GetDefaultDriverStationImpl();
void ForceDsInstance(MrcLibDs* ds);
} // namespace wpi::hal

10
hal/src/main/native/include/wpi/hal/simulation/MockHooks.h
View File

@@ -26,12 +26,14 @@ HAL_Bool HALSIM_IsTimingPaused(void);
void HALSIM_StepTiming(uint64_t delta);
void HALSIM_StepTimingAsync(uint64_t delta);
typedef int32_t (*HALSIM_SendErrorHandler)(
HAL_Bool isError, int32_t errorCode, HAL_Bool isLVCode, const char* details,
const char* location, const char* callStack, HAL_Bool printMsg);
typedef int32_t (*HALSIM_SendErrorHandler)(HAL_Bool isError, int32_t errorCode,
const struct WPI_String* details,
const struct WPI_String* location,
const struct WPI_String* callStack,
HAL_Bool printMsg);
void HALSIM_SetSendError(HALSIM_SendErrorHandler handler);
typedef int32_t (*HALSIM_SendConsoleLineHandler)(const char* line);
typedef int32_t (*HALSIM_SendConsoleLineHandler)(const struct WPI_String* line);
void HALSIM_SetSendConsoleLine(HALSIM_SendConsoleLineHandler handler);
typedef void (*HALSIM_SimPeriodicCallback)(void* param);

428
hal/src/main/native/sim/DriverStation.cpp
View File

@@ -20,16 +20,12 @@
#include "mockdata/DriverStationDataInternal.hpp"
#include "wpi/hal/DriverStationTypes.h"
#include "wpi/hal/Errors.h"
#include "wpi/hal/cpp/MrcLibDs.hpp"
#include "wpi/hal/monotonic_clock.hpp"
#include "wpi/hal/simulation/MockHooks.h"
#include "wpi/util/EventVector.hpp"
#include "wpi/util/mutex.hpp"
#include "wpi/util/string.h"
static wpi::util::mutex msgMutex;
static std::atomic<HALSIM_SendErrorHandler> sendErrorHandler{nullptr};
static std::atomic<HALSIM_SendConsoleLineHandler> sendConsoleLineHandler{
nullptr};
#include "wpi/util/string.hpp"
using namespace wpi::hal;
@@ -53,13 +49,6 @@ static_assert(std::is_standard_layout_v<JoystickDataCache>);
// static_assert(std::is_trivial_v<JoystickDataCache>);
static std::atomic_bool gShutdown{false};
struct FIRSTDriverStation {
~FIRSTDriverStation() { gShutdown = true; }
wpi::util::EventVector newDataEvents;
wpi::util::mutex cacheMutex;
wpi::util::mutex tcpCacheMutex;
};
} // namespace
void JoystickDataCache::Update() {
@@ -83,14 +72,6 @@ void JoystickDataCache::Update() {
if ((stickNum) < 0 || (stickNum) >= HAL_MAX_JOYSTICKS) \
return HAL_PARAMETER_OUT_OF_RANGE;
static HAL_ControlWord newestControlWord;
static JoystickDataCache caches[3];
static JoystickDataCache* currentRead = &caches[0];
static JoystickDataCache* currentReadLocal = &caches[0];
static std::atomic<JoystickDataCache*> currentCache{nullptr};
static JoystickDataCache* lastGiven = &caches[1];
static JoystickDataCache* cacheToUpdate = &caches[2];
namespace {
struct TcpCache {
TcpCache() { std::memset(this, 0, sizeof(*this)); }
@@ -103,9 +84,6 @@ struct TcpCache {
static_assert(std::is_standard_layout_v<TcpCache>);
} // namespace
static TcpCache tcpCache;
static TcpCache tcpCurrent;
void TcpCache::Update() {
SimDriverStationData->GetMatchInfo(&matchInfo);
@@ -114,23 +92,115 @@ void TcpCache::Update() {
}
}
static ::FIRSTDriverStation* driverStation;
class MrcLibDsSimImpl : public MrcLibDs {
public:
MrcLibDsSimImpl();
~MrcLibDsSimImpl() override { gShutdown = true; }
int32_t sendError(bool isError, int32_t errorCode,
const struct WPI_String* details,
const struct WPI_String* location,
const struct WPI_String* callStack, bool printMsg) override;
namespace wpi::hal::init {
void InitializeDriverStation() {
static FIRSTDriverStation ds;
driverStation = &ds;
int32_t sendConsoleLine(const struct WPI_String* line) override;
int32_t sendProgramCrash(const struct WPI_String* details,
const struct WPI_String* location,
const struct WPI_String* callStack) override;
int32_t getControlWord(HAL_ControlWord* controlWord) override;
int32_t getUncachedControlWord(HAL_ControlWord* controlWord) override;
int32_t setOpModeOptions(const struct HAL_OpModeOption* options,
int32_t count) override;
int32_t getAllianceStation(HAL_AllianceStationID* allianceStation) override;
int32_t getJoystickAxes(int32_t joystickNum, HAL_JoystickAxes* axes) override;
int32_t getJoystickPOVs(int32_t joystickNum, HAL_JoystickPOVs* povs) override;
int32_t getJoystickButtons(int32_t joystickNum,
HAL_JoystickButtons* buttons) override;
int32_t getJoystickTouchpads(int32_t joystickNum,
HAL_JoystickTouchpads* touchpads) override;
int32_t getAllJoystickData(int32_t joystickNum, HAL_JoystickAxes* axes,
HAL_JoystickPOVs* povs,
HAL_JoystickButtons* buttons,
HAL_JoystickTouchpads* touchpads) override;
int32_t getJoystickDescriptor(int32_t joystickNum,
HAL_JoystickDescriptor* desc) override;
int32_t getGameData(HAL_GameData* gameData) override;
int32_t setJoystickRumble(int32_t joystickNum, int32_t leftRumble,
int32_t rightRumble, int32_t leftTriggerRumble,
int32_t rightTriggerRumble) override;
int32_t setJoystickLeds(int32_t joystickNum, int32_t leds) override;
int32_t getMatchTime(double* matchTime) override;
int32_t getMatchInfo(HAL_MatchInfo* info) override;
int32_t getOutputsEnabled(bool* outputsEnabled) override;
int32_t refreshDSData(bool* wasRefreshed) override;
void provideNewDataEventHandle(WPI_EventHandle handle) override;
void removeNewDataEventHandle(WPI_EventHandle handle) override;
int32_t observeUserProgramStarting() override;
int32_t observeUserProgram(HAL_ControlWord controlWord) override;
int32_t getSystemTimeValid(bool* systemTimeValid) override;
wpi::util::EventVector newDataEvents;
void NewDriverStationData();
private:
int32_t BackendPrintFunctionImpl(bool isError, int32_t errorCode,
const struct WPI_String* details,
const struct WPI_String* location,
const struct WPI_String* callStack,
bool* forcePrintMsg);
wpi::util::mutex cacheMutex;
wpi::util::mutex tcpCacheMutex;
BackendPrintFunction backendPrintFunc;
HAL_ControlWord newestControlWord;
JoystickDataCache caches[3];
JoystickDataCache* currentRead = &caches[0];
JoystickDataCache* currentReadLocal = &caches[0];
std::atomic<JoystickDataCache*> currentCache{nullptr};
JoystickDataCache* lastGiven = &caches[1];
JoystickDataCache* cacheToUpdate = &caches[2];
TcpCache tcpCache;
TcpCache tcpCurrent;
};
MrcLibDsSimImpl::MrcLibDsSimImpl() {
newestControlWord.value = 0;
backendPrintFunc =
[this](bool isError, int32_t errorCode, const struct WPI_String* details,
const struct WPI_String* location,
const struct WPI_String* callStack, bool* forcePrintMsg) {
return BackendPrintFunctionImpl(isError, errorCode, details, location,
callStack, forcePrintMsg);
};
}
} // namespace wpi::hal::init
namespace wpi::hal {
static void DefaultPrintErrorImpl(const char* line, size_t size) {
std::fwrite(line, size, 1, stderr);
}
} // namespace wpi::hal
static std::atomic<void (*)(const char* line, size_t size)> gPrintErrorImpl{
wpi::hal::DefaultPrintErrorImpl};
static std::atomic<HALSIM_SendErrorHandler> sendErrorHandler{nullptr};
static std::atomic<HALSIM_SendConsoleLineHandler> sendConsoleLineHandler{
nullptr};
extern "C" {
@@ -141,94 +211,63 @@ void HALSIM_SetSendError(HALSIM_SendErrorHandler handler) {
void HALSIM_SetSendConsoleLine(HALSIM_SendConsoleLineHandler handler) {
sendConsoleLineHandler.store(handler);
}
} // extern "C"
int32_t HAL_SendError(HAL_Bool isError, int32_t errorCode, HAL_Bool isLVCode,
const char* details, const char* location,
const char* callStack, HAL_Bool printMsg) {
int32_t MrcLibDsSimImpl::BackendPrintFunctionImpl(
bool isError, int32_t errorCode, const struct WPI_String* details,
const struct WPI_String* location, const struct WPI_String* callStack,
bool* forcePrintMsg) {
// This will defer to the caller, which needs to force print to true.
*forcePrintMsg = true;
return 0;
}
int32_t MrcLibDsSimImpl::sendError(bool isError, int32_t errorCode,
const struct WPI_String* details,
const struct WPI_String* location,
const struct WPI_String* callStack,
bool printMsg) {
auto errorHandler = sendErrorHandler.load();
if (errorHandler) {
return errorHandler(isError, errorCode, isLVCode, details, location,
callStack, printMsg);
return errorHandler(isError, errorCode, details, location, callStack,
printMsg);
}
// Avoid flooding console by keeping track of previous 5 error
// messages and only printing again if they're longer than 1 second old.
static constexpr int KEEP_MSGS = 5;
std::scoped_lock lock(msgMutex);
static std::string prevMsg[KEEP_MSGS];
static monotonic_clock::time_point prevMsgTime[KEEP_MSGS];
static bool initialized = false;
if (!initialized) {
for (int i = 0; i < KEEP_MSGS; i++) {
prevMsgTime[i] = monotonic_clock::now() - std::chrono::seconds(2);
}
initialized = true;
}
auto curTime = monotonic_clock::now();
int i;
for (i = 0; i < KEEP_MSGS; ++i) {
if (prevMsg[i] == details) {
break;
}
}
int retval = 0;
if (i == KEEP_MSGS || (curTime - prevMsgTime[i]) >= std::chrono::seconds(1)) {
printMsg = true;
if (printMsg) {
fmt::memory_buffer buf;
if (location && location[0] != '\0') {
fmt::format_to(fmt::appender{buf},
"{} at {}: ", isError ? "Error" : "Warning", location);
}
fmt::format_to(fmt::appender{buf}, "{}\n", details);
if (callStack && callStack[0] != '\0') {
fmt::format_to(fmt::appender{buf}, "{}\n", callStack);
}
auto printError = gPrintErrorImpl.load();
printError(buf.data(), buf.size());
}
if (i == KEEP_MSGS) {
// replace the oldest one
i = 0;
auto first = prevMsgTime[0];
for (int j = 1; j < KEEP_MSGS; ++j) {
if (prevMsgTime[j] < first) {
first = prevMsgTime[j];
i = j;
}
}
prevMsg[i] = details;
}
prevMsgTime[i] = curTime;
}
return retval;
return DefaultSendErrorImpl(isError, errorCode, details, location, callStack,
printMsg, backendPrintFunc);
}
void HAL_SetPrintErrorImpl(void (*func)(const char* line, size_t size)) {
gPrintErrorImpl.store(func ? func : wpi::hal::DefaultPrintErrorImpl);
}
int32_t HAL_SendConsoleLine(const char* line) {
int32_t MrcLibDsSimImpl::sendConsoleLine(const struct WPI_String* line) {
auto handler = sendConsoleLineHandler.load();
if (handler) {
return handler(line);
}
std::puts(line);
fmt::print("{}\n", wpi::util::to_string_view(line));
std::fflush(stdout);
return 0;
}
int32_t HAL_GetControlWord(HAL_ControlWord* controlWord) {
int32_t MrcLibDsSimImpl::sendProgramCrash(const struct WPI_String* details,
const struct WPI_String* location,
const struct WPI_String* callStack) {
fmt::print(stderr, "Program Crash: {}\nLocation: {}\nCall Stack:\n{}\n",
wpi::util::to_string_view(details),
wpi::util::to_string_view(location),
wpi::util::to_string_view(callStack));
std::fflush(stderr);
return 0;
}
int32_t MrcLibDsSimImpl::getControlWord(HAL_ControlWord* controlWord) {
if (gShutdown) {
controlWord->value = 0;
return HAL_INCOMPATIBLE_STATE;
}
std::scoped_lock lock{driverStation->cacheMutex};
std::scoped_lock lock{cacheMutex};
*controlWord = newestControlWord;
return 0;
}
int32_t HAL_GetUncachedControlWord(HAL_ControlWord* controlWord) {
int32_t MrcLibDsSimImpl::getUncachedControlWord(HAL_ControlWord* controlWord) {
if (gShutdown) {
controlWord->value = 0;
return HAL_INCOMPATIBLE_STATE;
@@ -245,139 +284,114 @@ int32_t HAL_GetUncachedControlWord(HAL_ControlWord* controlWord) {
return 0;
}
int32_t HAL_SetOpModeOptions(const struct HAL_OpModeOption* options,
int32_t count) {
int32_t MrcLibDsSimImpl::setOpModeOptions(
const struct HAL_OpModeOption* options, int32_t count) {
if (gShutdown) {
return 0;
}
if (count < 0 || count > 1000 || (count != 0 && !options)) {
return HAL_PARAMETER_OUT_OF_RANGE;
}
if (count == 0) {
SimDriverStationData->SetOpModeOptions({});
} else {
SimDriverStationData->SetOpModeOptions({options, options + count});
}
return 0;
}
HAL_AllianceStationID HAL_GetAllianceStation(int32_t* status) {
int32_t MrcLibDsSimImpl::getAllianceStation(
HAL_AllianceStationID* allianceStation) {
if (gShutdown) {
return HAL_ALLIANCE_STATION_UNKNOWN;
*allianceStation = HAL_ALLIANCE_STATION_UNKNOWN;
return HAL_INCOMPATIBLE_STATE;
}
std::scoped_lock lock{driverStation->cacheMutex};
return currentRead->allianceStation;
std::scoped_lock lock{cacheMutex};
*allianceStation = currentRead->allianceStation;
return 0;
}
int32_t HAL_GetJoystickAxes(int32_t joystickNum, HAL_JoystickAxes* axes) {
int32_t MrcLibDsSimImpl::getJoystickAxes(int32_t joystickNum,
HAL_JoystickAxes* axes) {
if (gShutdown) {
return HAL_INCOMPATIBLE_STATE;
}
CHECK_JOYSTICK_NUMBER(joystickNum);
std::scoped_lock lock{driverStation->cacheMutex};
std::scoped_lock lock{cacheMutex};
*axes = currentRead->axes[joystickNum];
return 0;
}
int32_t HAL_GetJoystickPOVs(int32_t joystickNum, HAL_JoystickPOVs* povs) {
int32_t MrcLibDsSimImpl::getJoystickPOVs(int32_t joystickNum,
HAL_JoystickPOVs* povs) {
if (gShutdown) {
return HAL_INCOMPATIBLE_STATE;
}
CHECK_JOYSTICK_NUMBER(joystickNum);
std::scoped_lock lock{driverStation->cacheMutex};
std::scoped_lock lock{cacheMutex};
*povs = currentRead->povs[joystickNum];
return 0;
}
int32_t HAL_GetJoystickButtons(int32_t joystickNum,
int32_t MrcLibDsSimImpl::getJoystickButtons(int32_t joystickNum,
HAL_JoystickButtons* buttons) {
if (gShutdown) {
return HAL_INCOMPATIBLE_STATE;
}
CHECK_JOYSTICK_NUMBER(joystickNum);
std::scoped_lock lock{driverStation->cacheMutex};
std::scoped_lock lock{cacheMutex};
*buttons = currentRead->buttons[joystickNum];
return 0;
}
int32_t HAL_GetJoystickTouchpads(int32_t joystickNum,
HAL_JoystickTouchpads* touchpads) {
int32_t MrcLibDsSimImpl::getJoystickTouchpads(
int32_t joystickNum, HAL_JoystickTouchpads* touchpads) {
if (gShutdown) {
return HAL_INCOMPATIBLE_STATE;
}
CHECK_JOYSTICK_NUMBER(joystickNum);
std::scoped_lock lock{driverStation->cacheMutex};
std::scoped_lock lock{cacheMutex};
*touchpads = currentRead->touchpads[joystickNum];
return 0;
}
void HAL_GetAllJoystickData(int32_t joystickNum, HAL_JoystickAxes* axes,
int32_t MrcLibDsSimImpl::getAllJoystickData(int32_t joystickNum,
HAL_JoystickAxes* axes,
HAL_JoystickPOVs* povs,
HAL_JoystickButtons* buttons,
HAL_JoystickTouchpads* touchpads) {
if (gShutdown) {
return;
return HAL_INCOMPATIBLE_STATE;
}
std::scoped_lock lock{driverStation->cacheMutex};
std::scoped_lock lock{cacheMutex};
*axes = currentRead->axes[joystickNum];
*povs = currentRead->povs[joystickNum];
*buttons = currentRead->buttons[joystickNum];
*touchpads = currentRead->touchpads[joystickNum];
return 0;
}
int32_t HAL_GetJoystickDescriptor(int32_t joystickNum,
int32_t MrcLibDsSimImpl::getJoystickDescriptor(int32_t joystickNum,
HAL_JoystickDescriptor* desc) {
CHECK_JOYSTICK_NUMBER(joystickNum);
std::scoped_lock lock{driverStation->tcpCacheMutex};
std::scoped_lock lock{tcpCacheMutex};
*desc = tcpCurrent.descriptors[joystickNum];
return 0;
}
HAL_Bool HAL_GetJoystickIsGamepad(int32_t joystickNum) {
HAL_JoystickDescriptor joystickDesc;
if (HAL_GetJoystickDescriptor(joystickNum, &joystickDesc) < 0) {
return 0;
} else {
return joystickDesc.isGamepad;
}
}
int32_t HAL_GetJoystickGamepadType(int32_t joystickNum) {
HAL_JoystickDescriptor joystickDesc;
if (HAL_GetJoystickDescriptor(joystickNum, &joystickDesc) < 0) {
return -1;
} else {
return joystickDesc.gamepadType;
}
}
int32_t HAL_GetGameData(HAL_GameData* gameData) {
int32_t MrcLibDsSimImpl::getGameData(HAL_GameData* gameData) {
if (gShutdown) {
return HAL_INCOMPATIBLE_STATE;
}
std::scoped_lock lock{driverStation->cacheMutex};
std::scoped_lock lock{cacheMutex};
*gameData = currentRead->gameData;
return 0;
}
int32_t HAL_GetJoystickSupportedOutputs(int32_t joystickNum) {
HAL_JoystickDescriptor joystickDesc;
if (HAL_GetJoystickDescriptor(joystickNum, &joystickDesc) < 0) {
return -1;
} else {
return joystickDesc.supportedOutputs;
}
}
void HAL_GetJoystickName(struct WPI_String* name, int32_t joystickNum) {
HAL_JoystickDescriptor joystickDesc;
const char* cName = joystickDesc.name;
if (HAL_GetJoystickDescriptor(joystickNum, &joystickDesc) < 0) {
cName = "";
}
auto len = std::strlen(cName);
auto write = WPI_AllocateString(name, len);
std::memcpy(write, cName, len);
}
int32_t HAL_SetJoystickRumble(int32_t joystickNum, int32_t leftRumble,
int32_t rightRumble, int32_t leftTriggerRumble,
int32_t MrcLibDsSimImpl::setJoystickRumble(int32_t joystickNum,
int32_t leftRumble,
int32_t rightRumble,
int32_t leftTriggerRumble,
int32_t rightTriggerRumble) {
SimDriverStationData->SetJoystickRumbles(joystickNum, leftRumble, rightRumble,
leftTriggerRumble,
@@ -385,41 +399,46 @@ int32_t HAL_SetJoystickRumble(int32_t joystickNum, int32_t leftRumble,
return 0;
}
int32_t HAL_SetJoystickLeds(int32_t joystickNum, int32_t leds) {
int32_t MrcLibDsSimImpl::setJoystickLeds(int32_t joystickNum, int32_t leds) {
SimDriverStationData->SetJoystickLeds(joystickNum, leds);
return 0;
}
double HAL_GetMatchTime(int32_t* status) {
int32_t MrcLibDsSimImpl::getMatchTime(double* matchTime) {
if (gShutdown) {
return 0;
*matchTime = 0;
return HAL_INCOMPATIBLE_STATE;
}
std::scoped_lock lock{driverStation->cacheMutex};
return currentRead->matchTime;
std::scoped_lock lock{cacheMutex};
*matchTime = currentRead->matchTime;
return 0;
}
int32_t HAL_GetMatchInfo(HAL_MatchInfo* info) {
std::scoped_lock lock{driverStation->tcpCacheMutex};
int32_t MrcLibDsSimImpl::getMatchInfo(HAL_MatchInfo* info) {
std::scoped_lock lock{tcpCacheMutex};
*info = tcpCurrent.matchInfo;
return 0;
}
void HAL_ObserveUserProgramStarting(void) {
int32_t MrcLibDsSimImpl::observeUserProgramStarting(void) {
HALSIM_SetProgramStarted(true);
return 0;
}
void HAL_ObserveUserProgram(HAL_ControlWord word) {
int32_t MrcLibDsSimImpl::observeUserProgram(HAL_ControlWord word) {
HALSIM_SetProgramState(word);
return 0;
}
HAL_Bool HAL_RefreshDSData(void) {
int32_t MrcLibDsSimImpl::refreshDSData(bool* wasRefreshed) {
if (gShutdown) {
return false;
*wasRefreshed = false;
return HAL_INCOMPATIBLE_STATE;
}
bool dsAttached = SimDriverStationData->dsAttached;
JoystickDataCache* prev;
{
std::scoped_lock lock{driverStation->cacheMutex};
std::scoped_lock lock{cacheMutex};
prev = currentCache.exchange(nullptr);
if (prev != nullptr) {
currentRead = prev;
@@ -448,44 +467,46 @@ HAL_Bool HAL_RefreshDSData(void) {
{
tcpCache.Update();
std::scoped_lock tcpLock(driverStation->tcpCacheMutex);
std::scoped_lock tcpLock(tcpCacheMutex);
tcpCache.CloneTo(&tcpCurrent);
}
return prev != nullptr;
*wasRefreshed = prev != nullptr;
return 0;
}
void HAL_ProvideNewDataEventHandle(WPI_EventHandle handle) {
void MrcLibDsSimImpl::provideNewDataEventHandle(WPI_EventHandle handle) {
if (gShutdown) {
return;
}
wpi::hal::init::CheckInit();
driverStation->newDataEvents.Add(handle);
newDataEvents.Add(handle);
}
void HAL_RemoveNewDataEventHandle(WPI_EventHandle handle) {
void MrcLibDsSimImpl::removeNewDataEventHandle(WPI_EventHandle handle) {
if (gShutdown) {
return;
}
driverStation->newDataEvents.Remove(handle);
newDataEvents.Remove(handle);
}
HAL_Bool HAL_GetOutputsEnabled(void) {
int32_t MrcLibDsSimImpl::getOutputsEnabled(bool* outputsEnabled) {
if (gShutdown) {
return false;
*outputsEnabled = false;
return HAL_INCOMPATIBLE_STATE;
}
std::scoped_lock lock{driverStation->cacheMutex};
return HAL_ControlWord_IsEnabled(newestControlWord) &&
std::scoped_lock lock{cacheMutex};
*outputsEnabled = HAL_ControlWord_IsEnabled(newestControlWord) &&
HAL_ControlWord_IsDSAttached(newestControlWord);
return 0;
}
} // extern "C"
int32_t MrcLibDsSimImpl::getSystemTimeValid(bool* systemTimeValid) {
*systemTimeValid = true;
return 0;
}
namespace wpi::hal {
void NewDriverStationData() {
if (gShutdown) {
return;
}
void MrcLibDsSimImpl::NewDriverStationData() {
SimDriverStationData->dsAttached = true;
cacheToUpdate->Update();
@@ -500,11 +521,24 @@ void NewDriverStationData() {
}
lastGiven = given;
driverStation->newDataEvents.Wakeup();
newDataEvents.Wakeup();
SimDriverStationData->CallNewDataCallbacks();
}
void InitializeDriverStation() {
SimDriverStationData->ResetData();
namespace wpi::hal {
void NewDriverStationData() {
if (gShutdown) {
return;
}
MrcLibDsSimImpl* defaultImpl =
static_cast<MrcLibDsSimImpl*>(GetDefaultDriverStationImpl());
defaultImpl->NewDriverStationData();
}
MrcLibDs* GetDefaultDriverStationImpl() {
static MrcLibDsSimImpl impl;
return &impl;
}
} // namespace wpi::hal

5
hal/src/main/native/sim/HAL.cpp
View File

@@ -197,10 +197,6 @@ HAL_Bool HAL_GetRSLState(int32_t* status) {
return false;
}
HAL_Bool HAL_GetSystemTimeValid(int32_t* status) {
return true;
}
HAL_Bool HAL_Initialize(int32_t timeout, int32_t mode) {
static std::atomic_bool initialized{false};
static wpi::util::mutex initializeMutex;
@@ -220,6 +216,7 @@ HAL_Bool HAL_Initialize(int32_t timeout, int32_t mode) {
wpi::hal::init::HAL_IsInitialized.store(true);
wpi::hal::RestartTiming();
wpi::hal::InitializeDriverStation();
initialized = true;

1
hal/src/main/native/sim/HALInitializer.hpp
View File

@@ -42,7 +42,6 @@ extern void InitializeCAN();
extern void InitializeCounter();
extern void InitializeDigitalInternal();
extern void InitializeDIO();
extern void InitializeDriverStation();
extern void InitializeEncoder();
extern void InitializeExtensions();
extern void InitializeHAL();

12
hal/src/main/native/systemcore/DriverStation.cpp Normal file
View File

@@ -0,0 +1,12 @@
// 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 "HALInitializer.hpp"
#include "wpi/hal/cpp/MrcLibDs.hpp"
namespace wpi::hal {
MrcLibDs* GetDefaultDriverStationImpl() {
return wpi::hal::GetMrcLibDs();
}
} // namespace wpi::hal

766
hal/src/main/native/systemcore/FIRSTDriverStation.cpp
View File

@@ -1,766 +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 <atomic>
#include <chrono>
#include <cstdio>
#include <cstdlib>
#include <cstring>
#include <limits>
#include <string>
#include <string_view>
#include <utility>
#include <vector>
#include <fmt/format.h>
#include "HALInitializer.hpp"
#include "SystemServerInternal.hpp"
#include "mrc/NtNetComm.h"
#include "wpi/hal/DashboardOpMode.hpp"
#include "wpi/hal/DriverStation.h"
#include "wpi/hal/DriverStationTypes.h"
#include "wpi/hal/Errors.h"
#include "wpi/hal/proto/ControlData.h"
#include "wpi/hal/proto/ErrorInfo.h"
#include "wpi/hal/proto/JoystickDescriptors.h"
#include "wpi/hal/proto/JoystickOutput.h"
#include "wpi/hal/proto/MatchInfo.h"
#include "wpi/hal/proto/OpMode.h"
#include "wpi/nt/BooleanTopic.hpp"
#include "wpi/nt/IntegerTopic.hpp"
#include "wpi/nt/NetworkTableInstance.hpp"
#include "wpi/nt/ProtobufTopic.hpp"
#include "wpi/nt/StringArrayTopic.hpp"
#include "wpi/nt/StringTopic.hpp"
#include "wpi/util/EventVector.hpp"
#include "wpi/util/SafeThread.hpp"
#include "wpi/util/SmallVector.hpp"
#include "wpi/util/condition_variable.hpp"
#include "wpi/util/mutex.hpp"
#include "wpi/util/string.hpp"
#include "wpi/util/timestamp.hpp"
static_assert(sizeof(int32_t) >= sizeof(int),
"WPILIB_NetworkComm status variable is larger than 32 bits");
static_assert(MRC_MAX_NUM_AXES == HAL_MAX_JOYSTICK_AXES);
static_assert(MRC_MAX_NUM_POVS == HAL_MAX_JOYSTICK_POVS);
static_assert(MRC_MAX_NUM_JOYSTICKS == HAL_MAX_JOYSTICKS);
static_assert(MRC_MAX_NUM_TOUCHPADS == HAL_MAX_JOYSTICK_TOUCHPADS);
static_assert(MRC_MAX_NUM_TOUCHPAD_FINGERS ==
HAL_MAX_JOYSTICK_TOUCHPAD_FINGERS);
namespace {
struct JoystickDataCache {
JoystickDataCache() { std::memset(this, 0, sizeof(*this)); }
void Update(const mrc::ControlData& data);
HAL_JoystickAxes axes[HAL_MAX_JOYSTICKS];
HAL_JoystickPOVs povs[HAL_MAX_JOYSTICKS];
HAL_JoystickButtons buttons[HAL_MAX_JOYSTICKS];
HAL_JoystickTouchpads touchpads[HAL_MAX_JOYSTICKS];
HAL_AllianceStationID allianceStation;
float matchTime;
HAL_ControlWord controlWord;
HAL_GameData gameData;
};
static_assert(std::is_standard_layout_v<JoystickDataCache>);
// static_assert(std::is_trivial_v<JoystickDataCache>);
struct SystemServerDriverStation {
wpi::nt::NetworkTableInstance ntInst;
wpi::nt::BooleanPublisher hasUserCodePublisher;
wpi::nt::BooleanPublisher hasUserCodeReadyPublisher;
wpi::nt::BooleanSubscriber hasSetWallClockSubscriber;
wpi::nt::BooleanSubscriber serverReadySubscriber;
wpi::nt::ProtobufSubscriber<mrc::ControlData> controlDataSubscriber;
wpi::nt::ProtobufSubscriber<mrc::MatchInfo> matchInfoSubscriber;
wpi::nt::ProtobufSubscriber<mrc::JoystickDescriptors>
joystickDescriptorsTopic;
wpi::nt::StringPublisher versionPublisher;
wpi::nt::StringPublisher consoleLinePublisher;
wpi::nt::ProtobufPublisher<mrc::ErrorInfo> errorInfoPublisher;
std::array<wpi::nt::ProtobufPublisher<mrc::JoystickOutput>,
MRC_MAX_NUM_JOYSTICKS>
joystickOutputTopics;
wpi::nt::ProtobufPublisher<std::vector<mrc::OpMode>> opModeOptionsPublisher;
wpi::nt::IntegerPublisher traceOpModePublisher;
NT_Listener controlDataListener;
wpi::util::mutex controlDataMutex;
wpi::util::ProtobufMessage<mrc::ControlData> controlDataMsg;
wpi::nt::Value lastValue;
wpi::util::mutex joystickOutputMutexes[MRC_MAX_NUM_JOYSTICKS];
mrc::JoystickOutput joystickOutputs[MRC_MAX_NUM_JOYSTICKS];
explicit SystemServerDriverStation(wpi::nt::NetworkTableInstance inst) {
ntInst = inst;
wpi::nt::PubSubOptions options;
options.sendAll = true;
options.keepDuplicates = true;
options.periodic = 0.005;
hasUserCodeReadyPublisher =
ntInst.GetBooleanTopic(ROBOT_HAS_USER_CODE_READY_PATH).Publish(options);
for (size_t count = 0; count < joystickOutputTopics.size(); count++) {
std::string name = ROBOT_JOYSTICK_OUTPUTS_PATH;
name += std::to_string(count);
joystickOutputTopics[count] =
ntInst.GetProtobufTopic<mrc::JoystickOutput>(name).Publish(options);
}
hasUserCodePublisher =
ntInst.GetBooleanTopic(ROBOT_HAS_USER_CODE_PATH).Publish();
hasUserCodePublisher.Set(true);
consoleLinePublisher =
ntInst.GetStringTopic(ROBOT_CONSOLE_LINE_PATH).Publish(options);
hasSetWallClockSubscriber =
ntInst.GetBooleanTopic(ROBOT_HAS_SET_WALL_CLOCK_PATH)
.Subscribe(false, options);
errorInfoPublisher =
ntInst.GetProtobufTopic<mrc::ErrorInfo>(ROBOT_ERROR_INFO_PATH)
.Publish(options);
versionPublisher =
ntInst.GetStringTopic(ROBOT_LIB_VERSION_PATH).Publish(options);
controlDataSubscriber =
ntInst.GetProtobufTopic<mrc::ControlData>(ROBOT_CONTROL_DATA_PATH)
.Subscribe({}, options);
matchInfoSubscriber =
ntInst.GetProtobufTopic<mrc::MatchInfo>(ROBOT_MATCH_INFO_PATH)
.Subscribe({});
joystickDescriptorsTopic = ntInst
.GetProtobufTopic<mrc::JoystickDescriptors>(
ROBOT_JOYSTICK_DESCRIPTORS_PATH)
.Subscribe({});
opModeOptionsPublisher = ntInst
.GetProtobufTopic<std::vector<mrc::OpMode>>(
ROBOT_OP_MODE_OPTIONS_PATH)
.Publish();
opModeOptionsPublisher.Set({});
controlDataListener = ntInst.AddListener(
controlDataSubscriber, NT_EVENT_VALUE_REMOTE | NT_EVENT_UNPUBLISH,
[this](const wpi::nt::Event& event) { HandleListener(event); });
traceOpModePublisher =
ntInst.GetIntegerTopic(ROBOT_CURRENT_OPMODE_TRACE_PATH)
.Publish(options);
traceOpModePublisher.GetTopic().SetCached(false);
serverReadySubscriber =
ntInst.GetBooleanTopic(ROBOT_SERVER_READY_PATH).Subscribe(false);
int checkCount = 0;
while (!serverReadySubscriber.Get()) {
if (++checkCount > 500) {
fmt::print(stderr,
"Error: Waiting for server ready failed. Restarting app and "
"retrying...\n",
ROBOT_SERVER_READY_PATH);
std::terminate();
}
std::this_thread::sleep_for(std::chrono::milliseconds(20));
}
}
void HandleListener(const wpi::nt::Event& event);
bool GetLastControlData(mrc::ControlData* data, int64_t* time) {
std::scoped_lock lock{controlDataMutex};
if (!lastValue.IsRaw()) {
return false;
}
if (controlDataMsg.UnpackInto(data, lastValue.GetRaw())) {
*time = lastValue.time();
return true;
}
return false;
}
~SystemServerDriverStation() { ntInst.RemoveListener(controlDataListener); }
};
struct FIRSTDriverStation {
wpi::util::EventVector newDataEvents;
};
} // namespace
static ::SystemServerDriverStation* systemServerDs;
static ::FIRSTDriverStation* driverStation;
void SystemServerDriverStation::HandleListener(const wpi::nt::Event& event) {
auto valueEvent = event.GetValueEventData();
bool isValid = valueEvent && valueEvent->value.IsRaw();
{
std::scoped_lock lock{controlDataMutex};
if (isValid) {
lastValue = valueEvent->value;
} else {
// We've either been unpublished, or type changed.
// Treat either as a disconnect.
lastValue = wpi::nt::Value{};
}
}
if (isValid) {
driverStation->newDataEvents.Wakeup();
}
}
// Message and Data variables
static wpi::util::mutex msgMutex;
void JoystickDataCache::Update(const mrc::ControlData& data) {
matchTime = data.MatchTime;
uint32_t allianceInt = data.ControlWord.Alliance;
allianceInt += 1;
allianceStation = static_cast<HAL_AllianceStationID>(allianceInt);
auto gameData = data.GetGameData();
if (gameData.size() > 8) {
gameData = gameData.substr(0, 8);
}
std::memcpy(this->gameData.gameData, gameData.data(), gameData.size());
this->gameData.gameData[gameData.size()] = '\0';
if (data.ControlWord.SupportsOpModes) {
controlWord = HAL_MakeControlWord(
data.CurrentOpMode.ToValue(),
static_cast<HAL_RobotMode>(data.ControlWord.RobotMode),
data.ControlWord.Enabled, data.ControlWord.EStop,
data.ControlWord.FmsConnected, data.ControlWord.DsConnected);
} else {
wpi::hal::EnableDashboardOpMode();
auto robotMode = static_cast<HAL_RobotMode>(data.ControlWord.RobotMode);
controlWord = HAL_MakeControlWord(
wpi::hal::GetDashboardSelectedOpMode(robotMode), robotMode,
data.ControlWord.Enabled, data.ControlWord.EStop,
data.ControlWord.FmsConnected, data.ControlWord.DsConnected);
}
auto sticks = data.Joysticks();
for (size_t count = 0; count < sticks.size(); count++) {
auto& newStick = sticks[count];
auto newAxes = newStick.Axes.Axes();
auto newPovs = newStick.Povs.Povs();
axes[count].available = newStick.Axes.GetAvailable();
for (size_t i = 0; i < newAxes.size(); i++) {
axes[count].raw[i] = newAxes[i];
int16_t axisValue = newAxes[i];
if (axisValue < 0) {
axes[count].axes[i] = axisValue / 32768.0f;
} else {
axes[count].axes[i] = axisValue / 32767.0f;
}
}
// When mrccomm switches this to available, move to available
povs[count].available = (1lu << newPovs.size()) - 1;
for (size_t i = 0; i < newPovs.size(); i++) {
povs[count].povs[i] = static_cast<HAL_JoystickPOV>(newPovs[i]);
}
buttons[count].available = newStick.Buttons.GetAvailable();
buttons[count].buttons = newStick.Buttons.Buttons;
touchpads[count].count = newStick.Touchpads.GetTouchpadCount();
const auto& newTouchpads = newStick.Touchpads.Touchpads();
for (size_t i = 0; i < newTouchpads.size(); i++) {
const auto& touchpadFingers = newTouchpads[i].Fingers();
touchpads[count].touchpads[i].count = touchpadFingers.size();
for (size_t j = 0; j < touchpadFingers.size(); j++) {
auto& finger = touchpadFingers[j];
touchpads[count].touchpads[i].fingers[j].down = finger.Down ? 1 : 0;
touchpads[count].touchpads[i].fingers[j].x = finger.X;
touchpads[count].touchpads[i].fingers[j].y = finger.Y;
}
}
}
// Mark remaining sticks as unavailable
for (size_t i = sticks.size(); i < HAL_MAX_JOYSTICKS; i++) {
axes[i].available = 0;
povs[i].available = 0;
buttons[i].available = 0;
touchpads[i].count = 0;
}
}
#define CHECK_JOYSTICK_NUMBER(stickNum) \
if ((stickNum) < 0 || (stickNum) >= HAL_MAX_JOYSTICKS) \
return HAL_PARAMETER_OUT_OF_RANGE
static HAL_ControlWord newestControlWord;
static JoystickDataCache caches[2];
static JoystickDataCache* currentRead = &caches[0];
static JoystickDataCache* cacheToUpdate = &caches[1];
static wpi::util::mutex cacheMutex;
namespace {
struct TcpCache {
TcpCache() { std::memset(this, 0, sizeof(*this)); }
void Update();
void CloneTo(TcpCache* other) { std::memcpy(other, this, sizeof(*this)); }
HAL_MatchInfo matchInfo;
HAL_JoystickDescriptor descriptors[HAL_MAX_JOYSTICKS];
};
static_assert(std::is_standard_layout_v<TcpCache>);
} // namespace
static TcpCache tcpCaches[2];
static TcpCache* tcpCurrentRead = &tcpCaches[0];
static TcpCache* tcpCacheToUpdate = &tcpCaches[1];
static wpi::util::mutex tcpCacheMutex;
void TcpCache::Update() {
auto newMatchInfo = systemServerDs->matchInfoSubscriber.Get();
matchInfo.matchNumber = newMatchInfo.MatchNumber;
matchInfo.matchType = static_cast<HAL_MatchType>(newMatchInfo.Type);
matchInfo.replayNumber = newMatchInfo.ReplayNumber;
auto newEventName = newMatchInfo.GetEventName();
auto nameLen =
(std::min)(sizeof(matchInfo.eventName) - 1, newEventName.size());
if (nameLen > 0) {
std::memcpy(matchInfo.eventName, newEventName.data(), nameLen);
}
matchInfo.eventName[nameLen] = '\0';
const auto descriptorsMsg = systemServerDs->joystickDescriptorsTopic.Get();
size_t descriptorCount = descriptorsMsg.GetDescriptorCount();
for (size_t count = 0; count < descriptorCount; count++) {
const auto& newDesc = descriptorsMsg.Descriptors()[count];
auto& desc = descriptors[count];
desc.isGamepad = newDesc.IsGamepad;
desc.supportedOutputs = newDesc.SupportedOutputs;
desc.gamepadType = newDesc.GamepadType;
auto joystickName = newDesc.GetName();
auto joystickNameLen =
(std::min)(sizeof(desc.name) - 1, joystickName.size());
if (joystickNameLen > 0) {
std::memcpy(desc.name, joystickName.data(), joystickNameLen);
}
desc.name[joystickNameLen] = '\0';
}
}
namespace wpi::hal::init {
void InitializeFIRSTDriverStation() {
InitializeDashboardOpMode();
newestControlWord.value = 0;
static FIRSTDriverStation ds;
driverStation = &ds;
}
} // namespace wpi::hal::init
namespace wpi::hal {
static void DefaultPrintErrorImpl(const char* line, size_t size) {
std::fwrite(line, size, 1, stderr);
}
} // namespace wpi::hal
static std::atomic<void (*)(const char* line, size_t size)> gPrintErrorImpl{
wpi::hal::DefaultPrintErrorImpl};
extern "C" {
int32_t HAL_SendError(HAL_Bool isError, int32_t errorCode, HAL_Bool isLVCode,
const char* details, const char* location,
const char* callStack, HAL_Bool printMsg) {
// Avoid flooding console by keeping track of previous 5 error
// messages and only printing again if they're longer than 1 second old.
static constexpr int KEEP_MSGS = 5;
std::scoped_lock lock(msgMutex);
static std::string prevMsg[KEEP_MSGS];
static std::chrono::time_point<std::chrono::steady_clock>
prevMsgTime[KEEP_MSGS];
static bool initialized = false;
if (!initialized) {
for (int i = 0; i < KEEP_MSGS; i++) {
prevMsgTime[i] =
std::chrono::steady_clock::now() - std::chrono::seconds(2);
}
initialized = true;
}
auto curTime = std::chrono::steady_clock::now();
int i;
for (i = 0; i < KEEP_MSGS; ++i) {
if (prevMsg[i] == details) {
break;
}
}
int retval = 0;
if (i == KEEP_MSGS || (curTime - prevMsgTime[i]) >= std::chrono::seconds(1)) {
std::string_view detailsRef{details};
std::string_view locationRef{location};
std::string_view callStackRef{callStack};
mrc::ErrorInfo errorInfo;
errorInfo.IsError = isError ? 1 : 0;
errorInfo.ErrorCode = errorCode;
errorInfo.SetDetails(details);
errorInfo.SetLocation(location);
errorInfo.SetCallStack(callStack);
fmt::print("{}\n", errorInfo.GetCallStack());
systemServerDs->errorInfoPublisher.Set(errorInfo);
if (printMsg) {
fmt::memory_buffer buf;
if (location && location[0] != '\0') {
fmt::format_to(fmt::appender{buf},
"{} at {}: ", isError ? "Error" : "Warning", location);
}
fmt::format_to(fmt::appender{buf}, "{}\n", details);
if (callStack && callStack[0] != '\0') {
fmt::format_to(fmt::appender{buf}, "{}\n", callStack);
}
auto printError = gPrintErrorImpl.load();
printError(buf.data(), buf.size());
}
if (i == KEEP_MSGS) {
// replace the oldest one
i = 0;
auto first = prevMsgTime[0];
for (int j = 1; j < KEEP_MSGS; ++j) {
if (prevMsgTime[j] < first) {
first = prevMsgTime[j];
i = j;
}
}
prevMsg[i] = details;
}
prevMsgTime[i] = curTime;
}
return retval;
}
void HAL_SetPrintErrorImpl(void (*func)(const char* line, size_t size)) {
gPrintErrorImpl.store(func ? func : wpi::hal::DefaultPrintErrorImpl);
}
int32_t HAL_SendConsoleLine(const char* line) {
systemServerDs->consoleLinePublisher.Set(line);
return 0;
}
int32_t HAL_GetControlWord(HAL_ControlWord* controlWord) {
std::scoped_lock lock{cacheMutex};
*controlWord = newestControlWord;
return 0;
}
int32_t HAL_GetUncachedControlWord(HAL_ControlWord* controlWord) {
mrc::ControlData data;
int64_t dataTime{0};
bool dataValid = systemServerDs->GetLastControlData(&data, &dataTime);
if (dataValid && data.ControlWord.DsConnected) {
if (data.ControlWord.SupportsOpModes) {
*controlWord = HAL_MakeControlWord(
data.CurrentOpMode.ToValue(),
static_cast<HAL_RobotMode>(data.ControlWord.RobotMode),
data.ControlWord.Enabled, data.ControlWord.EStop,
data.ControlWord.FmsConnected, data.ControlWord.DsConnected);
} else {
wpi::hal::EnableDashboardOpMode();
auto robotMode = static_cast<HAL_RobotMode>(data.ControlWord.RobotMode);
*controlWord = HAL_MakeControlWord(
wpi::hal::GetDashboardSelectedOpMode(robotMode), robotMode,
data.ControlWord.Enabled, data.ControlWord.EStop,
data.ControlWord.FmsConnected, data.ControlWord.DsConnected);
}
} else {
// DS disconnected. Clear the control word
controlWord->value = 0;
}
return 0;
}
int32_t HAL_SetOpModeOptions(const struct HAL_OpModeOption* options,
int32_t count) {
if (count < 0 || count > 1000 || (count != 0 && !options)) {
return HAL_PARAMETER_OUT_OF_RANGE;
}
std::vector<mrc::OpMode> newOptions;
newOptions.reserve(count);
if (count != 0) {
for (auto&& option : std::span{options, options + count}) {
if (option.id == 0) {
continue;
}
newOptions.emplace_back(mrc::OpModeHash::FromValue(option.id),
wpi::util::to_string_view(&option.name),
wpi::util::to_string_view(&option.group),
wpi::util::to_string_view(&option.description),
option.textColor, option.backgroundColor);
}
}
{
std::scoped_lock lock{tcpCacheMutex};
systemServerDs->opModeOptionsPublisher.Set(newOptions);
}
wpi::hal::SetDashboardOpModeOptions({options, options + count});
return 0;
}
int32_t HAL_GetJoystickAxes(int32_t joystickNum, HAL_JoystickAxes* axes) {
CHECK_JOYSTICK_NUMBER(joystickNum);
std::scoped_lock lock{cacheMutex};
*axes = currentRead->axes[joystickNum];
return 0;
}
int32_t HAL_GetJoystickPOVs(int32_t joystickNum, HAL_JoystickPOVs* povs) {
CHECK_JOYSTICK_NUMBER(joystickNum);
std::scoped_lock lock{cacheMutex};
*povs = currentRead->povs[joystickNum];
return 0;
}
int32_t HAL_GetJoystickButtons(int32_t joystickNum,
HAL_JoystickButtons* buttons) {
CHECK_JOYSTICK_NUMBER(joystickNum);
std::scoped_lock lock{cacheMutex};
*buttons = currentRead->buttons[joystickNum];
return 0;
}
int32_t HAL_GetJoystickTouchpads(int32_t joystickNum,
HAL_JoystickTouchpads* touchpads) {
CHECK_JOYSTICK_NUMBER(joystickNum);
std::scoped_lock lock{cacheMutex};
*touchpads = currentRead->touchpads[joystickNum];
return 0;
}
void HAL_GetAllJoystickData(int32_t joystickNum, HAL_JoystickAxes* axes,
HAL_JoystickPOVs* povs,
HAL_JoystickButtons* buttons,
HAL_JoystickTouchpads* touchpads) {
std::scoped_lock lock{cacheMutex};
*axes = currentRead->axes[joystickNum];
*povs = currentRead->povs[joystickNum];
*buttons = currentRead->buttons[joystickNum];
*touchpads = currentRead->touchpads[joystickNum];
}
int32_t HAL_GetJoystickDescriptor(int32_t joystickNum,
HAL_JoystickDescriptor* desc) {
CHECK_JOYSTICK_NUMBER(joystickNum);
std::scoped_lock lock{tcpCacheMutex};
*desc = tcpCurrentRead->descriptors[joystickNum];
return 0;
}
int32_t HAL_GetMatchInfo(HAL_MatchInfo* info) {
std::scoped_lock lock{tcpCacheMutex};
*info = tcpCurrentRead->matchInfo;
return 0;
}
HAL_AllianceStationID HAL_GetAllianceStation(int32_t* status) {
std::scoped_lock lock{cacheMutex};
return currentRead->allianceStation;
}
HAL_Bool HAL_GetJoystickIsGamepad(int32_t joystickNum) {
HAL_JoystickDescriptor joystickDesc;
if (HAL_GetJoystickDescriptor(joystickNum, &joystickDesc) < 0) {
return 0;
} else {
return joystickDesc.isGamepad;
}
}
int32_t HAL_GetJoystickGamepadType(int32_t joystickNum) {
HAL_JoystickDescriptor joystickDesc;
if (HAL_GetJoystickDescriptor(joystickNum, &joystickDesc) < 0) {
return -1;
} else {
return joystickDesc.gamepadType;
}
}
int32_t HAL_GetGameData(HAL_GameData* gameData) {
std::scoped_lock lock{cacheMutex};
*gameData = currentRead->gameData;
return 0;
}
int32_t HAL_GetJoystickSupportedOutputs(int32_t joystickNum) {
HAL_JoystickDescriptor joystickDesc;
if (HAL_GetJoystickDescriptor(joystickNum, &joystickDesc) < 0) {
return -1;
} else {
return joystickDesc.supportedOutputs;
}
}
void HAL_GetJoystickName(struct WPI_String* name, int32_t joystickNum) {
HAL_JoystickDescriptor joystickDesc;
const char* cName = joystickDesc.name;
if (HAL_GetJoystickDescriptor(joystickNum, &joystickDesc) < 0) {
cName = "";
}
auto len = std::strlen(cName);
auto write = WPI_AllocateString(name, len);
std::memcpy(write, cName, len);
}
int32_t HAL_SetJoystickRumble(int32_t joystickNum, int32_t leftRumble,
int32_t rightRumble, int32_t leftTriggerRumble,
int32_t rightTriggerRumble) {
CHECK_JOYSTICK_NUMBER(joystickNum);
std::scoped_lock lock{systemServerDs->joystickOutputMutexes[joystickNum]};
systemServerDs->joystickOutputs[joystickNum].LeftRumble =
std::clamp(leftRumble, 0, UINT16_MAX);
systemServerDs->joystickOutputs[joystickNum].RightRumble =
std::clamp(rightRumble, 0, UINT16_MAX);
systemServerDs->joystickOutputs[joystickNum].LeftTriggerRumble =
std::clamp(leftTriggerRumble, 0, UINT16_MAX);
systemServerDs->joystickOutputs[joystickNum].RightTriggerRumble =
std::clamp(rightTriggerRumble, 0, UINT16_MAX);
systemServerDs->joystickOutputTopics[joystickNum].Set(
systemServerDs->joystickOutputs[joystickNum]);
return 0;
}
int32_t HAL_SetJoystickLeds(int32_t joystickNum, int32_t leds) {
CHECK_JOYSTICK_NUMBER(joystickNum);
std::scoped_lock lock{systemServerDs->joystickOutputMutexes[joystickNum]};
systemServerDs->joystickOutputs[joystickNum].R = (leds >> 16) & 0xFF;
systemServerDs->joystickOutputs[joystickNum].G = (leds >> 8) & 0xFF;
systemServerDs->joystickOutputs[joystickNum].B = leds & 0xFF;
systemServerDs->joystickOutputTopics[joystickNum].Set(
systemServerDs->joystickOutputs[joystickNum]);
return 0;
}
double HAL_GetMatchTime(int32_t* status) {
std::scoped_lock lock{cacheMutex};
return currentRead->matchTime;
}
void HAL_ObserveUserProgramStarting(void) {
systemServerDs->hasUserCodeReadyPublisher.Set(true);
}
void HAL_ObserveUserProgram(HAL_ControlWord word) {
systemServerDs->traceOpModePublisher.Set(word.value &
(HAL_CONTROLWORD_OPMODE_HASH_MASK |
HAL_CONTROLWORD_ROBOT_MODE_MASK |
HAL_CONTROLWORD_ENABLED_MASK));
}
HAL_Bool HAL_RefreshDSData(void) {
mrc::ControlData newestData;
int64_t dataTime{0};
bool dataValid = systemServerDs->GetLastControlData(&newestData, &dataTime);
// auto now = wpi::util::Now();
// auto delta = now - dataTime;
bool updatedData = false;
// Data newer then 125ms, and we have a DS connected
// TODO add a new way to detect if mrccomm has stopped.
if (dataValid /* && delta < 125000 */ && newestData.ControlWord.DsConnected) {
// Update the cache.
cacheToUpdate->Update(newestData);
updatedData = true;
} else {
// DS disconnected. Clear the control word
cacheToUpdate->controlWord.value = 0;
}
{
std::scoped_lock lock{cacheMutex};
std::swap(cacheToUpdate, currentRead);
newestControlWord = currentRead->controlWord;
}
tcpCacheToUpdate->Update();
std::scoped_lock tcpLock(tcpCacheMutex);
std::swap(tcpCacheToUpdate, tcpCurrentRead);
return updatedData;
}
void HAL_ProvideNewDataEventHandle(WPI_EventHandle handle) {
wpi::hal::init::CheckInit();
driverStation->newDataEvents.Add(handle);
}
void HAL_RemoveNewDataEventHandle(WPI_EventHandle handle) {
driverStation->newDataEvents.Remove(handle);
}
HAL_Bool HAL_GetOutputsEnabled(void) {
return systemServerDs->controlDataSubscriber.Get().ControlWord.WatchdogActive;
}
HAL_Bool HAL_GetSystemTimeValid(int32_t* status) {
return systemServerDs->hasSetWallClockSubscriber.Get(false);
}
} // extern "C"
namespace wpi::hal {
void InitializeDriverStation() {
StartDashboardOpMode();
systemServerDs = new ::SystemServerDriverStation{wpi::hal::GetSystemServer()};
}
void WaitForInitialPacket() {
wpi::util::Event waitForInitEvent;
driverStation->newDataEvents.Add(waitForInitEvent.GetHandle());
bool timed_out = false;
wpi::util::WaitForObject(waitForInitEvent.GetHandle(), 0.1, &timed_out);
// Don't care what the result is, just want to give it a chance.
driverStation->newDataEvents.Remove(waitForInitEvent.GetHandle());
}
} // namespace wpi::hal

1
hal/src/main/native/systemcore/HAL.cpp
View File

@@ -47,7 +47,6 @@ void InitializeHAL() {
InitializeDIO();
InitializeDutyCycle();
InitializeEncoder();
InitializeFIRSTDriverStation();
InitializeI2C();
InitializeIMU();
InitializeMain();

1
hal/src/main/native/systemcore/HALInitializer.hpp
View File

@@ -29,7 +29,6 @@ extern void InitializeDigitalInternal();
extern void InitializeDIO();
extern void InitializeDutyCycle();
extern void InitializeEncoder();
extern void InitializeFIRSTDriverStation();
extern void InitializeHAL();
extern void InitializeI2C();
extern void InitializeIMU();

12
hal/src/main/python/hal/src/hal.cpp
View File

@@ -1,7 +1,7 @@
#include "wpi/hal/HAL.h"
#include "wpi/hal/DriverStation.h"
#include "wpi/hal/DriverStation.hpp"
#include "wpi/hal/Value.h"
#include <semiwrap_init.hal._wpiHal.hpp>
@@ -68,7 +68,7 @@ SEMIWRAP_PYBIND11_MODULE(m) {
m.attr("__hal_simulation__") = true;
m.def("__test_senderr", []() {
HAL_SendError(1, 2, 0, "\xfa" "badmessage", "location", "callstack", 1);
wpi::hal::SendError(1, 2, "\xfa" "badmessage", "location", "callstack", 1);
}, release_gil());
#endif
@@ -76,16 +76,16 @@ SEMIWRAP_PYBIND11_MODULE(m) {
// Redirect stderr to python stderr
sys_module = py::module_::import("sys");
HAL_SetPrintErrorImpl([](const char *line, size_t size) {
if (size == 0) {
HAL_SetPrintErrorImpl([](const struct WPI_String* line) {
if (line == nullptr || line->str == nullptr || line->len == 0) {
return;
}
py::gil_scoped_acquire lock;
PyObject *o = PyUnicode_DecodeUTF8(line, size, "replace");
PyObject *o = PyUnicode_DecodeUTF8(line->str, line->len, "replace");
if (o == nullptr) {
PyErr_Clear();
py::print(py::bytes(line, size), "file"_a=sys_module.attr("stderr"));
py::print(py::bytes(line->str, line->len), "file"_a=sys_module.attr("stderr"));
} else {
py::print(py::reinterpret_steal<py::str>(o), "file"_a=sys_module.attr("stderr"));
}

23
hal/src/main/python/native-pyproject.toml
View File

@@ -32,6 +32,27 @@ version = "0.0.0"
extract_to = "src/native/wpihal"
libs = ["wpiHal"]
[[tool.hatch.build.hooks.robotpy.maven_lib_download]]
artifact_id = "mrclib-cpp"
group_id = "org.wpilib.mrclib"
repo_url = ""
version = "0.0.0"
extract_to = "src/native/wpihal"
libs = ["MrcLib"]
enable_if = 'platform_machine != "systemcore"'
[[tool.hatch.build.hooks.nativelib.pcfile]]
pcfile = "src/native/wpihal/robotpy-native-mrclib.pc"
name = "mrclib"
includedir = "src/native/wpihal/include"
libdir = "src/native/wpihal/lib"
shared_libraries = ["MrcLib"]
enable_if = 'platform_machine != "systemcore"'
[[tool.hatch.build.hooks.nativelib.pcfile]]
pcfile = "src/native/wpihal/robotpy-native-wpihal.pc"
name = "wpihal"
@@ -39,4 +60,4 @@ name = "wpihal"
includedir = "src/native/wpihal/include"
libdir = "src/native/wpihal/lib"
shared_libraries = ["wpiHal"]
requires = ["robotpy-native-wpiutil", "robotpy-native-ntcore"]
requires = ["robotpy-native-wpiutil", "robotpy-native-ntcore", "robotpy-native-mrclib"]

3
hal/src/main/python/semiwrap/DriverStation.yml
View File

@@ -1,3 +1,6 @@
functions:
GetControlWord:
GetUncachedControlWord:
SendError:
SendConsoleLine:
SendProgramCrash:

1
hal/src/main/python/semiwrap/DriverStation_c.yml
View File

@@ -6,6 +6,7 @@ functions:
HAL_SetPrintErrorImpl:
ignore: true
HAL_SendConsoleLine:
HAL_SendProgramCrash:
HAL_GetControlWord:
ignore: true
HAL_GetAllianceStation:

1
romiVendordep/BUILD.bazel
View File

@@ -120,6 +120,7 @@ define_native_wrapper(
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",

26
shared/bazel/thirdparty/mrclib/mrclib.MODULE.bazel vendored
View File

@@ -21,13 +21,21 @@ filegroup(
)
"""
mrclib_maven_base = "https://frcmaven.wpi.edu/artifactory/development-2027"
mrclib_version = "2027.1.0-alpha-1-42-g019903f"
mrclib_maven_base = "https://frcmaven.wpi.edu/artifactory/thirdparty-mvn-release"
mrclib_version = "2027.1.0-alpha-1-50-gd008523"
mrclib_headers_hash = "c71011e2c593749aca585eec308f394d5e7f34451c1a14cdd8e077ea3b1368b5"
mrclib_linuxarm64_hash = "4673c573c25d1d9a03f1cf3d73c1e8a121718291171213813b95b451eea53f3a"
mrclib_linuxx86_64_hash = "8d986ed0ffb03be1215a3cf88716c31da82848f4524e723ac89aee4eca45eb12"
mrclib_osx_hash = "af05c46bffa58a1ca63bd18f4d8865b827746d1cdf94c8512c7f86d58731d16f"
mrclib_systemcore_hash = "c1cad49fb96caa73fddbc852588c67a2110402539b39dba40255c21974114e84"
mrclib_windowsarm64_hash = "8205701efbde585c6822981c7246c0a0d8e4b4cdb702097ec77674aa98aff7bb"
mrclib_windowsx86_64_hash = "dd4eef6e1aaba5b7034889a1646e259b3e91435b3084d43b3ff3b2c6adcc2122"
http_archive(
name = "mrclib_headers",
url = mrclib_maven_base + "/org/wpilib/mrclib/mrclib-cpp/{version}/mrclib-cpp-{version}-headers.zip".format(version = mrclib_version),
sha256 = "c71011e2c593749aca585eec308f394d5e7f34451c1a14cdd8e077ea3b1368b5",
sha256 = mrclib_headers_hash,
build_file_content = """
cc_library(
name = "headers",
@@ -41,41 +49,41 @@ cc_library(
http_archive(
name = "mrclib_linuxarm64",
url = mrclib_maven_base + "/org/wpilib/mrclib/mrclib-cpp/{version}/mrclib-cpp-{version}-linuxarm64.zip".format(version = mrclib_version),
sha256 = "666b7ed14574495b4bf2bc2f9d5e130a5c17038e5620c33ffb4a56b5447f83e8",
sha256 = mrclib_linuxarm64_hash,
build_file_content = cc_shared_library_build_contents,
)
http_archive(
name = "mrclib_linuxx86-64",
url = mrclib_maven_base + "/org/wpilib/mrclib/mrclib-cpp/{version}/mrclib-cpp-{version}-linuxx86-64.zip".format(version = mrclib_version),
sha256 = "0e42d6565f7874dbfd014ea2008eeddb29ce0e1d77692b4158f2dc3a2edf7997",
sha256 = mrclib_linuxx86_64_hash,
build_file_content = cc_shared_library_build_contents,
)
http_archive(
name = "mrclib_osx",
url = mrclib_maven_base + "/org/wpilib/mrclib/mrclib-cpp/{version}/mrclib-cpp-{version}-osxuniversal.zip".format(version = mrclib_version),
sha256 = "91d6207feae81144342666f5f7676633b54c9bd83aa2296d05f90a451b108bb2",
sha256 = mrclib_osx_hash,
build_file_content = cc_shared_library_build_contents,
)
http_archive(
name = "mrclib_systemcore",
url = mrclib_maven_base + "/org/wpilib/mrclib/mrclib-cpp/{version}/mrclib-cpp-{version}-linuxsystemcore.zip".format(version = mrclib_version),
sha256 = "c1cad49fb96caa73fddbc852588c67a2110402539b39dba40255c21974114e84",
sha256 = mrclib_systemcore_hash,
build_file_content = cc_shared_library_build_contents,
)
http_archive(
name = "mrclib_windowsarm64",
url = mrclib_maven_base + "/org/wpilib/mrclib/mrclib-cpp/{version}/mrclib-cpp-{version}-windowsarm64.zip".format(version = mrclib_version),
sha256 = "4d05453713a9a43a8c0a01d23c528be1e4fb53e397b573ecdd8c1cccb27491cb",
sha256 = mrclib_windowsarm64_hash,
build_file_content = cc_shared_library_build_contents,
)
http_archive(
name = "mrclib_windowsx86-64",
url = mrclib_maven_base + "/org/wpilib/mrclib/mrclib-cpp/{version}/mrclib-cpp-{version}-windowsx86-64.zip".format(version = mrclib_version),
sha256 = "b36e96e2d523820e790f5bad824add60dc9c07d5dd70a96890a1b47644090d20",
sha256 = mrclib_windowsx86_64_hash,
build_file_content = cc_shared_library_build_contents,
)

14
shared/libmrclib.gradle Normal file
View File

@@ -0,0 +1,14 @@
nativeUtils {
nativeDependencyContainer {
mrclib(getNativeDependencyTypeClass('WPISharedMavenDependency')) {
groupId = "org.wpilib.mrclib"
artifactId = "mrclib-cpp"
headerClassifier = "headers"
ext = "zip"
version = '2027.1.0-alpha-1-50-gd008523'
skipAtRuntimePlatforms.add(nativeUtils.wpi.platforms.systemcore)
targetPlatforms.addAll(nativeUtils.wpi.platforms.allPlatforms)
noDebugSplit = true
}
}
}

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

311
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 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;
}
}
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);
}
MRC_JoystickDescriptors descriptors;
status = MRC_DsComms_GetJoystickDescriptors(&descriptors);
if (status == 0) {
for (int i = 0; i < MRCLIB_MAX_JOYSTICKS; ++i) {
HAL_JoystickDescriptor descriptor{};
if (i < descriptors.count) {
auto& mrcDescriptor = descriptors.descriptors[i];
descriptor.isGamepad = mrcDescriptor.isGamepad ? 1 : 0;
descriptor.gamepadType = mrcDescriptor.gamepadType;
descriptor.supportedOutputs = mrcDescriptor.supportedOutputs;
auto nameLength = std::min<size_t>(mrcDescriptor.nameLength,
sizeof(descriptor.name) - 1);
std::memcpy(descriptor.name, mrcDescriptor.name, nameLength);
descriptor.name[nameLength] = '\0';
}
HALSIM_SetJoystickDescriptor(i, &descriptor);
}
}
HALSIM_NotifyDriverStationNewData();
}
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
}
}
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)();
}
}
}
}
static void SetupTcp(wpi::net::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();
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);
}
});
});
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());
}
}
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();
}
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",

1
wpilibc/BUILD.bazel
View File

@@ -217,6 +217,7 @@ define_native_wrapper(
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",

7
wpilibc/src/main/native/cpp/simulation/DriverStationSim.cpp
View File

@@ -183,8 +183,9 @@ void DriverStationSim::SetSendError(bool shouldSend) {
HALSIM_SetSendError(nullptr);
} else {
HALSIM_SetSendError([](HAL_Bool isError, int32_t errorCode,
HAL_Bool isLVCode, const char* details,
const char* location, const char* callStack,
const struct WPI_String* details,
const struct WPI_String* location,
const struct WPI_String* callStack,
HAL_Bool printMsg) { return 0; });
}
}
@@ -193,7 +194,7 @@ void DriverStationSim::SetSendConsoleLine(bool shouldSend) {
if (shouldSend) {
HALSIM_SetSendConsoleLine(nullptr);
} else {
HALSIM_SetSendConsoleLine([](const char* line) { return 0; });
HALSIM_SetSendConsoleLine([](const struct WPI_String* line) { return 0; });
}
}

8
wpilibc/src/main/native/cpp/system/Errors.cpp
View File

@@ -7,7 +7,7 @@
#include <string>
#include <utility>
#include "wpi/hal/DriverStation.h"
#include "wpi/hal/DriverStation.hpp"
#include "wpi/hal/HAL.h"
#include "wpi/util/StackTrace.hpp"
#include "wpi/util/fs.hpp"
@@ -32,8 +32,8 @@ RuntimeError::RuntimeError(int32_t code, const char* fileName, int lineNumber,
std::move(stack), std::move(message)} {}
void RuntimeError::Report() const {
HAL_SendError(m_data->code < 0, m_data->code, 0, what(), m_data->loc.c_str(),
m_data->stack.c_str(), 1);
wpi::hal::SendError(m_data->code < 0, m_data->code, what(),
m_data->loc.c_str(), m_data->stack.c_str(), 1);
}
const char* wpi::GetErrorMessage(int32_t* code) {
@@ -63,7 +63,7 @@ void wpi::ReportErrorV(int32_t status, const char* fileName, int lineNumber,
fmt::format_to(fmt::appender{out}, "{}: ", GetErrorMessage(&status));
fmt::vformat_to(fmt::appender{out}, format, args);
out.push_back('\0');
HAL_SendError(status < 0, status, 0, out.data(), funcName,
wpi::hal::SendError(status < 0, status, out.data(), funcName,
wpi::util::GetStackTrace(2).c_str(), 1);
}

4
wpilibc/src/main/native/cpp/system/Notifier.cpp
View File

@@ -6,7 +6,7 @@
#include <utility>
#include "wpi/hal/DriverStation.h"
#include "wpi/hal/DriverStation.hpp"
#include "wpi/hal/Notifier.hpp"
#include "wpi/hal/Threads.h"
#include "wpi/system/Errors.hpp"
@@ -57,7 +57,7 @@ Notifier::Notifier(int priority, std::function<void()> callback) {
" See https://wpilib.org/stacktrace for more information.\n");
throw;
} catch (const std::exception& e) {
HAL_SendError(1, err::Error, 0, e.what(), "", "", 1);
wpi::hal::SendError(1, err::Error, e.what(), "", "", 1);
throw;
}
}

4
wpilibc/src/main/native/include/wpi/framework/RobotBase.hpp
View File

@@ -8,7 +8,7 @@
#include <string>
#include <thread>
#include "wpi/hal/DriverStation.h"
#include "wpi/hal/DriverStation.hpp"
#include "wpi/hal/HAL.h"
#include "wpi/hal/Main.h"
#include "wpi/nt/NetworkTable.hpp"
@@ -47,7 +47,7 @@ void RunRobot(wpi::util::mutex& m, Robot** robot) {
" See https://wpilib.org/stacktrace for more information.\n");
throw;
} catch (const std::exception& e) {
HAL_SendError(1, err::Error, 0, e.what(), "", "", 1);
wpi::hal::SendError(1, err::Error, e.what(), "", "", 1);
throw;
}
}

1
wpilibc/src/main/python/wpilib/_impl/report_error.py
View File

@@ -61,7 +61,6 @@ def reportErrorInternal(
hal.sendError(
not isWarning,
code,
False,
error,
locString,
traceString,

10
wpilibj/src/main/java/org/wpilib/driverstation/DriverStationErrors.java
View File

@@ -49,4 +49,14 @@ public final class DriverStationErrors {
public static void reportWarning(String warning, StackTraceElement[] stackTrace) {
DriverStationBackend.reportWarning(warning, stackTrace);
}
/**
* Report crash to Driver Station. Appends provided stack trace to crash message.
*
* @param details The crash details to report.
* @param stackTrace The stack trace to append
*/
public static void reportCrash(String details, StackTraceElement[] stackTrace) {
DriverStationBackend.reportCrash(details, stackTrace);
}
}

41
wpilibj/src/main/java/org/wpilib/driverstation/internal/DriverStationBackend.java
View File

@@ -571,8 +571,45 @@ public final class DriverStationBackend {
}
}
}
DriverStationJNI.sendError(
isError, code, false, error, locString, traceString.toString(), true);
DriverStationJNI.sendError(isError, code, error, locString, traceString.toString(), true);
}
/**
* Report crash to Driver Station. Appends provided stack trace to crash message.
*
* @param error The error message
* @param stackTrace The stack trace to append
*/
public static void reportCrash(String error, StackTraceElement[] stackTrace) {
reportCrashImpl(error, stackTrace, 0);
}
private static void reportCrashImpl(
String error, StackTraceElement[] stackTrace, int stackTraceFirst) {
String locString;
if (stackTrace.length >= stackTraceFirst + 1) {
locString = stackTrace[stackTraceFirst].toString();
} else {
locString = "";
}
StringBuilder traceString = new StringBuilder();
boolean haveLoc = false;
for (int i = stackTraceFirst; i < stackTrace.length; i++) {
String loc = stackTrace[i].toString();
traceString.append("\tat ").append(loc).append('\n');
// get first user function
if (!haveLoc && !loc.startsWith("org.wpilib")) {
locString = loc;
haveLoc = true;
}
}
DriverStationJNI.sendProgramCrash(error, locString, traceString.toString());
// Sleep to ensure message is sent before crash
try {
Thread.sleep(100);
} catch (InterruptedException e) {
Thread.currentThread().interrupt();
}
}
/**

3
wpilibj/src/main/java/org/wpilib/framework/RobotBase.java
View File

@@ -347,6 +347,7 @@ public abstract class RobotBase implements AutoCloseable {
+ " See https://wpilib.org/stacktrace for more information.\n",
false);
DriverStationErrors.reportError("Could not instantiate robot " + robotName + "!", false);
DriverStationErrors.reportCrash("Could not instantiate robot " + robotName + "!", elements);
return;
}
@@ -364,6 +365,8 @@ public abstract class RobotBase implements AutoCloseable {
}
DriverStationErrors.reportError(
"Unhandled exception: " + throwable, throwable.getStackTrace());
DriverStationErrors.reportCrash(
"Unhandled exception: " + throwable, throwable.getStackTrace());
errorOnExit = true;
} finally {
m_runMutex.lock();

1
xrpVendordep/BUILD.bazel
View File

@@ -118,6 +118,7 @@ define_native_wrapper(
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",