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;
}