diff --git a/hal/src/main/native/sim/Encoder.cpp b/hal/src/main/native/sim/Encoder.cpp index 7407b60f46..870719afaa 100644 --- a/hal/src/main/native/sim/Encoder.cpp +++ b/hal/src/main/native/sim/Encoder.cpp @@ -254,6 +254,7 @@ void HAL_SetEncoderDistancePerPulse(HAL_EncoderHandle encoderHandle, return; } encoder->distancePerPulse = distancePerPulse; + SimEncoderData[encoder->index].SetDistancePerPulse(distancePerPulse); } void HAL_SetEncoderReverseDirection(HAL_EncoderHandle encoderHandle, HAL_Bool reverseDirection, diff --git a/hal/src/main/native/sim/MockData/EncoderData.cpp b/hal/src/main/native/sim/MockData/EncoderData.cpp index 64a2e20c35..b8da1b7496 100644 --- a/hal/src/main/native/sim/MockData/EncoderData.cpp +++ b/hal/src/main/native/sim/MockData/EncoderData.cpp @@ -38,6 +38,8 @@ void EncoderData::ResetData() { m_reverseDirectionCallbacks = nullptr; m_samplesToAverage = 0; m_samplesToAverageCallbacks = nullptr; + m_distancePerPulse = 0; + m_distancePerPulseCallbacks = nullptr; } int32_t EncoderData::RegisterInitializedCallback(HAL_NotifyCallback callback, @@ -330,6 +332,42 @@ void EncoderData::SetSamplesToAverage(int32_t samplesToAverage) { } } +int32_t EncoderData::RegisterDistancePerPulseCallback( + HAL_NotifyCallback callback, void* param, HAL_Bool initialNotify) { + // Must return -1 on a null callback for error handling + if (callback == nullptr) return -1; + int32_t newUid = 0; + { + std::lock_guard lock(m_registerMutex); + m_distancePerPulseCallbacks = + RegisterCallback(m_distancePerPulseCallbacks, "DistancePerPulse", + callback, param, &newUid); + } + if (initialNotify) { + // We know that the callback is not null because of earlier null check + HAL_Value value = MakeDouble(GetDistancePerPulse()); + callback("DistancePerPulse", param, &value); + } + return newUid; +} +void EncoderData::CancelDistancePerPulseCallback(int32_t uid) { + m_distancePerPulseCallbacks = + CancelCallback(m_distancePerPulseCallbacks, uid); +} + +void EncoderData::InvokeDistancePerPulseCallback(HAL_Value value) { + InvokeCallback(m_distancePerPulseCallbacks, "DistancePerPulse", &value); +} + +double EncoderData::GetDistancePerPulse() { return m_distancePerPulse; } + +void EncoderData::SetDistancePerPulse(double distancePerPulse) { + double oldValue = m_distancePerPulse.exchange(distancePerPulse); + if (oldValue != distancePerPulse) { + InvokeDistancePerPulseCallback(MakeDouble(distancePerPulse)); + } +} + extern "C" { void HALSIM_ResetEncoderData(int32_t index) { SimEncoderData[index].ResetData(); @@ -511,5 +549,7 @@ void HALSIM_RegisterEncoderAllCallbacks(int32_t index, initialNotify); SimEncoderData[index].RegisterSamplesToAverageCallback(callback, param, initialNotify); + SimEncoderData[index].RegisterDistancePerPulseCallback(callback, param, + initialNotify); } } // extern "C" diff --git a/hal/src/main/native/sim/MockData/EncoderDataInternal.h b/hal/src/main/native/sim/MockData/EncoderDataInternal.h index c61eba5fec..df86ec6230 100644 --- a/hal/src/main/native/sim/MockData/EncoderDataInternal.h +++ b/hal/src/main/native/sim/MockData/EncoderDataInternal.h @@ -75,6 +75,13 @@ class EncoderData { int32_t GetSamplesToAverage(); void SetSamplesToAverage(int32_t samplesToAverage); + int32_t RegisterDistancePerPulseCallback(HAL_NotifyCallback callback, + void* param, HAL_Bool initialNotify); + void CancelDistancePerPulseCallback(int32_t uid); + void InvokeDistancePerPulseCallback(HAL_Value value); + double GetDistancePerPulse(); + void SetDistancePerPulse(double distancePerPulse); + virtual void ResetData(); private: @@ -95,6 +102,8 @@ class EncoderData { std::shared_ptr m_reverseDirectionCallbacks = nullptr; std::atomic m_samplesToAverage{0}; std::shared_ptr m_samplesToAverageCallbacks = nullptr; + std::atomic m_distancePerPulse{0}; + std::shared_ptr m_distancePerPulseCallbacks = nullptr; }; extern EncoderData* SimEncoderData; } // namespace hal