Reset DMA pulse width reader state when stale
We should always be updating at a regular interval.
Signed-off-by: milind-u <milind.upadhyay@gmail.com>
Change-Id: Ibf3071831eb42123697b4f47f1b8056557ea42ee
diff --git a/frc971/wpilib/dma_edge_counting.cc b/frc971/wpilib/dma_edge_counting.cc
index 8864116..ba8fdf4 100644
--- a/frc971/wpilib/dma_edge_counting.cc
+++ b/frc971/wpilib/dma_edge_counting.cc
@@ -27,15 +27,29 @@
void DMAPulseWidthReader::UpdateFromSample(const DMASample &sample) {
if (have_prev_sample_ && high_time_ != 0 && prev_sample_.Get(input_) &&
!sample.Get(input_)) {
- last_width_ = (sample.GetTime() - high_time_) * 0.000001;
+ last_width_ = (sample.GetTime() - high_time_) * 1e-6;
+ high_time_ = 0;
+ poll_count_ = 0;
} else if (have_prev_sample_ && !prev_sample_.Get(input_) &&
sample.Get(input_)) {
high_time_ = prev_sample_.GetTime();
+ poll_count_ = 0;
}
have_prev_sample_ = true;
prev_sample_ = sample;
}
+void DMAPulseWidthReader::UpdatePolledValue() {
+ // If we are polled without an update for too long, reset
+ constexpr size_t kMaxPollCount = 4;
+ if (poll_count_ > kMaxPollCount) {
+ high_time_ = 0;
+ have_prev_sample_ = false;
+ last_width_ = ::std::numeric_limits<double>::quiet_NaN();
+ }
+ poll_count_++;
+}
+
void DMAPulseSeparationReader::UpdateFromSample(const DMASample &sample) {
// save the time of the falling edge of the input one
if (have_prev_sample_ && !sample.Get(input_one_) &&