tracker/trackhat: implement LED tracking indicator

9 files changed, 250 insertions, 31 deletions

diff --git a/tracker-trackhat/camera.cpp b/tracker-trackhat/camera.cpp
index d518621f..74e35423 100644
--- a/tracker-trackhat/camera.cpp
+++ b/tracker-trackhat/camera.cpp
@@ -1,5 +1,6 @@
 #include "trackhat.hpp"
 #include "compat/sleep.hpp"
+#include <algorithm>
 #include <cstdio>
 
 namespace trackhat_impl {
@@ -83,6 +84,15 @@ pt_camera::result trackhat_camera::get_frame(pt_frame& frame_)
             goto error;
         auto& frame = *frame_.as<trackhat_frame>();
         frame.init_points(points, t.min_pt_size, t.max_pt_size);
+
+        using trackhat_impl::led_state;
+        int count =
+            std::count_if(frame.points.cbegin(), frame.points.cend(),
+                          [](const point& pt) { return pt.ok; });
+        led.update(&*device, *t.led,
+                   count == 3
+                   ? led_state::tracking
+                   : led_state::not_tracking);
     }
 
     return {true, get_desired()};
@@ -117,10 +127,14 @@ bool trackhat_camera::start(const pt_settings&)
 
     set_pt_options();
 
+    led.update(&*device, *t.led, trackhat_impl::led_state::stopped);
+
     return true;
 }
 
 void trackhat_camera::stop()
 {
+    if (device)
+        led.update(&*device, *t.led, trackhat_impl::led_state::stopped);
     device.disconnect();
 }
diff --git a/tracker-trackhat/dialog.cpp b/tracker-trackhat/dialog.cpp
index 02ecc637..4ef64f50 100644
--- a/tracker-trackhat/dialog.cpp
+++ b/tracker-trackhat/dialog.cpp
@@ -82,6 +82,15 @@ trackhat_dialog::trackhat_dialog()
     connect(&t.point_filter_deadzone, value_::value_changed<slider_value>(), ui.point_filter_deadzone_label,
         [this] { ui.point_filter_deadzone_label->setValue(*t.point_filter_deadzone); }, Qt::QueuedConnection);
 
+    // led
+
+    using trackhat_impl::led_mode;
+    ui.led_mode->setItemData(0, (int)led_mode::off);
+    ui.led_mode->setItemData(1, (int)led_mode::constant);
+    ui.led_mode->setItemData(2, (int)led_mode::dynamic);
+
+    tie_setting(t.led, ui.led_mode);
+
     // stuff
 
     connect(&poll_timer, &QTimer::timeout, this, &trackhat_dialog::poll_tracker_info);
diff --git a/tracker-trackhat/dialog.ui b/tracker-trackhat/dialog.ui
index 927cb945..7c2ea8ee 100644
--- a/tracker-trackhat/dialog.ui
+++ b/tracker-trackhat/dialog.ui
@@ -7,7 +7,7 @@
     <x>0</x>
     <y>0</y>
     <width>365</width>
-    <height>460</height>
+    <height>485</height>
    </rect>
   </property>
   <property name="minimumSize">
@@ -30,27 +30,8 @@
       <string>Camera</string>
      </property>
      <layout class="QGridLayout" name="gridLayout">
-      <item row="0" column="1">
-       <widget class="QSlider" name="exposure_slider">
-        <property name="sizePolicy">
-         <sizepolicy hsizetype="Expanding" vsizetype="Maximum">
-          <horstretch>0</horstretch>
-          <verstretch>0</verstretch>
-         </sizepolicy>
-        </property>
-        <property name="maximum">
-         <number>239</number>
-        </property>
-        <property name="pageStep">
-         <number>10</number>
-        </property>
-        <property name="orientation">
-         <enum>Qt::Horizontal</enum>
-        </property>
-       </widget>
-      </item>
-      <item row="0" column="2">
-       <widget class="QSpinBox" name="exposure_label">
+      <item row="1" column="2">
+       <widget class="QSpinBox" name="gain_label">
         <property name="focusPolicy">
          <enum>Qt::NoFocus</enum>
         </property>
@@ -75,15 +56,27 @@
         </property>
        </widget>
       </item>
