blob: a1265e808cbe625ea67085a23c392ef9db66ebbe (
plain)
1
2
3
4
5
6
7
8
9
10
11
12
13
14
15
16
17
18
19
20
21
22
23
24
25
26
27
28
29
30
31
32
33
34
35
36
37
38
39
40
41
42
43
44
45
46
47
48
49
50
51
52
53
54
55
56
57
58
59
60
61
62
63
64
65
66
67
68
69
70
71
72
73
74
75
76
77
78
79
80
81
82
83
84
85
86
87
88
89
90
91
92
93
94
95
96
97
98
99
100
101
102
103
104
105
106
107
108
109
110
111
112
113
114
115
116
117
118
119
120
121
122
123
124
|
#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 = {};
}
|
