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

[hal] HAL_RefreshDSData: Zero out control word on DS disconnect, use cache in sim (#6380)

Browse Source
This commit is contained in:
Thad House
2024-02-18 14:30:40 -08:00
committed by GitHub
parent 0ad6b3acb3
commit 63d9e945b8
4 changed files with 59 additions and 27 deletions
Download Patch File Download Diff File Expand all files Collapse all files

43
hal/src/main/native/sim/DriverStation.cpp
View File

@@ -44,6 +44,7 @@ struct JoystickDataCache {
HAL_JoystickButtons buttons[kJoystickPorts];
HAL_AllianceStationID allianceStation;
double matchTime;
HAL_ControlWord controlWord;
};
static_assert(std::is_standard_layout_v<JoystickDataCache>);
// static_assert(std::is_trivial_v<JoystickDataCache>);
@@ -65,6 +66,16 @@ void JoystickDataCache::Update() {
}
allianceStation = SimDriverStationData->allianceStationId;
matchTime = SimDriverStationData->matchTime;
HAL_ControlWord tmpControlWord;
std::memset(&tmpControlWord, 0, sizeof(tmpControlWord));
tmpControlWord.enabled = SimDriverStationData->enabled;
tmpControlWord.autonomous = SimDriverStationData->autonomous;
tmpControlWord.test = SimDriverStationData->test;
tmpControlWord.eStop = SimDriverStationData->eStop;
tmpControlWord.fmsAttached = SimDriverStationData->fmsAttached;
tmpControlWord.dsAttached = SimDriverStationData->dsAttached;
this->controlWord = tmpControlWord;
}
#define CHECK_JOYSTICK_NUMBER(stickNum) \
@@ -322,20 +333,32 @@ HAL_Bool HAL_RefreshDSData(void) {
if (gShutdown) {
return false;
}
HAL_ControlWord controlWord;
std::memset(&controlWord, 0, sizeof(controlWord));
controlWord.enabled = SimDriverStationData->enabled;
controlWord.autonomous = SimDriverStationData->autonomous;
controlWord.test = SimDriverStationData->test;
controlWord.eStop = SimDriverStationData->eStop;
controlWord.fmsAttached = SimDriverStationData->fmsAttached;
controlWord.dsAttached = SimDriverStationData->dsAttached;
bool dsAttached = SimDriverStationData->dsAttached;
std::scoped_lock lock{driverStation->cacheMutex};
JoystickDataCache* prev = currentCache.exchange(nullptr);
if (prev != nullptr) {
currentRead = prev;
}
newestControlWord = controlWord;
// If newest state shows we have a DS attached, just use the
// control word out of the cache, As it will be the one in sync
// with the data. If no data has been updated, at this point,
// and a DS wasn't attached previously, this will still return
// a zeroed out control word, with is the correct state for
// no new data.
if (!dsAttached) {
// If the DS is not attached, we need to zero out the control word.
// This is because HAL_RefreshDSData is called asynchronously from
// the DS data. The dsAttached variable comes directly from netcomm
// and could be updated before the caches are. If that happens,
// we would end up returning the previous cached control word,
// which is out of sync with the current control word and could
// break invariants such as which alliance station is in used.
// Also, when the DS has never been connected the rest of the fields
// in control word are garbage, so we also need to zero out in that
// case too
std::memset(&currentRead->controlWord, 0, sizeof(currentRead->controlWord));
}
newestControlWord = currentRead->controlWord;
return prev != nullptr;
}
@@ -369,6 +392,7 @@ void NewDriverStationData() {
if (gShutdown) {
return;
}
SimDriverStationData->dsAttached = true;
cacheToUpdate->Update();
JoystickDataCache* given = cacheToUpdate;
@@ -382,7 +406,6 @@ void NewDriverStationData() {
}
lastGiven = given;
SimDriverStationData->dsAttached = true;
driverStation->newDataEvents.Wakeup();
SimDriverStationData->CallNewDataCallbacks();
}