-      <item row="1" column="0">
-       <widget class="QLabel" name="label_2">
-        <property name="text">
-         <string>Gain</string>
+      <item row="1" column="1">
+       <widget class="QSlider" name="gain_slider">
+        <property name="sizePolicy">
+         <sizepolicy hsizetype="Expanding" vsizetype="Maximum">
+          <horstretch>0</horstretch>
+          <verstretch>0</verstretch>
+         </sizepolicy>
+        </property>
+        <property name="maximum">
+         <number>239</number>
+        </property>
+        <property name="pageStep">
+         <number>10</number>
+        </property>
+        <property name="orientation">
+         <enum>Qt::Horizontal</enum>
         </property>
        </widget>
       </item>
-      <item row="1" column="1">
-       <widget class="QSlider" name="gain_slider">
+      <item row="0" column="1">
+       <widget class="QSlider" name="exposure_slider">
         <property name="sizePolicy">
          <sizepolicy hsizetype="Expanding" vsizetype="Maximum">
           <horstretch>0</horstretch>
@@ -101,8 +94,8 @@
         </property>
        </widget>
       </item>
-      <item row="1" column="2">
-       <widget class="QSpinBox" name="gain_label">
+      <item row="0" column="2">
+       <widget class="QSpinBox" name="exposure_label">
         <property name="focusPolicy">
          <enum>Qt::NoFocus</enum>
         </property>
@@ -120,6 +113,45 @@
         </property>
        </widget>
       </item>
+      <item row="1" column="0">
+       <widget class="QLabel" name="label_2">
+        <property name="text">
+         <string>Gain</string>
+        </property>
+       </widget>
+      </item>
+      <item row="2" column="0">
+       <widget class="QLabel" name="label_3">
+        <property name="text">
+         <string>LED</string>
+        </property>
+       </widget>
+      </item>
+      <item row="2" column="1">
+       <widget class="QComboBox" name="led_mode">
+        <property name="sizePolicy">
+         <sizepolicy hsizetype="Preferred" vsizetype="Preferred">
+          <horstretch>0</horstretch>
+          <verstretch>0</verstretch>
+         </sizepolicy>
+        </property>
+        <item>
+         <property name="text">
+          <string>Off</string>
+         </property>
+        </item>
+        <item>
+         <property name="text">
+          <string>Constant</string>
+         </property>
+        </item>
+        <item>
+         <property name="text">
+          <string>Dynamic</string>
+         </property>
+        </item>
+       </widget>
+      </item>
      </layout>
     </widget>
    </item>
@@ -553,12 +585,15 @@
  </widget>
  <tabstops>
   <tabstop>exposure_slider</tabstop>
+  <tabstop>gain_slider</tabstop>
+  <tabstop>led_mode</tabstop>
   <tabstop>model_type</tabstop>
   <tabstop>min_point_size</tabstop>
   <tabstop>max_point_size</tabstop>
   <tabstop>enable_point_filter</tabstop>
   <tabstop>point_filter_slider</tabstop>
   <tabstop>point_filter_limit</tabstop>
+  <tabstop>point_filter_deadzone</tabstop>
  </tabstops>
  <resources>
   <include location="trackhat-res.qrc"/>
diff --git a/tracker-trackhat/lang/nl_NL.ts b/tracker-trackhat/lang/nl_NL.ts
index 8d29d6c4..9cc67677 100644
--- a/tracker-trackhat/lang/nl_NL.ts
+++ b/tracker-trackhat/lang/nl_NL.ts
@@ -127,6 +127,22 @@
         <source>Gain</source>
         <translation type="unfinished"></translation>
     </message>
+    <message>
+        <source>LED</source>
+        <translation type="unfinished"></translation>
+    </message>
+    <message>
+        <source>Off</source>
+        <translation type="unfinished"></translation>
+    </message>
+    <message>
+        <source>Constant</source>
+        <translation type="unfinished"></translation>
+    </message>
+    <message>
+        <source>Dynamic</source>
+        <translation type="unfinished"></translation>
+    </message>
 </context>
 <context>
     <name>trackhat_module</name>
