From 800c13a72ace78be48537906898e35dead5b51b8 Mon Sep 17 00:00:00 2001 From: Stanislaw Halik Date: Sun, 29 May 2022 16:46:19 +0200 Subject: tracker/trackhat: implement LED tracking indicator --- tracker-trackhat/camera.cpp | 14 +++++++ tracker-trackhat/dialog.cpp | 9 ++++ tracker-trackhat/dialog.ui | 95 +++++++++++++++++++++++++++++------------- tracker-trackhat/lang/nl_NL.ts | 16 +++++++ tracker-trackhat/lang/ru_RU.ts | 16 +++++++ tracker-trackhat/lang/stub.ts | 16 +++++++ tracker-trackhat/lang/zh_CN.ts | 16 +++++++ tracker-trackhat/led.cpp | 66 +++++++++++++++++++++++++++++ tracker-trackhat/trackhat.hpp | 33 ++++++++++++++- 9 files changed, 250 insertions(+), 31 deletions(-) create mode 100644 tracker-trackhat/led.cpp (limited to 'tracker-trackhat') 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 #include namespace trackhat_impl { @@ -83,6 +84,15 @@ pt_camera::result trackhat_camera::get_frame(pt_frame& frame_) goto error; auto& frame = *frame_.as(); 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(), 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 @@ 0 0 365 - 460 + 485 @@ -30,27 +30,8 @@ Camera - - - - - 0 - 0 - - - - 239 - - - 10 - - - Qt::Horizontal - - - - - + + Qt::NoFocus @@ -75,15 +56,27 @@ - - - - Gain + + + + + 0 + 0 + + + + 239 + + + 10 + + + Qt::Horizontal - - + + 0 @@ -101,8 +94,8 @@ - - + + Qt::NoFocus @@ -120,6 +113,45 @@ + + + + Gain + + + + + + + LED + + + + + + + + 0 + 0 + + + + + Off + + + + + Constant + + + + + Dynamic + + + + @@ -553,12 +585,15 @@ exposure_slider + gain_slider + led_mode model_type min_point_size max_point_size enable_point_filter point_filter_slider point_filter_limit + point_filter_deadzone 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 @@ Gain + + LED + + + + Off + + + + Constant + + + + Dynamic + + trackhat_module 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 @@ Gain + + LED + + + + Off + + + + Constant + + + + Dynamic + + trackhat_module 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 @@ Gain + + LED + + + + Off + + + + Constant + + + + Dynamic + + trackhat_module 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 @@ Gain + + LED + + + + Off + + + + Constant + + + + Dynamic + + trackhat_module 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 #include #include +#include #include 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 point_filter_coefficient{b, "point-filter-coefficient", { 1.5, 1, 4 }}; value point_filter_limit { b, "point-filter-limit", { 0.1, 0.01, 1 }}; value point_filter_deadzone { b, "point-filter-deadzone", {0, 0, 1}}; + value led { b, "led-mode", led_mode::dynamic }; }; class setting_receiver : public QObject @@ -59,9 +66,31 @@ private: std::atomic 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_; + 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 -- cgit v1.2.3