diff --git a/hal/src/main/native/athena/CANAPI.cpp b/hal/src/main/native/athena/CANAPI.cpp index 3d4035fc63..01c06bda10 100644 --- a/hal/src/main/native/athena/CANAPI.cpp +++ b/hal/src/main/native/athena/CANAPI.cpp @@ -22,7 +22,7 @@ using namespace hal; namespace { struct Receives { - uint64_t lastTimeStamp; + uint32_t lastTimeStamp; uint8_t data[8]; uint8_t length; }; @@ -40,30 +40,13 @@ struct CANStorage { static UnlimitedHandleResource* canHandles; -static std::atomic_bool HasFixedTime{false}; -static uint64_t timeSpanDiff; - -static void CheckDeltaTime() { - if (HasFixedTime) return; - HasFixedTime = true; - - // TODO: Fix locking +static uint32_t GetPacketBaseTime() { timespec t; clock_gettime(CLOCK_MONOTONIC, &t); - int32_t status = 0; - uint64_t fpgaTime = HAL_GetFPGATime(&status); - - // Convert t to microseconds - uint64_t us = t.tv_sec * 1000000 + t.tv_nsec / 1000; - - timeSpanDiff = - us - fpgaTime; // This assumes CLOCK_MONOTONIC is greater then FPGA Time. -} - -static inline uint64_t ConvertToFPGATime(uint32_t canMs) { - uint64_t canMsToUs = canMs * 1000; - return canMsToUs - timeSpanDiff; + // Convert t to milliseconds + uint64_t ms = t.tv_sec * 1000ull + t.tv_nsec / 1000000ull; + return ms & 0xFFFFFFFF; } namespace hal { @@ -89,7 +72,6 @@ HAL_CANHandle HAL_InitializeCAN(HAL_CANManufacturer manufacturer, int32_t deviceId, HAL_CANDeviceType deviceType, int32_t* status) { hal::init::CheckInit(); - CheckDeltaTime(); auto can = std::make_shared(); auto handle = canHandles->Allocate(can); @@ -189,18 +171,16 @@ void HAL_ReadCANPacketNew(HAL_CANHandle handle, int32_t apiId, uint8_t* data, uint32_t ts = 0; HAL_CAN_ReceiveMessage(&messageId, 0x1FFFFFFF, data, &dataSize, &ts, status); - uint64_t timestamp = ConvertToFPGATime(ts); - if (*status == 0) { std::lock_guard lock(can->mapMutex); auto& msg = can->receives[messageId]; msg.length = dataSize; - msg.lastTimeStamp = timestamp; + msg.lastTimeStamp = ts; // The NetComm call placed in data, copy into the msg std::memcpy(msg.data, data, dataSize); } *length = dataSize; - *receivedTimestamp = timestamp; + *receivedTimestamp = ts; } void HAL_ReadCANPacketLatest(HAL_CANHandle handle, int32_t apiId, uint8_t* data, @@ -217,16 +197,14 @@ void HAL_ReadCANPacketLatest(HAL_CANHandle handle, int32_t apiId, uint8_t* data, uint32_t ts = 0; HAL_CAN_ReceiveMessage(&messageId, 0x1FFFFFFF, data, &dataSize, &ts, status); - uint64_t timestamp = ConvertToFPGATime(ts); - std::lock_guard lock(can->mapMutex); if (*status == 0) { // fresh update auto& msg = can->receives[messageId]; msg.length = dataSize; *length = dataSize; - msg.lastTimeStamp = timestamp; - *receivedTimestamp = timestamp; + msg.lastTimeStamp = ts; + *receivedTimestamp = ts; // The NetComm call placed in data, copy into the msg std::memcpy(msg.data, data, dataSize); } else { @@ -256,25 +234,22 @@ void HAL_ReadCANPacketTimeout(HAL_CANHandle handle, int32_t apiId, uint32_t ts = 0; HAL_CAN_ReceiveMessage(&messageId, 0x1FFFFFFF, data, &dataSize, &ts, status); - uint64_t timestamp = ConvertToFPGATime(ts); - std::lock_guard lock(can->mapMutex); if (*status == 0) { // fresh update auto& msg = can->receives[messageId]; msg.length = dataSize; *length = dataSize; - msg.lastTimeStamp = timestamp; - *receivedTimestamp = timestamp; + msg.lastTimeStamp = ts; + *receivedTimestamp = ts; // The NetComm call placed in data, copy into the msg std::memcpy(msg.data, data, dataSize); } else { auto i = can->receives.find(messageId); if (i != can->receives.end()) { // Found, check if new enough - uint64_t now = HAL_GetFPGATime(status); - if (now - i->second.lastTimeStamp > - static_cast(timeoutMs) * 1000) { + uint32_t now = GetPacketBaseTime(); + if (now - i->second.lastTimeStamp > static_cast(timeoutMs)) { // Timeout, return bad status *status = HAL_CAN_TIMEOUT; return; @@ -304,15 +279,14 @@ void HAL_ReadCANPeriodicPacket(HAL_CANHandle handle, int32_t apiId, std::lock_guard lock(can->mapMutex); auto i = can->receives.find(messageId); if (i != can->receives.end()) { - uint64_t now = HAL_GetFPGATime(status); // Found, check if new enough - if (now - i->second.lastTimeStamp < - static_cast(periodMs) * 1000) { - *status = 0; + uint32_t now = GetPacketBaseTime(); + if (now - i->second.lastTimeStamp < static_cast(periodMs)) { // Read the data from the stored message into the output std::memcpy(data, i->second.data, i->second.length); *length = i->second.length; *receivedTimestamp = i->second.lastTimeStamp; + *status = 0; return; } } @@ -322,25 +296,22 @@ void HAL_ReadCANPeriodicPacket(HAL_CANHandle handle, int32_t apiId, uint32_t ts = 0; HAL_CAN_ReceiveMessage(&messageId, 0x1FFFFFFF, data, &dataSize, &ts, status); - uint64_t timestamp = ConvertToFPGATime(ts); - std::lock_guard lock(can->mapMutex); if (*status == 0) { // fresh update auto& msg = can->receives[messageId]; msg.length = dataSize; *length = dataSize; - msg.lastTimeStamp = timestamp; - *receivedTimestamp = timestamp; + msg.lastTimeStamp = ts; + *receivedTimestamp = ts; // The NetComm call placed in data, copy into the msg std::memcpy(msg.data, data, dataSize); } else { auto i = can->receives.find(messageId); if (i != can->receives.end()) { // Found, check if new enough - uint64_t now = HAL_GetFPGATime(status); - if (now - i->second.lastTimeStamp > - static_cast(timeoutMs) * 1000) { + uint32_t now = GetPacketBaseTime(); + if (now - i->second.lastTimeStamp > static_cast(timeoutMs)) { // Timeout, return bad status *status = HAL_CAN_TIMEOUT; return; diff --git a/hal/src/main/native/athena/HAL.cpp b/hal/src/main/native/athena/HAL.cpp index 9e8c11bab1..dbdb826ce5 100644 --- a/hal/src/main/native/athena/HAL.cpp +++ b/hal/src/main/native/athena/HAL.cpp @@ -239,7 +239,7 @@ uint64_t HAL_GetFPGATime(int32_t* status) { *status = NiFpga_Status_ResourceNotInitialized; return 0; } - + *status = 0; uint64_t upper1 = global->readLocalTimeUpper(status); uint32_t lower = global->readLocalTime(status); uint64_t upper2 = global->readLocalTimeUpper(status); diff --git a/hal/src/main/native/athena/PDP.cpp b/hal/src/main/native/athena/PDP.cpp index 8298e9d274..ff1eb8149a 100644 --- a/hal/src/main/native/athena/PDP.cpp +++ b/hal/src/main/native/athena/PDP.cpp @@ -31,7 +31,7 @@ static constexpr int32_t StatusEnergy = 0x5D; static constexpr int32_t Control1 = 0x70; -static constexpr int32_t TimeoutMs = 50; +static constexpr int32_t TimeoutMs = 100; static constexpr int32_t StatusPeriodMs = 25; /* encoder/decoders */ diff --git a/hal/src/main/native/sim/CANAPI.cpp b/hal/src/main/native/sim/CANAPI.cpp index 3b1a00eba2..a54f06f51c 100644 --- a/hal/src/main/native/sim/CANAPI.cpp +++ b/hal/src/main/native/sim/CANAPI.cpp @@ -41,13 +41,11 @@ struct CANStorage { static UnlimitedHandleResource* canHandles; -static void CheckDeltaTime() { - // Noop on sim -} - -static inline uint64_t ConvertToFPGATime(uint32_t canMs) { - uint64_t canMsToUs = canMs * 1000; - return canMsToUs; +static uint32_t GetPacketBaseTime() { + int status = 0; + auto basetime = HAL_GetFPGATime(&status); + // us to ms + return (basetime / 1000ull) & 0xFFFFFFFF; } namespace hal { @@ -83,7 +81,6 @@ HAL_CANHandle HAL_InitializeCAN(HAL_CANManufacturer manufacturer, int32_t deviceId, HAL_CANDeviceType deviceType, int32_t* status) { hal::init::CheckInit(); - CheckDeltaTime(); auto can = std::make_shared(); auto handle = canHandles->Allocate(can); @@ -183,18 +180,16 @@ void HAL_ReadCANPacketNew(HAL_CANHandle handle, int32_t apiId, uint8_t* data, uint32_t ts = 0; HAL_CAN_ReceiveMessage(&messageId, 0x1FFFFFFF, data, &dataSize, &ts, status); - uint64_t timestamp = ConvertToFPGATime(ts); - if (*status == 0) { std::lock_guard lock(can->mapMutex); auto& msg = can->receives[messageId]; msg.length = dataSize; - msg.lastTimeStamp = timestamp; + msg.lastTimeStamp = ts; // The NetComm call placed in data, copy into the msg std::memcpy(msg.data, data, dataSize); } *length = dataSize; - *receivedTimestamp = timestamp; + *receivedTimestamp = ts; } void HAL_ReadCANPacketLatest(HAL_CANHandle handle, int32_t apiId, uint8_t* data, @@ -211,16 +206,14 @@ void HAL_ReadCANPacketLatest(HAL_CANHandle handle, int32_t apiId, uint8_t* data, uint32_t ts = 0; HAL_CAN_ReceiveMessage(&messageId, 0x1FFFFFFF, data, &dataSize, &ts, status); - uint64_t timestamp = ConvertToFPGATime(ts); - std::lock_guard lock(can->mapMutex); if (*status == 0) { // fresh update auto& msg = can->receives[messageId]; msg.length = dataSize; *length = dataSize; - msg.lastTimeStamp = timestamp; - *receivedTimestamp = timestamp; + msg.lastTimeStamp = ts; + *receivedTimestamp = ts; // The NetComm call placed in data, copy into the msg std::memcpy(msg.data, data, dataSize); } else { @@ -250,25 +243,22 @@ void HAL_ReadCANPacketTimeout(HAL_CANHandle handle, int32_t apiId, uint32_t ts = 0; HAL_CAN_ReceiveMessage(&messageId, 0x1FFFFFFF, data, &dataSize, &ts, status); - uint64_t timestamp = ConvertToFPGATime(ts); - std::lock_guard lock(can->mapMutex); if (*status == 0) { // fresh update auto& msg = can->receives[messageId]; msg.length = dataSize; *length = dataSize; - msg.lastTimeStamp = timestamp; - *receivedTimestamp = timestamp; + msg.lastTimeStamp = ts; + *receivedTimestamp = ts; // The NetComm call placed in data, copy into the msg std::memcpy(msg.data, data, dataSize); } else { auto i = can->receives.find(messageId); if (i != can->receives.end()) { // Found, check if new enough - uint64_t now = HAL_GetFPGATime(status); - if (now - i->second.lastTimeStamp > - static_cast(timeoutMs) * 1000) { + uint32_t now = GetPacketBaseTime(); + if (now - i->second.lastTimeStamp > static_cast(timeoutMs)) { // Timeout, return bad status *status = HAL_CAN_TIMEOUT; return; @@ -298,10 +288,9 @@ void HAL_ReadCANPeriodicPacket(HAL_CANHandle handle, int32_t apiId, std::lock_guard lock(can->mapMutex); auto i = can->receives.find(messageId); if (i != can->receives.end()) { - uint64_t now = HAL_GetFPGATime(status); // Found, check if new enough - if (now - i->second.lastTimeStamp < - static_cast(periodMs) * 1000) { + uint32_t now = GetPacketBaseTime(); + if (now - i->second.lastTimeStamp < static_cast(periodMs)) { *status = 0; // Read the data from the stored message into the output std::memcpy(data, i->second.data, i->second.length); @@ -316,25 +305,22 @@ void HAL_ReadCANPeriodicPacket(HAL_CANHandle handle, int32_t apiId, uint32_t ts = 0; HAL_CAN_ReceiveMessage(&messageId, 0x1FFFFFFF, data, &dataSize, &ts, status); - uint64_t timestamp = ConvertToFPGATime(ts); - std::lock_guard lock(can->mapMutex); if (*status == 0) { // fresh update auto& msg = can->receives[messageId]; msg.length = dataSize; *length = dataSize; - msg.lastTimeStamp = timestamp; - *receivedTimestamp = timestamp; + msg.lastTimeStamp = ts; + *receivedTimestamp = ts; // The NetComm call placed in data, copy into the msg std::memcpy(msg.data, data, dataSize); } else { auto i = can->receives.find(messageId); if (i != can->receives.end()) { // Found, check if new enough - uint64_t now = HAL_GetFPGATime(status); - if (now - i->second.lastTimeStamp > - static_cast(timeoutMs) * 1000) { + uint32_t now = GetPacketBaseTime(); + if (now - i->second.lastTimeStamp > static_cast(timeoutMs)) { // Timeout, return bad status *status = HAL_CAN_TIMEOUT; return; diff --git a/wpilibcIntegrationTests/src/main/native/cpp/PowerDistributionPanelTest.cpp b/wpilibcIntegrationTests/src/main/native/cpp/PowerDistributionPanelTest.cpp index 959f03d455..881b949145 100644 --- a/wpilibcIntegrationTests/src/main/native/cpp/PowerDistributionPanelTest.cpp +++ b/wpilibcIntegrationTests/src/main/native/cpp/PowerDistributionPanelTest.cpp @@ -7,6 +7,8 @@ #include "frc/PowerDistributionPanel.h" // NOLINT(build/include_order) +#include + #include "TestBench.h" #include "frc/Jaguar.h" #include "frc/Talon.h" @@ -40,6 +42,20 @@ class PowerDistributionPanelTest : public testing::Test { } }; +TEST_F(PowerDistributionPanelTest, CheckRepeatedCalls) { + auto numChannels = HAL_GetNumPDPChannels(); + // 1 second + for (int i = 0; i < 50; i++) { + for (int j = 0; j < numChannels; j++) { + m_pdp->GetCurrent(j); + ASSERT_TRUE(m_pdp->GetError().GetCode() == 0); + } + m_pdp->GetVoltage(); + ASSERT_TRUE(m_pdp->GetError().GetCode() == 0); + } + std::this_thread::sleep_for(std::chrono::milliseconds(20)); +} + /** * Test if the current changes when the motor is driven using a talon */