Merge "Use an amd64 Debian Buster sysroot for building"
diff --git a/aos/json_to_flatbuffer.cc b/aos/json_to_flatbuffer.cc
index 6508786..249a59e 100644
--- a/aos/json_to_flatbuffer.cc
+++ b/aos/json_to_flatbuffer.cc
@@ -120,25 +120,21 @@
// each message. Same goes for vectors.
class JsonParser {
public:
- JsonParser() { fbb_.ForceDefaults(1); }
+ JsonParser(flatbuffers::FlatBufferBuilder *fbb) : fbb_(fbb) {}
~JsonParser() {}
// Parses the json into a flatbuffer. Returns either an empty vector on
// error, or a vector with the flatbuffer data in it.
- flatbuffers::DetachedBuffer Parse(const std::string_view data,
- const flatbuffers::TypeTable *typetable) {
+ flatbuffers::Offset<flatbuffers::Table> Parse(
+ const std::string_view data, const flatbuffers::TypeTable *typetable) {
flatbuffers::uoffset_t end = 0;
bool result = DoParse(typetable, data, &end);
if (result) {
// On success, finish the table and build the vector.
- auto o = flatbuffers::Offset<flatbuffers::Table>(end);
- fbb_.Finish(o);
-
- return fbb_.Release();
+ return flatbuffers::Offset<flatbuffers::Table>(end);
} else {
- // Otherwise return an empty vector.
- return flatbuffers::DetachedBuffer();
+ return flatbuffers::Offset<flatbuffers::Table>(0);
}
}
@@ -170,7 +166,7 @@
bool PushElement(flatbuffers::ElementaryType elementary_type,
flatbuffers::Offset<flatbuffers::String> offset_value);
- flatbuffers::FlatBufferBuilder fbb_;
+ flatbuffers::FlatBufferBuilder *fbb_;
// This holds the state information that is needed as you recurse into
// nested structures.
@@ -254,7 +250,7 @@
} else {
// End of a nested struct! Add it.
const flatbuffers::uoffset_t end = WriteTable(
- stack_.back().typetable, stack_.back().elements, &fbb_);
+ stack_.back().typetable, stack_.back().elements, fbb_);
// We now want to talk about the parent structure. Pop the child.
stack_.pop_back();
@@ -453,9 +449,9 @@
}
if (in_vector()) {
- stack_.back().vector_elements.emplace_back(fbb_.CreateString(data));
+ stack_.back().vector_elements.emplace_back(fbb_->CreateString(data));
} else {
- stack_.back().elements.emplace_back(field_index, fbb_.CreateString(data));
+ stack_.back().elements.emplace_back(field_index, fbb_->CreateString(data));
}
return true;
}
@@ -631,7 +627,7 @@
static_cast<flatbuffers::ElementaryType>(type_code.base_type);
// Vectors have a start (unfortunately which needs to know the size)
- fbb_.StartVector(
+ fbb_->StartVector(
stack_.back().vector_elements.size(),
flatbuffers::InlineSize(elementary_type, stack_.back().typetable));
@@ -656,7 +652,7 @@
// Then an End which is placed into the buffer the same as any other offset.
stack_.back().elements.emplace_back(
field_index, flatbuffers::Offset<flatbuffers::String>(
- fbb_.EndVector(stack_.back().vector_elements.size())));
+ fbb_->EndVector(stack_.back().vector_elements.size())));
stack_.back().vector_elements.clear();
return true;
}
@@ -665,37 +661,37 @@
int64_t int_value) {
switch (elementary_type) {
case flatbuffers::ET_BOOL:
- fbb_.PushElement<bool>(int_value);
+ fbb_->PushElement<bool>(int_value);
return true;
case flatbuffers::ET_CHAR:
- fbb_.PushElement<int8_t>(int_value);
+ fbb_->PushElement<int8_t>(int_value);
return true;
case flatbuffers::ET_UCHAR:
- fbb_.PushElement<uint8_t>(int_value);
+ fbb_->PushElement<uint8_t>(int_value);
return true;
case flatbuffers::ET_SHORT:
- fbb_.PushElement<int16_t>(int_value);
+ fbb_->PushElement<int16_t>(int_value);
return true;
case flatbuffers::ET_USHORT:
- fbb_.PushElement<uint16_t>(int_value);
+ fbb_->PushElement<uint16_t>(int_value);
return true;
case flatbuffers::ET_INT:
- fbb_.PushElement<int32_t>(int_value);
+ fbb_->PushElement<int32_t>(int_value);
return true;
case flatbuffers::ET_UINT:
- fbb_.PushElement<uint32_t>(int_value);
+ fbb_->PushElement<uint32_t>(int_value);
return true;
case flatbuffers::ET_LONG:
- fbb_.PushElement<int64_t>(int_value);
+ fbb_->PushElement<int64_t>(int_value);
return true;
case flatbuffers::ET_ULONG:
- fbb_.PushElement<uint64_t>(int_value);
+ fbb_->PushElement<uint64_t>(int_value);
return true;
case flatbuffers::ET_FLOAT:
- fbb_.PushElement<float>(int_value);
+ fbb_->PushElement<float>(int_value);
return true;
case flatbuffers::ET_DOUBLE:
- fbb_.PushElement<double>(int_value);
+ fbb_->PushElement<double>(int_value);
return true;
case flatbuffers::ET_STRING:
case flatbuffers::ET_UTYPE:
@@ -730,10 +726,10 @@
ElementaryTypeName(elementary_type));
return false;
case flatbuffers::ET_FLOAT:
- fbb_.PushElement<float>(double_value);
+ fbb_->PushElement<float>(double_value);
return true;
case flatbuffers::ET_DOUBLE:
- fbb_.PushElement<double>(double_value);
+ fbb_->PushElement<double>(double_value);
return true;
}
return false;
@@ -762,7 +758,7 @@
return false;
case flatbuffers::ET_STRING:
case flatbuffers::ET_SEQUENCE:
- fbb_.PushElement(offset_value);
+ fbb_->PushElement(offset_value);
return true;
}
return false;
@@ -770,11 +766,29 @@
} // namespace
+flatbuffers::Offset<flatbuffers::Table> JsonToFlatbuffer(
+ const std::string_view data, const flatbuffers::TypeTable *typetable,
+ flatbuffers::FlatBufferBuilder *fbb) {
+ JsonParser p(fbb);
+ return p.Parse(data, typetable);
+}
+
flatbuffers::DetachedBuffer JsonToFlatbuffer(
const std::string_view data,
const flatbuffers::TypeTable *typetable) {
- JsonParser p;
- return p.Parse(data, typetable);
+ flatbuffers::FlatBufferBuilder fbb;
+ fbb.ForceDefaults(1);
+
+ const flatbuffers::Offset<flatbuffers::Table> result =
+ JsonToFlatbuffer(data, typetable, &fbb);
+ if (result.o != 0) {
+ fbb.Finish(result);
+
+ return fbb.Release();
+ } else {
+ // Otherwise return an empty vector.
+ return flatbuffers::DetachedBuffer();
+ }
}
::std::string BufferFlatbufferToJson(const uint8_t *buffer,
diff --git a/aos/json_to_flatbuffer.h b/aos/json_to_flatbuffer.h
index f90a602..c061c69 100644
--- a/aos/json_to_flatbuffer.h
+++ b/aos/json_to_flatbuffer.h
@@ -11,10 +11,28 @@
namespace aos {
-// Parses the flatbuffer into the vector, or returns an empty vector.
+// Parses the flatbuffer into the buffer, or returns an empty buffer.
flatbuffers::DetachedBuffer JsonToFlatbuffer(
const std::string_view data, const flatbuffers::TypeTable *typetable);
+// Parses the flatbuffer into the builder, and returns the offset.
+flatbuffers::Offset<flatbuffers::Table> JsonToFlatbuffer(
+ const std::string_view data, const flatbuffers::TypeTable *typetable,
+ flatbuffers::FlatBufferBuilder *fbb);
+
+// Typed versions of the above methods.
+template <typename T>
+inline flatbuffers::DetachedBuffer JsonToFlatbuffer(
+ const std::string_view data) {
+ return JsonToFlatbuffer(data, T::MiniReflectTypeTable());
+}
+template <typename T>
+inline flatbuffers::Offset<T> JsonToFlatbuffer(
+ const std::string_view data, flatbuffers::FlatBufferBuilder *fbb) {
+ return flatbuffers::Offset<T>(
+ JsonToFlatbuffer(data, T::MiniReflectTypeTable(), fbb).o);
+}
+
// Converts a flatbuffer into a Json string.
// multi_line controls if the Json is written out on multiple lines or one.
// The methods below are generally more useful than BufferFlatbufferToJson and
diff --git a/frc971/wpilib/gyro_sender.cc b/frc971/wpilib/gyro_sender.cc
index e7273ee..c62d40a 100644
--- a/frc971/wpilib/gyro_sender.cc
+++ b/frc971/wpilib/gyro_sender.cc
@@ -137,7 +137,7 @@
if (joystick_state_fetcher_.get() &&
joystick_state_fetcher_->outputs_enabled() &&
zeroing_data_.full()) {
- zero_offset_ = -zeroing_data_.GetAverage();
+ zero_offset_ = -zeroing_data_.GetAverage()(0, 0);
AOS_LOG(INFO, "total zero offset %f\n", zero_offset_);
zeroed_ = true;
}
diff --git a/frc971/zeroing/BUILD b/frc971/zeroing/BUILD
index b066dea..a2b908b 100644
--- a/frc971/zeroing/BUILD
+++ b/frc971/zeroing/BUILD
@@ -7,6 +7,10 @@
hdrs = [
"averager.h",
],
+ deps = [
+ "@com_github_google_glog//:glog",
+ "@org_tuxfamily_eigen//:eigen",
+ ],
)
cc_test(
@@ -17,6 +21,36 @@
deps = [
":averager",
"//aos/testing:googletest",
+ "@org_tuxfamily_eigen//:eigen",
+ ],
+)
+
+cc_library(
+ name = "imu_zeroer",
+ srcs = [
+ "imu_zeroer.cc",
+ ],
+ hdrs = [
+ "imu_zeroer.h",
+ ],
+ deps = [
+ ":averager",
+ "//frc971/wpilib:imu_fbs",
+ "@com_github_google_glog//:glog",
+ "@org_tuxfamily_eigen//:eigen",
+ ],
+)
+
+cc_test(
+ name = "imu_zeroer_test",
+ srcs = [
+ "imu_zeroer_test.cc",
+ ],
+ deps = [
+ ":imu_zeroer",
+ "//aos:flatbuffers",
+ "//aos/testing:googletest",
+ "@org_tuxfamily_eigen//:eigen",
],
)
diff --git a/frc971/zeroing/averager.h b/frc971/zeroing/averager.h
index 879c246..b6ff09d 100644
--- a/frc971/zeroing/averager.h
+++ b/frc971/zeroing/averager.h
@@ -5,45 +5,79 @@
#include <array>
#include <stdint.h>
+#include "Eigen/Dense"
+#include "glog/logging.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>
+// TODO(james): Look at deduplicating this with some of the work in the
+// MoveDetector.
+template <typename Scalar, size_t num_samples, int rows_per_sample = 1>
class Averager {
public:
+ typedef Eigen::Matrix<Scalar, rows_per_sample, 1> Vector;
// 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
+ // If more than "num_samples" samples are added, they will start overwriting
// the oldest ones.
- void AddData(data_type data) {
+ void AddData(Scalar data) {
+ CHECK_EQ(1, rows_per_sample);
+ AddData(Vector(data));
+ }
+ void AddData(const Vector &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;
+ num_data_points_ = std::min(num_samples, num_data_points_ + 1);
+ data_point_index_ = (data_point_index_ + 1) % num_samples;
}
// Returns the average of the data points.
- data_type GetAverage() const {
- // TODO(phil): What do we want to do without any elements?
+ Vector GetAverage() const {
if (num_data_points_ == 0) {
- return 0;
+ return Vector::Zero();
}
- data_type average = 0;
- for (data_type data : data_) {
+ Vector average;
+ average.setZero();
+ for (const Vector &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; };
+ // Return the difference between the min and max values in the data buffer.
+ Scalar GetRange() const {
+ if (num_data_points_ == 0) {
+ return 0.0;
+ }
+ Vector min_value;
+ min_value.setConstant(std::numeric_limits<Scalar>::max());
+ Vector max_value;
+ max_value.setConstant(std::numeric_limits<Scalar>::lowest());
+ // The array will always fill up starting at zero, so we can iterate from
+ // zero safely.
+ for (size_t ii = 0; ii < num_data_points_; ++ii) {
+ const Vector &value = data_[ii];
+ min_value = min_value.cwiseMin(value);
+ max_value = max_value.cwiseMax(value);
+ }
+ return (max_value - min_value).maxCoeff();
+ }
- size_t size() const { return data_size; }
+ void Reset() {
+ num_data_points_ = 0;
+ data_point_index_ = 0;
+ }
+
+ // Returns true when we've gathered num_samples data points.
+ bool full() const { return num_data_points_ >= num_samples; };
+
+ size_t size() const { return num_samples; }
private:
// Data points to be averaged.
- ::std::array<data_type, data_size> data_;
+ std::array<Vector, num_samples> data_;
// Which data point in "data_" will be filled in next.
size_t data_point_index_ = 0;
// Number of data points added via AddData().
diff --git a/frc971/zeroing/averager_test.cc b/frc971/zeroing/averager_test.cc
index 930a3cb..9f8f727 100644
--- a/frc971/zeroing/averager_test.cc
+++ b/frc971/zeroing/averager_test.cc
@@ -17,7 +17,7 @@
averager.AddData(static_cast<int>(i));
}
ASSERT_TRUE(averager.full());
- ASSERT_EQ(2, averager.GetAverage());
+ ASSERT_EQ(2, averager.GetAverage()(0, 0));
}
// Makes sure that we can compute the average of a bunch of floats.
@@ -28,7 +28,32 @@
averager.AddData(static_cast<float>(i) / 3.0);
}
ASSERT_TRUE(averager.full());
- ASSERT_NEAR(16.5, averager.GetAverage(), 0.001);
+ ASSERT_NEAR(16.5, averager.GetAverage()(0, 0), 0.001);
+}
+
+TEST_F(AveragerTest, CalculateRange) {
+ Averager<float, 5, 2> averager;
+ ASSERT_EQ(0, averager.GetRange());
+ averager.AddData({100, 10});
+ averager.AddData({105, 15});
+ averager.AddData({90, 9});
+ ASSERT_EQ(15, averager.GetRange());
+ for (size_t ii = 0; ii < averager.size(); ++ii) {
+ averager.AddData({10, 20});
+ }
+ ASSERT_EQ(0, averager.GetRange());
+}
+
+TEST_F(AveragerTest, ResetAverager) {
+ Averager<float, 5> averager;
+ for (size_t ii = 0; ii < averager.size(); ++ii) {
+ averager.AddData(10);
+ }
+ ASSERT_TRUE(averager.full());
+ ASSERT_EQ(10.0, averager.GetAverage()(0, 0));
+ averager.Reset();
+ ASSERT_FALSE(averager.full());
+ ASSERT_EQ(0.0, averager.GetAverage()(0, 0));
}
} // namespace zeroing
diff --git a/frc971/zeroing/imu_zeroer.cc b/frc971/zeroing/imu_zeroer.cc
new file mode 100644
index 0000000..25bf6f6
--- /dev/null
+++ b/frc971/zeroing/imu_zeroer.cc
@@ -0,0 +1,52 @@
+#include "frc971/zeroing/imu_zeroer.h"
+
+namespace frc971::zeroing {
+
+ImuZeroer::ImuZeroer() {
+ gyro_average_.setZero();
+ last_gyro_sample_.setZero();
+ last_accel_sample_.setZero();
+}
+
+bool ImuZeroer::Zeroed() const { return zeroed_ || Faulted(); }
+
+bool ImuZeroer::Faulted() const { return faulted_; }
+
+Eigen::Vector3d ImuZeroer::ZeroedGyro() const {
+ return last_gyro_sample_ - gyro_average_;
+}
+Eigen::Vector3d ImuZeroer::ZeroedAccel() const { return last_accel_sample_; }
+Eigen::Vector3d ImuZeroer::GyroOffset() const { return gyro_average_; }
+
+bool ImuZeroer::GyroZeroReady() const {
+ return gyro_averager_.full() && gyro_averager_.GetRange() < kGyroMaxVariation;
+}
+
+bool ImuZeroer::AccelZeroReady() const {
+ return accel_averager_.full() &&
+ accel_averager_.GetRange() < kAccelMaxVariation;
+}
+
+void ImuZeroer::ProcessMeasurement(const IMUValues &values) {
+ last_gyro_sample_ << values.gyro_x(), values.gyro_y(), values.gyro_z();
+ gyro_averager_.AddData(last_gyro_sample_);
+ last_accel_sample_ << values.accelerometer_x(), values.accelerometer_y(),
+ values.accelerometer_z();
+ accel_averager_.AddData(last_accel_sample_);
+ if (GyroZeroReady() && AccelZeroReady()) {
+ if (!zeroed_) {
+ zeroed_ = true;
+ gyro_average_ = gyro_averager_.GetAverage();
+ } else {
+ // If we got a new zero and it is substantially different from the
+ // original zero, fault.
+ if ((gyro_averager_.GetAverage() - gyro_average_).norm() >
+ kGyroFaultVariation) {
+ faulted_ = true;
+ }
+ }
+ gyro_averager_.Reset();
+ }
+}
+
+} // namespace frc971::zeroing
diff --git a/frc971/zeroing/imu_zeroer.h b/frc971/zeroing/imu_zeroer.h
new file mode 100644
index 0000000..e95c231
--- /dev/null
+++ b/frc971/zeroing/imu_zeroer.h
@@ -0,0 +1,54 @@
+#ifndef FRC971_ZEROING_IMU_ZEROER_H_
+#define FRC971_ZEROING_IMU_ZEROER_H_
+
+#include "frc971/wpilib/imu_generated.h"
+#include "frc971/zeroing/averager.h"
+
+namespace frc971::zeroing {
+
+// This class handles processing IMU measurements and zeroing them once it is
+// able to do so.
+class ImuZeroer {
+ public:
+ // Average 5 seconds of data (assuming 2kHz sampling rate).
+ static constexpr size_t kSamplesToAverage = 10000.0;
+
+ ImuZeroer();
+ bool Zeroed() const;
+ bool Faulted() const;
+ void ProcessMeasurement(const IMUValues &values);
+ Eigen::Vector3d GyroOffset() const;
+ Eigen::Vector3d ZeroedGyro() const;
+ Eigen::Vector3d ZeroedAccel() const;
+ private:
+ // Max variation (difference between the maximum and minimum value) in a
+ // kSamplesToAverage range before we allow using the samples for zeroing.
+ // These values are currently based on looking at results from the ADIS16448.
+ static constexpr double kGyroMaxVariation = 0.02; // rad / sec
+ // Max variation in the range before we consider the accelerometer readings to
+ // be steady.
+ static constexpr double kAccelMaxVariation = 0.02; // g's
+ // If we ever are able to rezero and get a zero that is more than
+ // kGyroFaultVariation away from the original zeroing, fault.
+ static constexpr double kGyroFaultVariation = 0.005; // rad / sec
+
+ bool GyroZeroReady() const;
+ bool AccelZeroReady() const;
+
+ Averager<double, kSamplesToAverage, 3> gyro_averager_;
+ // Averager for the accelerometer readings--we don't currently actually
+ // average the readings, but we do check that the accelerometer readings have
+ // stayed roughly constant during the calibration period.
+ Averager<double, kSamplesToAverage, 3> accel_averager_;
+ // The average zero position of the gyro.
+ Eigen::Vector3d gyro_average_;
+ Eigen::Vector3d last_gyro_sample_;
+ Eigen::Vector3d last_accel_sample_;
+ // Whether we have currently zeroed yet.
+ bool zeroed_ = false;
+ // Whether the zeroing has faulted at any point thus far.
+ bool faulted_ = false;
+};
+
+} // namespace frc971::zeroing
+#endif // FRC971_ZEROING_IMU_ZEROER_H_
diff --git a/frc971/zeroing/imu_zeroer_test.cc b/frc971/zeroing/imu_zeroer_test.cc
new file mode 100644
index 0000000..9919e2f
--- /dev/null
+++ b/frc971/zeroing/imu_zeroer_test.cc
@@ -0,0 +1,159 @@
+#include "aos/flatbuffers.h"
+#include "gtest/gtest.h"
+#include "frc971/zeroing/imu_zeroer.h"
+
+namespace frc971::zeroing {
+
+aos::FlatbufferDetachedBuffer<IMUValues> MakeMeasurement(
+ const Eigen::Vector3d &gyro, const Eigen::Vector3d &accel) {
+ flatbuffers::FlatBufferBuilder fbb;
+ fbb.ForceDefaults(1);
+ IMUValuesBuilder builder(fbb);
+ builder.add_gyro_x(gyro.x());
+ builder.add_gyro_y(gyro.y());
+ builder.add_gyro_z(gyro.z());
+ builder.add_accelerometer_x(accel.x());
+ builder.add_accelerometer_y(accel.y());
+ builder.add_accelerometer_z(accel.z());
+ fbb.Finish(builder.Finish());
+ return fbb.Release();
+}
+
+// Tests that when we initialize everything is in a sane state.
+TEST(ImuZeroerTest, InitializeUnzeroed) {
+ ImuZeroer zeroer;
+ ASSERT_FALSE(zeroer.Zeroed());
+ ASSERT_FALSE(zeroer.Faulted());
+ ASSERT_EQ(0.0, zeroer.GyroOffset().norm());
+ ASSERT_EQ(0.0, zeroer.ZeroedGyro().norm());
+ ASSERT_EQ(0.0, zeroer.ZeroedAccel().norm());
+ // A measurement before we are zeroed should just result in the measurement
+ // being passed through without modification.
+ zeroer.ProcessMeasurement(MakeMeasurement({1, 2, 3}, {4, 5, 6}).message());
+ ASSERT_FALSE(zeroer.Zeroed());
+ ASSERT_FALSE(zeroer.Faulted());
+ ASSERT_EQ(0.0, zeroer.GyroOffset().norm());
+ ASSERT_EQ(1.0, zeroer.ZeroedGyro().x());
+ ASSERT_EQ(2.0, zeroer.ZeroedGyro().y());
+ ASSERT_EQ(3.0, zeroer.ZeroedGyro().z());
+ ASSERT_EQ(4.0, zeroer.ZeroedAccel().x());
+ ASSERT_EQ(5.0, zeroer.ZeroedAccel().y());
+ ASSERT_EQ(6.0, zeroer.ZeroedAccel().z());
+}
+
+// Tests that we zero if we receive a bunch of identical measurements.
+TEST(ImuZeroerTest, ZeroOnConstantData) {
+ ImuZeroer zeroer;
+ ASSERT_FALSE(zeroer.Zeroed());
+ for (size_t ii = 0; ii < ImuZeroer::kSamplesToAverage; ++ii) {
+ ASSERT_FALSE(zeroer.Zeroed());
+ zeroer.ProcessMeasurement(MakeMeasurement({1, 2, 3}, {4, 5, 6}).message());
+ }
+ ASSERT_TRUE(zeroer.Zeroed());
+ ASSERT_FALSE(zeroer.Faulted());
+ // Gyro should be zeroed to {1, 2, 3}.
+ ASSERT_EQ(1.0, zeroer.GyroOffset().x());
+ ASSERT_EQ(2.0, zeroer.GyroOffset().y());
+ ASSERT_EQ(3.0, zeroer.GyroOffset().z());
+ ASSERT_EQ(0.0, zeroer.ZeroedGyro().x());
+ ASSERT_EQ(0.0, zeroer.ZeroedGyro().y());
+ ASSERT_EQ(0.0, zeroer.ZeroedGyro().z());
+ // Accelerometer readings should not be affected.
+ ASSERT_EQ(4.0, zeroer.ZeroedAccel().x());
+ ASSERT_EQ(5.0, zeroer.ZeroedAccel().y());
+ ASSERT_EQ(6.0, zeroer.ZeroedAccel().z());
+ // If we get another measurement offset by {1, 1, 1} we should read the result
+ // as {1, 1, 1}.
+ zeroer.ProcessMeasurement(MakeMeasurement({2, 3, 4}, {0, 0, 0}).message());
+ ASSERT_FALSE(zeroer.Faulted());
+ ASSERT_EQ(1.0, zeroer.ZeroedGyro().x());
+ ASSERT_EQ(1.0, zeroer.ZeroedGyro().y());
+ ASSERT_EQ(1.0, zeroer.ZeroedGyro().z());
+}
+
+// Tests that we tolerate small amounts of noise in the incoming data and can
+// still zero.
+TEST(ImuZeroerTest, ZeroOnLowNoiseData) {
+ ImuZeroer zeroer;
+ ASSERT_FALSE(zeroer.Zeroed());
+ for (size_t ii = 0; ii < ImuZeroer::kSamplesToAverage; ++ii) {
+ ASSERT_FALSE(zeroer.Zeroed());
+ const double offset =
+ (static_cast<double>(ii) / (ImuZeroer::kSamplesToAverage - 1) - 0.5) *
+ 0.01;
+ zeroer.ProcessMeasurement(
+ MakeMeasurement({1 + offset, 2 + offset, 3 + offset},
+ {4 + offset, 5 + offset, 6 + offset})
+ .message());
+ }
+ ASSERT_TRUE(zeroer.Zeroed());
+ ASSERT_FALSE(zeroer.Faulted());
+ // Gyro should be zeroed to {1, 2, 3}.
+ ASSERT_NEAR(1.0, zeroer.GyroOffset().x(), 1e-10);
+ ASSERT_NEAR(2.0, zeroer.GyroOffset().y(), 1e-10);
+ ASSERT_NEAR(3.0, zeroer.GyroOffset().z(), 1e-10);
+ // If we get another measurement offset by {1, 1, 1} we should read the result
+ // as {1, 1, 1}.
+ zeroer.ProcessMeasurement(MakeMeasurement({2, 3, 4}, {0, 0, 0}).message());
+ ASSERT_FALSE(zeroer.Faulted());
+ ASSERT_NEAR(1.0, zeroer.ZeroedGyro().x(), 1e-10);
+ ASSERT_NEAR(1.0, zeroer.ZeroedGyro().y(), 1e-10);
+ ASSERT_NEAR(1.0, zeroer.ZeroedGyro().z(), 1e-10);
+ ASSERT_EQ(0.0, zeroer.ZeroedAccel().x());
+ ASSERT_EQ(0.0, zeroer.ZeroedAccel().y());
+ ASSERT_EQ(0.0, zeroer.ZeroedAccel().z());
+}
+
+// Tests that we do not zero if there is too much noise in the input data.
+TEST(ImuZeroerTest, NoZeroOnHighNoiseData) {
+ ImuZeroer zeroer;
+ ASSERT_FALSE(zeroer.Zeroed());
+ for (size_t ii = 0; ii < ImuZeroer::kSamplesToAverage; ++ii) {
+ ASSERT_FALSE(zeroer.Zeroed());
+ const double offset =
+ (static_cast<double>(ii) / (ImuZeroer::kSamplesToAverage - 1) - 0.5) *
+ 1.0;
+ zeroer.ProcessMeasurement(
+ MakeMeasurement({1 + offset, 2 + offset, 3 + offset},
+ {4 + offset, 5 + offset, 6 + offset})
+ .message());
+ }
+ ASSERT_FALSE(zeroer.Zeroed());
+ ASSERT_FALSE(zeroer.Faulted());
+}
+
+// Tests that we fault if we successfully rezero and get a significantly offset
+// zero.
+TEST(ImuZeroerTest, FaultOnNewZero) {
+ ImuZeroer zeroer;
+ ASSERT_FALSE(zeroer.Zeroed());
+ for (size_t ii = 0; ii < ImuZeroer::kSamplesToAverage; ++ii) {
+ ASSERT_FALSE(zeroer.Zeroed());
+ zeroer.ProcessMeasurement(MakeMeasurement({1, 2, 3}, {4, 5, 6}).message());
+ }
+ ASSERT_TRUE(zeroer.Zeroed());
+ for (size_t ii = 0; ii < ImuZeroer::kSamplesToAverage; ++ii) {
+ ASSERT_FALSE(zeroer.Faulted())
+ << "We should not fault until we complete a second cycle of zeroing.";
+ zeroer.ProcessMeasurement(MakeMeasurement({1, 5, 3}, {4, 5, 6}).message());
+ }
+ ASSERT_TRUE(zeroer.Faulted());
+}
+
+// Tests that we do not fault if the zero only changes by a small amount.
+TEST(ImuZeroerTest, NoFaultOnSimilarZero) {
+ ImuZeroer zeroer;
+ ASSERT_FALSE(zeroer.Zeroed());
+ for (size_t ii = 0; ii < ImuZeroer::kSamplesToAverage; ++ii) {
+ ASSERT_FALSE(zeroer.Zeroed());
+ zeroer.ProcessMeasurement(MakeMeasurement({1, 2, 3}, {4, 5, 6}).message());
+ }
+ ASSERT_TRUE(zeroer.Zeroed());
+ for (size_t ii = 0; ii < ImuZeroer::kSamplesToAverage; ++ii) {
+ zeroer.ProcessMeasurement(
+ MakeMeasurement({1, 2.0001, 3}, {4, 5, 6}).message());
+ }
+ ASSERT_FALSE(zeroer.Faulted());
+}
+
+} // namespace frc971::zeroing