diff --git a/tracker-trackhat/lang/ru_RU.ts b/tracker-trackhat/lang/ru_RU.ts
index f2920f58..844c5519 100644
--- a/tracker-trackhat/lang/ru_RU.ts
+++ b/tracker-trackhat/lang/ru_RU.ts
@@ -127,6 +127,22 @@
         <source>Gain</source>
         <translation type="unfinished"></translation>
     </message>
+    <message>
+        <source>LED</source>
+        <translation type="unfinished"></translation>
+    </message>
+    <message>
+        <source>Off</source>
+        <translation type="unfinished"></translation>
+    </message>
+    <message>
+        <source>Constant</source>
+        <translation type="unfinished"></translation>
+    </message>
+    <message>
+        <source>Dynamic</source>
+        <translation type="unfinished"></translation>
+    </message>
 </context>
 <context>
     <name>trackhat_module</name>
diff --git a/tracker-trackhat/lang/stub.ts b/tracker-trackhat/lang/stub.ts
index 0d8616c8..127d0691 100644
--- a/tracker-trackhat/lang/stub.ts
+++ b/tracker-trackhat/lang/stub.ts
@@ -127,6 +127,22 @@
         <source>Gain</source>
         <translation type="unfinished"></translation>
     </message>
+    <message>
+        <source>LED</source>
+        <translation type="unfinished"></translation>
+    </message>
+    <message>
+        <source>Off</source>
+        <translation type="unfinished"></translation>
+    </message>
+    <message>
+        <source>Constant</source>
+        <translation type="unfinished"></translation>
+    </message>
+    <message>
+        <source>Dynamic</source>
+        <translation type="unfinished"></translation>
+    </message>
 </context>
 <context>
     <name>trackhat_module</name>
diff --git a/tracker-trackhat/lang/zh_CN.ts b/tracker-trackhat/lang/zh_CN.ts
index c8c9ed4f..179d4261 100644
--- a/tracker-trackhat/lang/zh_CN.ts
+++ b/tracker-trackhat/lang/zh_CN.ts
@@ -127,6 +127,22 @@
         <source>Gain</source>
         <translation type="unfinished"></translation>
     </message>
+    <message>
+        <source>LED</source>
+        <translation type="unfinished"></translation>
+    </message>
+    <message>
+        <source>Off</source>
+        <translation type="unfinished"></translation>
+    </message>
+    <message>
+        <source>Constant</source>
+        <translation type="unfinished"></translation>
+    </message>
+    <message>
+        <source>Dynamic</source>
+        <translation type="unfinished"></translation>
+    </message>
 </context>
 <context>
     <name>trackhat_module</name>
diff --git a/tracker-trackhat/led.cpp b/tracker-trackhat/led.cpp
new file mode 100644
index 00000000..db5ab6c2
--- /dev/null
+++ b/tracker-trackhat/led.cpp
@@ -0,0 +1,66 @@
+#include "trackhat.hpp"
+
+namespace trackhat_impl {
+
+void led_updater::update_(trackHat_Device_t* device, trackHat_SetLeds_t leds)
+{
+    if (leds_.ledRedState == leds.ledRedState &&
+        leds_.ledGreenState == leds.ledGreenState &&
+        leds_.ledBlueState == leds.ledBlueState)
+        return;
+    (void)trackHat_SetLeds(device, &leds);
+    leds_ = leds;
+}
+
+trackHat_SetLeds_t led_updater::next_state(led_mode mode, led_state new_state)
+{
+    switch (mode)
+    {
+    case led_mode::off:
+        state_ = led_state::stopped;
+        timer_ = std::nullopt;
+        return LED_off;
+    default:
+    case led_mode::constant:
+        state_ = led_state::stopped;
+        timer_ = std::nullopt;
+        return LED_idle;
+    case led_mode::dynamic:
+        break;
+    }
+
+    if (new_state <= led_state::stopped)
+    {
+        state_ = new_state;
+        timer_ = std::nullopt;
+        return LED_idle;
+    }
+    else if (new_state == state_)
+    {
+        timer_ = std::nullopt;
+        return leds_;
+    }
+    else if (!timer_)
+    {
+        timer_ = Timer{};
+        return leds_;
+    }
+    else if (timer_->elapsed_ms() > SWITCH_TIME_MS)
+    {
+        state_ = new_state;
+        timer_ = std::nullopt;
+        return new_state == led_state::not_tracking
+               ? LED_not_tracking
+               : LED_tracking;
+    }
+    else
+        return leds_;
+}
+
+void led_updater::update(trackHat_Device_t* device, led_mode mode, led_state new_state)
+{
+    auto leds = next_state(mode, new_state);
+    update_(device, leds);
+}
+
+} // namespace trackhat_impl
diff --git a/tracker-trackhat/trackhat.hpp b/tracker-trackhat/trackhat.hpp
index b80e4c2f..5599e963 100644
--- a/tracker-trackhat/trackhat.hpp
+++ b/tracker-trackhat/trackhat.hpp
@@ -2,12 +2,14 @@
 
 #include "../tracker-pt/pt-api.hpp"
 #include "compat/macros.hpp"
