tracker/trackhat: simplify connection state tracking

4 files changed, 150 insertions, 99 deletions

diff --git a/tracker-trackhat/camera.cpp b/tracker-trackhat/camera.cpp
index 486ff66f..c6639391 100644
--- a/tracker-trackhat/camera.cpp
+++ b/tracker-trackhat/camera.cpp
@@ -1,6 +1,20 @@
 #include "trackhat.hpp"
 #include "compat/sleep.hpp"
-#include <QDebug>
+#include <cstdio>
+
+namespace trackhat_impl {
+
+TH_ErrorCode log_error(TH_ErrorCode error, const char* source)
+{
+    if (error == TH_ERROR_DEVICE_ALREADY_OPEN)
+        error = TH_SUCCESS;
+    if (error)
+        fprintf(stderr, "error 0x%x in %s\n", -error, source);
+    fflush(stderr);
+    return error;
+}
+
+} // ns trackhat_impl
 
 pt_camera::result trackhat_camera::get_info() const
 {
@@ -50,116 +64,57 @@ trackhat_camera::~trackhat_camera()
     stop();
 }
 
-#define CHECK(x)                                                \
-    do {                                                        \
-        if (TH_ErrorCode status_ = (x); status_ != TH_SUCCESS)  \
-        {                                                       \
-            qDebug() << "trackhat: error"                       \
-                     << (void*)-status_ << "in" << #x;          \
-            error_code = status_;                               \
-            goto error;                                         \
-        }                                                       \
-    } while (false)
-
 pt_camera::result trackhat_camera::get_frame(pt_frame& frame_)
 {
-start:
-    switch (status)
-    {
-    case th_noinit:
-    case th_init:
-        return {false, get_desired()};
-    case th_detect:
-        CHECK(trackHat_Connect(&device, TH_FRAME_EXTENDED));
-        status = th_connect;
-        goto start;
-    case th_connect:
-        uint32_t uptime;
-        CHECK(trackHat_GetUptime(&device, &uptime));
-        status = th_running;
-        break;
-    case th_running:
-        break;
-    }
+    if (!device.ensure_connected())
+        goto error;
 
     if (sig.test_and_clear())
     {
         set_pt_options();
-        if ((error_code = (decltype(error_code))init_regs()) != TH_SUCCESS)
+        TH_ErrorCode status;
+        int i;
+        for (i = 0; i < 5; i++)
+        {
+            status = (TH_ErrorCode)init_regs();
+            if (status != TH_FAILED_TO_SET_REGISTER)
+                break;
+        }
+        if (i == 5)
             goto error;
     }
 
-    if (trackHat_ExtendedPoints_t points = {};
-        (error_code = trackHat_GetDetectedPointsExtended(&device, &points)) == TH_SUCCESS)
     {
+        trackHat_ExtendedPoints_t points;
+        if (!!th_check(trackHat_GetDetectedPointsExtended(&*device, &points)))
+            goto error;
         auto& frame = *frame_.as<trackhat_frame>();
         frame.init_points(points, t.min_pt_size, t.max_pt_size);
     }
-    else
-        goto error;
 
     return {true, get_desired()};
 
 error:
-    if (status >= th_running)
-        qDebug() << "trackhat: error" << (void*)error_code;
     stop();
-    return {false, get_desired()};
+    return {false, {}};
 }
 
 bool trackhat_camera::start(const pt_settings&)
 {
-    int attempts = 0;
-    constexpr int max_attempts = 5;
+    if constexpr(debug_mode)
+        trackHat_EnableDebugMode();
+    else
+        trackHat_DisableDebugMode();
 
-    if (status != th_noinit)
-        return true;
+    if (!device.ensure_device_exists())
+        return false;
 
-start:
-    trackHat_EnableDebugMode();
     set_pt_options();
 
-    error_code = TH_SUCCESS;
-    status = th_noinit;
-
-    CHECK(trackHat_Initialize(&device)); status = th_init;
-    CHECK(trackHat_DetectDevice(&device)); status = th_detect;
-
     return true;
-error:
-    stop();
-    switch (error_code)
-    {
-    case TH_ERROR_DEVICE_NOT_DETECTED:
-    case TH_ERROR_CAMERA_SELFT_TEST_FAILD:
-    case TH_ERROR_CAMERA_INTERNAL_BROKEN:
-        break;
-    default:
-        if (attempts++ < max_attempts)
-        {
-            portable::sleep(10);
-            goto start;
-        }
-    }
-    return false;
 }
 
 void trackhat_camera::stop()
 {
-#if 0
-    if (status >= th_connect)
-    {
-        uint32_t uptime = 0;
-        if (TH_ErrorCode status = trackHat_GetUptime(&device, &uptime); status == TH_SUCCESS)
-            qDebug() << "trackhat stop: device uptime" << uptime << "seconds";
-    }
-#endif
-
-    if (status >= th_connect)
-        (void)trackHat_Disconnect(&device);
-    if (status >= th_init)
-        (void)trackHat_Deinitialize(&device);
-
-    status = th_noinit;
-    device = {};
+    device.disconnect();
 }
