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
125
126
127
128
129
130
131
132
133
134
135
136
137
138
139
140
141
142
143
144
145
146
147
148
149
150
151
152
153
154
155
156
157
158
159
160
161
162
163
164
165
166
167
168
169
170
171
|
#include "survive_api.h"
#include "os_generic.h"
#include "survive.h"
#include "stdio.h"
struct SurviveAsyncObject {
struct SurviveAsyncContext* actx;
enum SurviveAsyncObject_type {
SurviveAsyncObject_LIGHTHOUSE,
SurviveAsyncObject_OBJECT
} type;
union {
int lighthouse;
struct SurviveObject* so;
} data;
char name[32];
bool has_update;
};
struct SurviveAsyncContext {
SurviveContext* ctx;
bool running;
og_thread_t thread;
og_mutex_t poll_mutex;
size_t object_ct;
struct SurviveAsyncObject objects[];
};
static void pose_fn(SurviveObject *so, uint32_t timecode, SurvivePose *pose) {
struct SurviveAsyncContext *actx = so->ctx->user_ptr;
OGLockMutex(actx->poll_mutex);
survive_default_raw_pose_process(so, timecode, pose);
int idx = (int)so->user_ptr;
actx->objects[idx].has_update = true;
OGUnlockMutex(actx->poll_mutex);
}
static void lh_fn(SurviveContext *ctx, uint8_t lighthouse, SurvivePose *lighthouse_pose,
SurvivePose *object_pose) {
struct SurviveAsyncContext *actx = ctx->user_ptr;
OGLockMutex(actx->poll_mutex);
survive_default_lighthouse_pose_process(ctx, lighthouse, lighthouse_pose, object_pose);
actx->objects[lighthouse].has_update = true;
OGUnlockMutex(actx->poll_mutex);
}
struct SurviveAsyncContext *survive_async_init(int argc, char *const *argv) {
SurviveContext* ctx = survive_init(argc, argv);
if (ctx == 0)
return 0;
survive_startup(ctx);
int object_ct = ctx->activeLighthouses + ctx->objs_ct;
struct SurviveAsyncContext * actx = calloc(1, sizeof(struct SurviveAsyncContext) + sizeof(struct SurviveAsyncObject) * object_ct );
actx->object_ct = object_ct;
actx->ctx = ctx;
actx->poll_mutex = OGCreateMutex();
ctx->user_ptr = actx;
int i = 0;
for (i = 0; i < ctx->activeLighthouses; i++) {
struct SurviveAsyncObject* obj = &actx->objects[i];
obj->data.lighthouse = i;
obj->type = SurviveAsyncObject_LIGHTHOUSE;
obj->actx = actx;
obj->has_update = ctx->bsd[i].PositionSet;
snprintf(obj->name, 32, "LH%d", i);
}
for (; i < object_ct; i++) {
struct SurviveAsyncObject* obj = &actx->objects[i];
int so_idx = i - ctx->activeLighthouses;
obj->data.so = ctx->objs[so_idx];
obj->type = SurviveAsyncObject_OBJECT;
obj->actx = actx;
obj->data.so->user_ptr = (void*)i;
snprintf(obj->name, 32, "%s", obj->data.so->codename);
}
survive_install_pose_fn(ctx, pose_fn);
survive_install_lighthouse_pose_fn(ctx, lh_fn);
return actx;
}
void survive_async_close(struct SurviveAsyncContext* actx) {
if (actx->running) {
survive_async_stop_thread(actx);
}
survive_close(actx->ctx);
}
static inline void* __async_thread(void* _actx) {
struct SurviveAsyncContext* actx = _actx;
int error = 0;
while (actx->running && error == 0) {
error = survive_poll(actx->ctx);
}
actx->running = false;
return (void*)error;
}
bool survive_async_is_running(struct SurviveAsyncContext* actx) {
return actx->running;
}
void survive_async_start_thread(struct SurviveAsyncContext* actx) {
actx->running = true;
actx->thread = OGCreateThread(__async_thread, actx);
}
int survive_async_stop_thread(struct SurviveAsyncContext* actx) {
actx->running = false;
int error = (int)OGJoinThread(actx->thread);
if (error != 0) {
SurviveContext* ctx = actx->ctx;
SV_INFO("Warning: Loope exited with error %d", error);
}
return error;
}
const struct SurviveAsyncObject *survive_async_get_next_object(struct SurviveAsyncContext *actx,
const struct SurviveAsyncObject *curr) {
const struct SurviveAsyncObject* next = curr + 1;
if (next >= actx->objects + actx->object_ct)
return 0;
return next;
}
const struct SurviveAsyncObject *survive_async_get_first_object(struct SurviveAsyncContext *actx) {
return actx->objects;
}
const struct SurviveAsyncObject* survive_async_get_next_updated(struct SurviveAsyncContext* actx) {
for (int i = 0; i < actx->object_ct; i++) {
if (actx->objects[i].has_update) {
actx->objects[i].has_update = false;
return &actx->objects[i];
}
}
return 0;
}
uint32_t survive_async_object_get_latest_pose(const struct SurviveAsyncObject* sao, SurvivePose* pose) {
uint32_t timecode = 0;
OGLockMutex(sao->actx->poll_mutex);
switch (sao->type) {
case SurviveAsyncObject_LIGHTHOUSE: {
if(pose)
*pose = sao->actx->ctx->bsd[sao->data.lighthouse].Pose;
break;
}
case SurviveAsyncObject_OBJECT:
if(pose)
*pose = sao->data.so->OutPose;
timecode = sao->data.so->OutPose_timecode;
break;
}
OGUnlockMutex(sao->actx->poll_mutex);
return timecode;
}
const char * survive_async_object_name(const SurviveAsyncObject * sao)
{
return sao->name;
}
|