+#include "compat/timer.hpp"
 #include "options/options.hpp"
 
 #include <track_hat_driver.h>
 
 #include <array>
 #include <atomic>
+#include <optional>
 #include <opencv2/core/mat.hpp>
 
 enum model_type : int
@@ -28,6 +30,10 @@ TH_ErrorCode log_error(TH_ErrorCode error, const char* source, const char* file,
 #define th_check_(expr, expr2) ::trackhat_impl::log_error((expr), expr2)
 #define th_check(expr) ::trackhat_impl::log_error((expr), #expr, __FILE__, __LINE__, function_name)
 
+enum class led_mode : unsigned char {
+    off, constant, dynamic,
+};
+
 struct trackhat_settings : opts
 {
     static constexpr int min_gain = 1, max_gain = 47,
@@ -44,6 +50,7 @@ struct trackhat_settings : opts
     value<slider_value> point_filter_coefficient{b, "point-filter-coefficient", { 1.5, 1, 4 }};
     value<slider_value> point_filter_limit { b, "point-filter-limit", { 0.1, 0.01, 1 }};
     value<slider_value> point_filter_deadzone { b, "point-filter-deadzone", {0, 0, 1}};
+    value<led_mode> led { b, "led-mode", led_mode::dynamic };
 };
 
 class setting_receiver : public QObject
@@ -59,9 +66,31 @@ private:
     std::atomic<bool> changed{false};
 };
 
+enum class led_state : unsigned char {
+    invalid, stopped, not_tracking, tracking,
+};
+
+struct led_updater final {
+    trackHat_SetLeds_t leds_ {TH_UNCHANGED, TH_UNCHANGED, TH_UNCHANGED};
+    std::optional<Timer> timer_;
+    led_state state_ = led_state::invalid;
+
+    trackHat_SetLeds_t next_state(led_mode mode, led_state new_state);
+    void update_(trackHat_Device_t* device, trackHat_SetLeds_t leds);
+    void update(trackHat_Device_t* device, led_mode mode, led_state new_state);
+
+    static constexpr int SWITCH_TIME_MS = 2000;
+    static constexpr
+        trackHat_SetLeds_t LED_idle = {TH_OFF, TH_SOLID, TH_OFF},
+                           LED_off  = {TH_OFF, TH_OFF, TH_OFF},
+                           LED_tracking {TH_OFF, TH_SOLID, TH_SOLID},
+                           LED_not_tracking {TH_SOLID, TH_OFF, TH_OFF};
+};
+
 } // ns trackhat_impl
 
-using typename trackhat_impl::trackhat_settings;
+using trackhat_impl::trackhat_settings;
+using trackhat_impl::led_updater;
 
 struct trackhat_metadata final : pt_runtime_traits
 {
@@ -94,6 +123,7 @@ struct camera_handle final
     camera_handle() = default;
     ~camera_handle() = default;
 
+    constexpr operator bool() const { return state_ >= st_streaming; }
     [[nodiscard]] bool ensure_connected();
     [[nodiscard]] bool ensure_device_exists();
     void disconnect();
@@ -139,6 +169,7 @@ private:
     camera_handle device;
     pt_settings s{trackhat_metadata::module_name};
     trackhat_settings t;
+    led_updater led;
 };
 
 struct trackhat_frame final : pt_frame