Team2890/allwpilib
4
0
Fork 0
mirror of https://github.com/wpilibsuite/allwpilib synced 2026-06-25 01:41:43 +00:00
Code Issues Packages Projects Releases Wiki Activity

[sim] Disable the robot program when DS disconnects (#5818)

Browse Source
This commit is contained in:
Gold856
2023-10-28 13:10:23 -04:00
committed by GitHub
parent abe1cec90c
commit 80c47da237
1 changed files with 35 additions and 22 deletions
Download Patch File Download Diff File Expand all files Collapse all files

57
simulation/halsim_ds_socket/src/main/native/cpp/main.cpp
View File

@@ -16,6 +16,7 @@
#include <atomic>
#include <cstdio>
#include <cstring>
#include <exception>
#include <string_view>
#include <DSCommPacket.h>
@@ -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<halsim::DSCommPacket>();
ds->DecodeUDP({reinterpret_cast<uint8_t*>(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<halsim::DSCommPacket>();
ds->DecodeUDP({reinterpret_cast<uint8_t*>(buf.base), len});
struct sockaddr_in outAddr;
std::memcpy(&outAddr, &recSock, sizeof(sockaddr_in));
outAddr.sin_family = PF_INET;
outAddr.sin_port = htons(1150);
struct sockaddr_in outAddr;
std::memcpy(&outAddr, &recSock, sizeof(sockaddr_in));
outAddr.sin_family = PF_INET;
outAddr.sin_port = htons(1150);
wpi::SmallVector<wpi::uv::Buffer, 4> sendBufs;
wpi::raw_uv_ostream stream{sendBufs,
[] { return GetBufferPool().Allocate(); }};
ds->SetupSendBuffer(stream);
wpi::SmallVector<wpi::uv::Buffer, 4> 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();
}