commit c341986332c54fdfd7972640a6c0f9483f200004
author Austin Schuh <austin.linux@gmail.com> Sun Jan 08 13:54:36 2023 -0800
committer Austin Schuh <austin.linux@gmail.com> Sun Jan 08 14:42:10 2023 -0800
tree cb21877d821020f45943e29be4eccd647590bc8e
parent 035247f0391dc56fbaec834cc99e78bacde06288

Configure ImageCallback to work with ftrace

We want to be able to stop a trace when we have a timing violation.  Add
flags to support that.

Change-Id: I9a66c59576692ecceeb796dfba223207b7c1234b
Signed-off-by: Austin Schuh <austin.linux@gmail.com>

frc971/vision/charuco_lib.cc

diff --git a/frc971/vision/charuco_lib.cc b/frc971/vision/charuco_lib.cc
index 82cf56b..01e18e8 100644
--- a/frc971/vision/charuco_lib.cc
+++ b/frc971/vision/charuco_lib.cc
@@ -29,6 +29,11 @@
               "Type of target: april_tag|aruco|charuco|charuco_diamond");
 DEFINE_bool(visualize, false, "Whether to visualize the resulting data.");
 
+DEFINE_uint32(age, 5, "Age to start dropping frames at.");
+DEFINE_uint32(disable_delay, 100, "Time after an issue to disable tracing at.");
+
+DECLARE_bool(enable_ftrace);
+
 namespace frc971 {
 namespace vision {
 namespace chrono = std::chrono;
@@ -102,7 +107,8 @@
           event_loop_->GetChannel<CameraImage>(channel)
               ->source_node()
               ->string_view())),
-      handle_image_(std::move(handle_image_fn)) {
+      handle_image_(std::move(handle_image_fn)),
+      timer_fn_(event_loop->AddTimer([this]() { DisableTracing(); })) {
   event_loop_->MakeWatcher(channel, [this](const CameraImage &image) {
     const monotonic_clock::time_point eof_source_node =
         monotonic_clock::time_point(
@@ -141,7 +147,20 @@
     const monotonic_clock::duration age = event_loop_->monotonic_now() - eof;
     const double age_double =
         std::chrono::duration_cast<std::chrono::duration<double>>(age).count();
-    if (age > std::chrono::milliseconds(100)) {
+    if (age > std::chrono::milliseconds(FLAGS_age)) {
+      if (FLAGS_enable_ftrace) {
+        ftrace_.FormatMessage("Too late receiving image, age: %f\n",
+                              age_double);
+        if (FLAGS_disable_delay > 0) {
+          if (!disabling_) {
+            timer_fn_->Setup(event_loop_->monotonic_now() +
+                             chrono::milliseconds(FLAGS_disable_delay));
+            disabling_ = true;
+          }
+        } else {
+          DisableTracing();
+        }
+      }
       VLOG(2) << "Age: " << age_double << ", getting behind, skipping";
       return;
     }
@@ -155,6 +174,11 @@
   });
 }
 
+void ImageCallback::DisableTracing() {
+  disabling_ = false;
+  ftrace_.TurnOffOrDie();
+}
+
 void CharucoExtractor::SetupTargetData() {
   // TODO(Jim): Put correct values here
   marker_length_ = 0.15;

frc971/vision/charuco_lib.h

diff --git a/frc971/vision/charuco_lib.h b/frc971/vision/charuco_lib.h
index 362bb7d..fdf25e1 100644
--- a/frc971/vision/charuco_lib.h
+++ b/frc971/vision/charuco_lib.h
@@ -56,10 +56,17 @@
                     &&handle_image_fn);
 
  private:
+  void DisableTracing();
+
   aos::EventLoop *event_loop_;
   aos::Fetcher<aos::message_bridge::ServerStatistics> server_fetcher_;
   const aos::Node *source_node_;
   std::function<void(cv::Mat, aos::monotonic_clock::time_point)> handle_image_;
+  aos::TimerHandler *timer_fn_;
+
+  bool disabling_ = false;
+
+  aos::Ftrace ftrace_;
 };
 
 // Class which calls a callback each time an image arrives with the information