From 8e16c0474b635ea274abf885f58e44194b7aefe9 Mon Sep 17 00:00:00 2001 From: Thad House Date: Tue, 15 Apr 2025 20:50:55 -0700 Subject: [PATCH] Add ability to detect crossed CAN Streams --- .vscode/settings.json | 3 +- CMakeLists.txt | 3 +- crossstreamdetect/CMakeLists.txt | 10 ++ crossstreamdetect/main.cpp | 198 +++++++++++++++++++++++++++++++ powerdistribution/main.cpp | 2 +- 5 files changed, 213 insertions(+), 3 deletions(-) create mode 100644 crossstreamdetect/CMakeLists.txt create mode 100644 crossstreamdetect/main.cpp diff --git a/.vscode/settings.json b/.vscode/settings.json index e7ed16c..6e07029 100644 --- a/.vscode/settings.json +++ b/.vscode/settings.json @@ -100,6 +100,7 @@ "xtree": "cpp", "xutility": "cpp", "*.tcc": "cpp", - "string_view": "cpp" + "string_view": "cpp", + "codecvt": "cpp" } } diff --git a/CMakeLists.txt b/CMakeLists.txt index c1d41d3..707f66a 100644 --- a/CMakeLists.txt +++ b/CMakeLists.txt @@ -94,11 +94,12 @@ add_subdirectory(radio) if (MRC_BUILD) add_subdirectory(powerdistribution) +add_subdirectory(crossstreamdetect) endif() if (MRC_BUILD) install( - TARGETS RadioDaemon PowerDistributionDaemon + TARGETS RadioDaemon PowerDistributionDaemon CanCrossStreamDetectorDaemon RUNTIME DESTINATION bin LIBRARY DESTINATION lib ARCHIVE DESTINATION lib diff --git a/crossstreamdetect/CMakeLists.txt b/crossstreamdetect/CMakeLists.txt new file mode 100644 index 0000000..e126852 --- /dev/null +++ b/crossstreamdetect/CMakeLists.txt @@ -0,0 +1,10 @@ +project(CanCrossStreamDetectorDaemon) + +add_executable(CanCrossStreamDetectorDaemon main.cpp) +target_compile_features(CanCrossStreamDetectorDaemon PRIVATE cxx_std_20) +wpilib_target_warnings(CanCrossStreamDetectorDaemon) +target_link_libraries(CanCrossStreamDetectorDaemon PRIVATE Common ntcore wpinet wpiutil) + +if (MRC_BUILD) +target_compile_definitions(CanCrossStreamDetectorDaemon PRIVATE MRC_DAEMON_BUILD) +endif() diff --git a/crossstreamdetect/main.cpp b/crossstreamdetect/main.cpp new file mode 100644 index 0000000..c15b965 --- /dev/null +++ b/crossstreamdetect/main.cpp @@ -0,0 +1,198 @@ +#if defined(__linux__) && defined(MRC_DAEMON_BUILD) +#include +#endif +#include + +#include "version.h" + +#include +#include +#include +#include + +#include +#include +#include + +#include "networktables/NetworkTableInstance.h" +#include "networktables/IntegerTopic.h" + +#define NUM_CAN_BUSES 5 + +// Robot Controller, NI, 1023 API Id +static constexpr uint32_t crossMessageId = 0x101FFF8; +static constexpr uint32_t crossMessageMask = 0x1FFFFFF8; + +struct CanState { + int socketHandle{-1}; + nt::IntegerPublisher crossDetectPublisher; + unsigned busId{0}; + uint32_t crossedBusses = 0; + + ~CanState() { + if (socketHandle != -1) { + close(socketHandle); + } + } + + void handleCanFrame(const can_frame& frame); + bool startUvLoop(unsigned bus, const nt::NetworkTableInstance& ntInst, + wpi::uv::Loop& loop); +}; + +void CanState::handleCanFrame(const can_frame& frame) { + uint32_t deviceId = (frame.can_id & ~crossMessageId) & 0x7; + + crossedBusses |= (1 << deviceId); + + crossDetectPublisher.Set(crossedBusses); +} + +bool CanState::startUvLoop(unsigned bus, const nt::NetworkTableInstance& ntInst, + wpi::uv::Loop& loop) { + if (bus >= NUM_CAN_BUSES) { + return false; + } + + busId = bus; + + auto busIdStr = std::to_string(busId); + + crossDetectPublisher = + ntInst.GetIntegerTopic("/cancross/" + busIdStr).Publish(); + crossDetectPublisher.Set(crossedBusses); + + socketHandle = + socket(PF_CAN, SOCK_RAW | SOCK_NONBLOCK | SOCK_CLOEXEC, CAN_RAW); + + if (socketHandle == -1) { + return false; + } + + struct can_filter filter{ + .can_id = crossMessageId | CAN_EFF_FLAG, + .can_mask = crossMessageMask | CAN_EFF_FLAG, + }; + + if (setsockopt(socketHandle, SOL_CAN_RAW, CAN_RAW_FILTER, &filter, + sizeof(filter)) == -1) { + return false; + } + + ifreq ifr; + std::snprintf(ifr.ifr_name, sizeof(ifr.ifr_name), "can%u", busId); + + if (ioctl(socketHandle, SIOCGIFINDEX, &ifr) == -1) { + return false; + } + + sockaddr_can addr; + std::memset(&addr, 0, sizeof(addr)); + addr.can_family = AF_CAN; + addr.can_ifindex = ifr.ifr_ifindex; + + if (bind(socketHandle, reinterpret_cast(&addr), + sizeof(addr)) == -1) { + return false; + } + + auto poll = wpi::uv::Poll::Create(loop, socketHandle); + if (!poll) { + return false; + } + + poll->pollEvent.connect([this](int mask) { + if (mask & UV_READABLE) { + can_frame frame; + int rVal = read(socketHandle, &frame, sizeof(frame)); + + if (rVal != CAN_MTU) { + // TODO Error handling, do we need to reopen the socket? + return; + } + + if (frame.can_id & CAN_ERR_FLAG) { + // Do nothing if this is an error frame + return; + } + + handleCanFrame(frame); + } + }); + + auto timer = wpi::uv::Timer::Create(loop); + if (!timer) { + return false; + } + + timer->timeout.connect([this]() { + // If we didn't have a cross last iteration, write the value. + if (crossedBusses == 0) { + crossDetectPublisher.Set(crossedBusses); + } + crossedBusses = 0; + + can_frame frame; + std::memset(&frame, 0, sizeof(frame)); + frame.can_id = crossMessageId | busId | CAN_EFF_FLAG; + write(socketHandle, &frame, sizeof(frame)); + // TODO do we need to error here? + }); + + poll->Start(UV_READABLE); + + // Write every 5 seconds. + timer->Start(wpi::uv::Timer::Time{100}, wpi::uv::Timer::Time{5000}); + + return true; +} + +int main() { + printf("Starting CanCrossStreamDetectorDaemon\n"); + printf("\tBuild Hash: %s\n", MRC_GetGitHash()); + printf("\tBuild Timestamp: %s\n", MRC_GetBuildTimestamp()); + +#if defined(__linux__) && defined(MRC_DAEMON_BUILD) + sigset_t signal_set; + sigemptyset(&signal_set); + sigaddset(&signal_set, SIGTERM); + sigaddset(&signal_set, SIGINT); + sigprocmask(SIG_BLOCK, &signal_set, nullptr); +#endif + + std::array states; + + auto ntInst = nt::NetworkTableInstance::Create(); + ntInst.SetServer({"localhost"}, 6810); + ntInst.StartClient("CanCrossStreamDetectorDaemon"); + + wpi::EventLoopRunner loopRunner; + + bool success = false; + loopRunner.ExecSync([&success, &states, &ntInst](wpi::uv::Loop& loop) { + for (size_t i = 0; i < states.size(); i++) { + success = states[i].startUvLoop(i, ntInst, loop); + if (!success) { + return; + } + } + }); + + if (!success) { + loopRunner.Stop(); + return -1; + } + + { +#if defined(__linux__) && defined(MRC_DAEMON_BUILD) + int sig = 0; + sigwait(&signal_set, &sig); +#else + (void)getchar(); +#endif + } + ntInst.StopClient(); + nt::NetworkTableInstance::Destroy(ntInst); + + return 0; +} diff --git a/powerdistribution/main.cpp b/powerdistribution/main.cpp index db830ab..c86466f 100644 --- a/powerdistribution/main.cpp +++ b/powerdistribution/main.cpp @@ -17,7 +17,7 @@ #include "networktables/RawTopic.h" #include "networktables/IntegerTopic.h" -#define NUM_CAN_BUSES 2 +#define NUM_CAN_BUSES 5 static constexpr uint32_t deviceTypeMask = 0x3F000000; static constexpr uint32_t powerDistributionFilter = 0x08000000;