Added zeroing to ADIS16448
Broke out averaging from gyro_sender into a new class.
Change-Id: Iedaa0a7bd39337dbbe61a6d856ce28227f2747a5
diff --git a/frc971/zeroing/averager.h b/frc971/zeroing/averager.h
new file mode 100644
index 0000000..879c246
--- /dev/null
+++ b/frc971/zeroing/averager.h
@@ -0,0 +1,56 @@
+#ifndef FRC971_ZEROING_AVERAGER_H_
+#define FRC971_ZEROING_AVERAGER_H_
+
+#include <algorithm>
+#include <array>
+#include <stdint.h>
+
+namespace frc971 {
+namespace zeroing {
+
+// Averages a set of given numbers. Numbers are given one at a time. Once full
+// the average may be requested.
+template <typename data_type, size_t data_size>
+class Averager {
+ public:
+ // Adds one data point to the set of data points to be averaged.
+ // If more than "data_size" samples are added, they will start overwriting
+ // the oldest ones.
+ void AddData(data_type data) {
+ data_[data_point_index_] = data;
+ num_data_points_ = ::std::min(data_size, num_data_points_ + 1);
+ data_point_index_ = (data_point_index_ + 1) % data_size;
+ }
+
+ // Returns the average of the data points.
+ data_type GetAverage() const {
+ // TODO(phil): What do we want to do without any elements?
+ if (num_data_points_ == 0) {
+ return 0;
+ }
+
+ data_type average = 0;
+ for (data_type data : data_) {
+ average += data;
+ }
+ return average / num_data_points_;
+ }
+
+ // Returns true when we've gathered data_size data points.
+ bool full() const { return num_data_points_ >= data_size; };
+
+ size_t size() const { return data_size; }
+
+ private:
+ // Data points to be averaged.
+ ::std::array<data_type, data_size> data_;
+ // Which data point in "data_" will be filled in next.
+ size_t data_point_index_ = 0;
+ // Number of data points added via AddData().
+ size_t num_data_points_ = 0;
+};
+
+} // namespace zeroing
+} // namespace frc971
+
+#endif // FRC971_ZEROING_AVERAGER_H_