diff --git a/glass/src/lib/native/cpp/other/FMS.cpp b/glass/src/lib/native/cpp/other/FMS.cpp index 965c6e1bb6..6cfa25ff88 100644 --- a/glass/src/lib/native/cpp/other/FMS.cpp +++ b/glass/src/lib/native/cpp/other/FMS.cpp @@ -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()) { diff --git a/hal/BUILD.bazel b/hal/BUILD.bazel index 6a9c568c97..ab7b406eb1 100644 --- a/hal/BUILD.bazel +++ b/hal/BUILD.bazel @@ -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", diff --git a/hal/build.gradle b/hal/build.gradle index d22e5aed60..0a0c997b3f 100644 --- a/hal/build.gradle +++ b/hal/build.gradle @@ -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' diff --git a/hal/robotpy_native_build_info.bzl b/hal/robotpy_native_build_info.bzl index 211714f76f..70d68b443a 100644 --- a/hal/robotpy_native_build_info.bzl +++ b/hal/robotpy_native_build_info.bzl @@ -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", ], }, diff --git a/hal/src/main/java/org/wpilib/hardware/hal/DriverStationJNI.java b/hal/src/main/java/org/wpilib/hardware/hal/DriverStationJNI.java index c90f407d6e..e6ba322878 100644 --- a/hal/src/main/java/org/wpilib/hardware/hal/DriverStationJNI.java +++ b/hal/src/main/java/org/wpilib/hardware/hal/DriverStationJNI.java @@ -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. * diff --git a/hal/src/main/native/cpp/FIRSTDriverStation.cpp b/hal/src/main/native/cpp/FIRSTDriverStation.cpp new file mode 100644 index 0000000000..0c262e67fe --- /dev/null +++ b/hal/src/main/native/cpp/FIRSTDriverStation.cpp @@ -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 +#include +#include +#include +#include + +#include + +#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 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 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 diff --git a/hal/src/main/native/cpp/jni/DriverStationJNI.cpp b/hal/src/main/native/cpp/jni/DriverStationJNI.cpp index c48190d3eb..f8eba6da89 100644 --- a/hal/src/main/native/cpp/jni/DriverStationJNI.cpp +++ b/hal/src/main/native/cpp/jni/DriverStationJNI.cpp @@ -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; } diff --git a/hal/src/main/native/cpp/jni/HALUtil.cpp b/hal/src/main/native/cpp/jni/HALUtil.cpp index 0131c4a369..7bc5fb379d 100644 --- a/hal/src/main/native/cpp/jni/HALUtil.cpp +++ b/hal/src/main/native/cpp/jni/HALUtil.cpp @@ -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); } } diff --git a/hal/src/main/native/cpp/jni/simulation/DriverStationDataJNI.cpp b/hal/src/main/native/cpp/jni/simulation/DriverStationDataJNI.cpp index 73877062ab..5454a377fa 100644 --- a/hal/src/main/native/cpp/jni/simulation/DriverStationDataJNI.cpp +++ b/hal/src/main/native/cpp/jni/simulation/DriverStationDataJNI.cpp @@ -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; }); } } diff --git a/hal/src/main/native/cpp/mrclib/MrcLibDs.cpp b/hal/src/main/native/cpp/mrclib/MrcLibDs.cpp new file mode 100644 index 0000000000..f0660ca2c9 --- /dev/null +++ b/hal/src/main/native/cpp/mrclib/MrcLibDs.cpp @@ -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 +#include +#include +#include +#include +#include + +#include + +#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); + +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); +} // 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(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( + 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( + mrclib::GetRobotMode(controlData.controlFlags)); + controlWord = + HAL_MakeControlWord(wpi::hal::GetDashboardSelectedOpMode(robotMode), + static_cast( + 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(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(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(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(mrclib::GetRobotMode(data.controlFlags)); + *controlWord = HAL_MakeControlWord( + wpi::hal::GetDashboardSelectedOpMode(robotMode), + static_cast(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 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(option.name.len), + static_cast(MRCLIB_MAX_OPMODE_NAME_LENGTH)); + std::memcpy(newValue.name, option.name.str, nameLen); + newValue.nameLength = static_cast(nameLen); + + auto groupLen = + std::min(static_cast(option.group.len), + static_cast(MRCLIB_MAX_OPMODE_GROUP_LENGTH)); + std::memcpy(newValue.group, option.group.str, groupLen); + newValue.groupLength = static_cast(groupLen); + + auto descLen = + std::min(static_cast(option.description.len), + static_cast(MRCLIB_MAX_OPMODE_DESCRIPTION_LENGTH)); + std::memcpy(newValue.description, option.description.str, descLen); + newValue.descriptionLength = static_cast(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( + 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(leftRumble, 0, UINT16_MAX); + joystickOutputs[joystickNum].rightRumble = + std::clamp(rightRumble, 0, UINT16_MAX); + joystickOutputs[joystickNum].leftTriggerRumble = + std::clamp(leftTriggerRumble, 0, UINT16_MAX); + joystickOutputs[joystickNum].rightTriggerRumble = + std::clamp(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); +} diff --git a/hal/src/main/native/include/wpi/hal/DriverStation.h b/hal/src/main/native/include/wpi/hal/DriverStation.h index a595370311..015f8ad63f 100644 --- a/hal/src/main/native/include/wpi/hal/DriverStation.h +++ b/hal/src/main/native/include/wpi/hal/DriverStation.h @@ -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. diff --git a/hal/src/main/native/include/wpi/hal/DriverStation.hpp b/hal/src/main/native/include/wpi/hal/DriverStation.hpp index 53307465aa..006de008d8 100644 --- a/hal/src/main/native/include/wpi/hal/DriverStation.hpp +++ b/hal/src/main/native/include/wpi/hal/DriverStation.hpp @@ -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); diff --git a/hal/src/main/native/include/wpi/hal/DriverStationTypes.h b/hal/src/main/native/include/wpi/hal/DriverStationTypes.h index bdb991d935..8829ac24fb 100644 --- a/hal/src/main/native/include/wpi/hal/DriverStationTypes.h +++ b/hal/src/main/native/include/wpi/hal/DriverStationTypes.h @@ -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; diff --git a/hal/src/main/native/include/wpi/hal/cpp/MrcLibDs.hpp b/hal/src/main/native/include/wpi/hal/cpp/MrcLibDs.hpp new file mode 100644 index 0000000000..1c848ecf7c --- /dev/null +++ b/hal/src/main/native/include/wpi/hal/cpp/MrcLibDs.hpp @@ -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 + +#include + +#include "wpi/hal/DriverStationTypes.h" +#include "wpi/util/Synchronization.h" +#include "wpi/util/string.hpp" + +namespace wpi::hal { +using BackendPrintFunction = std::function; + +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 diff --git a/hal/src/main/native/include/wpi/hal/simulation/MockHooks.h b/hal/src/main/native/include/wpi/hal/simulation/MockHooks.h index 8f389c5d69..3c105e3681 100644 --- a/hal/src/main/native/include/wpi/hal/simulation/MockHooks.h +++ b/hal/src/main/native/include/wpi/hal/simulation/MockHooks.h @@ -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); diff --git a/hal/src/main/native/sim/DriverStation.cpp b/hal/src/main/native/sim/DriverStation.cpp index 673c2f81f4..b1fcab94a2 100644 --- a/hal/src/main/native/sim/DriverStation.cpp +++ b/hal/src/main/native/sim/DriverStation.cpp @@ -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 sendErrorHandler{nullptr}; -static std::atomic sendConsoleLineHandler{ - nullptr}; +#include "wpi/util/string.hpp" using namespace wpi::hal; @@ -53,13 +49,6 @@ static_assert(std::is_standard_layout_v); // static_assert(std::is_trivial_v); 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 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); } // 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 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 gPrintErrorImpl{ - wpi::hal::DefaultPrintErrorImpl}; +static std::atomic sendErrorHandler{nullptr}; +static std::atomic 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,181 +284,161 @@ 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; } - SimDriverStationData->SetOpModeOptions({options, options + count}); + 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, - HAL_JoystickButtons* buttons) { +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, - HAL_JoystickPOVs* povs, - HAL_JoystickButtons* buttons, - HAL_JoystickTouchpads* touchpads) { +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, - HAL_JoystickDescriptor* desc) { +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 rightTriggerRumble) { +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, rightTriggerRumble); 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) && - HAL_ControlWord_IsDSAttached(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(GetDefaultDriverStationImpl()); + defaultImpl->NewDriverStationData(); } + +MrcLibDs* GetDefaultDriverStationImpl() { + static MrcLibDsSimImpl impl; + return &impl; +} + } // namespace wpi::hal diff --git a/hal/src/main/native/sim/HAL.cpp b/hal/src/main/native/sim/HAL.cpp index 379fed41e9..913ab32043 100644 --- a/hal/src/main/native/sim/HAL.cpp +++ b/hal/src/main/native/sim/HAL.cpp @@ -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; diff --git a/hal/src/main/native/sim/HALInitializer.hpp b/hal/src/main/native/sim/HALInitializer.hpp index 41d35109fd..cd49488e01 100644 --- a/hal/src/main/native/sim/HALInitializer.hpp +++ b/hal/src/main/native/sim/HALInitializer.hpp @@ -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(); diff --git a/hal/src/main/native/systemcore/DriverStation.cpp b/hal/src/main/native/systemcore/DriverStation.cpp new file mode 100644 index 0000000000..18141e3a76 --- /dev/null +++ b/hal/src/main/native/systemcore/DriverStation.cpp @@ -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 diff --git a/hal/src/main/native/systemcore/FIRSTDriverStation.cpp b/hal/src/main/native/systemcore/FIRSTDriverStation.cpp deleted file mode 100644 index ceb0e527ce..0000000000 --- a/hal/src/main/native/systemcore/FIRSTDriverStation.cpp +++ /dev/null @@ -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 -#include -#include -#include -#include -#include -#include -#include -#include -#include - -#include - -#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); -// static_assert(std::is_trivial_v); - -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 controlDataSubscriber; - wpi::nt::ProtobufSubscriber matchInfoSubscriber; - - wpi::nt::ProtobufSubscriber - joystickDescriptorsTopic; - - wpi::nt::StringPublisher versionPublisher; - wpi::nt::StringPublisher consoleLinePublisher; - wpi::nt::ProtobufPublisher errorInfoPublisher; - - std::array, - MRC_MAX_NUM_JOYSTICKS> - joystickOutputTopics; - - wpi::nt::ProtobufPublisher> opModeOptionsPublisher; - wpi::nt::IntegerPublisher traceOpModePublisher; - - NT_Listener controlDataListener; - - wpi::util::mutex controlDataMutex; - wpi::util::ProtobufMessage 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(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(ROBOT_ERROR_INFO_PATH) - .Publish(options); - - versionPublisher = - ntInst.GetStringTopic(ROBOT_LIB_VERSION_PATH).Publish(options); - - controlDataSubscriber = - ntInst.GetProtobufTopic(ROBOT_CONTROL_DATA_PATH) - .Subscribe({}, options); - - matchInfoSubscriber = - ntInst.GetProtobufTopic(ROBOT_MATCH_INFO_PATH) - .Subscribe({}); - - joystickDescriptorsTopic = ntInst - .GetProtobufTopic( - ROBOT_JOYSTICK_DESCRIPTORS_PATH) - .Subscribe({}); - - opModeOptionsPublisher = ntInst - .GetProtobufTopic>( - 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(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(data.ControlWord.RobotMode), - data.ControlWord.Enabled, data.ControlWord.EStop, - data.ControlWord.FmsConnected, data.ControlWord.DsConnected); - } else { - wpi::hal::EnableDashboardOpMode(); - auto robotMode = static_cast(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(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); -} // 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(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 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 - 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(data.ControlWord.RobotMode), - data.ControlWord.Enabled, data.ControlWord.EStop, - data.ControlWord.FmsConnected, data.ControlWord.DsConnected); - } else { - wpi::hal::EnableDashboardOpMode(); - auto robotMode = static_cast(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 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 diff --git a/hal/src/main/native/systemcore/HAL.cpp b/hal/src/main/native/systemcore/HAL.cpp index 70c7b243cb..481ad475dd 100644 --- a/hal/src/main/native/systemcore/HAL.cpp +++ b/hal/src/main/native/systemcore/HAL.cpp @@ -47,7 +47,6 @@ void InitializeHAL() { InitializeDIO(); InitializeDutyCycle(); InitializeEncoder(); - InitializeFIRSTDriverStation(); InitializeI2C(); InitializeIMU(); InitializeMain(); diff --git a/hal/src/main/native/systemcore/HALInitializer.hpp b/hal/src/main/native/systemcore/HALInitializer.hpp index 300a88ecc2..4efe24d9b5 100644 --- a/hal/src/main/native/systemcore/HALInitializer.hpp +++ b/hal/src/main/native/systemcore/HALInitializer.hpp @@ -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(); diff --git a/hal/src/main/python/hal/src/hal.cpp b/hal/src/main/python/hal/src/hal.cpp index 176ec66d37..04a3ebcd8a 100644 --- a/hal/src/main/python/hal/src/hal.cpp +++ b/hal/src/main/python/hal/src/hal.cpp @@ -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 @@ -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(o), "file"_a=sys_module.attr("stderr")); } diff --git a/hal/src/main/python/native-pyproject.toml b/hal/src/main/python/native-pyproject.toml index 47a4ebedda..234985b559 100644 --- a/hal/src/main/python/native-pyproject.toml +++ b/hal/src/main/python/native-pyproject.toml @@ -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"] \ No newline at end of file diff --git a/hal/src/main/python/semiwrap/DriverStation.yml b/hal/src/main/python/semiwrap/DriverStation.yml index f093a586a1..fc5f42bfcf 100644 --- a/hal/src/main/python/semiwrap/DriverStation.yml +++ b/hal/src/main/python/semiwrap/DriverStation.yml @@ -1,3 +1,6 @@ functions: GetControlWord: GetUncachedControlWord: + SendError: + SendConsoleLine: + SendProgramCrash: diff --git a/hal/src/main/python/semiwrap/DriverStation_c.yml b/hal/src/main/python/semiwrap/DriverStation_c.yml index 2328fdbe21..b7928a749b 100644 --- a/hal/src/main/python/semiwrap/DriverStation_c.yml +++ b/hal/src/main/python/semiwrap/DriverStation_c.yml @@ -6,6 +6,7 @@ functions: HAL_SetPrintErrorImpl: ignore: true HAL_SendConsoleLine: + HAL_SendProgramCrash: HAL_GetControlWord: ignore: true HAL_GetAllianceStation: diff --git a/romiVendordep/BUILD.bazel b/romiVendordep/BUILD.bazel index 9c65bd98d0..be857334bd 100644 --- a/romiVendordep/BUILD.bazel +++ b/romiVendordep/BUILD.bazel @@ -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", diff --git a/shared/bazel/thirdparty/mrclib/mrclib.MODULE.bazel b/shared/bazel/thirdparty/mrclib/mrclib.MODULE.bazel index 898b7e1f04..f6a911acf3 100644 --- a/shared/bazel/thirdparty/mrclib/mrclib.MODULE.bazel +++ b/shared/bazel/thirdparty/mrclib/mrclib.MODULE.bazel @@ -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, ) diff --git a/shared/libmrclib.gradle b/shared/libmrclib.gradle new file mode 100644 index 0000000000..571687e501 --- /dev/null +++ b/shared/libmrclib.gradle @@ -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 + } + } +} diff --git a/simulation/CMakeLists.txt b/simulation/CMakeLists.txt index e6bc35c7d1..eadd549a2e 100644 --- a/simulation/CMakeLists.txt +++ b/simulation/CMakeLists.txt @@ -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) diff --git a/simulation/halsim_ds_socket/src/main/native/cpp/DSCommPacket.cpp b/simulation/halsim_ds_socket/src/main/native/cpp/DSCommPacket.cpp deleted file mode 100644 index 042e1c1129..0000000000 --- a/simulation/halsim_ds_socket/src/main/native/cpp/DSCommPacket.cpp +++ /dev/null @@ -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 -#include -#include - -#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(station_code); -} - -void DSCommPacket::ReadMatchtimeTag(std::span 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 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::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 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 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 data) { - // Size 2 bytes, tag 1 byte - if (data.size() <= 3) { - return; - } - - int nameLength = std::min(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( - 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 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(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(HALSIM_GetProgramStarted() ? kRobotHasCode : 0); - - // Battery voltage high and low - buf << static_cast(12) << static_cast(0); - - // Request (Always 0) - buf << static_cast(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(); -} diff --git a/simulation/halsim_ds_socket/src/main/native/cpp/main.cpp b/simulation/halsim_ds_socket/src/main/native/cpp/main.cpp index 3d6a92e3d3..294eb64562 100644 --- a/simulation/halsim_ds_socket/src/main/native/cpp/main.cpp +++ b/simulation/halsim_ds_socket/src/main/native/cpp/main.cpp @@ -13,6 +13,7 @@ #include +#include #include #include #include @@ -20,169 +21,158 @@ #include #include +#include + +#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 singleByte; -static std::atomic gDSConnected = false; +static void WriteSimBackend(); namespace { -struct DataStore { - wpi::util::SmallVector m_frame; - size_t m_frameSize = (std::numeric_limits::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 gDSConnected = true; -static void HandleTcpDataStream(Buffer& buf, size_t size, DataStore& store) { - std::string_view data{buf.base, size}; - while (!data.empty()) { - if (store.m_frameSize == (std::numeric_limits::max)()) { - if (store.m_frame.size() < 2u) { - size_t toCopy = (std::min)(2u - store.m_frame.size(), data.size()); - store.m_frame.append(data.data(), data.data() + toCopy); - data.remove_prefix(toCopy); - if (store.m_frame.size() < 2u) { - return; // need more data +static void WriteSimBackend() { + MRC_Joysticks joysticks; + MRC_ControlData controlData; + + int32_t status = + MRC_DsComms_GetControlDataWithJoysticks(&controlData, &joysticks); + if (status != 0) { + return; + } + + HALSIM_SetDriverStationOpMode(controlData.currentOpMode); + HALSIM_SetDriverStationAllianceStationId(static_cast( + mrclib::GetAlliance(controlData.controlFlags) + 1)); + HALSIM_SetDriverStationMatchTime(controlData.matchTime); + HALSIM_SetDriverStationRobotMode(static_cast( + mrclib::GetRobotMode(controlData.controlFlags))); + HALSIM_SetDriverStationDsAttached( + mrclib::GetDsConnected(controlData.controlFlags)); + HALSIM_SetDriverStationFmsAttached( + mrclib::GetFmsConnected(controlData.controlFlags)); + HALSIM_SetDriverStationEnabled(mrclib::GetEnabled(controlData.controlFlags)); + HALSIM_SetDriverStationEStop(mrclib::GetEStop(controlData.controlFlags)); + + HAL_GameData gameData; + auto gameDataSize = controlData.gameDataLength; + if (gameDataSize > MRCLIB_MAX_GAMEDATA_LENGTH) { + gameDataSize = MRCLIB_MAX_GAMEDATA_LENGTH; + } + std::memcpy(gameData.gameData, controlData.gameData, gameDataSize); + gameData.gameData[gameDataSize] = '\0'; + HALSIM_SetGameData(&gameData); + + for (int i = 0; i < MRCLIB_MAX_JOYSTICKS; ++i) { + HAL_JoystickAxes axes{}; + HAL_JoystickPOVs povs{}; + HAL_JoystickButtons buttons{}; + HAL_JoystickTouchpads touchpads{}; + + if (i < joysticks.count) { + auto& joystick = joysticks.joysticks[i]; + + axes.available = joystick.availableAxes; + for (size_t j = 0; j < MRCLIB_MAX_AXES; ++j) { + auto raw = joystick.axes[j]; + axes.raw[j] = raw; + if (raw < 0) { + axes.axes[j] = raw / 32768.0f; + } else { + axes.axes[j] = raw / 32767.0f; } } - store.m_frameSize = (static_cast(store.m_frame[0]) << 8) | - static_cast(store.m_frame[1]); - } - if (store.m_frameSize != (std::numeric_limits::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::max)(); + + povs.available = joystick.availablePovs; + for (size_t j = 0; j < MRCLIB_MAX_POVS; ++j) { + povs.povs[j] = static_cast(joystick.povs[j]); + } + + buttons.available = joystick.availableButtons; + buttons.buttons = joystick.buttons; + + touchpads.count = joystick.touchpadCount; + for (size_t j = 0; j < MRCLIB_MAX_TOUCHPADS; ++j) { + touchpads.touchpads[j].count = joystick.touchpads[j].count; + for (size_t k = 0; k < MRCLIB_MAX_TOUCHPAD_FINGERS; ++k) { + auto& finger = joystick.touchpads[j].fingers[k]; + touchpads.touchpads[j].fingers[k].down = finger.down ? 1 : 0; + touchpads.touchpads[j].fingers[k].x = finger.x / 65535.0f; + touchpads.touchpads[j].fingers[k].y = finger.y / 65535.0f; + } } } + + HALSIM_SetJoystickAxes(i, &axes); + HALSIM_SetJoystickPOVs(i, &povs); + HALSIM_SetJoystickButtons(i, &buttons); + HALSIM_SetJoystickTouchpads(i, &touchpads); } -} -static void SetupTcp(wpi::net::uv::Loop& loop) { - auto tcp = Tcp::Create(loop); - auto tcpWaitTimer = Timer::Create(loop); + MRC_JoystickDescriptors descriptors; + status = MRC_DsComms_GetJoystickDescriptors(&descriptors); + if (status == 0) { + for (int i = 0; i < MRCLIB_MAX_JOYSTICKS; ++i) { + HAL_JoystickDescriptor descriptor{}; - auto recStore = std::make_shared(); - recStore->dsPacket = loop.GetData().get(); + if (i < descriptors.count) { + auto& mrcDescriptor = descriptors.descriptors[i]; - tcp->SetData(recStore); + descriptor.isGamepad = mrcDescriptor.isGamepad ? 1 : 0; + descriptor.gamepadType = mrcDescriptor.gamepadType; + descriptor.supportedOutputs = mrcDescriptor.supportedOutputs; - tcp->Bind("0.0.0.0", 1740); - - tcp->Listen([t = tcp.get()] { - auto client = t->Accept(); - gDSConnected = true; - wpi::hal::EnableDashboardOpMode(); - - client->data.connect([t](Buffer& buf, size_t len) { - HandleTcpDataStream(buf, len, *t->GetData()); - }); - client->StartRead(); - client->end.connect([c = client.get()] { - c->Close(); - gDSConnected = false; - }); - }); -} - -static void SetupUdp(wpi::net::uv::Loop& loop) { - auto udp = wpi::net::uv::Udp::Create(loop); - udp->Bind("0.0.0.0", 1110); - - // Simulation mode packet - auto simLoopTimer = Timer::Create(loop); - struct sockaddr_in simAddr; - NameToAddr("127.0.0.1", 1135, &simAddr); - simLoopTimer->timeout.connect([udpLocal = udp.get(), simAddr] { - udpLocal->Send(simAddr, {singleByte.get(), 1}, [](auto buf, Error err) { - if (err) { - wpi::util::print(stderr, "{}\n", err.str()); - std::fflush(stderr); + auto nameLength = std::min(mrcDescriptor.nameLength, + sizeof(descriptor.name) - 1); + std::memcpy(descriptor.name, mrcDescriptor.name, nameLength); + descriptor.name[nameLength] = '\0'; } - }); - }); - simLoopTimer->Start(Timer::Time{100}, Timer::Time{100}); - // DS Timeout - int timeoutMs = 100; - if (auto envTimeout = std::getenv("DS_TIMEOUT_MS")) { - try { - timeoutMs = std::stoi(envTimeout); - } catch (const std::exception& e) { - wpi::util::print(stderr, "Error parsing DS_TIMEOUT_MS: {}\n", e.what()); + + HALSIM_SetJoystickDescriptor(i, &descriptor); } } - auto autoDisableTimer = Timer::Create(loop); - autoDisableTimer->timeout.connect([] { HALSIM_SetDriverStationEnabled(0); }); - // UDP Receive then send - udp->received.connect( - [udpLocal = udp.get(), autoDisableTimer, timeoutMs]( - Buffer& buf, size_t len, const sockaddr& recSock, unsigned int port) { - autoDisableTimer->Start(Timer::Time(timeoutMs)); - auto ds = udpLocal->GetLoop()->GetData(); - ds->DecodeUDP({reinterpret_cast(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 sendBufs; - wpi::net::raw_uv_ostream stream{ - sendBufs, [] { return GetBufferPool().Allocate(); }}; - ds->SetupSendBuffer(stream); - - udpLocal->Send(outAddr, sendBufs, [](auto bufs, Error err) { - GetBufferPool().Release(bufs); - if (err) { - wpi::util::print(stderr, "{}\n", err.str()); - std::fflush(stderr); - } - }); - ds->SendUDPToHALSim(); - }); - - udp->StartRecv(); + HALSIM_NotifyDriverStationNewData(); } -static void SetupEventLoop(wpi::net::uv::Loop& loop) { - auto loopData = std::make_shared(); - loop.SetData(loopData); - SetupUdp(loop); - SetupTcp(loop); -} - -static std::unique_ptr eventLoopRunner; - -void ThisIsAHackDontCallThis() { - eventLoopRunner = std::make_unique(); - eventLoopRunner->ExecAsync(SetupEventLoop); +static void SetupBackend() { + static wpi::util::SafeThreadOwner thread; + thread.Start(); } /*---------------------------------------------------------------------------- @@ -206,7 +196,22 @@ int HALSIM_InitExtension(void) { HAL_RegisterExtension("ds_socket", &gDSConnected); - singleByte = std::make_unique("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; diff --git a/simulation/halsim_ds_socket/src/main/native/include/wpi/halsim/ds_socket/DSCommJoystickPacket.hpp b/simulation/halsim_ds_socket/src/main/native/include/wpi/halsim/ds_socket/DSCommJoystickPacket.hpp deleted file mode 100644 index f1177b081e..0000000000 --- a/simulation/halsim_ds_socket/src/main/native/include/wpi/halsim/ds_socket/DSCommJoystickPacket.hpp +++ /dev/null @@ -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 - -#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 diff --git a/simulation/halsim_ds_socket/src/main/native/include/wpi/halsim/ds_socket/DSCommPacket.hpp b/simulation/halsim_ds_socket/src/main/native/include/wpi/halsim/ds_socket/DSCommPacket.hpp deleted file mode 100644 index 16e55860ce..0000000000 --- a/simulation/halsim_ds_socket/src/main/native/include/wpi/halsim/ds_socket/DSCommPacket.hpp +++ /dev/null @@ -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 -#include - -#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 packet); - void DecodeUDP(std::span 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 tagData); - void ReadJoystickTag(std::span data, int index); - void ReadNewMatchInfoTag(std::span data); - void ReadJoystickDescriptionTag(std::span 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 m_joystick_packets; - double m_match_time = -1; -}; - -} // namespace halsim diff --git a/simulation/halsim_ds_socket/src/main/native/include/wpi/halsim/ds_socket/DsSocket.h b/simulation/halsim_ds_socket/src/main/native/include/wpi/halsim/ds_socket/DsSocket.h new file mode 100644 index 0000000000..2ed74d5bb9 --- /dev/null +++ b/simulation/halsim_ds_socket/src/main/native/include/wpi/halsim/ds_socket/DsSocket.h @@ -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. diff --git a/simulation/halsim_ds_socket/src/test/native/cpp/DSCommPacketTest.cpp b/simulation/halsim_ds_socket/src/test/native/cpp/DSCommPacketTest.cpp deleted file mode 100644 index e3f62f224e..0000000000 --- a/simulation/halsim_ds_socket/src/test/native/cpp/DSCommPacketTest.cpp +++ /dev/null @@ -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 - -class DSCommPacketTest : public ::testing::Test { - public: - DSCommPacketTest() = default; - - void SendJoysticks() { commPacket.SendJoysticks(); } - - halsim::DSCommJoystickPacket& ReadJoystickTag(std::span data, - int index) { - commPacket.ReadJoystickTag(data, index); - return commPacket.m_joystick_packets[index]; - } - - halsim::DSCommJoystickPacket& ReadDescriptorTag( - std::span data) { - commPacket.ReadJoystickDescriptionTag(data); - return commPacket.m_joystick_packets[data[3]]; - } - - HAL_MatchInfo& ReadNewMatchInfoTag(std::span 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 _buttons{{0, 1, 0, 0, 1, 1, 0, 1, 0, 1, 0, 1}}; - - std::array _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(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); -} diff --git a/simulation/halsim_gui/BUILD.bazel b/simulation/halsim_gui/BUILD.bazel index 0f530fd648..23f6bc0fa5 100644 --- a/simulation/halsim_gui/BUILD.bazel +++ b/simulation/halsim_gui/BUILD.bazel @@ -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", diff --git a/wpilibc/BUILD.bazel b/wpilibc/BUILD.bazel index 3314831b9b..781c754535 100644 --- a/wpilibc/BUILD.bazel +++ b/wpilibc/BUILD.bazel @@ -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", diff --git a/wpilibc/src/main/native/cpp/simulation/DriverStationSim.cpp b/wpilibc/src/main/native/cpp/simulation/DriverStationSim.cpp index 7cc6ca06cd..ef82affe00 100644 --- a/wpilibc/src/main/native/cpp/simulation/DriverStationSim.cpp +++ b/wpilibc/src/main/native/cpp/simulation/DriverStationSim.cpp @@ -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; }); } } diff --git a/wpilibc/src/main/native/cpp/system/Errors.cpp b/wpilibc/src/main/native/cpp/system/Errors.cpp index a98ac846d1..d092d2b3b6 100644 --- a/wpilibc/src/main/native/cpp/system/Errors.cpp +++ b/wpilibc/src/main/native/cpp/system/Errors.cpp @@ -7,7 +7,7 @@ #include #include -#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,8 +63,8 @@ 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::util::GetStackTrace(2).c_str(), 1); + wpi::hal::SendError(status < 0, status, out.data(), funcName, + wpi::util::GetStackTrace(2).c_str(), 1); } RuntimeError wpi::MakeErrorV(int32_t status, const char* fileName, diff --git a/wpilibc/src/main/native/cpp/system/Notifier.cpp b/wpilibc/src/main/native/cpp/system/Notifier.cpp index 5a7fa4f01c..3b3e27a0bd 100644 --- a/wpilibc/src/main/native/cpp/system/Notifier.cpp +++ b/wpilibc/src/main/native/cpp/system/Notifier.cpp @@ -6,7 +6,7 @@ #include -#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 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; } } diff --git a/wpilibc/src/main/native/include/wpi/framework/RobotBase.hpp b/wpilibc/src/main/native/include/wpi/framework/RobotBase.hpp index ee1db0fcd7..cf7bfb72a6 100644 --- a/wpilibc/src/main/native/include/wpi/framework/RobotBase.hpp +++ b/wpilibc/src/main/native/include/wpi/framework/RobotBase.hpp @@ -8,7 +8,7 @@ #include #include -#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; } } diff --git a/wpilibc/src/main/python/wpilib/_impl/report_error.py b/wpilibc/src/main/python/wpilib/_impl/report_error.py index f9e447dcec..218e54c4ec 100644 --- a/wpilibc/src/main/python/wpilib/_impl/report_error.py +++ b/wpilibc/src/main/python/wpilib/_impl/report_error.py @@ -61,7 +61,6 @@ def reportErrorInternal( hal.sendError( not isWarning, code, - False, error, locString, traceString, diff --git a/wpilibj/src/main/java/org/wpilib/driverstation/DriverStationErrors.java b/wpilibj/src/main/java/org/wpilib/driverstation/DriverStationErrors.java index 2ea24ce1ad..b4db8ce917 100644 --- a/wpilibj/src/main/java/org/wpilib/driverstation/DriverStationErrors.java +++ b/wpilibj/src/main/java/org/wpilib/driverstation/DriverStationErrors.java @@ -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); + } } diff --git a/wpilibj/src/main/java/org/wpilib/driverstation/internal/DriverStationBackend.java b/wpilibj/src/main/java/org/wpilib/driverstation/internal/DriverStationBackend.java index 7e7d39e117..6088e505d6 100644 --- a/wpilibj/src/main/java/org/wpilib/driverstation/internal/DriverStationBackend.java +++ b/wpilibj/src/main/java/org/wpilib/driverstation/internal/DriverStationBackend.java @@ -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(); + } } /** diff --git a/wpilibj/src/main/java/org/wpilib/framework/RobotBase.java b/wpilibj/src/main/java/org/wpilib/framework/RobotBase.java index bf890ce2a7..8426477f6d 100644 --- a/wpilibj/src/main/java/org/wpilib/framework/RobotBase.java +++ b/wpilibj/src/main/java/org/wpilib/framework/RobotBase.java @@ -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(); diff --git a/xrpVendordep/BUILD.bazel b/xrpVendordep/BUILD.bazel index 02aee050aa..08f3580822 100644 --- a/xrpVendordep/BUILD.bazel +++ b/xrpVendordep/BUILD.bazel @@ -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",