Send 0.95 from TOF controller if there is no cone.
This lets us detect cone presence separately from unplugged sensor.
Change-Id: I0f70c1229bd88b5c50ed2fdf043361d072c41402
Signed-off-by: Austin Schuh <austin.linux@gmail.com>
diff --git a/frc971/wpilib/dma_edge_counting.cc b/frc971/wpilib/dma_edge_counting.cc
index ba8fdf4..4e8a158 100644
--- a/frc971/wpilib/dma_edge_counting.cc
+++ b/frc971/wpilib/dma_edge_counting.cc
@@ -28,11 +28,11 @@
if (have_prev_sample_ && high_time_ != 0 && prev_sample_.Get(input_) &&
!sample.Get(input_)) {
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();
+ last_period_ = (sample.GetTime() - high_time_) * 1e-6;
+ high_time_ = sample.GetTime();
poll_count_ = 0;
}
have_prev_sample_ = true;
@@ -46,6 +46,7 @@
high_time_ = 0;
have_prev_sample_ = false;
last_width_ = ::std::numeric_limits<double>::quiet_NaN();
+ last_period_ = ::std::numeric_limits<double>::quiet_NaN();
}
poll_count_++;
}
diff --git a/frc971/wpilib/dma_edge_counting.h b/frc971/wpilib/dma_edge_counting.h
index 4d1246c..f51318d 100644
--- a/frc971/wpilib/dma_edge_counting.h
+++ b/frc971/wpilib/dma_edge_counting.h
@@ -47,6 +47,7 @@
// Last pulse width in seconds
double last_width() const { return last_width_; }
+ double last_period() const { return last_period_; }
private:
void UpdateFromSample(const DMASample & /*sample*/) override;
@@ -70,6 +71,7 @@
size_t poll_count_ = 0;
double last_width_ = ::std::numeric_limits<double>::quiet_NaN();
+ double last_period_ = ::std::numeric_limits<double>::quiet_NaN();
DISALLOW_COPY_AND_ASSIGN(DMAPulseWidthReader);
};