Added zeroing to ADIS16448
Broke out averaging from gyro_sender into a new class.
Change-Id: Iedaa0a7bd39337dbbe61a6d856ce28227f2747a5
diff --git a/frc971/wpilib/ADIS16448.cc b/frc971/wpilib/ADIS16448.cc
index f2d5119..c5e083e 100644
--- a/frc971/wpilib/ADIS16448.cc
+++ b/frc971/wpilib/ADIS16448.cc
@@ -11,6 +11,7 @@
#include "aos/linux_code/init.h"
#include "frc971/wpilib/imu.q.h"
+#include "frc971/zeroing/averager.h"
namespace frc971 {
namespace wpilib {
@@ -130,6 +131,13 @@
}
LOG(INFO, "IMU initialized successfully\n");
+ // Rounded to approximate the 204.8 Hz.
+ constexpr size_t kImuSendRate = 205;
+
+ zeroing::Averager<double, 6 * kImuSendRate> average_gyro_x;
+ zeroing::Averager<double, 6 * kImuSendRate> average_gyro_y;
+ zeroing::Averager<double, 6 * kImuSendRate> average_gyro_z;
+
bool got_an_interrupt = false;
while (run_) {
{
@@ -188,6 +196,24 @@
message->gyro_z =
ConvertValue(&to_receive[8], kGyroLsbDegreeSecond * M_PI / 180.0);
+ // The first few seconds of samples are averaged and subtracted from
+ // subsequent samples for zeroing purposes.
+ if (!gyros_are_zeroed()) {
+ average_gyro_x.AddData(message->gyro_x);
+ average_gyro_y.AddData(message->gyro_y);
+ average_gyro_z.AddData(message->gyro_z);
+
+ if (average_gyro_x.full() && average_gyro_y.full() &&
+ average_gyro_z.full()) {
+ gyro_x_zeroed_offset_ = average_gyro_x.GetAverage();
+ gyro_y_zeroed_offset_ = average_gyro_y.GetAverage();
+ gyro_z_zeroed_offset_ = average_gyro_z.GetAverage();
+ LOG(DEBUG, "total gyro zero offset X:%f, Y:%f, Z:%f\n",
+ gyro_x_zeroed_offset_, gyro_y_zeroed_offset_, gyro_z_zeroed_offset_);
+ gyros_are_zeroed_.store(true);
+ }
+ }
+
message->accelerometer_x =
ConvertValue(&to_receive[10], kAccelerometerLsbG);
message->accelerometer_y =
diff --git a/frc971/wpilib/ADIS16448.h b/frc971/wpilib/ADIS16448.h
index 778ab49..4fefd07 100644
--- a/frc971/wpilib/ADIS16448.h
+++ b/frc971/wpilib/ADIS16448.h
@@ -37,6 +37,12 @@
void Quit() { run_ = false; }
+ bool gyros_are_zeroed() const { return gyros_are_zeroed_.load(); }
+
+ double gyro_x_zeroed_offset() const { return gyro_x_zeroed_offset_; }
+ double gyro_y_zeroed_offset() const { return gyro_y_zeroed_offset_; }
+ double gyro_z_zeroed_offset() const { return gyro_z_zeroed_offset_; }
+
private:
// Converts a 16-bit value at data to a scaled output value where a value of 1
// corresponds to lsb_per_output.
@@ -70,6 +76,12 @@
DigitalInput *const dio1_;
::std::atomic<bool> run_{true};
+
+ // The averaged values of the gyro over 6 seconds after power up.
+ ::std::atomic<bool> gyros_are_zeroed_{false};
+ double gyro_x_zeroed_offset_ = 0.0;
+ double gyro_y_zeroed_offset_ = 0.0;
+ double gyro_z_zeroed_offset_ = 0.0;
};
} // namespace wpilib
diff --git a/frc971/wpilib/BUILD b/frc971/wpilib/BUILD
index 6ea778a..5304207 100644
--- a/frc971/wpilib/BUILD
+++ b/frc971/wpilib/BUILD
@@ -99,8 +99,9 @@
'gyro_sender.h',
],
deps = [
- '//frc971/queues:gyro',
':gyro_interface',
+ '//frc971/queues:gyro',
+ '//frc971/zeroing:averager',
'//aos/common/logging',
'//aos/common/logging:queue_logging',
'//aos/common/util:phased_loop',
@@ -225,12 +226,13 @@
'ADIS16448.cc',
],
deps = [
- '//third_party/allwpilib_2016:wpilib',
+ ':imu_queue',
'//aos/common/logging',
'//aos/common/logging:queue_logging',
'//aos/common:time',
'//aos/linux_code:init',
- ':imu_queue',
+ '//frc971/zeroing:averager',
+ '//third_party/allwpilib_2016:wpilib',
],
)
diff --git a/frc971/wpilib/gyro_sender.cc b/frc971/wpilib/gyro_sender.cc
index dd3b3e1..82e9775 100644
--- a/frc971/wpilib/gyro_sender.cc
+++ b/frc971/wpilib/gyro_sender.cc
@@ -10,6 +10,7 @@
#include "aos/linux_code/init.h"
#include "frc971/queues/gyro.q.h"
+#include "frc971/zeroing/averager.h"
namespace frc971 {
namespace wpilib {
@@ -35,10 +36,8 @@
int startup_cycles_left = 2 * kReadingRate;
- double zeroing_data[6 * kReadingRate];
- size_t zeroing_index = 0;
+ zeroing::Averager<double, 6 * kReadingRate> zeroing_data;
bool zeroed = false;
- bool have_zeroing_data = false;
double zero_offset = 0;
::aos::time::PhasedLoop phased_loop(
@@ -118,22 +117,12 @@
message->velocity = 0.0;
message.Send();
}
- zeroing_data[zeroing_index] = new_angle;
- ++zeroing_index;
- if (zeroing_index >= sizeof(zeroing_data) / sizeof(zeroing_data[0])) {
- zeroing_index = 0;
- have_zeroing_data = true;
- }
+ zeroing_data.AddData(new_angle);
::aos::joystick_state.FetchLatest();
if (::aos::joystick_state.get() && ::aos::joystick_state->enabled &&
- have_zeroing_data) {
- zero_offset = 0;
- for (size_t i = 0; i < sizeof(zeroing_data) / sizeof(zeroing_data[0]);
- ++i) {
- zero_offset -= zeroing_data[i];
- }
- zero_offset /= sizeof(zeroing_data) / sizeof(zeroing_data[0]);
+ zeroing_data.full()) {
+ zero_offset = -zeroing_data.GetAverage();
LOG(INFO, "total zero offset %f\n", zero_offset);
zeroed = true;
}
diff --git a/frc971/zeroing/BUILD b/frc971/zeroing/BUILD
index 8a8d82c..c63d97d 100644
--- a/frc971/zeroing/BUILD
+++ b/frc971/zeroing/BUILD
@@ -2,6 +2,24 @@
load('/aos/build/queues', 'queue_library')
+cc_library(
+ name = 'averager',
+ hdrs = [
+ 'averager.h',
+ ],
+)
+
+cc_test(
+ name = 'averager_test',
+ srcs = [
+ 'averager_test.cc',
+ ],
+ deps = [
+ ':averager',
+ '//aos/testing:googletest',
+ ],
+)
+
queue_library(
name = 'zeroing_queue',
srcs = [
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_
diff --git a/frc971/zeroing/averager_test.cc b/frc971/zeroing/averager_test.cc
new file mode 100644
index 0000000..930a3cb
--- /dev/null
+++ b/frc971/zeroing/averager_test.cc
@@ -0,0 +1,35 @@
+#include "gtest/gtest.h"
+#include "frc971/zeroing/averager.h"
+
+namespace frc971 {
+namespace zeroing {
+
+class AveragerTest : public ::testing::Test {
+ protected:
+ void SetUp() override {}
+};
+
+// Makes sure that we can compute the average of a bunch of integers.
+TEST_F(AveragerTest, ComputeIntegerAverage) {
+ Averager<int, 6> averager;
+ for (size_t i = 0; i < averager.size(); ++i) {
+ ASSERT_FALSE(averager.full());
+ averager.AddData(static_cast<int>(i));
+ }
+ ASSERT_TRUE(averager.full());
+ ASSERT_EQ(2, averager.GetAverage());
+}
+
+// Makes sure that we can compute the average of a bunch of floats.
+TEST_F(AveragerTest, ComputeFloatAverage) {
+ Averager<float, 100> averager;
+ for (size_t i = 0; i < averager.size(); ++i) {
+ ASSERT_FALSE(averager.full());
+ averager.AddData(static_cast<float>(i) / 3.0);
+ }
+ ASSERT_TRUE(averager.full());
+ ASSERT_NEAR(16.5, averager.GetAverage(), 0.001);
+}
+
+} // namespace zeroing
+} // namespace frc971