commit 726deb025fb2b0519ffcec05ed01d21930ff5d78
author Ravago Jones <ravagojones@gmail.com> Sat May 29 14:36:43 2021 -0700
committer Austin Schuh <austin.linux@gmail.com> Wed Mar 01 20:50:28 2023 -0800
tree 6433fb563c022d6e744f688767ce342e5f0bb4fc
parent 43b537669576a7c1edfa2a7703f6bc7ae9132459

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>

frc971/zeroing/absolute_encoder.cc

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