diff --git a/tracker-trackhat/handle.cpp b/tracker-trackhat/handle.cpp
new file mode 100644
index 00000000..0c2ce822
--- /dev/null
+++ b/tracker-trackhat/handle.cpp
@@ -0,0 +1,72 @@
+#undef NDEBUG
+#include "trackhat.hpp"
+#include "compat/sleep.hpp"
+#include "compat/timer.hpp"
+#include <cassert>
+
+bool camera_handle::ensure_connected()
+{
+    if (state_ >= st_streaming)
+        return true;
+    else if (state_ == st_stopped)
+        return false;
+
+    Timer t;
+
+    constexpr int max_attempts = 5;
+
+    if (!ensure_device_exists())
+        goto error;
+
+    for (int i = 0; i < max_attempts; i++)
+    {
+        if (!th_check(trackHat_Connect(&device_, TH_FRAME_EXTENDED)))
+        {
+            state_ = st_streaming;
+            if (int ms = (int)t.elapsed_ms(); ms > 1000)
+                qDebug() << "tracker/trackhat: connecting took" << ms << "ms";
+            return true;
+        }
+
+        auto dbg = qDebug();
+        dbg << "tracker/trackhat: connect failed, retry";
+        dbg.space(); dbg.nospace();
+        dbg << i << "/" << max_attempts;
+        portable::sleep(50);
+    }
+
+error:
+    disconnect();
+    return false;
+}
+
+bool camera_handle::ensure_device_exists()
+{
+    switch (state_)
+    {
+    case st_streaming:
+        return true;
+    case st_detected:
+        disconnect();
+        [[fallthrough]];
+    case st_stopped:
+        assert(!th_check(trackHat_Initialize(&device_)) && device_.m_pInternal);
+        if (auto error = th_check(trackHat_DetectDevice(&device_)); error)
+        {
+            disconnect();
+            return false;
+        }
+        state_ = st_detected;
+        return true;
+    }
+}
+
+void camera_handle::disconnect()
+{
+    state_ = st_stopped;
+    if (device_.m_pInternal)
+    {
+        (void)!th_check(trackHat_Disconnect(&device_));
+        trackHat_Deinitialize(&device_);
+    }
+}
diff --git a/tracker-trackhat/settings.cpp b/tracker-trackhat/settings.cpp
index 28b97c0a..ecc693b8 100644
--- a/tracker-trackhat/settings.cpp
+++ b/tracker-trackhat/settings.cpp
@@ -75,11 +75,10 @@ void trackhat_camera::set_pt_options()
     s.point_filter_coefficient = *t.point_filter_coefficient;
 }
 
