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 af84632935..e602ecc08b 100644 --- a/simulation/halsim_ds_socket/src/main/native/cpp/main.cpp +++ b/simulation/halsim_ds_socket/src/main/native/cpp/main.cpp @@ -16,6 +16,7 @@ #include #include #include +#include #include #include @@ -124,33 +125,45 @@ static void SetupUdp(wpi::uv::Loop& loop) { }); }); 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) { + fmt::print(stderr, "Error parsing DS_TIMEOUT_MS: {}\n", e.what()); + } + } + auto autoDisableTimer = Timer::Create(loop); + autoDisableTimer->timeout.connect([] { HALSIM_SetDriverStationEnabled(0); }); // UDP Receive then send - udp->received.connect([udpLocal = udp.get()](Buffer& buf, size_t len, - const sockaddr& recSock, - unsigned int port) { - auto ds = udpLocal->GetLoop()->GetData(); - ds->DecodeUDP({reinterpret_cast(buf.base), len}); + 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); + struct sockaddr_in outAddr; + std::memcpy(&outAddr, &recSock, sizeof(sockaddr_in)); + outAddr.sin_family = PF_INET; + outAddr.sin_port = htons(1150); - wpi::SmallVector sendBufs; - wpi::raw_uv_ostream stream{sendBufs, - [] { return GetBufferPool().Allocate(); }}; - ds->SetupSendBuffer(stream); + wpi::SmallVector sendBufs; + wpi::raw_uv_ostream stream{sendBufs, + [] { return GetBufferPool().Allocate(); }}; + ds->SetupSendBuffer(stream); - udpLocal->Send(outAddr, sendBufs, [](auto bufs, Error err) { - GetBufferPool().Release(bufs); - if (err) { - fmt::print(stderr, "{}\n", err.str()); - std::fflush(stderr); - } - }); - ds->SendUDPToHALSim(); - }); + udpLocal->Send(outAddr, sendBufs, [](auto bufs, Error err) { + GetBufferPool().Release(bufs); + if (err) { + fmt::print(stderr, "{}\n", err.str()); + std::fflush(stderr); + } + }); + ds->SendUDPToHALSim(); + }); udp->StartRecv(); }