commit d67e9cf45df9b1f71ef1ab3c5bc3e0c0994225e0
author Milind Upadhyay <milind.upadhyay@gmail.com> Sun Mar 13 13:56:57 2022 -0700
committer Milind Upadhyay <milind.upadhyay@gmail.com> Sun Mar 13 14:55:33 2022 -0700
tree b77c5ffedc1f40371be2f44a5bf363010e3031f1
parent 2da80bbdd79a136f91fa7c23cf867ceb5e02e026

Add boilerplate localizer code for LED control

Will use this to turn LEDs facing away from the target off

Signed-off-by: Milind Upadhyay <milind.upadhyay@gmail.com>
Change-Id: I4e03778f6b06db87b26ab3986dfebab7840d6de1

y2022/localizer/localizer.cc

diff --git a/y2022/localizer/localizer.cc b/y2022/localizer/localizer.cc
index 8f9ffa1..1aa2458 100644
--- a/y2022/localizer/localizer.cc
+++ b/y2022/localizer/localizer.cc
@@ -102,6 +102,8 @@
   DiscretizeQAFast(Q_continuous_accel_, A_continuous_accel_, kNominalDt,
                    &Q_discrete_accel_, &A_discrete_accel_);
   P_accel_ = Q_discrete_accel_;
+
+  led_outputs_.fill(LedOutput::ON);
 }
 
 Eigen::Matrix<double, ModelBasedLocalizer::kNModelStates,
@@ -487,7 +489,8 @@
 }
 
 // Node names of the pis to listen for cameras from.
-const std::array<std::string_view, 4> kPisToUse{"pi1", "pi2", "pi3", "pi4"};
+constexpr std::array<std::string_view, ModelBasedLocalizer::kNumPis> kPisToUse{
+    "pi1", "pi2", "pi3", "pi4"};
 }  // namespace
 
 const Eigen::Matrix<double, 4, 4> ModelBasedLocalizer::CameraTransform(
@@ -667,6 +670,9 @@
   const double camera_yaw =
       std::atan2(camera_z_in_field.y(), camera_z_in_field.x());
 
+  // TODO(milind): actually control this
+  led_outputs_[camera_index] = LedOutput::ON;
+
   TargetEstimateDebugT debug;
   debug.camera = static_cast<uint8_t>(camera_index);
   debug.camera_x = H_field_camera_measured(0, 3);
@@ -1010,6 +1016,11 @@
           if (last_output_send_ + std::chrono::milliseconds(5) <
               event_loop_->context().monotonic_event_time) {
             auto builder = output_sender_.MakeBuilder();
+
+            const auto led_outputs_offset =
+                builder.fbb()->CreateVector(model_based_.led_outputs().data(),
+                                            model_based_.led_outputs().size());
+
             LocalizerOutput::Builder output_builder =
                 builder.MakeBuilder<LocalizerOutput>();
             // TODO(james): Should we bother to try to estimate time offsets for
@@ -1027,6 +1038,7 @@
             quaternion.mutate_z(orientation.z());
             quaternion.mutate_w(orientation.w());
             output_builder.add_orientation(&quaternion);
+            output_builder.add_led_outputs(led_outputs_offset);
             builder.CheckOk(builder.Send(output_builder.Finish()));
             last_output_send_ = event_loop_->monotonic_now();
           }