commit 61227f27364eb0e5cb01858b4add30912fd96157
author milind-u <milind.upadhyay@gmail.com> Sun Aug 29 15:58:33 2021 -0700
committer Milind Upadhyay <milind.upadhyay@gmail.com> Sat Sep 11 16:36:28 2021 -0700
tree bb55ae4318be4b7419a160a2213e1898136d1951
parent 33a311140fe6ebe026e4bfdea8248b73f46472f9

Use a timer in SensorReader

Since we know what time the next iteration should occur at,
it makes more sense to set a timer for that time instead of
modifying the PhasedLoop's offset.

Change-Id: Iddacb4726ea7278d910ba15e7780ebcad76b7858
Signed-off-by: milind-u <milind.upadhyay@gmail.com>

aos/events/event_loop.h

diff --git a/aos/events/event_loop.h b/aos/events/event_loop.h
index 8ef8d57..1000703 100644
--- a/aos/events/event_loop.h
+++ b/aos/events/event_loop.h
@@ -586,6 +586,8 @@
 
   // Creates a timer that executes callback when the timer expires
   // Returns a TimerHandle for configuration of the timer
+  // TODO(milind): callback should take the number of cycles elapsed as a
+  // parameter.
   virtual TimerHandler *AddTimer(::std::function<void()> callback) = 0;
 
   // Creates a timer that executes callback periodically at the specified

frc971/wpilib/sensor_reader.cc

diff --git a/frc971/wpilib/sensor_reader.cc b/frc971/wpilib/sensor_reader.cc
index d8377d8..16b0e09 100644
--- a/frc971/wpilib/sensor_reader.cc
+++ b/frc971/wpilib/sensor_reader.cc
@@ -7,7 +7,6 @@
 #include "aos/init.h"
 #include "aos/logging/logging.h"
 #include "aos/util/compiler_memory_barrier.h"
-#include "aos/util/phased_loop.h"
 #include "frc971/wpilib/ahal/DigitalInput.h"
 #include "frc971/wpilib/ahal/DriverStation.h"
 #include "frc971/wpilib/ahal/Utility.h"
@@ -31,9 +30,8 @@
   event_loop->SetRuntimeRealtimePriority(40);
 
   // Fill in the no pwm trigger defaults.
-  phased_loop_handler_ =
-      event_loop_->AddPhasedLoop([this](int iterations) { Loop(iterations); },
-                                 period_, chrono::milliseconds(4));
+  timer_handler_ = event_loop_->AddTimer([this]() { Loop(); });
+  timer_handler_->set_name("SensorReader Loop");
 
   event_loop->set_name("SensorReader");
   event_loop->OnRun([this]() { DoStart(); });
@@ -94,18 +92,15 @@
   }
 
   // Now that we are configured, actually fill in the defaults.
-  phased_loop_handler_->set_interval_and_offset(
-      period_,
-      pwm_trigger_ ? ::std::chrono::milliseconds(3) : chrono::milliseconds(4));
+  timer_handler_->Setup(
+      event_loop_->monotonic_now() +
+          (pwm_trigger_ ? chrono::milliseconds(3) : chrono::milliseconds(4)),
+      period_);
 
   last_monotonic_now_ = monotonic_clock::now();
 }
 
-void SensorReader::Loop(const int iterations) {
-  if (iterations != 1) {
-    AOS_LOG(WARNING, "SensorReader skipped %d iterations\n", iterations - 1);
-  }
-
+void SensorReader::Loop() {
   const monotonic_clock::time_point monotonic_now =
       event_loop_->monotonic_now();
 
@@ -142,11 +137,10 @@
     // after the falling edge.  This gives us a little bit of buffer for
     // errors in waking up.  The PWM cycle starts at the falling edge of the
     // PWM pulse.
-    chrono::nanoseconds new_offset =
-        ::aos::time::PhasedLoop::OffsetFromIntervalAndTime(
-            period_, last_tick_timepoint + chrono::microseconds(50));
+    const auto next_time =
+        last_tick_timepoint + period_ + chrono::microseconds(50);
 
-    phased_loop_handler_->set_interval_and_offset(period_, new_offset);
+    timer_handler_->Setup(next_time, period_);
   }
 }
 

frc971/wpilib/sensor_reader.h

diff --git a/frc971/wpilib/sensor_reader.h b/frc971/wpilib/sensor_reader.h
index 8efdf75..5ae7ffd 100644
--- a/frc971/wpilib/sensor_reader.h
+++ b/frc971/wpilib/sensor_reader.h
@@ -221,7 +221,7 @@
   void DoStart();
 
   // Runs a single iteration.
-  void Loop(int iterations);
+  void Loop();
 
   // Returns the monotonic time of the start of the first PWM cycle.
   // Returns min_time if no start time could be calculated.
@@ -236,8 +236,8 @@
 
   const int32_t my_pid_;
 
-  // Pointer to the phased loop handler used to modify the wakeup.
-  ::aos::PhasedLoopHandler *phased_loop_handler_;
+  // Pointer to the timer handler used to modify the wakeup.
+  ::aos::TimerHandler *timer_handler_;
 
   // Last time we got called.
   ::aos::monotonic_clock::time_point last_monotonic_now_ =