diff --git a/.vscode/c_cpp_properties.json b/.vscode/c_cpp_properties.json index 168cc9a..406176d 100644 --- a/.vscode/c_cpp_properties.json +++ b/.vscode/c_cpp_properties.json @@ -10,12 +10,10 @@ "UNICODE", "_UNICODE" ], - "windowsSdkVersion": "10.0.26100.0", - "compilerPath": "cl.exe", "cStandard": "c17", "cppStandard": "c++17", - "intelliSenseMode": "windows-msvc-x64", - "compileCommands": "${workspaceFolder}/build/compile_commands.json" + "intelliSenseMode": "linux-gcc-arm64", + "compileCommands": "${workspaceFolder}/buildmrc/compile_commands.json" } ], "version": 4 diff --git a/.vscode/settings.json b/.vscode/settings.json new file mode 100644 index 0000000..57bd4f7 --- /dev/null +++ b/.vscode/settings.json @@ -0,0 +1,104 @@ +{ + "files.associations": { + "filesystem": "cpp", + "xlocale": "cpp", + "algorithm": "cpp", + "any": "cpp", + "array": "cpp", + "atomic": "cpp", + "bit": "cpp", + "bitset": "cpp", + "cctype": "cpp", + "charconv": "cpp", + "chrono": "cpp", + "cinttypes": "cpp", + "clocale": "cpp", + "cmath": "cpp", + "compare": "cpp", + "complex": "cpp", + "concepts": "cpp", + "condition_variable": "cpp", + "coroutine": "cpp", + "csignal": "cpp", + "cstdarg": "cpp", + "cstddef": "cpp", + "cstdint": "cpp", + "cstdio": "cpp", + "cstdlib": "cpp", + "cstring": "cpp", + "ctime": "cpp", + "cwchar": "cpp", + "cwctype": "cpp", + "deque": "cpp", + "exception": "cpp", + "expected": "cpp", + "format": "cpp", + "forward_list": "cpp", + "fstream": "cpp", + "functional": "cpp", + "future": "cpp", + "initializer_list": "cpp", + "iomanip": "cpp", + "ios": "cpp", + "iosfwd": "cpp", + "iostream": "cpp", + "istream": "cpp", + "iterator": "cpp", + "limits": "cpp", + "list": "cpp", + "locale": "cpp", + "map": "cpp", + "memory": "cpp", + "memory_resource": "cpp", + "mutex": "cpp", + "new": "cpp", + "numbers": "cpp", + "numeric": "cpp", + "optional": "cpp", + "ostream": "cpp", + "queue": "cpp", + "random": "cpp", + "ranges": "cpp", + "ratio": "cpp", + "regex": "cpp", + "scoped_allocator": "cpp", + "semaphore": "cpp", + "set": "cpp", + "shared_mutex": "cpp", + "source_location": "cpp", + "span": "cpp", + "sstream": "cpp", + "stack": "cpp", + "stdexcept": "cpp", + "stop_token": "cpp", + "streambuf": "cpp", + "string": "cpp", + "system_error": "cpp", + "thread": "cpp", + "tuple": "cpp", + "type_traits": "cpp", + "typeinfo": "cpp", + "unordered_map": "cpp", + "unordered_set": "cpp", + "utility": "cpp", + "valarray": "cpp", + "variant": "cpp", + "vector": "cpp", + "xfacet": "cpp", + "xhash": "cpp", + "xiosbase": "cpp", + "xlocbuf": "cpp", + "xlocinfo": "cpp", + "xlocmes": "cpp", + "xlocmon": "cpp", + "xlocnum": "cpp", + "xloctime": "cpp", + "xmemory": "cpp", + "xstring": "cpp", + "xtr1common": "cpp", + "xtree": "cpp", + "xutility": "cpp", + "*.tcc": "cpp", + "string_view": "cpp" + } +} diff --git a/CMakeLists.txt b/CMakeLists.txt index ed94b69..c1d41d3 100644 --- a/CMakeLists.txt +++ b/CMakeLists.txt @@ -91,3 +91,24 @@ include(Catch) add_subdirectory(common) add_subdirectory(radio) + +if (MRC_BUILD) +add_subdirectory(powerdistribution) +endif() + +if (MRC_BUILD) +install( + TARGETS RadioDaemon PowerDistributionDaemon + RUNTIME DESTINATION bin + LIBRARY DESTINATION lib + ARCHIVE DESTINATION lib +) + +else() +install( + TARGETS RadioDaemon + RUNTIME DESTINATION bin + LIBRARY DESTINATION lib + ARCHIVE DESTINATION lib +) +endif() diff --git a/powerdistribution/CMakeLists.txt b/powerdistribution/CMakeLists.txt new file mode 100644 index 0000000..d5466f2 --- /dev/null +++ b/powerdistribution/CMakeLists.txt @@ -0,0 +1,10 @@ +project(PowerDistributionDaemon) + +add_executable(PowerDistributionDaemon main.cpp) +target_compile_features(PowerDistributionDaemon PRIVATE cxx_std_20) +wpilib_target_warnings(PowerDistributionDaemon) +target_link_libraries(PowerDistributionDaemon PRIVATE Common ntcore wpinet wpiutil) + +if (MRC_BUILD) +target_compile_definitions(PowerDistributionDaemon PRIVATE MRC_DAEMON_BUILD) +endif() diff --git a/powerdistribution/main.cpp b/powerdistribution/main.cpp new file mode 100644 index 0000000..9365da8 --- /dev/null +++ b/powerdistribution/main.cpp @@ -0,0 +1,214 @@ +#if defined(__linux__) && defined(MRC_DAEMON_BUILD) +#include +#endif +#include + +#include "version.h" + +#include +#include +#include +#include + +#include +#include + +#include "networktables/NetworkTableInstance.h" +#include "networktables/RawTopic.h" +#include "networktables/IntegerTopic.h" + +struct CanState { + int socketHandle{-1}; + nt::IntegerPublisher deviceIdPublisher; + std::array framePublishers; + + ~CanState() { + if (socketHandle != -1) { + close(socketHandle); + } + } + + void handleCanFrame(const canfd_frame& frame); + bool startUvLoop(wpi::uv::Loop& loop); +}; + +static CanState* canState; + +void CanState::handleCanFrame(const canfd_frame& frame) { + // Can't support FD frames + if (frame.flags & CANFD_FDF) { + return; + } + + uint16_t apiId = (frame.can_id >> 6) & 0x3FF; + + int frameNum = 0; + uint32_t deviceId = frame.can_id & 0x1FFF003F; + + if (frame.can_id & 0x10000) { + // Rev Frame + if (apiId < 0x60 || apiId > 0x63) { + // Not valid + return; + } + + frameNum = apiId - 0x60; + } else { + // CTRE frame + + if (apiId == 0x5D) { + // Special case + frameNum = 3; + } else { + if (apiId < 0x50 || apiId > 0x52) { + // Not valid + return; + } + + frameNum = apiId - 0x50; + } + } + + deviceIdPublisher.Set(deviceId); + + std::span frameSpan = { + reinterpret_cast(frame.data), frame.len}; + + if (frameNum < 0 || frameNum >= static_cast(framePublishers.size())) { + printf("BUGBUG logic error invalid frame number\n"); + return; + } + + framePublishers[frameNum].Set(frameSpan); +} + +bool CanState::startUvLoop(wpi::uv::Loop& loop) { + socketHandle = + socket(PF_CAN, SOCK_RAW | SOCK_NONBLOCK | SOCK_CLOEXEC, CAN_RAW); + + if (socketHandle == -1) { + return false; + } + + // Filter to PD device type. + // Both mfg types have the "4" bit set. They just + // differ on the 1 bit. So a single filter can be used, + // ignoring that bit. + struct can_filter filter { + .can_id = 0x08040000 | CAN_EFF_FLAG, + .can_mask = 0x1FFE0000 | 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), "can0"); + + 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, fd = socketHandle](int mask) { + if (mask & UV_READABLE) { + canfd_frame frame; + int rVal = read(fd, &frame, sizeof(frame)); + + if (rVal != CAN_MTU && rVal != CANFD_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; + } + + if (rVal == CANFD_MTU) { + frame.flags = CANFD_FDF; + } + + handleCanFrame(frame); + } + }); + + poll->Start(UV_READABLE); + + return true; +} + +int main() { + printf("Starting PowerDistributionDaemon\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 + + CanState state; + canState = &state; + + auto ntInst = nt::NetworkTableInstance::Create(); + ntInst.SetServer({"localhost"}, 6810); + ntInst.StartClient4("PowerDistributionDaemon"); + + nt::PubSubOptions options; + options.sendAll = true; + options.keepDuplicates = true; + options.periodic = 0.005; + + for (size_t i = 0; i < state.framePublishers.size(); i++) { + auto iStr = std::to_string(i); + state.framePublishers[i] = + ntInst.GetRawTopic("/pd/frame" + iStr).Publish("pd", options); + } + + state.deviceIdPublisher = ntInst.GetIntegerTopic("/pd/deviceid").Publish(); + + wpi::EventLoopRunner loopRunner; + + bool success = false; + loopRunner.ExecSync([&success](wpi::uv::Loop& loop) { + success = canState->startUvLoop(loop); + }); + + 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/radio/main.cpp b/radio/main.cpp index 1b27df4..bb4c79c 100644 --- a/radio/main.cpp +++ b/radio/main.cpp @@ -9,15 +9,19 @@ #include #include #include "wpinet/HttpUtil.h" +#include "wpinet/ParallelTcpConnector.h" +#include "wpi/Logger.h" #include "networktables/NetworkTableInstance.h" #include "networktables/StringTopic.h" #include "wpi/StringExtras.h" struct DataStorage { + wpi::Logger logger; nt::StringSubscriber teamSubscriber; - std::shared_ptr tcpConn; + std::shared_ptr tcpConnector; + std::weak_ptr tcpConn; std::optional oldTeamNumber; }; @@ -71,31 +75,27 @@ int main() { return 0; } -static std::optional checkTeamNumber(DataStorage& instData) { - auto teamStr = instData.teamSubscriber.Get(); - auto maybeTeam = wpi::parse_integer(teamStr, 10); - if (!maybeTeam.has_value()) { - printf("failed to parse team number\n"); - return {}; - } +// static std::optional checkTeamNumber(DataStorage& instData) { +// auto teamStr = instData.teamSubscriber.Get(); +// auto maybeTeam = wpi::parse_integer(teamStr, 10); +// if (!maybeTeam.has_value()) { +// printf("failed to parse team number\n"); +// return {}; +// } - auto team = *maybeTeam; +// auto team = *maybeTeam; - if (team > 25599) { - printf("Team number too large\n"); - return {}; - } +// if (team > 25599) { +// printf("Team number too large\n"); +// return {}; +// } - return team; -} +// return team; +// } -static void handleNewConnection(wpi::uv::Loop& loop, DataStorage& instData) { - auto tcp = wpi::uv::Tcp::Create(loop, 0); - - tcp->error.connect -} +// static void tryRequest(wpi::uv::Loop& loop, DataStorage& instData) { -static void handleTeamUpdate(wpi::uv::Loop& loop, DataStorage& instData) {} +// } static bool startUvLoop(wpi::uv::Loop& loop, DataStorage& instData) { auto timer = wpi::uv::Timer::Create(loop); @@ -103,43 +103,52 @@ static bool startUvLoop(wpi::uv::Loop& loop, DataStorage& instData) { return false; } - timer->timeout.connect([&loop, &instData] { - auto maybeTeam = checkTeamNumber(instData); - if (!maybeTeam.has_value()) { - instData.oldTeamNumber = maybeTeam; - return; - } + // instData.tcpConnector = wpi::ParallelTcpConnector::Create( + // loop, wpi::uv::Timer::Time{2000}, instData.logger, + // [](wpi::uv::Tcp& tcp) { + // }, + // true); - if (maybeTeam != instData.oldTeamNumber) { - // New team number. - instData.oldTeamNumber = maybeTeam; - // Restart TCP if it exists. + // if (!instData.tcpConnector) { + // return false; + // } - if (instData.tcpConn) { - instData.tcpConn->Shutdown(); - instData.tcpConn->Close(); - instData.tcpConn = nullptr; - } + // timer->timeout.connect([&loop, &instData] { + // auto maybeTeam = checkTeamNumber(instData); + // if (!maybeTeam.has_value()) { + // instData.oldTeamNumber = maybeTeam; + // return; + // } - handleNewConnection(loop, instData); - return; - } + // if (maybeTeam != instData.oldTeamNumber) { + // // New team number. + // instData.oldTeamNumber = maybeTeam; + // // Restart TCP if it exists. - handleTeamUpdate(loop, instData); + // std::array, 1> servers; + // servers[0] = {"localhost", 80}; - // fmt::format("10.{}.{}.1", static_cast(team / 100), - // static_cast(team % 100)); + // instData.tcpConnector->SetServers(servers); + // instData.tcpConnector->Disconnected(); - // auto tcp = wpi::uv::Tcp::Create(loop, 0); - // if (!tcp) { - // printf("Failed to allocate tcp\n"); - // return; - // } + // return; + // } - // tcp->Reuse - }); + // tryRequest(loop, instData); + + // // fmt::format("10.{}.{}.1", static_cast(team / 100), + // // static_cast(team % 100)); + + // // auto tcp = wpi::uv::Tcp::Create(loop, 0); + // // if (!tcp) { + // // printf("Failed to allocate tcp\n"); + // // return; + // // } + + // // tcp->Reuse + // }); - timer->Start(wpi::uv::Timer::Time{100}, wpi::uv::Timer::Time{3000}); + // timer->Start(wpi::uv::Timer::Time{100}, wpi::uv::Timer::Time{3000}); return true; }