diff --git a/hal/src/main/native/athena/HAL.cpp b/hal/src/main/native/athena/HAL.cpp index 798c99639b..1bf6624252 100644 --- a/hal/src/main/native/athena/HAL.cpp +++ b/hal/src/main/native/athena/HAL.cpp @@ -35,11 +35,6 @@ using namespace hal; static std::unique_ptr global; static std::unique_ptr watchdog; -static std::mutex timeMutex; -static uint32_t timeEpoch = 0; -static uint32_t prevFPGATime = 0; -static HAL_NotifierHandle rolloverNotifier = 0; - using namespace hal; extern "C" { @@ -224,14 +219,16 @@ uint64_t HAL_GetFPGATime(int32_t* status) { *status = NiFpga_Status_ResourceNotInitialized; return 0; } - std::lock_guard lock(timeMutex); - uint32_t fpgaTime = global->readLocalTime(status); + uint64_t upper1 = global->readLocalTimeUpper(status); + uint32_t lower = global->readLocalTime(status); + uint64_t upper2 = global->readLocalTimeUpper(status); if (*status != 0) return 0; - // check for rollover - if (fpgaTime < prevFPGATime) ++timeEpoch; - prevFPGATime = fpgaTime; - return static_cast(timeEpoch) << 32 | - static_cast(fpgaTime); + if (upper1 != upper2) { + // Rolled over between the lower call, reread lower + lower = global->readLocalTime(status); + if (*status != 0) return 0; + } + return (upper2 << 32) + lower; } /** @@ -262,12 +259,6 @@ HAL_Bool HAL_GetBrownedOut(int32_t* status) { return !(watchdog->readStatus_PowerAlive(status)); } -static void timerRollover(uint64_t currentTime, HAL_NotifierHandle handle) { - // reschedule timer for next rollover - int32_t status = 0; - HAL_UpdateNotifierAlarm(handle, currentTime + 0x80000000ULL, &status); -} - void HAL_BaseInitialize(int32_t* status) { static std::atomic_bool initialized{false}; static std::mutex initializeMutex; @@ -361,23 +352,6 @@ HAL_Bool HAL_Initialize(int32_t timeout, int32_t mode) { int32_t status = 0; HAL_BaseInitialize(&status); - if (!rolloverNotifier) - rolloverNotifier = HAL_InitializeNotifierNonThreadedUnsafe( - timerRollover, nullptr, &status); - if (status == 0) { - uint64_t curTime = HAL_GetFPGATime(&status); - if (status == 0) { - HAL_UpdateNotifierAlarm(rolloverNotifier, curTime + 0x80000000ULL, - &status); - } else { - // return false if status failed. - return false; - } - } else { - // return false if status failed. - return false; - } - HAL_InitializeDriverStation(); initialized = true; diff --git a/hal/src/main/native/athena/PWM.cpp b/hal/src/main/native/athena/PWM.cpp index 34c8d096a2..d4117ab85e 100644 --- a/hal/src/main/native/athena/PWM.cpp +++ b/hal/src/main/native/athena/PWM.cpp @@ -450,4 +450,24 @@ int32_t HAL_GetLoopTiming(int32_t* status) { return pwmSystem->readLoopTiming(status); } +/** + * Get the pwm starting cycle time + * + * @return The pwm cycle start time. + */ +uint64_t HAL_GetCycleStartTime(int32_t* status) { + initializeDigital(status); + if (*status != 0) return 0; + uint64_t upper1 = pwmSystem->readCycleStartTimeUpper(status); + uint32_t lower = pwmSystem->readCycleStartTime(status); + uint64_t upper2 = pwmSystem->readCycleStartTimeUpper(status); + if (*status != 0) return 0; + if (upper1 != upper2) { + // Rolled over between the lower call, reread lower + lower = pwmSystem->readCycleStartTime(status); + if (*status != 0) return 0; + } + return (upper2 << 32) + lower; +} + } // extern "C" diff --git a/hal/src/main/native/include/HAL/PWM.h b/hal/src/main/native/include/HAL/PWM.h index b581701b95..894c738669 100644 --- a/hal/src/main/native/include/HAL/PWM.h +++ b/hal/src/main/native/include/HAL/PWM.h @@ -50,6 +50,7 @@ void HAL_LatchPWMZero(HAL_DigitalHandle pwmPortHandle, int32_t* status); void HAL_SetPWMPeriodScale(HAL_DigitalHandle pwmPortHandle, int32_t squelchMask, int32_t* status); int32_t HAL_GetLoopTiming(int32_t* status); +uint64_t HAL_GetCycleStartTime(int32_t* status); #ifdef __cplusplus } // extern "C" #endif diff --git a/hal/src/main/native/sim/PWM.cpp b/hal/src/main/native/sim/PWM.cpp index f69afbb638..9bc75b1768 100644 --- a/hal/src/main/native/sim/PWM.cpp +++ b/hal/src/main/native/sim/PWM.cpp @@ -341,4 +341,11 @@ void HAL_SetPWMPeriodScale(HAL_DigitalHandle pwmPortHandle, int32_t squelchMask, * @return The loop time */ int32_t HAL_GetLoopTiming(int32_t* status) { return kExpectedLoopTiming; } + +/** + * Get the pwm starting cycle time + * + * @return The pwm cycle start time. + */ +uint64_t HAL_GetCycleStartTime(int32_t* status) { return 0; } }