-int trackhat_camera::init_regs()
+bool trackhat_camera::init_regs()
 {
     unsigned attempts = 0;
     constexpr unsigned max_attempts = 5;
-    TH_ErrorCode error;
 
     auto exp    = (uint8_t)t.exposure;
     auto exp2   = (uint8_t)(exp == 0xff ? 0xf0 : 0xff);
@@ -109,26 +108,32 @@ start:
     for (const auto& reg : regs)
     {
         trackHat_SetRegister_t r{reg[0], reg[1], reg[2]};
-        error = trackHat_SetRegisterValue(&device, &r);
-        if (error != TH_SUCCESS)
+        auto status = th_check(trackHat_SetRegisterValue(&*device, &r));
+        if (status == TH_SUCCESS)
+            continue;
+        else if (status == TH_FAILED_TO_SET_REGISTER)
+            goto retry;
+        else
             goto error;
     }
 
     if (int elapsed = (int)t.elapsed_ms(); elapsed > 250)
         qDebug() << "tracker/trackhat: setting registers took" << elapsed << "ms";
 
-    return TH_SUCCESS;
+    return true;
 
-error:
+retry:
     if (attempts++ < max_attempts)
     {
-        qDebug() << "tracker/trackhat: set register retry attempt"
-                 << attempts << "/" << max_attempts;
-        portable::sleep(10);
+        auto dbg = qDebug();
+        dbg << "tracker/trackhat: set register retry attempt";
+        dbg.space(); dbg.nospace();
+        dbg << attempts << "/" << max_attempts;
+        portable::sleep(50);
         goto start;
     }
 
-    qDebug() << "tracker/trackhat: failed to set registers";
-
-    return error;
+error:
+    device.disconnect();
+    return false;
 }
diff --git a/tracker-trackhat/trackhat.hpp b/tracker-trackhat/trackhat.hpp
index b1118c1c..a5b4bb3c 100644
--- a/tracker-trackhat/trackhat.hpp
+++ b/tracker-trackhat/trackhat.hpp
@@ -24,6 +24,9 @@ namespace trackhat_impl
 {
 using namespace options;
 
+TH_ErrorCode log_error(TH_ErrorCode error, const char* source);
+#define th_check(expr) ::trackhat_impl::log_error((expr), #expr)
+
 struct trackhat_settings : opts
 {
     trackhat_settings();
@@ -77,6 +80,24 @@ struct point
     bool ok = false;
 };
 
+struct camera_handle final
+{
+    OTR_DISABLE_MOVE_COPY(camera_handle);
+    trackHat_Device_t* operator->() { return &device_; }
+    trackHat_Device_t& operator*() { return device_; }
+
+    camera_handle() = default;
+    ~camera_handle() = default;
+
+    [[nodiscard]] bool ensure_connected();
+    [[nodiscard]] bool ensure_device_exists();
+    void disconnect();
+private:
+    trackHat_Device_t device_ = {};
+    enum state { st_stopped, st_detected, st_streaming, };
+    state state_ = st_stopped;
+};
+
 struct trackhat_camera final : pt_camera
 {
     trackhat_camera();
@@ -100,17 +121,15 @@ struct trackhat_camera final : pt_camera
     static constexpr int sensor_size = 2940;
     static constexpr int sensor_fov = 52;
     static constexpr int point_count = TRACK_HAT_NUMBER_OF_POINTS;
+    static constexpr bool debug_mode = true;
 
 private:
-    enum device_status { th_noinit, th_init, th_detect, th_connect, th_running, };
     trackhat_impl::setting_receiver sig{true};
 
-    [[nodiscard]] int init_regs();
+    [[nodiscard]] bool init_regs();
     void set_pt_options();
 
-    trackHat_Device_t device {};
-    device_status status = th_noinit;
-    TH_ErrorCode error_code = TH_SUCCESS;
+    camera_handle device;
     pt_settings s{trackhat_metadata::module_name};
     trackhat_settings t;
 };