Convert to use the FPGA PWM cycle synchronization
This removes the need for the pwm feedback wire.
Change-Id: I8af9252cc81fd00d3904e785356deffb2ee89b00
diff --git a/frc971/wpilib/sensor_reader.cc b/frc971/wpilib/sensor_reader.cc
index f5372bf..d25db27 100644
--- a/frc971/wpilib/sensor_reader.cc
+++ b/frc971/wpilib/sensor_reader.cc
@@ -7,8 +7,10 @@
#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"
#include "frc971/wpilib/wpilib_interface.h"
+#include "hal/PWM.h"
namespace frc971 {
namespace wpilib {
@@ -18,6 +20,7 @@
// just work with them.
UpdateFastEncoderFilterHz(500000);
UpdateMediumEncoderFilterHz(100000);
+ ds_ = &::frc::DriverStation::GetInstance();
}
void SensorReader::UpdateFastEncoderFilterHz(int hz) {
@@ -42,77 +45,37 @@
drivetrain_right_encoder_->SetMaxPeriod(0.005);
}
-void SensorReader::set_pwm_trigger(
- ::std::unique_ptr<frc::DigitalInput> pwm_trigger) {
- fast_encoder_filter_.Add(pwm_trigger.get());
- pwm_trigger_ = ::std::move(pwm_trigger);
-}
+monotonic_clock::time_point SensorReader::GetPWMStartTime() {
+ int32_t status = 0;
+ const hal::fpga_clock::time_point new_fpga_time = hal::fpga_clock::time_point(
+ hal::fpga_clock::duration(HAL_GetPWMCycleStartTime(&status)));
-void SensorReader::RunPWMDetecter() {
- ::aos::SetCurrentThreadRealtimePriority(41);
+ aos_compiler_memory_barrier();
+ const hal::fpga_clock::time_point fpga_time_before = hal::fpga_clock::now();
+ aos_compiler_memory_barrier();
+ const monotonic_clock::time_point monotonic_now = monotonic_clock::now();
+ aos_compiler_memory_barrier();
+ const hal::fpga_clock::time_point fpga_time_after = hal::fpga_clock::now();
+ aos_compiler_memory_barrier();
- pwm_trigger_->RequestInterrupts();
- // Rising edge only.
- pwm_trigger_->SetUpSourceEdge(true, false);
+ const chrono::nanoseconds fpga_sample_length =
+ fpga_time_after - fpga_time_before;
+ const chrono::nanoseconds fpga_offset =
+ hal::fpga_clock::time_point((fpga_time_after.time_since_epoch() +
+ fpga_time_before.time_since_epoch()) /
+ 2) -
+ new_fpga_time;
- monotonic_clock::time_point last_posedge_monotonic =
- monotonic_clock::min_time;
-
- while (run_) {
- auto ret = pwm_trigger_->WaitForInterrupt(1.0, true);
- if (ret == frc::InterruptableSensorBase::WaitResult::kRisingEdge) {
- // Grab all the clocks.
- const double pwm_fpga_time = pwm_trigger_->ReadRisingTimestamp();
-
- aos_compiler_memory_barrier();
- const double fpga_time_before = frc::GetFPGATime() * 1e-6;
- aos_compiler_memory_barrier();
- const monotonic_clock::time_point monotonic_now = monotonic_clock::now();
- aos_compiler_memory_barrier();
- const double fpga_time_after = frc::GetFPGATime() * 1e-6;
- aos_compiler_memory_barrier();
-
- const double fpga_offset =
- (fpga_time_after + fpga_time_before) / 2.0 - pwm_fpga_time;
-
- // Compute when the edge was.
- const monotonic_clock::time_point monotonic_edge =
- monotonic_now - chrono::duration_cast<chrono::nanoseconds>(
- chrono::duration<double>(fpga_offset));
-
- LOG(DEBUG, "Got PWM pulse %f spread, %f offset, %lld trigger\n",
- fpga_time_after - fpga_time_before, fpga_offset,
- monotonic_edge.time_since_epoch().count());
-
- // Compute bounds on the timestep and sampling times.
- const double fpga_sample_length = fpga_time_after - fpga_time_before;
- const chrono::nanoseconds elapsed_time =
- monotonic_edge - last_posedge_monotonic;
-
- last_posedge_monotonic = monotonic_edge;
-
- // Verify that the values are sane.
- if (fpga_sample_length > 2e-5 || fpga_sample_length < 0) {
- continue;
- }
- if (fpga_offset < 0 || fpga_offset > 0.00015) {
- continue;
- }
- if (elapsed_time > chrono::microseconds(5050) + chrono::microseconds(4) ||
- elapsed_time < chrono::microseconds(5050) - chrono::microseconds(4)) {
- continue;
- }
- // Good edge!
- {
- ::std::unique_lock<::aos::stl_mutex> locker(tick_time_mutex_);
- last_tick_time_monotonic_timepoint_ = last_posedge_monotonic;
- last_period_ = elapsed_time;
- }
- } else {
- LOG(INFO, "PWM triggered %d\n", ret);
- }
+ // Make sure that there wasn't a context switch while we were sampling the
+ // clocks. If there was, we are better off rejecting the sample than using
+ // it.
+ if (ds_->IsSysActive() && fpga_sample_length <= chrono::microseconds(20) &&
+ fpga_sample_length >= chrono::microseconds(0)) {
+ // Compute when the edge was.
+ return monotonic_now - fpga_offset;
+ } else {
+ return monotonic_clock::min_time;
}
- pwm_trigger_->CancelInterrupts();
}
void SensorReader::operator()() {
@@ -125,24 +88,19 @@
dma_synchronizer_->Start();
}
+ const chrono::microseconds period =
+ pwm_trigger_ ? chrono::microseconds(5050) : chrono::microseconds(5000);
if (pwm_trigger_) {
- last_period_ = chrono::microseconds(5050);
LOG(INFO, "Using PWM trigger and a 5.05 ms period\n");
} else {
LOG(INFO, "Defaulting to open loop pwm synchronization\n");
- last_period_ = chrono::microseconds(5000);
}
::aos::time::PhasedLoop phased_loop(
- last_period_,
+ period,
pwm_trigger_ ? ::std::chrono::milliseconds(3) : chrono::milliseconds(4));
- ::std::thread pwm_detecter_thread;
- if (pwm_trigger_) {
- pwm_detecter_thread =
- ::std::thread(::std::bind(&SensorReader::RunPWMDetecter, this));
- }
-
::aos::SetCurrentThreadRealtimePriority(40);
+ monotonic_clock::time_point last_monotonic_now = monotonic_clock::now();
while (run_) {
{
const int iterations = phased_loop.SleepUntilNext();
@@ -150,6 +108,7 @@
LOG(WARNING, "SensorReader skipped %d iterations\n", iterations - 1);
}
}
+ const monotonic_clock::time_point monotonic_now = monotonic_clock::now();
::frc971::wpilib::SendRobotState(my_pid);
RunIteration();
@@ -159,30 +118,33 @@
}
if (pwm_trigger_) {
- monotonic_clock::time_point last_tick_timepoint;
- chrono::nanoseconds period;
- {
- ::std::unique_lock<::aos::stl_mutex> locker(tick_time_mutex_);
- last_tick_timepoint = last_tick_time_monotonic_timepoint_;
- period = last_period_;
- }
+ LOG(DEBUG, "PWM wakeup delta: %lld\n",
+ (monotonic_now - last_monotonic_now).count());
+ last_monotonic_now = monotonic_now;
+ monotonic_clock::time_point last_tick_timepoint = GetPWMStartTime();
if (last_tick_timepoint == monotonic_clock::min_time) {
continue;
}
- chrono::nanoseconds new_offset = phased_loop.OffsetFromIntervalAndTime(
- period, last_tick_timepoint + chrono::microseconds(2050));
- // TODO(austin): If this is the first edge in a while, skip to it (plus
- // an offset). Otherwise, slowly drift time to line up.
+ last_tick_timepoint +=
+ (monotonic_now - last_tick_timepoint) / period * period;
+ // If it's over 1/2 of a period back in time, that's wrong. Move it
+ // forwards to now.
+ if (last_tick_timepoint - monotonic_now < -period / 2) {
+ last_tick_timepoint += period;
+ }
+
+ // We should be sampling our sensors to kick off the control cycle 50 uS
+ // 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 = phased_loop.OffsetFromIntervalAndTime(
+ period, last_tick_timepoint + chrono::microseconds(50));
phased_loop.set_interval_and_offset(period, new_offset);
}
}
-
- if (pwm_trigger_) {
- pwm_detecter_thread.join();
- }
}
} // namespace wpilib
diff --git a/frc971/wpilib/sensor_reader.h b/frc971/wpilib/sensor_reader.h
index be96eea..12bcf37 100644
--- a/frc971/wpilib/sensor_reader.h
+++ b/frc971/wpilib/sensor_reader.h
@@ -9,6 +9,7 @@
#include "frc971/control_loops/control_loops.q.h"
#include "frc971/wpilib/ahal/DigitalGlitchFilter.h"
#include "frc971/wpilib/ahal/DigitalInput.h"
+#include "frc971/wpilib/ahal/DriverStation.h"
#include "frc971/wpilib/dma.h"
#include "frc971/wpilib/dma_edge_counting.h"
#include "frc971/wpilib/encoder_and_potentiometer.h"
@@ -43,8 +44,10 @@
dma_synchronizer_->Add(handler);
}
- // Sets the pwm trigger.
- void set_pwm_trigger(::std::unique_ptr<frc::DigitalInput> pwm_trigger);
+ // Sets PWM trigger mode. If true, synchronize the control loops with the PWM
+ // pulses. The sensors are sampled 50 uS after the falling edge of the PWM
+ // pulse.
+ void set_pwm_trigger(bool trigger) { pwm_trigger_ = trigger; }
// Stops the pwm trigger on the next iteration.
void Quit() { run_ = false; }
@@ -177,21 +180,16 @@
// Gets called right before the DMA synchronizer is up and running.
virtual void Start() {}
- // Uses the pwm trigger to find the pwm cycle width and offset for that
- // iteration.
- void RunPWMDetecter();
+ // Returns the monotonic time of the start of the first PWM cycle.
+ // Returns min_time if no start time could be calculated.
+ monotonic_clock::time_point GetPWMStartTime();
- ::std::unique_ptr<frc::DigitalInput> pwm_trigger_;
-
- // Mutex to manage access to the period and tick time variables.
- ::aos::stl_mutex tick_time_mutex_;
- monotonic_clock::time_point last_tick_time_monotonic_timepoint_ =
- monotonic_clock::min_time;
- chrono::nanoseconds last_period_;
+ bool pwm_trigger_;
::std::unique_ptr<::frc971::wpilib::DMASynchronizer> dma_synchronizer_;
::std::atomic<bool> run_{true};
+ ::frc::DriverStation *ds_;
};
} // namespace wpilib
diff --git a/y2017/wpilib_interface.cc b/y2017/wpilib_interface.cc
index a5235c9..12e5760 100644
--- a/y2017/wpilib_interface.cc
+++ b/y2017/wpilib_interface.cc
@@ -465,7 +465,7 @@
reader.set_autonomous_mode(0, make_unique<DigitalInput>(9));
reader.set_autonomous_mode(1, make_unique<DigitalInput>(8));
- reader.set_pwm_trigger(make_unique<DigitalInput>(7));
+ reader.set_pwm_trigger(true);
::std::thread reader_thread(::std::ref(reader));
diff --git a/y2018/wpilib_interface.cc b/y2018/wpilib_interface.cc
index 2de1741..968de9e 100644
--- a/y2018/wpilib_interface.cc
+++ b/y2018/wpilib_interface.cc
@@ -683,7 +683,7 @@
reader.set_claw_beambreak(make_unique<frc::DigitalInput>(8));
reader.set_box_back_beambreak(make_unique<frc::DigitalInput>(9));
- reader.set_pwm_trigger(make_unique<frc::DigitalInput>(25));
+ reader.set_pwm_trigger(true);
reader.set_lidar_lite_input(make_unique<frc::DigitalInput>(22));
diff --git a/y2019/wpilib_interface.cc b/y2019/wpilib_interface.cc
index eee7342..70cd6a2 100644
--- a/y2019/wpilib_interface.cc
+++ b/y2019/wpilib_interface.cc
@@ -460,7 +460,7 @@
reader.set_stilts_absolute_pwm(make_unique<frc::DigitalInput>(3));
reader.set_stilts_potentiometer(make_unique<frc::AnalogInput>(3));
- reader.set_pwm_trigger(make_unique<frc::DigitalInput>(25));
+ reader.set_pwm_trigger(true);
reader.set_vacuum_sensor(7);
::std::thread reader_thread(::std::ref(reader));