Use ErrorList in estimators
Support it in both the abs + abs, and pot + abs and just abs.
Change-Id: I1c11d5d1fdc3d644f9d391bde7c06b6c6e8ec57d
Signed-off-by: Ravago Jones <ravagojones@gmail.com>
diff --git a/frc971/control_loops/control_loops.fbs b/frc971/control_loops/control_loops.fbs
index 243e2cb..da2dc05 100644
--- a/frc971/control_loops/control_loops.fbs
+++ b/frc971/control_loops/control_loops.fbs
@@ -86,6 +86,13 @@
encoder:double (id: 0);
}
+// An enum to represent the different types of errors
+// a zeroing estimator could have.
+enum ZeroingError : short {
+ OFFSET_MOVED_TOO_FAR,
+ LOST_ABSOLUTE_ENCODER
+}
+
// The internal state of a zeroing estimator.
table EstimatorState {
// If true, there has been a fatal error for the estimator.
@@ -114,6 +121,9 @@
// The estimated absolute position of the encoder. This is filtered, so it
// can be easily used when zeroing.
absolute_position:double (id: 4);
+
+ // If errored, this contains the causes for the error.
+ errors: [ZeroingError] (id: 5);
}
// The internal state of a zeroing estimator.
@@ -128,6 +138,9 @@
// The estimated absolute position of the encoder. This is filtered, so it
// can be easily used when zeroing.
absolute_position:double (id: 3);
+
+ // If errored, this contains the causes for the error.
+ errors: [ZeroingError] (id: 4);
}
// The internal state of a zeroing estimator.
@@ -145,8 +158,12 @@
// Estimated absolute position of the single turn absolute encoder.
single_turn_absolute_position:double (id: 4);
+
+ // If errored, this contains the causes for the error.
+ errors: [ZeroingError] (id: 5);
}
+
table RelativeEncoderEstimatorState {
// If true, there has been a fatal error for the estimator.
error:bool (id: 0);
diff --git a/frc971/zeroing/BUILD b/frc971/zeroing/BUILD
index 9ec2772..d50f181 100644
--- a/frc971/zeroing/BUILD
+++ b/frc971/zeroing/BUILD
@@ -80,6 +80,7 @@
target_compatible_with = ["@platforms//os:linux"],
deps = [
":wrap",
+ "//aos/containers:error_list",
"//aos/logging",
"//frc971:constants",
"//frc971/control_loops:control_loops_fbs",
diff --git a/frc971/zeroing/absolute_and_absolute_encoder.cc b/frc971/zeroing/absolute_and_absolute_encoder.cc
index 40b0519..03ef3f1 100644
--- a/frc971/zeroing/absolute_and_absolute_encoder.cc
+++ b/frc971/zeroing/absolute_and_absolute_encoder.cc
@@ -71,6 +71,7 @@
if (zeroed_) {
VLOG(1) << "NAN on one of the absolute encoders.";
error_ = true;
+ errors_.Set(ZeroingError::LOST_ABSOLUTE_ENCODER);
} else {
++nan_samples_;
VLOG(1) << "NAN on one of the absolute encoders while zeroing"
@@ -78,6 +79,7 @@
if (nan_samples_ >= constants_.average_filter_size) {
error_ = true;
zeroed_ = true;
+ errors_.Set(ZeroingError::LOST_ABSOLUTE_ENCODER);
}
}
// Throw some dummy values in for now.
@@ -189,11 +191,6 @@
(-constants_.single_turn_measured_absolute_position +
what_Unwrap_added));
- /*
- filtered_single_turn_absolute_encoder_ =
- sample.encoder - single_turn_to_relative_encoder_offset_;
- */
-
if (!zeroed_) {
first_offset_ = offset_;
}
@@ -209,6 +206,7 @@
constants_.allowable_encoder_error *
constants_.one_revolution_distance);
error_ = true;
+ errors_.Set(ZeroingError::OFFSET_MOVED_TOO_FAR);
}
zeroed_ = true;
@@ -222,8 +220,12 @@
flatbuffers::Offset<AbsoluteAndAbsoluteEncoderZeroingEstimator::State>
AbsoluteAndAbsoluteEncoderZeroingEstimator::GetEstimatorState(
flatbuffers::FlatBufferBuilder *fbb) const {
+ flatbuffers::Offset<flatbuffers::Vector<ZeroingError>> errors_offset =
+ errors_.ToFlatbuffer(fbb);
+
State::Builder builder(*fbb);
builder.add_error(error_);
+ builder.add_errors(errors_offset);
builder.add_zeroed(zeroed_);
builder.add_position(position_);
builder.add_absolute_position(filtered_absolute_encoder_);
diff --git a/frc971/zeroing/absolute_and_absolute_encoder.h b/frc971/zeroing/absolute_and_absolute_encoder.h
index 499f7d1..509d5c5 100644
--- a/frc971/zeroing/absolute_and_absolute_encoder.h
+++ b/frc971/zeroing/absolute_and_absolute_encoder.h
@@ -5,6 +5,7 @@
#include "flatbuffers/flatbuffers.h"
+#include "aos/containers/error_list.h"
#include "frc971/zeroing/zeroing.h"
namespace frc971 {
@@ -73,6 +74,8 @@
bool zeroed_;
// Marker to track whether an error has occurred.
bool error_;
+ // Marker to track what kind of error has occured.
+ aos::ErrorList<ZeroingError> errors_;
// The first valid offset we recorded. This is only set after zeroed_ first
// changes to true.
double first_offset_;
diff --git a/frc971/zeroing/absolute_and_absolute_encoder_test.cc b/frc971/zeroing/absolute_and_absolute_encoder_test.cc
index cc1872d..59b421a 100644
--- a/frc971/zeroing/absolute_and_absolute_encoder_test.cc
+++ b/frc971/zeroing/absolute_and_absolute_encoder_test.cc
@@ -1,15 +1,15 @@
#include "frc971/zeroing/absolute_and_absolute_encoder.h"
-#include "gtest/gtest.h"
-
#include "frc971/zeroing/zeroing_test.h"
+#include "glog/logging.h"
+#include "gmock/gmock.h"
+#include "gtest/gtest.h"
namespace frc971 {
namespace zeroing {
namespace testing {
using constants::AbsoluteAndAbsoluteEncoderZeroingConstants;
-using EstimatorState = AbsoluteAndAbsoluteEncoderZeroingEstimator::State;
class AbsoluteAndAbsoluteEncoderZeroingTest : public ZeroingTest {
protected:
@@ -17,7 +17,7 @@
AbsoluteAndAbsoluteEncoderZeroingEstimator *estimator,
double new_position) {
simulator->MoveTo(new_position);
- FBB fbb;
+ flatbuffers::FlatBufferBuilder fbb;
estimator->UpdateEstimate(
*simulator->FillSensorValues<AbsoluteAndAbsolutePosition>(&fbb));
}
@@ -107,7 +107,7 @@
AbsoluteAndAbsoluteEncoderZeroingEstimator estimator(constants);
// We tolerate a couple NANs before we start.
- FBB fbb;
+ flatbuffers::FlatBufferBuilder fbb;
fbb.Finish(CreateAbsoluteAndAbsolutePosition(
fbb, 0.0, ::std::numeric_limits<double>::quiet_NaN(), 0.0));
for (size_t i = 0; i < kSampleSize - 1; ++i) {
@@ -180,7 +180,7 @@
AbsoluteAndAbsoluteEncoderZeroingEstimator estimator(constants);
- FBB fbb;
+ flatbuffers::FlatBufferBuilder fbb;
fbb.Finish(CreateAbsoluteAndAbsolutePosition(
fbb, 0.0, ::std::numeric_limits<double>::quiet_NaN(), 0.0));
const auto sensor_values =
@@ -192,6 +192,14 @@
estimator.UpdateEstimate(*sensor_values);
ASSERT_TRUE(estimator.error());
+
+ fbb.Finish(estimator.GetEstimatorState(&fbb));
+ const AbsoluteAndAbsoluteEncoderEstimatorState *state =
+ flatbuffers::GetRoot<AbsoluteAndAbsoluteEncoderEstimatorState>(
+ fbb.GetBufferPointer());
+
+ ASSERT_GT(state->errors()->size(), 0);
+ EXPECT_EQ(state->errors()->Get(0), ZeroingError::LOST_ABSOLUTE_ENCODER);
}
TEST_F(AbsoluteAndAbsoluteEncoderZeroingTest,
@@ -234,12 +242,13 @@
ASSERT_TRUE(estimator.zeroed());
EXPECT_DOUBLE_EQ(start_pos, estimator.offset());
- FBB fbb;
+ flatbuffers::FlatBufferBuilder fbb;
fbb.Finish(estimator.GetEstimatorState(&fbb));
- const EstimatorState *state =
- flatbuffers::GetRoot<EstimatorState>(fbb.GetBufferPointer());
+ const AbsoluteAndAbsoluteEncoderEstimatorState *state =
+ flatbuffers::GetRoot<AbsoluteAndAbsoluteEncoderEstimatorState>(
+ fbb.GetBufferPointer());
EXPECT_NEAR(state->position(), position, 1e-10);
@@ -253,6 +262,74 @@
EXPECT_NEAR(state->single_turn_absolute_position(), 0.3, 1e-10);
}
+// Tests that errors() adds the OFFSET_MOVED_TOO_FAR error when we move too far.
+TEST_F(AbsoluteAndAbsoluteEncoderZeroingTest,
+ TestAbsoluteAndAbsoluteEncoderZeroingStateErrors) {
+ const double full_range = 4.0;
+ const double distance_per_revolution = 1.0;
+ const double single_turn_distance_per_revolution = full_range;
+ const double start_pos = 2.1;
+ const double single_turn_middle_position = full_range * 0.5;
+ const double measured_absolute_position = 0.3 * distance_per_revolution;
+ const double single_turn_measured_absolute_position =
+ 0.4 * single_turn_distance_per_revolution;
+
+ AbsoluteAndAbsoluteEncoderZeroingConstants constants{
+ kSampleSize,
+ distance_per_revolution,
+ measured_absolute_position,
+ single_turn_distance_per_revolution,
+ single_turn_measured_absolute_position,
+ single_turn_middle_position,
+ 0.1,
+ kMovingBufferSize,
+ kIndexErrorFraction};
+
+ PositionSensorSimulator sim(distance_per_revolution,
+ single_turn_distance_per_revolution);
+ sim.Initialize(start_pos, distance_per_revolution / 3.0, 0.0,
+ measured_absolute_position,
+ single_turn_measured_absolute_position);
+
+ AbsoluteAndAbsoluteEncoderZeroingEstimator estimator(constants);
+
+ const double position = 2.7;
+
+ for (size_t i = 0; i < kSampleSize + kMovingBufferSize - 1; ++i) {
+ MoveTo(&sim, &estimator, position);
+ ASSERT_FALSE(estimator.zeroed());
+ }
+ MoveTo(&sim, &estimator, position);
+ ASSERT_TRUE(estimator.zeroed());
+ EXPECT_DOUBLE_EQ(start_pos, estimator.offset());
+
+ // If the ratios suddenly get very messed up
+ flatbuffers::FlatBufferBuilder fbb;
+ fbb.Finish(CreateAbsoluteAndAbsolutePosition(fbb, 0.0, 0.0, 3.0));
+
+ const auto sensor_values =
+ flatbuffers::GetRoot<AbsoluteAndAbsolutePosition>(fbb.GetBufferPointer());
+
+ ASSERT_FALSE(estimator.error());
+
+ for (size_t i = 0; i < kSampleSize + kMovingBufferSize - 1; ++i) {
+ estimator.UpdateEstimate(*sensor_values);
+ }
+ ASSERT_TRUE(estimator.error());
+
+ flatbuffers::FlatBufferBuilder fbb2;
+ fbb2.Finish(estimator.GetEstimatorState(&fbb2));
+ const AbsoluteAndAbsoluteEncoderEstimatorState *state =
+ flatbuffers::GetRoot<AbsoluteAndAbsoluteEncoderEstimatorState>(
+ fbb2.GetBufferPointer());
+
+ for (ZeroingError err : *state->errors()) {
+ LOG(INFO) << "error: " << EnumNameZeroingError(err);
+ }
+ EXPECT_THAT(*state->errors(),
+ ::testing::ElementsAre(ZeroingError::OFFSET_MOVED_TOO_FAR));
+}
+
} // namespace testing
} // namespace zeroing
} // namespace frc971
diff --git a/frc971/zeroing/absolute_encoder.cc b/frc971/zeroing/absolute_encoder.cc
index ffdf9da..d0cd0d9 100644
--- a/frc971/zeroing/absolute_encoder.cc
+++ b/frc971/zeroing/absolute_encoder.cc
@@ -3,9 +3,9 @@
#include <cmath>
#include <numeric>
-#include "glog/logging.h"
-
+#include "aos/containers/error_list.h"
#include "frc971/zeroing/wrap.h"
+#include "glog/logging.h"
namespace frc971 {
namespace zeroing {
@@ -29,7 +29,6 @@
move_detector_.Reset();
}
-
// The math here is a bit backwards, but I think it'll be less error prone that
// way and more similar to the version with a pot as well.
//
@@ -49,11 +48,13 @@
if (::std::isnan(info.absolute_encoder())) {
if (zeroed_) {
VLOG(1) << "NAN on absolute encoder.";
+ errors_.Set(ZeroingError::LOST_ABSOLUTE_ENCODER);
error_ = true;
} else {
++nan_samples_;
VLOG(1) << "NAN on absolute encoder while zeroing " << nan_samples_;
if (nan_samples_ >= constants_.average_filter_size) {
+ errors_.Set(ZeroingError::LOST_ABSOLUTE_ENCODER);
error_ = true;
zeroed_ = true;
}
@@ -152,6 +153,7 @@
<< ", current " << offset_ << ", allowable change: "
<< constants_.allowable_encoder_error *
constants_.one_revolution_distance;
+ errors_.Set(ZeroingError::OFFSET_MOVED_TOO_FAR);
error_ = true;
}
@@ -166,11 +168,15 @@
flatbuffers::Offset<AbsoluteEncoderZeroingEstimator::State>
AbsoluteEncoderZeroingEstimator::GetEstimatorState(
flatbuffers::FlatBufferBuilder *fbb) const {
+ flatbuffers::Offset<flatbuffers::Vector<ZeroingError>> errors_offset =
+ errors_.ToFlatbuffer(fbb);
+
State::Builder builder(*fbb);
builder.add_error(error_);
builder.add_zeroed(zeroed_);
builder.add_position(position_);
builder.add_absolute_position(filtered_absolute_encoder_);
+ builder.add_errors(errors_offset);
return builder.Finish();
}
diff --git a/frc971/zeroing/absolute_encoder.h b/frc971/zeroing/absolute_encoder.h
index 0021e13..df40ec3 100644
--- a/frc971/zeroing/absolute_encoder.h
+++ b/frc971/zeroing/absolute_encoder.h
@@ -85,6 +85,9 @@
// The filtered position.
double position_ = 0.0;
+
+ // Marker to track what kind of error has occured.
+ aos::ErrorList<ZeroingError> errors_;
};
} // namespace zeroing
diff --git a/frc971/zeroing/absolute_encoder_test.cc b/frc971/zeroing/absolute_encoder_test.cc
index 38ce069..ce485eb 100644
--- a/frc971/zeroing/absolute_encoder_test.cc
+++ b/frc971/zeroing/absolute_encoder_test.cc
@@ -1,8 +1,8 @@
#include "frc971/zeroing/absolute_encoder.h"
-#include "gtest/gtest.h"
-
#include "frc971/zeroing/zeroing_test.h"
+#include "gmock/gmock.h"
+#include "gtest/gtest.h"
namespace frc971 {
namespace zeroing {
@@ -15,7 +15,7 @@
void MoveTo(PositionSensorSimulator *simulator,
AbsoluteEncoderZeroingEstimator *estimator, double new_position) {
simulator->MoveTo(new_position);
- FBB fbb;
+ flatbuffers::FlatBufferBuilder fbb;
estimator->UpdateEstimate(
*simulator->FillSensorValues<AbsolutePosition>(&fbb));
}
@@ -71,7 +71,7 @@
AbsoluteEncoderZeroingEstimator estimator(constants);
// We tolerate a couple NANs before we start.
- FBB fbb;
+ flatbuffers::FlatBufferBuilder fbb;
fbb.Finish(CreateAbsolutePosition(
fbb, 0.0, ::std::numeric_limits<double>::quiet_NaN()));
const auto sensor_values =
@@ -126,7 +126,7 @@
AbsoluteEncoderZeroingEstimator estimator(constants);
- FBB fbb;
+ flatbuffers::FlatBufferBuilder fbb;
fbb.Finish(CreateAbsolutePosition(
fbb, 0.0, ::std::numeric_limits<double>::quiet_NaN()));
const auto sensor_values =
@@ -138,6 +138,16 @@
estimator.UpdateEstimate(*sensor_values);
ASSERT_TRUE(estimator.error());
+
+ flatbuffers::FlatBufferBuilder fbb2;
+ fbb2.Finish(estimator.GetEstimatorState(&fbb2));
+
+ const AbsoluteEncoderEstimatorState *state =
+ flatbuffers::GetRoot<AbsoluteEncoderEstimatorState>(
+ fbb2.GetBufferPointer());
+
+ EXPECT_THAT(*state->errors(),
+ ::testing::ElementsAre(ZeroingError::LOST_ABSOLUTE_ENCODER));
}
} // namespace testing
diff --git a/frc971/zeroing/pot_and_absolute_encoder.cc b/frc971/zeroing/pot_and_absolute_encoder.cc
index 200f399..32c4f60 100644
--- a/frc971/zeroing/pot_and_absolute_encoder.cc
+++ b/frc971/zeroing/pot_and_absolute_encoder.cc
@@ -3,9 +3,9 @@
#include <cmath>
#include <numeric>
-#include "glog/logging.h"
-
+#include "aos/containers/error_list.h"
#include "frc971/zeroing/wrap.h"
+#include "glog/logging.h"
namespace frc971 {
namespace zeroing {
@@ -57,11 +57,13 @@
if (::std::isnan(info.absolute_encoder())) {
if (zeroed_) {
VLOG(1) << "NAN on absolute encoder.";
+ errors_.Set(ZeroingError::LOST_ABSOLUTE_ENCODER);
error_ = true;
} else {
++nan_samples_;
- VLOG(1) << "NAN on absolute encoder while zeroing" << nan_samples_;
+ VLOG(1) << "NAN on absolute encoder while zeroing " << nan_samples_;
if (nan_samples_ >= constants_.average_filter_size) {
+ errors_.Set(ZeroingError::LOST_ABSOLUTE_ENCODER);
error_ = true;
zeroed_ = true;
}
@@ -168,6 +170,7 @@
<< ", current " << offset_ << ", allowable change: "
<< constants_.allowable_encoder_error *
constants_.one_revolution_distance;
+ errors_.Set(ZeroingError::OFFSET_MOVED_TOO_FAR);
error_ = true;
}
@@ -183,12 +186,16 @@
flatbuffers::Offset<PotAndAbsoluteEncoderZeroingEstimator::State>
PotAndAbsoluteEncoderZeroingEstimator::GetEstimatorState(
flatbuffers::FlatBufferBuilder *fbb) const {
+ flatbuffers::Offset<flatbuffers::Vector<ZeroingError>> errors_offset =
+ errors_.ToFlatbuffer(fbb);
+
State::Builder builder(*fbb);
builder.add_error(error_);
builder.add_zeroed(zeroed_);
builder.add_position(position_);
builder.add_pot_position(filtered_position_);
builder.add_absolute_position(filtered_absolute_encoder_);
+ builder.add_errors(errors_offset);
return builder.Finish();
}
diff --git a/frc971/zeroing/pot_and_absolute_encoder.h b/frc971/zeroing/pot_and_absolute_encoder.h
index 133eaf5..2ff141f 100644
--- a/frc971/zeroing/pot_and_absolute_encoder.h
+++ b/frc971/zeroing/pot_and_absolute_encoder.h
@@ -3,8 +3,8 @@
#include <vector>
+#include "aos/containers/error_list.h"
#include "flatbuffers/flatbuffers.h"
-
#include "frc971/zeroing/zeroing.h"
namespace frc971 {
@@ -92,6 +92,9 @@
double filtered_position_ = 0.0;
// The filtered position.
double position_ = 0.0;
+
+ // Marker to track what kind of error has occured.
+ aos::ErrorList<ZeroingError> errors_;
};
} // namespace zeroing
diff --git a/frc971/zeroing/pot_and_absolute_encoder_test.cc b/frc971/zeroing/pot_and_absolute_encoder_test.cc
index ba89834..1784fed 100644
--- a/frc971/zeroing/pot_and_absolute_encoder_test.cc
+++ b/frc971/zeroing/pot_and_absolute_encoder_test.cc
@@ -1,8 +1,8 @@
#include "frc971/zeroing/pot_and_absolute_encoder.h"
-#include "gtest/gtest.h"
-
#include "frc971/zeroing/zeroing_test.h"
+#include "gmock/gmock.h"
+#include "gtest/gtest.h"
namespace frc971 {
namespace zeroing {
@@ -16,7 +16,7 @@
PotAndAbsoluteEncoderZeroingEstimator *estimator,
double new_position) {
simulator->MoveTo(new_position);
- FBB fbb;
+ flatbuffers::FlatBufferBuilder fbb;
estimator->UpdateEstimate(
*simulator->FillSensorValues<PotAndAbsolutePosition>(&fbb));
}
@@ -70,7 +70,7 @@
PotAndAbsoluteEncoderZeroingEstimator estimator(constants);
// We tolerate a couple NANs before we start.
- FBB fbb;
+ flatbuffers::FlatBufferBuilder fbb;
fbb.Finish(CreatePotAndAbsolutePosition(
fbb, 0.0, ::std::numeric_limits<double>::quiet_NaN(), 0.0));
for (size_t i = 0; i < kSampleSize - 1; ++i) {
@@ -124,7 +124,7 @@
PotAndAbsoluteEncoderZeroingEstimator estimator(constants);
- FBB fbb;
+ flatbuffers::FlatBufferBuilder fbb;
fbb.Finish(CreatePotAndAbsolutePosition(
fbb, 0.0, ::std::numeric_limits<double>::quiet_NaN(), 0.0));
const auto sensor_values =
@@ -136,6 +136,16 @@
estimator.UpdateEstimate(*sensor_values);
ASSERT_TRUE(estimator.error());
+
+ flatbuffers::FlatBufferBuilder fbb2;
+ fbb2.Finish(estimator.GetEstimatorState(&fbb2));
+
+ const PotAndAbsoluteEncoderEstimatorState *state =
+ flatbuffers::GetRoot<PotAndAbsoluteEncoderEstimatorState>(
+ fbb2.GetBufferPointer());
+
+ EXPECT_THAT(*state->errors(),
+ ::testing::ElementsAre(ZeroingError::LOST_ABSOLUTE_ENCODER));
}
} // namespace testing