1 files changed, 129 insertions, 0 deletions
diff --git a/tracker-trackhat/camera.cpp b/tracker-trackhat/camera.cpp
new file mode 100644
index 00000000..19a8d6ca
--- /dev/null
+++ b/tracker-trackhat/camera.cpp
@@ -0,0 +1,129 @@
+#include "trackhat.hpp"
+#include <QDebug>
+
+pt_camera::result trackhat_camera::get_info() const
+{
+    return {true, get_desired() };
+}
+
+pt_camera_info trackhat_camera::get_desired() const
+{
+    pt_camera_info ret = {};
+
+    ret.fov = sensor_fov;
+    ret.fps = 250;
+    ret.res_x = sensor_size;
+    ret.res_y = sensor_size;
+
+    return ret;
+}
+
+QString trackhat_camera::get_desired_name() const
+{
+    return QStringLiteral("TrackHat sensor");
+}
+
+QString trackhat_camera::get_active_name() const
+{
+    return get_desired_name();
+}
+
+void trackhat_camera::set_fov(pt_camera::f) {}
+void trackhat_camera::show_camera_settings() {}
+
+trackhat_camera::trackhat_camera() = default;
+
+trackhat_camera::~trackhat_camera()
+{
+    stop();
+}
+
+pt_camera::result trackhat_camera::get_frame(pt_frame& frame_)
+{
+    auto& ret = *frame_.as<trackhat_frame>();
+    trackhat_frame frame;
+
+    if (status < th_running || error_code != TH_SUCCESS)
+    {
+        if (status >= th_running)
+            qDebug() << "trackhat: disconnected, status" << (void*)error_code;
+        goto error;
+    }
+
+    if (TH_ErrorCode error = trackHat_GetDetectedPoints(&device, &frame.points); error != TH_SUCCESS)
+    {
+        error_code = error;
+        goto error;
+    }
+
+    ret.points = frame.points;
+    return {true, get_desired()};
+
+error:
+    if (error_code != TH_SUCCESS)
+        qDebug() << "trackhat: error" << (void*)error_code;
+    ret.points = {};
+    stop();
+    return {false, get_desired()};
+}
+
+#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)
+
+bool trackhat_camera::start(const pt_settings&)
+{
+    stop();
+    trackHat_EnableDebugMode();
+
+    [[maybe_unused]] uint32_t uptime = 0;
+    error_code = TH_SUCCESS;
+    status = th_noinit;
+
+    CHECK(trackHat_Initialize(&device)); status = th_init;
+    CHECK(trackHat_DetectDevice(&device)); status = th_detect;
+    CHECK(trackHat_Connect(&device)); status = th_connect;
+    CHECK(trackHat_GetUptime(&device, &uptime)); status = th_running;
+
+#if 0
+    qDebug() << "trackhat start: device uptime" << uptime << "seconds";
+#endif
+
+    return true;
+error:
+    stop();
+    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 = {};
+}
+
+trackhat_preview::trackhat_preview(int w, int h)
+{
+    frame_bgr.create(h, w, CV_8UC3);
+    frame_bgra.create(h, w, CV_8UC4);
+}