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

26
hal/src/main/native/athena/FRCDriverStation.cpp
View File

@@ -539,15 +539,25 @@ HAL_Bool HAL_RefreshDSData(void) {
}
// 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. Otherwise use the state that shows disconnected.
if (controlWord.dsAttached) {
newestControlWord = currentRead->controlWord;
} else {
// Zero out the control word. When the DS has never been connected
// this returns garbage. And there is no way we can detect that.
std::memset(&controlWord, 0, sizeof(controlWord));
newestControlWord = controlWord;
// 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 (!controlWord.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;
}
uint32_t mask = tcpMask.exchange(0);

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();
}

9
wpilibNewCommands/src/test/java/edu/wpi/first/wpilibj2/command/button/RobotModeTriggersTest.java
View File

@@ -6,7 +6,6 @@ package edu.wpi.first.wpilibj2.command.button;
import static org.junit.jupiter.api.Assertions.assertTrue;
import edu.wpi.first.wpilibj.DriverStation;
import edu.wpi.first.wpilibj.simulation.DriverStationSim;
import edu.wpi.first.wpilibj2.command.CommandTestBase;
import org.junit.jupiter.api.Test;
@@ -18,7 +17,7 @@ class RobotModeTriggersTest extends CommandTestBase {
DriverStationSim.setAutonomous(true);
DriverStationSim.setTest(false);
DriverStationSim.setEnabled(true);
DriverStation.refreshData();
DriverStationSim.notifyNewData();
Trigger auto = RobotModeTriggers.autonomous();
assertTrue(auto.getAsBoolean());
}
@@ -29,7 +28,7 @@ class RobotModeTriggersTest extends CommandTestBase {
DriverStationSim.setAutonomous(false);
DriverStationSim.setTest(false);
DriverStationSim.setEnabled(true);
DriverStation.refreshData();
DriverStationSim.notifyNewData();
Trigger teleop = RobotModeTriggers.teleop();
assertTrue(teleop.getAsBoolean());
}
@@ -40,7 +39,7 @@ class RobotModeTriggersTest extends CommandTestBase {
DriverStationSim.setAutonomous(false);
DriverStationSim.setTest(true);
DriverStationSim.setEnabled(true);
DriverStation.refreshData();
DriverStationSim.notifyNewData();
Trigger test = RobotModeTriggers.test();
assertTrue(test.getAsBoolean());
}
@@ -51,7 +50,7 @@ class RobotModeTriggersTest extends CommandTestBase {
DriverStationSim.setAutonomous(false);
DriverStationSim.setTest(false);
DriverStationSim.setEnabled(false);
DriverStation.refreshData();
DriverStationSim.notifyNewData();
Trigger disabled = RobotModeTriggers.disabled();
assertTrue(disabled.getAsBoolean());
}

8
wpilibNewCommands/src/test/native/cpp/frc2/command/button/RobotModeTriggersTest.cpp
View File

@@ -18,7 +18,7 @@ TEST(RobotModeTriggersTest, Autonomous) {
DriverStationSim::SetAutonomous(true);
DriverStationSim::SetTest(false);
DriverStationSim::SetEnabled(true);
frc::DriverStation::RefreshData();
DriverStationSim::NotifyNewData();
Trigger autonomous = RobotModeTriggers::Autonomous();
EXPECT_TRUE(autonomous.Get());
}
@@ -28,7 +28,7 @@ TEST(RobotModeTriggersTest, Teleop) {
DriverStationSim::SetAutonomous(false);
DriverStationSim::SetTest(false);
DriverStationSim::SetEnabled(true);
frc::DriverStation::RefreshData();
DriverStationSim::NotifyNewData();
Trigger teleop = RobotModeTriggers::Teleop();
EXPECT_TRUE(teleop.Get());
}
@@ -38,7 +38,7 @@ TEST(RobotModeTriggersTest, Disabled) {
DriverStationSim::SetAutonomous(false);
DriverStationSim::SetTest(false);
DriverStationSim::SetEnabled(false);
frc::DriverStation::RefreshData();
DriverStationSim::NotifyNewData();
Trigger disabled = RobotModeTriggers::Disabled();
EXPECT_TRUE(disabled.Get());
}
@@ -48,7 +48,7 @@ TEST(RobotModeTriggersTest, TestMode) {
DriverStationSim::SetAutonomous(false);
DriverStationSim::SetTest(true);
DriverStationSim::SetEnabled(true);
frc::DriverStation::RefreshData();
DriverStationSim::NotifyNewData();
Trigger test = RobotModeTriggers::Test();
EXPECT_TRUE(test.Get());
}