summaryrefslogtreecommitdiffhomepage
diff options
context:
space:
mode:
authorStanislaw Halik <sthalik@misaki.pl>2022-05-29 16:46:19 +0200
committerStanislaw Halik <sthalik@misaki.pl>2022-06-05 14:44:25 +0200
commit800c13a72ace78be48537906898e35dead5b51b8 (patch)
tree5e4ce313495568d99fa3f21a9f097cc4fc5edeb1
parentfc7847ae9fafdd7cbca64f4619eb481bee58a5d7 (diff)
tracker/trackhat: implement LED tracking indicator
-rw-r--r--tracker-trackhat/camera.cpp14
-rw-r--r--tracker-trackhat/dialog.cpp9
-rw-r--r--tracker-trackhat/dialog.ui95
-rw-r--r--tracker-trackhat/lang/nl_NL.ts16
-rw-r--r--tracker-trackhat/lang/ru_RU.ts16
-rw-r--r--tracker-trackhat/lang/stub.ts16
-rw-r--r--tracker-trackhat/lang/zh_CN.ts16
-rw-r--r--tracker-trackhat/led.cpp66
-rw-r--r--tracker-trackhat/trackhat.hpp33
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