split out the interrupt-based edge-counting code
Change-Id: I57f7b451e1ce16e092c9c86f44b6b45e71bbc80d
diff --git a/frc971/wpilib/interrupt_edge_counting.cc b/frc971/wpilib/interrupt_edge_counting.cc
new file mode 100644
index 0000000..4bdb2d2
--- /dev/null
+++ b/frc971/wpilib/interrupt_edge_counting.cc
@@ -0,0 +1,104 @@
+#include "frc971/wpilib/interrupt_edge_counting.h"
+
+#include "aos/common/time.h"
+#include "aos/linux_code/init.h"
+
+namespace frc971 {
+namespace wpilib {
+
+void EdgeCounter::GatherPolledValue() {
+ shadow_values_.polled_value = input_->Get();
+ bool miss_match = (shadow_values_.polled_value != current_value_);
+ if (miss_match && last_miss_match_) {
+ current_value_ = shadow_values_.polled_value;
+ last_miss_match_ = false;
+ } else {
+ last_miss_match_ = miss_match;
+ }
+}
+
+void EdgeCounter::operator()() {
+ ::aos::SetCurrentThreadName("EdgeCounter_" +
+ ::std::to_string(input_->GetChannelForRouting()));
+
+ input_->RequestInterrupts();
+ input_->SetUpSourceEdge(true, true);
+
+ {
+ ::std::unique_lock<::aos::stl_mutex> mutex_guard(*mutex());
+ current_value_ = input_->Get();
+ }
+
+ ::aos::SetCurrentThreadRealtimePriority(priority());
+ InterruptableSensorBase::WaitResult result = InterruptableSensorBase::kBoth;
+ while (should_run()) {
+ result = input_->WaitForInterrupt(
+ 0.1, result != InterruptableSensorBase::kTimeout);
+ if (result == InterruptableSensorBase::kTimeout) {
+ continue;
+ }
+ interrupt_received();
+
+ ::std::unique_lock<::aos::stl_mutex> mutex_guard(*mutex());
+ int32_t encoder_value = encoder_->GetRaw();
+ bool hall_value = input_->Get();
+ if (current_value_ != hall_value) {
+ if (hall_value) {
+ ++shadow_values_.positive_interrupt_count;
+ shadow_values_.last_positive_encoder_value = encoder_value;
+ } else {
+ ++shadow_values_.negative_interrupt_count;
+ shadow_values_.last_negative_encoder_value = encoder_value;
+ }
+ current_value_ = hall_value;
+ } else {
+ LOG(WARNING, "Detected spurious edge on %d. Dropping it.\n",
+ input_->GetChannelForRouting());
+ }
+ }
+}
+
+void InterruptSynchronizer::RunIteration() {
+ while (true) {
+ if (!TryStartIteration()) continue;
+
+ // Wait more than the amount of time it takes for a digital input change
+ // to go from visible to software to having triggered an interrupt.
+ ::aos::time::SleepFor(::aos::time::Time::InUS(120));
+
+ if (TryFinishingIteration()) return;
+ }
+}
+
+bool InterruptSynchronizer::TryStartIteration() {
+ for (auto &c : handlers_) {
+ c->save_interrupt_count();
+ }
+
+ {
+ ::std::unique_lock<::aos::stl_mutex> mutex_guard(mutex_);
+ for (auto &c : handlers_) {
+ c->GatherPolledValue();
+ }
+ }
+ return true;
+}
+
+bool InterruptSynchronizer::TryFinishingIteration() {
+ // Make sure no interrupts have occurred while we were waiting. If they
+ // have, we are in an inconsistent state and need to try again.
+ ::std::unique_lock<::aos::stl_mutex> mutex_guard(mutex_);
+ for (auto &c : handlers_) {
+ if (c->interrupt_count_changed()) {
+ LOG(WARNING, "got an interrupt while sampling. retrying\n");
+ return false;
+ }
+ }
+ for (auto &c : handlers_) {
+ c->CommitValue();
+ }
+ return true;
+}
+
+} // namespace wpilib
+} // namespace frc971
diff --git a/frc971/wpilib/interrupt_edge_counting.h b/frc971/wpilib/interrupt_edge_counting.h
new file mode 100644
index 0000000..bd76d7c
--- /dev/null
+++ b/frc971/wpilib/interrupt_edge_counting.h
@@ -0,0 +1,217 @@
+#ifndef FRC971_WPILIB_INTERRUPT_EDGE_COUNTING_H_
+#define FRC971_WPILIB_INTERRUPT_EDGE_COUNTING_H_
+
+#include <memory>
+#include <atomic>
+#include <thread>
+#include <vector>
+
+#include "aos/common/stl_mutex.h"
+#include "aos/common/macros.h"
+
+#include "DigitalSource.h"
+#include "Encoder.h"
+#include "AnalogInput.h"
+#include "Utility.h"
+
+namespace frc971 {
+namespace wpilib {
+
+class InterruptSynchronizer;
+
+// Handles interrupts arriving from a single source.
+//
+// Instances of subclasses should be passed to InterruptSynchronizer::Add to use
+// them. All methods which are called with the lock held should avoid taking too
+// long because they directly contribute to interrupt-handling latency for all
+// InterruptHandlers in the same InterruptSynchronizer.
+//
+// Each instance handles any important sensor reading which must happen right
+// after an interrupt triggers from a single source. It is also important that
+// some sensors are sampled after any pending interrupts have been processed.
+// This is handled using a per-InterruptSynchronizer mutex. Each
+// InterruptHandler records that it has received an interrupt, locks the mutex,
+// and then updates a "shadow" state. The InterruptSynchronizer then triggers
+// making this "shadow" state visible after making sure no more interrupts have
+// arrived while holding the mutex.
+class InterruptHandler {
+ public:
+ virtual ~InterruptHandler() {}
+
+ // Stops the thread which actually does the sampling and waits for it to
+ // finish.
+ void Quit() {
+ run_ = false;
+ thread_.join();
+ }
+
+ // Starts the thread running.
+ // set_priority and set_mutex must be called first.
+ void Start() {
+ CHECK_NE(nullptr, mutex_);
+ CHECK_NE(0, priority_);
+ thread_ = ::std::thread(::std::ref(*this));
+ }
+
+ // Polls the current values and saves them to the "shadow" output.
+ // Called while the lock is held.
+ virtual void GatherPolledValue() = 0;
+
+ // Actually outputs the "shadow" state collected during the most recent
+ // GatherPolledValue.
+ // Called while the lock is held.
+ virtual void CommitValue() = 0;
+
+ // Saves the current interrupt count to be compared when
+ // interrupt_count_changed() is called.
+ void save_interrupt_count() { saved_interrupt_count_ = interrupt_count_; }
+ // Returns whether or not the interrupt count has changed since
+ // save_interrupt_count() was last called.
+ bool interrupt_count_changed() const {
+ return saved_interrupt_count_ != interrupt_count_;
+ }
+
+ // Sets the priority the thread will run at.
+ // This must be called before Start.
+ void set_priority(int priority) { priority_ = priority; }
+
+ // Sets the mutex to use for synchronizing readings.
+ // This must be called before Start.
+ void set_mutex(::aos::stl_mutex *mutex) { mutex_ = mutex; }
+
+ // Waits for interrupts, locks the mutex, and updates the internal state.
+ // Should only be called by the (internal) ::std::thread.
+ virtual void operator()() = 0;
+
+ protected:
+ // Indicates that another interrupt has been received (not handled yet).
+ void interrupt_received() { ++interrupt_count_; }
+
+ int priority() const { return priority_; }
+
+ ::aos::stl_mutex *mutex() { return mutex_; }
+
+ // Returns true if the thread should continue running.
+ bool should_run() const { return run_; }
+
+ private:
+ ::std::atomic<int> interrupt_count_{0};
+ int saved_interrupt_count_;
+
+ ::std::atomic<bool> run_{true};
+ ::std::thread thread_;
+
+ int priority_ = 0;
+ ::aos::stl_mutex *mutex_ = nullptr;
+};
+
+// Latches the value of an encoder on rising and falling edges of a digital
+// input.
+class EdgeCounter : public InterruptHandler {
+ public:
+ EdgeCounter(Encoder *encoder, DigitalSource *input)
+ : encoder_(encoder), input_(input) {}
+
+ // Returns the current interrupt edge counts and encoder values.
+ int positive_interrupt_count() const {
+ return output_.positive_interrupt_count;
+ }
+ int negative_interrupt_count() const {
+ return output_.negative_interrupt_count;
+ }
+ int32_t last_positive_encoder_value() const {
+ return output_.last_positive_encoder_value;
+ }
+ int32_t last_negative_encoder_value() const {
+ return output_.last_negative_encoder_value;
+ }
+ // Returns the current polled value.
+ bool polled_value() const { return output_.polled_value; }
+
+ private:
+ struct OutputValues {
+ bool polled_value = false;
+ int positive_interrupt_count = 0, negative_interrupt_count = 0;
+ int32_t last_positive_encoder_value = 0, last_negative_encoder_value = 0;
+ };
+
+ void GatherPolledValue() override;
+ void CommitValue() override { output_ = shadow_values_; }
+ void operator()() override;
+
+ Encoder *encoder_;
+ DigitalSource *input_;
+
+ // The following variables represent the current "shadow" state.
+ bool current_value_ = false;
+ bool last_miss_match_ = true;
+ OutputValues shadow_values_;
+
+ // The actual output values.
+ OutputValues output_;
+
+ DISALLOW_COPY_AND_ASSIGN(EdgeCounter);
+};
+
+// Synchronizes interrupts with poll-based sampling on multiple
+// InterruptHandlers.
+//
+// See InterruptHandler for an overview of the logic.
+//
+// Usage is to create an instance, call Add 1 or more times, call Start, and
+// then call RunIteration during normal sensor sampling. After RunIteration
+// returns, the output values from the various InterruptHandlers can be
+// retrieved.
+class InterruptSynchronizer {
+ public:
+ InterruptSynchronizer(int interrupt_priority)
+ : interrupt_priority_(interrupt_priority) {}
+
+ void Add(::std::unique_ptr<InterruptHandler> handler) {
+ handler->set_mutex(&mutex_);
+ handler->set_priority(interrupt_priority_);
+ handlers_.emplace_back(::std::move(handler));
+ }
+
+ void Start() {
+ for (auto &c : handlers_) {
+ c->Start();
+ }
+ }
+
+ // Updates all of the counts and makes sure everything is synchronized.
+ // IMPORTANT: This will usually only take 120uS but WILL occasionally take
+ // longer, so be careful about letting that jitter get into control loops.
+ void RunIteration();
+
+ // Asks all of the InterruptHandlers to stop and waits until they have done
+ // so.
+ void Quit() {
+ for (auto &c : handlers_) {
+ c->Quit();
+ }
+ }
+
+ private:
+ // Starts a sampling iteration. See RunIteration for usage.
+ // Returns true if we are ready to go or false if we already need to retry.
+ bool TryStartIteration();
+
+ // Attempts to finish a sampling iteration. See RunIteration for usage.
+ // Returns true if the iteration succeeded, and false otherwise.
+ bool TryFinishingIteration();
+
+ const int interrupt_priority_;
+
+ // The mutex used to synchronize all the sampling.
+ ::aos::stl_mutex mutex_;
+
+ ::std::vector<::std::unique_ptr<InterruptHandler>> handlers_;
+
+ DISALLOW_COPY_AND_ASSIGN(InterruptSynchronizer);
+};
+
+} // namespace wpilib
+} // namespace frc971
+
+#endif // FRC971_WPILIB_INTERRUPT_EDGE_COUNTING_H_
diff --git a/frc971/wpilib/wpilib.gyp b/frc971/wpilib/wpilib.gyp
index 39a5036..fce7e55 100644
--- a/frc971/wpilib/wpilib.gyp
+++ b/frc971/wpilib/wpilib.gyp
@@ -26,6 +26,26 @@
'loop_output_handler',
'buffered_pcm',
'gyro_sender',
+ 'interrupt_edge_counting',
+ ],
+ },
+ {
+ 'target_name': 'interrupt_edge_counting',
+ 'type': 'static_library',
+ 'sources': [
+ 'interrupt_edge_counting.cc',
+ ],
+ 'dependencies': [
+ '<(EXTERNALS):WPILib',
+ '<(AOS)/build/aos.gyp:logging',
+ '<(AOS)/common/common.gyp:stl_mutex',
+ '<(AOS)/common/common.gyp:time',
+ '<(AOS)/linux_code/linux_code.gyp:init',
+ ],
+ 'export_dependent_settings': [
+ '<(EXTERNALS):WPILib',
+ '<(AOS)/build/aos.gyp:logging',
+ '<(AOS)/common/common.gyp:stl_mutex',
],
},
{
diff --git a/frc971/wpilib/wpilib_interface.cc b/frc971/wpilib/wpilib_interface.cc
index 207c481..3d5958b 100644
--- a/frc971/wpilib/wpilib_interface.cc
+++ b/frc971/wpilib/wpilib_interface.cc
@@ -29,6 +29,7 @@
#include "frc971/wpilib/buffered_solenoid.h"
#include "frc971/wpilib/buffered_pcm.h"
#include "frc971/wpilib/gyro_sender.h"
+#include "frc971/wpilib/interrupt_edge_counting.h"
#include "Encoder.h"
#include "Talon.h"
@@ -48,254 +49,6 @@
namespace frc971 {
namespace wpilib {
-// TODO(brian): Split this out into a separate file once DMA is in.
-class EdgeCounter {
- public:
- EdgeCounter(int priority, Encoder *encoder, HallEffect *input,
- ::aos::stl_mutex *mutex)
- : priority_(priority),
- encoder_(encoder),
- input_(input),
- mutex_(mutex),
- run_(true),
- any_interrupt_count_(0) {
- thread_.reset(new ::std::thread(::std::ref(*this)));
- }
-
- // Waits for interrupts, locks the mutex, and updates the internal state.
- // Updates the any_interrupt_count count when the interrupt comes in without
- // the lock.
- void operator()() {
- ::aos::SetCurrentThreadName("EdgeCounter_" +
- ::std::to_string(input_->GetChannel()));
-
- input_->RequestInterrupts();
- input_->SetUpSourceEdge(true, true);
-
- {
- ::std::unique_lock<::aos::stl_mutex> mutex_guard(*mutex_);
- current_value_ = input_->GetHall();
- }
-
- ::aos::SetCurrentThreadRealtimePriority(priority_);
- InterruptableSensorBase::WaitResult result = InterruptableSensorBase::kBoth;
- while (run_) {
- result = input_->WaitForInterrupt(
- 0.1, result != InterruptableSensorBase::kTimeout);
- if (result == InterruptableSensorBase::kTimeout) {
- continue;
- }
- ++any_interrupt_count_;
-
- ::std::unique_lock<::aos::stl_mutex> mutex_guard(*mutex_);
- int32_t encoder_value = encoder_->GetRaw();
- bool hall_value = input_->GetHall();
- if (current_value_ != hall_value) {
- if (hall_value) {
- ++positive_interrupt_count_;
- last_positive_encoder_value_ = encoder_value;
- } else {
- ++negative_interrupt_count_;
- last_negative_encoder_value_ = encoder_value;
- }
- } else {
- LOG(WARNING, "Detected spurious edge on %d. Dropping it.\n",
- input_->GetChannel());
- }
-
- current_value_ = hall_value;
- }
- }
-
- // Updates the internal hall effect value given this new observation.
- // The mutex provided at construction time must be held during this operation.
- void set_polled_value(bool value) {
- polled_value_ = value;
- bool miss_match = (value != current_value_);
- if (miss_match && last_miss_match_) {
- current_value_ = value;
- last_miss_match_ = false;
- } else {
- last_miss_match_ = miss_match;
- }
- }
-
- // Signals the thread to quit next time it gets an interrupt.
- void Quit() {
- run_ = false;
- thread_->join();
- }
-
- // Returns the total number of interrupts since construction time. This
- // should be done without the mutex held.
- int any_interrupt_count() const { return any_interrupt_count_; }
- // Returns the current interrupt edge counts and encoder values.
- // The mutex provided at construction time must be held during this operation.
- int positive_interrupt_count() const { return positive_interrupt_count_; }
- int negative_interrupt_count() const { return negative_interrupt_count_; }
- int32_t last_positive_encoder_value() const {
- return last_positive_encoder_value_;
- }
- int32_t last_negative_encoder_value() const {
- return last_negative_encoder_value_;
- }
- // Returns the current polled value.
- bool polled_value() const { return polled_value_; }
-
- private:
- int priority_;
- Encoder *encoder_;
- HallEffect *input_;
- ::aos::stl_mutex *mutex_;
- ::std::atomic<bool> run_;
-
- ::std::atomic<int> any_interrupt_count_;
-
- // The following variables represent the current state. They must be
- // synchronized by mutex_;
- bool current_value_ = false;
- bool polled_value_ = false;
- bool last_miss_match_ = true;
- int positive_interrupt_count_ = 0;
- int negative_interrupt_count_ = 0;
- int32_t last_positive_encoder_value_ = 0;
- int32_t last_negative_encoder_value_ = 0;
-
- ::std::unique_ptr<::std::thread> thread_;
-};
-
-// This class will synchronize sampling edges on a bunch of HallEffects with
-// the periodic poll.
-//
-// The data is provided to subclasses by calling SaveState when the state is
-// consistent and ready.
-//
-// TODO(brian): Split this out into a separate file once DMA is in.
-template <int num_sensors>
-class PeriodicHallSynchronizer {
- public:
- PeriodicHallSynchronizer(
- const char *name, int priority, int interrupt_priority,
- ::std::unique_ptr<Encoder> encoder,
- ::std::array<::std::unique_ptr<HallEffect>, num_sensors> *sensors)
- : name_(name),
- priority_(priority),
- encoder_(::std::move(encoder)),
- run_(true) {
- for (int i = 0; i < num_sensors; ++i) {
- sensors_[i] = ::std::move((*sensors)[i]);
- edge_counters_[i] = ::std::unique_ptr<EdgeCounter>(new EdgeCounter(
- interrupt_priority, encoder_.get(), sensors_[i].get(), &mutex_));
- }
- }
-
- const char *name() const { return name_.c_str(); }
-
- void StartThread() { thread_.reset(new ::std::thread(::std::ref(*this))); }
-
- // Called when the state is consistent and up to date.
- virtual void SaveState() = 0;
-
- // Starts a sampling iteration. See RunIteration for usage.
- void StartIteration() {
- // Start by capturing the current interrupt counts.
- for (int i = 0; i < num_sensors; ++i) {
- interrupt_counts_[i] = edge_counters_[i]->any_interrupt_count();
- }
-
- {
- // Now, update the encoder and sensor values.
- ::std::unique_lock<::aos::stl_mutex> mutex_guard(mutex_);
- encoder_value_ = encoder_->GetRaw();
- for (int i = 0; i < num_sensors; ++i) {
- edge_counters_[i]->set_polled_value(sensors_[i]->GetHall());
- }
- }
- }
-
- // Attempts to finish a sampling iteration. See RunIteration for usage.
- // Returns true if the iteration succeeded, and false otherwise.
- bool TryFinishingIteration() {
- // Make sure no interrupts have occurred while we were waiting. If they
- // have, we are in an inconsistent state and need to try again.
- ::std::unique_lock<::aos::stl_mutex> mutex_guard(mutex_);
- bool retry = false;
- for (int i = 0; i < num_sensors; ++i) {
- retry = retry || (interrupt_counts_[i] !=
- edge_counters_[i]->any_interrupt_count());
- }
- if (!retry) {
- SaveState();
- return true;
- }
- LOG(WARNING, "Got an interrupt while sampling encoder %s, retrying\n",
- name());
- return false;
- }
-
- void RunIteration() {
- while (true) {
- StartIteration();
-
- // Wait more than the amount of time it takes for a digital input change
- // to go from visible to software to having triggered an interrupt.
- ::aos::time::SleepFor(::aos::time::Time::InUS(120));
-
- if (TryFinishingIteration()) {
- return;
- }
- }
- }
-
- void operator()() {
- ::aos::SetCurrentThreadName("HallSync" + ::std::to_string(num_sensors));
- ::aos::SetCurrentThreadRealtimePriority(priority_);
- while (run_) {
- ::aos::time::PhasedLoopXMS(10, 9000);
- RunIteration();
- }
- }
-
- void Quit() {
- run_ = false;
- for (int i = 0; i < num_sensors; ++i) {
- edge_counters_[i]->Quit();
- }
- if (thread_) {
- thread_->join();
- }
- }
-
- protected:
- // These values are only safe to fetch from inside SaveState()
- int32_t encoder_value() const { return encoder_value_; }
- ::std::array<::std::unique_ptr<EdgeCounter>, num_sensors> &edge_counters() {
- return edge_counters_;
- }
-
- private:
- // A descriptive name for error messages.
- ::std::string name_;
- // The priority of the polling thread.
- int priority_;
- // The Encoder to sample.
- ::std::unique_ptr<Encoder> encoder_;
- // A list of all the digital inputs.
- ::std::array<::std::unique_ptr<HallEffect>, num_sensors> sensors_;
- // The mutex used to synchronize all the state.
- ::aos::stl_mutex mutex_;
- ::std::atomic<bool> run_;
-
- // The state.
- // The current encoder value.
- int32_t encoder_value_ = 0;
- // The current edge counters.
- ::std::array<::std::unique_ptr<EdgeCounter>, num_sensors> edge_counters_;
-
- ::std::unique_ptr<::std::thread> thread_;
- ::std::array<int, num_sensors> interrupt_counts_;
-};
-
double drivetrain_translate(int32_t in) {
return static_cast<double>(in) /
(256.0 /*cpr*/ * 2.0 /*2x. Stupid WPILib*/) *