mmo-ref/og-libsurvive
1
0
Fork 0
mirror of https://github.com/collabora/libsurvive.git synced 2026-08-04 00:51:21 -04:00
Code Issues Projects Releases Packages Wiki Activity Actions

Merge branch 'master' into gen2_wip

Browse source
This commit is contained in:
Justin Berger 2019-11-23 23:30:55 -07:00
commit 52c62ce2db
52 changed files with 466 additions and 221 deletions
Download patch file Download diff file Expand all files Collapse all files

25
CMakeLists.txt
View file

@ -1,9 +1,8 @@
project(libsurvive C)
cmake_minimum_required(VERSION 3.5.1)
enable_testing()
IF(UNIX)
SET(CMAKE_C_FLAGS "${CMAKE_C_FLAGS} -fPIC -std=gnu99 -rdynamic -Werror=incompatible-pointer-types -Wall -Wno-unused-variable -Wno-switch -Wno-parentheses -Wno-missing-braces")
SET(CMAKE_C_FLAGS "${CMAKE_C_FLAGS} -fPIC -std=gnu99 -rdynamic -Werror=incompatible-pointer-types -Wall -Wno-unused-variable -Wno-switch -Wno-parentheses -Wno-missing-braces -Werror=implicit-function-declaration")
ENDIF()
include_directories(redist include/libsurvive include)
@ -11,6 +10,11 @@ include_directories(redist include/libsurvive include)
option(USE_HIDAPI "Use HIDAPI instead of libusb" OFF)
option(USE_ASAN "Use address sanitizer" OFF)
option(ENABLE_TESTS "Enable build / execution of tests" OFF)
IF (ENABLE_TESTS)
enable_testing()
ENDIF()
IF(NOT WIN32)
include(CheckIncludeFile)
@ -48,6 +52,8 @@ endif()
IF(WIN32)
add_definitions(-DNOZLIB)
set( CMAKE_RUNTIME_OUTPUT_DIRECTORY ${CMAKE_BINARY_DIR}/$<CONFIG>)
ENDIF()
list (APPEND CMAKE_REQUIRED_DEFINITIONS -D_GNU_SOURCE)
@ -131,12 +137,14 @@ IF(USE_LAPACKE)
LIST(APPEND PLUGINS poser_sba)
ENDIF()
find_library(PCAP_LIBRARY pcap)
if(PCAP_LIBRARY)
IF(NOT WIN32)
find_library(PCAP_LIBRARY pcap)
if(PCAP_LIBRARY)
list(APPEND PLUGINS driver_usbmon)
set(driver_usbmon_ADDITIONAL_LIBS "${PCAP_LIBRARY}")
else()
set(driver_usbmon_ADDITIONAL_LIBS "${PCAP_LIBRARY}")
else()
message("Can't build usbmon plugin -- pcap library was not found")
endif()
endif()
if(UNIX)
@ -177,8 +185,9 @@ foreach(executable data_recorder simple_pose_test survive-cli api_example sensor
endforeach()
target_link_libraries(simple_pose_test CNGFX)
add_subdirectory(src/test_cases)
IF (ENABLE_TESTS)
add_subdirectory(src/test_cases)
ENDIF()
find_package(catkin QUIET COMPONENTS
roscpp

2
README.md
View file

@ -335,7 +335,7 @@ Nothing will happen until you connect to that page. When you do, the app lifetim
The arrow keys will move you to the left / right / up / down and the UI response to orbital mouse controls.
![Visuzliation Screenshot](https://raw.githubusercontent.com/cnlohr/libsurvive/master/useful_files/viz_screenshot.jpg)
![Visuzliation Screenshot](https://raw.githubusercontent.com/cnlohr/libsurvive/master/useful_files/viz_screenshot.png)
## Notes about coordinate frames.

2
api_example.c
View file

@ -30,7 +30,7 @@ int main(int argc, char **argv) {
pose.Rot[1], pose.Rot[2], pose.Rot[3]);
}
struct SurviveSimpleEvent event = {};
struct SurviveSimpleEvent event = {0};
while (survive_simple_next_event(actx, &event) != SurviveSimpleEventType_None) {
switch (event.event_type) {

13
include/libsurvive/survive.h
View file

@ -6,6 +6,7 @@
#include <stdbool.h>
#include <stdint.h>
#include <stdio.h>
#include <stdlib.h>
#ifdef __cplusplus
extern "C" {
@ -479,6 +480,18 @@ SURVIVE_EXPORT void handle_lightcap(SurviveObject *so, const LightcapElement *le
SV_LOG_NULL_GUARD ctx->logproc(ctx, SURVIVE_LOG_LEVEL_INFO, stbuff); \
}
inline static void *sv_dynamic_ptr_check(char *file, int line, void *ptr) {
if (ptr == NULL) {
fprintf(stderr, "Survive: memory allocation request failed in file %s, line %d, exiting", file, line);
exit(EXIT_FAILURE);
}
return ptr;
}
#define SV_MALLOC(size) sv_dynamic_ptr_check(__FILE__, __LINE__, malloc(size))
#define SV_CALLOC(num, size) sv_dynamic_ptr_check(__FILE__, __LINE__, calloc((num), (size)))
#define SV_REALLOC(ptr, size) sv_dynamic_ptr_check(__FILE__, __LINE__, realloc(ptr, (size)))
static inline void survive_notify_gen2(struct SurviveObject *so, const char *msg) {
if (so->ctx->lh_version_forced != -1 && so->ctx->lh_version_forced != 1) {
return;

19
redist/CMakeLists.txt
View file

@ -52,7 +52,7 @@ IF(USE_OPENCV)
target_link_libraries(minimal_opencv ${OpenCV_LIBS})
ENDIF()
IF(USE_LAPACKE) # NOT USE_OPENCV -- lapack / blas
IF(USE_LAPACKE OR WIN32) # NOT USE_OPENCV -- lapack / blas
IF(UNIX)
target_link_libraries(minimal_opencv cblas lapacke)
else()
@ -85,12 +85,15 @@ IF(USE_LAPACKE) # NOT USE_OPENCV -- lapack / blas
endif()
endif()
add_executable(lintest linmath.c linmath.h lintest.c)
target_link_libraries(lintest minimal_opencv)
set_target_properties(lintest PROPERTIES FOLDER "tests")
add_executable(minimal_opencvtest minimal_opencvtest.c)
target_link_libraries(minimal_opencvtest minimal_opencv)
set_target_properties(minimal_opencvtest PROPERTIES FOLDER "tests")
IF (ENABLE_TESTS)
add_executable(lintest linmath.c linmath.h lintest.c)
target_link_libraries(lintest minimal_opencv)
set_target_properties(lintest PROPERTIES FOLDER "tests")
add_test(NAME lintest COMMAND lintest)
add_executable(minimal_opencvtest minimal_opencvtest.c)
target_link_libraries(minimal_opencvtest minimal_opencv)
set_target_properties(minimal_opencvtest PROPERTIES FOLDER "tests")
add_test(NAME lintest COMMAND lintest)
ENDIF()

2
redist/lintest.c
View file

@ -144,7 +144,7 @@ void testKabsch() {
}
static void testKabsch2() {
LinmathQuat q = {};
LinmathQuat q = {0};
LinmathPoint3d survivePts[] = {{1, 0, 0}, {0, 1, 0}, {0, 0, 1}};

10
redist/minimal_opencvtest.c
View file

@ -47,7 +47,7 @@ static void test_solve() {
{
double _A[3] = {1, 2, 3};
double _B[3] = {4, 8, 12};
double _x[1] = {};
double _x[1] = {0};
CvMat A = cvMat(3, 1, CV_64F, _A);
CvMat B = cvMat(3, 1, CV_64F, _B);
@ -61,7 +61,7 @@ static void test_solve() {
{
double _A[3] = {1, 2, 3};
double _B[9] = {4, 5, 6, 7, 8, 9, 10, 11, 12};
double _x[3] = {};
double _x[3] = {0};
CvMat A = cvMat(3, 1, CV_64F, _A);
CvMat B = cvMat(3, 3, CV_64F, _B);
@ -77,13 +77,13 @@ static void test_svd() {
double _3x3[3 * 3] = {1, 2, 3, 4, 5, 6, 7, 8, 12};
CvMat m3x3 = cvMat(3, 3, CV_64F, _3x3);
double _w[3] = {};
double _w[3] = {0};
CvMat w = cvMat(1, 3, CV_64F, _w);
double _u[9] = {};
double _u[9] = {0};
CvMat u = cvMat(3, 3, CV_64F, _u);
double _v[9] = {};
double _v[9] = {0};
CvMat v = cvMat(3, 3, CV_64F, _v);
cvSVD(&m3x3, &w, &u, &v, 0);

35
redist/os_generic.h
View file

@ -74,11 +74,15 @@ extern "C" {
#ifdef USE_WINDOWS
#include <stdint.h>
#include <windows.h>
OSG_INLINE void OGSleep(int is) { Sleep(is * 1000); }
OSG_INLINE void OGUSleep(int ius) { Sleep(ius / 1000); }
OSG_INLINE int OGUSleep(int ius) {
Sleep(ius / 1000);
return 0;
}
OSG_INLINE double OGGetAbsoluteTime() {
static LARGE_INTEGER lpf;
@ -119,6 +123,8 @@ OSG_INLINE double OGGetFileTime(const char *file) {
return ft.dwHighDateTime + ft.dwLowDateTime;
}
OSG_INLINE void OGNameThread(og_thread_t t, const char *name) {}
OSG_INLINE og_thread_t OGCreateThread(void *(routine)(void *), void *parameter) {
return (og_thread_t)CreateThread(0, 0, (LPTHREAD_START_ROUTINE)routine, parameter, 0, 0);
}
@ -202,10 +208,6 @@ OSG_INLINE og_cv_t OGCreateConditionVariable() {
#else
#ifndef _GNU_SOURCE
#define _GNU_SOURCE
#endif
#include <pthread.h>
#include <semaphore.h>
#include <stdint.h>
@ -219,7 +221,9 @@ OSG_INLINE og_cv_t OGCreateConditionVariable() {
OSG_INLINE void OGSleep(int is) { sleep(is); }
OSG_INLINE int OGUSleep(int ius) {
struct timespec sleep = {.tv_nsec = ius * 1000};
struct timespec sleep = {0};
sleep.tv_nsec = (ius % 1000000) * 1000;
sleep.tv_sec = ius / 1000000;
return nanosleep(&sleep, 0);
}
@ -248,6 +252,11 @@ OSG_INLINE double OGGetFileTime(const char *file) {
return buff.st_mtime;
}
OSG_INLINE void OGNameThread(og_thread_t t, const char *name) {
#ifdef _GNU_SOURCE
pthread_setname_np(*(pthread_t *)t, name);
#endif
}
OSG_INLINE og_thread_t OGCreateThread(void *(routine)(void *), void *parameter) {
pthread_t *ret = (pthread_t *)malloc(sizeof(pthread_t));
int r = pthread_create(ret, 0, routine, parameter);
@ -338,12 +347,18 @@ OSG_INLINE void OGDeleteSema(og_sema_t os) {
free(os);
}
OSG_INLINE void OGSignalCond(og_cv_t cv) { _OGHandlePosixError("OGSignalCond", pthread_cond_signal(cv)); }
OSG_INLINE void OGBroadcastCond(og_cv_t cv) { _OGHandlePosixError("OGBroadcastCond", pthread_cond_broadcast(cv)); }
OSG_INLINE void OGWaitCond(og_cv_t cv, og_mutex_t m) { _OGHandlePosixError("OGWaitCond", pthread_cond_wait(cv, m)); }
OSG_INLINE void OGSignalCond(og_cv_t cv) {
_OGHandlePosixError("OGSignalCond", pthread_cond_signal((pthread_cond_t *)cv));
}
OSG_INLINE void OGBroadcastCond(og_cv_t cv) {
_OGHandlePosixError("OGBroadcastCond", pthread_cond_broadcast((pthread_cond_t *)cv));
}
OSG_INLINE void OGWaitCond(og_cv_t cv, og_mutex_t m) {
_OGHandlePosixError("OGWaitCond", pthread_cond_wait((pthread_cond_t *)cv, (pthread_mutex_t *)m));
}
OSG_INLINE void OGDeleteConditionVariable(og_cv_t cv) {
pthread_cond_destroy(cv);
pthread_cond_destroy((pthread_cond_t *)cv);
free(cv);
}

9
sensors-readout.c
View file

@ -2,14 +2,17 @@
static volatile int keepRunning = 1;
#include "math.h"
#include <os_generic.h>
#include <stdlib.h>
#ifdef __linux__
#include "math.h"
#include <assert.h>
#include <ctype.h>
#include <os_generic.h>
#include <signal.h>
#include <stdlib.h>
void intHandler(int dummy) {
if (keepRunning == 0)
@ -204,4 +207,4 @@ int main(int argc, char **argv) {
survive_close(ctx);
return 0;
}
}

12
src/barycentric_svd/barycentric_svd.c
View file

@ -3,6 +3,8 @@
#include "stdbool.h"
#include "stdio.h"
#include "stdlib.h"
#include "survive.h"
#include <malloc.h>
static void bc_svd_choose_control_points(bc_svd *self) {
// Take C0 as the reference points centroid:
@ -72,8 +74,8 @@ void bc_svd_bc_svd(bc_svd *self, void *user, bc_svd_fill_M_fn fillFn, const Linm
self->setup.obj_cnt = obj_cnt;
self->setup.obj_pts = obj_pts;
self->setup.alphas = calloc(obj_cnt, sizeof(self->setup.alphas[0]));
self->object_pts_in_camera = calloc(obj_cnt, sizeof(self->setup.alphas[0]));
self->setup.alphas = SV_CALLOC(obj_cnt, sizeof(self->setup.alphas[0]));
self->object_pts_in_camera = SV_CALLOC(obj_cnt, sizeof(self->setup.alphas[0]));
bc_svd_choose_control_points(self);
bc_svd_compute_barycentric_coordinates(self);
@ -121,7 +123,7 @@ void bc_svd_add_correspondence(bc_svd *self, size_t idx, double u, double v) {
if (self->meas_space <= self->meas_cnt) {
self->meas_space = self->meas_space * 2 + 1;
self->meas = realloc(self->meas, sizeof(self->meas[0]) * self->meas_space);
self->meas = SV_REALLOC(self->meas, sizeof(self->meas[0]) * self->meas_space);
}
self->meas[self->meas_cnt] = (bc_svd_meas_t){.angle = angle, .axis = i, .obj_idx = idx};
@ -268,8 +270,8 @@ void qr_solve(CvMat *A, CvMat *b, CvMat *X) {
}
if (max_nr < nr) {
max_nr = nr;
A1 = malloc(sizeof(double) * nr);
A2 = malloc(sizeof(double) * nr);
A1 = SV_MALLOC(sizeof(double) * nr);
A2 = SV_MALLOC(sizeof(double) * nr);
}
double *pA = A->data.db, *ppAkk = pA;

23
src/disambiguator_statebased.c
View file

@ -572,7 +572,7 @@ static void ProcessStateChange(Disambiguator_data_t *d, const LightcapElement *l
SV_WARN("Drift in timecodes %s %u", d->so->codename, delta);
}
d->mod_offset[LS_Params[d->state].lh] = new_offset;
DEBUG_TB("New offset %d (%d)", new_offset, delta);
// Figure out if it looks more like it has data or doesn't. We need this for OOX
int lengthData = ACODE_TIMING(LSParam_acode(d->state) | DATA_BIT);
int lengthNoData = ACODE_TIMING(LSParam_acode(d->state));
@ -611,7 +611,7 @@ static void ProcessStateChange(Disambiguator_data_t *d, const LightcapElement *l
}
if (cnt > 0) {
double var = 3;
size_t minl = 10;
size_t minl = DIV_ROUND_CLOSEST(avg_length, cnt * 4);
size_t maxl = var * DIV_ROUND_CLOSEST(avg_length, cnt);
for (int i = 0; i < d->so->sensor_ct; i++) {
@ -639,6 +639,18 @@ static void ProcessStateChange(Disambiguator_data_t *d, const LightcapElement *l
SetState(d, le, new_state);
}
static uint32_t offset_from_state(Disambiguator_data_t *d, const LightcapElement *le) {
struct SurviveContext *ctx = d->so->ctx;
Global_Disambiguator_data_t *g = ctx->disambiguator_data;
int end_of_mod = g->single_60hz_mode ? LS_WaitLHB_ACode0 : LS_END;
int lh = LS_Params[d->state].lh;
int le_offset = apply_mod_offset(le->timestamp + le->length / 2, d->mod_offset[lh], end_of_mod);
int state_offset = le_offset - LSParam_offset_for_state(d->state);
if (state_offset > LS_Params[d->state].window)
state_offset = state_offset - LS_Params[d->state].window;
return state_offset;
}
static void PropagateState(Disambiguator_data_t *d, const LightcapElement *le) {
struct SurviveContext *ctx = d->so->ctx;
if (le->sensor_id >= d->so->sensor_ct) {
@ -701,14 +713,14 @@ void DisambiguatorStateBased(SurviveObject *so, const LightcapElement *le) {
if (so->ctx->disambiguator_data == NULL) {
DEBUG_TB("Initializing Global Disambiguator Data");
Global_Disambiguator_data_t *d = calloc(1, sizeof(Global_Disambiguator_data_t));
Global_Disambiguator_data_t *d = SV_CALLOC(1, sizeof(Global_Disambiguator_data_t));
d->ctx = ctx;
ctx->disambiguator_data = d;
}
if (so->disambiguator_data == NULL) {
DEBUG_TB("Initializing Disambiguator Data for TB %d", so->sensor_ct);
Disambiguator_data_t *d = calloc(1, sizeof(Disambiguator_data_t) + sizeof(LightcapElement) * so->sensor_ct);
Disambiguator_data_t *d = SV_CALLOC(1, sizeof(Disambiguator_data_t) + sizeof(LightcapElement) * so->sensor_ct);
d->so = so;
so->disambiguator_data = d;
}
@ -720,7 +732,8 @@ void DisambiguatorStateBased(SurviveObject *so, const LightcapElement *le) {
return;
}
DEBUG_TB("%s LE: %2u\t%4u\t%10u\t%2u", so->codename, le->sensor_id, le->length, le->timestamp, d->state);
DEBUG_TB("%s LE: %2u\t%4u\t%10u\t%2u\t%7u", so->codename, le->sensor_id, le->length, le->timestamp, d->state,
offset_from_state(d, le));
if (d->state == LS_UNKNOWN) {
enum LighthouseState new_state = AttemptFindState(d, le);

2
src/disambiguator_turvey.c
View file

@ -441,7 +441,7 @@ void DisambiguatorTurvey(SurviveObject *so, LightcapElement *le) {
if (so->disambiguator_data == NULL) {
fprintf(stderr, "Initializing Disambiguator Data\n");
so->disambiguator_data = malloc(sizeof(lightcap2_data));
so->disambiguator_data = SV_MALLOC(sizeof(lightcap2_data));
memset(so->disambiguator_data, 0, sizeof(lightcap2_data));
}

8
src/driver_dummy.c
View file

@ -53,20 +53,20 @@ int dummy_haptic(SurviveObject *so, uint8_t reserved, uint16_t pulseHigh, uint16
}
int DriverRegDummy(SurviveContext *ctx) {
SurviveDriverDummy *sp = calloc(1, sizeof(SurviveDriverDummy));
SurviveDriverDummy *sp = SV_CALLOC(1, sizeof(SurviveDriverDummy));
sp->ctx = ctx;
SV_INFO("Setting up dummy driver.");
// Create a new SurviveObject...
SurviveObject *device = calloc(1, sizeof(SurviveObject));
SurviveObject *device = SV_CALLOC(1, sizeof(SurviveObject));
device->ctx = ctx;
device->driver = sp;
memcpy(device->codename, "DM0", 4);
memcpy(device->drivername, "DUM", 4);
device->sensor_ct = 1;
device->sensor_locations = malloc(sizeof(FLT) * 3);
device->sensor_normals = malloc(sizeof(FLT) * 3);
device->sensor_locations = SV_MALLOC(sizeof(FLT) * 3);
device->sensor_normals = SV_MALLOC(sizeof(FLT) * 3);
device->sensor_locations[0] = 0;
device->sensor_locations[1] = 0;
device->sensor_locations[2] = 0;

4
src/driver_simulator.c
View file

@ -228,7 +228,7 @@ void str_append(char **pString, const char *str) {
size_t l1 = *pString ? strlen(*pString) : 0;
size_t l2 = strlen(str);
*pString = realloc(*pString, l1 + l2 + 1);
*pString = SV_REALLOC(*pString, l1 + l2 + 1);
(*pString)[l1] = 0;
strcat(*pString, str);
@ -248,7 +248,7 @@ const BaseStationData simulated_bsd[2] = {
};
int DriverRegSimulator(SurviveContext *ctx) {
SurviveDriverSimulator *sp = calloc(1, sizeof(SurviveDriverSimulator));
SurviveDriverSimulator *sp = SV_CALLOC(1, sizeof(SurviveDriverSimulator));
sp->ctx = ctx;
sp->position.Rot[0] = 1;

8
src/driver_udp.c
View file

@ -104,20 +104,20 @@ int DriverRegUDP(SurviveContext *ctx) {
if (!enable_UDP_driver)
return 0;
SurviveDriverUDP *sp = calloc(1, sizeof(SurviveDriverUDP));
SurviveDriverUDP *sp = SV_CALLOC(1, sizeof(SurviveDriverUDP));
sp->ctx = ctx;
SV_INFO("Setting up UDP driver.");
// Create a new SurviveObject...
SurviveObject *device = calloc(1, sizeof(SurviveObject));
SurviveObject *device = SV_CALLOC(1, sizeof(SurviveObject));
device->ctx = ctx;
device->driver = sp;
memcpy(device->codename, "UD0", 4);
memcpy(device->drivername, "UDP", 4);
device->sensor_ct = 1;
device->sensor_locations = malloc(sizeof(FLT) * 3);
device->sensor_normals = malloc(sizeof(FLT) * 3);
device->sensor_locations = SV_MALLOC(sizeof(FLT) * 3);
device->sensor_normals = SV_MALLOC(sizeof(FLT) * 3);
device->sensor_locations[0] = 0;
device->sensor_locations[1] = 0;
device->sensor_locations[2] = 0;

57
src/driver_usbmon.c
View file

@ -21,6 +21,7 @@
STATIC_CONFIG_ITEM(USBMON_RECORD, "usbmon-record", 's', "File to save .pcap to.", 0);
STATIC_CONFIG_ITEM(USBMON_PLAYBACK, "usbmon-playback", 's', "File to replay .pcap from.", 0);
STATIC_CONFIG_ITEM(USBMON_RECORD_ALL, "usbmon-record-all", 'i', "Whether or not to record all usb traffic", 0);
typedef struct vive_device_t {
uint16_t vid, pid;
@ -62,6 +63,7 @@ typedef struct SurviveDriverUSBMon {
pcap_t *pcap;
pcap_dumper_t *pcapDumper;
bool record_all;
char errbuf[PCAP_ERRBUF_SIZE];
vive_device_inst_t usb_devices[VIVE_DEVICE_INST_MAX];
@ -94,7 +96,7 @@ static char *read_file(const char *fn, size_t *size) {
return 0;
}
source = malloc(sizeof(char) * (bufsize + 1));
source = SV_MALLOC(sizeof(char) * (bufsize + 1));
if (fseek(fp, 0L, SEEK_SET) != 0) {
fprintf(stderr, "fseek file '%s' failed with %d", fn, errno);
@ -126,7 +128,7 @@ static int interface_lookup(const vive_device_inst_t *dev, int endpoint) {
int32_t id = dev->device->pid + (endpoint << 16);
switch (id) {
case 0x812000:
return USB_IF_HMD_HEADSET_INFO;
return USB_IF_HMD_IMU;
case 0x812101:
return USB_IF_WATCHMAN1;
case 0x812022:
@ -183,10 +185,16 @@ static void ingest_config_request(vive_device_inst_t *dev, const struct _usb_hea
SurviveContext *ctx = dev->so->ctx;
if (cnt) {
memcpy(&dev->compressed_data[dev->compressed_data_idx], pktData + 2, cnt);
dev->compressed_data_idx += cnt;
// Some (Tracker at least?) devices send a uint64_t before data; not sure what it means but skip it for now.
if (dev->compressed_data_idx == 0 && cnt >= 2 && pktData[2] != 0x78) {
} else {
memcpy(&dev->compressed_data[dev->compressed_data_idx], pktData + 2, cnt);
dev->compressed_data_idx += cnt;
}
} else {
char *uncompressed_data = malloc(65536);
char *uncompressed_data = SV_MALLOC(65536);
SurviveContext *ctx = dev->so->ctx;
int len = survive_simple_inflate(dev->so->ctx, dev->compressed_data, dev->compressed_data_idx,
(uint8_t *)uncompressed_data, 65536 - 1);
@ -236,7 +244,7 @@ static int usbmon_close(struct SurviveContext *ctx, void *_driver) {
return 0;
}
static usb_info_t *get_usb_info_from_file(const char *fname) {
usb_info_t *rtn = calloc(MAX_USB_DEVS, sizeof(usb_info_t));
usb_info_t *rtn = SV_CALLOC(MAX_USB_DEVS, sizeof(usb_info_t));
size_t count = 0;
FILE *f = fopen(fname, "r");
while (!feof(f)) {
@ -262,7 +270,7 @@ static usb_info_t *get_usb_info_from_libusb() {
return 0;
count = libusb_get_device_list(context, &list);
rtn = (usb_info_t *)calloc(count + 1, sizeof(usb_info_t));
rtn = (usb_info_t *)SV_CALLOC(count + 1, sizeof(usb_info_t));
size_t fill_cnt = 0;
for (size_t idx = 0; idx < count; ++idx) {
libusb_device *device = list[idx];
@ -286,11 +294,14 @@ static usb_info_t *get_usb_info_from_libusb() {
return rtn;
}
static size_t fill_device_inst(vive_device_inst_t *insts, const usb_info_t *usb_dev, FILE *save_file) {
static size_t fill_device_inst(SurviveContext *ctx, vive_device_inst_t *insts, const usb_info_t *usb_dev,
FILE *save_file) {
size_t rtn = 0;
while (usb_dev->vid != 0 && usb_dev->pid != 0) {
bool foundDevice = false;
for (vive_device_t *dev = devices; dev->vid != 0; dev++) {
if (usb_dev->vid == dev->vid && usb_dev->pid == dev->pid) {
foundDevice = true;
insts->device = dev;
insts->bus_id = usb_dev->bus_id;
insts->dev_id = usb_dev->dev_id;
@ -305,6 +316,10 @@ static size_t fill_device_inst(vive_device_inst_t *insts, const usb_info_t *usb_
}
}
if (!foundDevice && usb_dev->vid == 0x28de) {
SV_WARN("Didn't find device instance for %04x:%04x", usb_dev->vid, usb_dev->pid);
}
usb_dev++;
}
@ -338,7 +353,7 @@ static int setup_usb_devices(SurviveDriverUSBMon *sp) {
usbInfo = get_usb_info_from_libusb();
}
sp->usb_devices_cnt = fill_device_inst(sp->usb_devices, usbInfo, listing_file);
sp->usb_devices_cnt = fill_device_inst(ctx, sp->usb_devices, usbInfo, listing_file);
if (listing_file) {
fclose(listing_file);
}
@ -382,15 +397,16 @@ void *pcap_thread_fn(void *_driver) {
case 1: {
// if (usbp = (usb_header_t *)pcap_next(driver->pcap, &pkthdr)) {
vive_device_inst_t *dev = find_device_inst(driver, usbp->bus_id, usbp->device_address);
// Packet data is directly after the packet header
uint8_t *pktData = (uint8_t *)&usbp[1];
if (driver->pcapDumper && (dev || driver->record_all)) {
pcap_dump((uint8_t *)driver->pcapDumper, pkthdr, (uint8_t *)usbp);
}
if (dev) {
driver->packet_cnt++;
const char *dev_name = dev->so ? dev->so->codename : "(unknown)";
// Packet data is directly after the packet header
uint8_t *pktData = (uint8_t *)&usbp[1];
if (driver->pcapDumper) {
pcap_dump((uint8_t *)driver->pcapDumper, pkthdr, (uint8_t *)usbp);
}
// Print setup flags, then just bail
if (!usbp->setup_flag) {
@ -479,9 +495,7 @@ void *pcap_thread_fn(void *_driver) {
memset(si.buffer, 0xCA, sizeof(si.buffer));
memcpy(si.buffer, pktData, usbp->data_len);
survive_get_ctx_lock(ctx);
survive_data_cb(&si);
survive_release_ctx_lock(ctx);
}
}
}
@ -516,7 +530,7 @@ static int gzip_cookie_close(void *cookie) { return gzclose((gzFile)cookie); }
static ssize_t gzip_cookie_read(void *cookie, char *buf, size_t nbytes) { return gzread((gzFile)cookie, buf, nbytes); }
int gzip_cookie_seek(void *cookie, _IO_off64_t *pos, int __w) { return gzseek((gzFile)cookie, *pos, __w); }
int gzip_cookie_seek(void *cookie, off64_t *pos, int __w) { return gzseek((gzFile)cookie, *pos, __w); }
cookie_io_functions_t gzip_cookie = {
.close = gzip_cookie_close, .write = gzip_cookie_write, .read = gzip_cookie_read, .seek = gzip_cookie_seek};
@ -536,7 +550,7 @@ int DriverRegUSBMon(SurviveContext *ctx) {
const char *usbmon_record = survive_configs(ctx, "usbmon-record", SC_GET, 0);
const char *usbmon_playback = survive_configs(ctx, "usbmon-playback", SC_GET, 0);
SurviveDriverUSBMon *sp = calloc(1, sizeof(SurviveDriverUSBMon));
SurviveDriverUSBMon *sp = SV_CALLOC(1, sizeof(SurviveDriverUSBMon));
sp->ctx = ctx;
sp->passiveMode = !enable && usbmon_record;
if (sp->passiveMode) {
@ -560,6 +574,11 @@ int DriverRegUSBMon(SurviveContext *ctx) {
FILE *fd = open_playback(usbmon_record, "w");
SV_INFO("Opening %s for usb recording (%p)", usbmon_record, fd);
sp->pcapDumper = pcap_dump_fopen(sp->pcap, fd);
sp->record_all = survive_configi(ctx, "usbmon-record-all", SC_GET, 0);
if (sp->record_all) {
SV_WARN("All USB traffic is being captured. Don't use 'usbmon-record-all' if you don't want to expose "
"things like input from keyboards.");
}
}
int device_count = setup_usb_devices(sp);

70
src/driver_vive.c
View file

@ -12,6 +12,8 @@
#include <errno.h>
#include <jsmn.h>
#include <os_generic.h>
#include <stddef.h>
#include <stdint.h>
#include <stdio.h>
#include <stdlib.h>
#include <string.h>
@ -345,7 +347,7 @@ int libusb_control_transfer_async(libusb_device_handle *dev_handle, uint8_t bmRe
if (!transfer)
return LIBUSB_ERROR_NO_MEM;
buffer = malloc(LIBUSB_CONTROL_SETUP_SIZE + wLength);
buffer = SV_MALLOC(LIBUSB_CONTROL_SETUP_SIZE + wLength);
if (!buffer) {
libusb_free_transfer(transfer);
return LIBUSB_ERROR_NO_MEM;
@ -438,7 +440,7 @@ static int survive_get_ids(survive_usb_device_t d, uint16_t *idVendor, uint16_t
static const char *survive_usb_error_name(int ret) { return ""; }
static int survive_open_usb_device(SurviveViveData *sv, survive_usb_device_t d, struct SurviveUSBInfo *usbInfo) {
usbInfo->handle = calloc(1, sizeof(struct HIDAPI_USB_Handle_t));
usbInfo->handle = SV_CALLOC(1, sizeof(struct HIDAPI_USB_Handle_t));
survive_usb_device_t c = d;
struct SurviveContext *ctx = sv->ctx;
@ -840,9 +842,12 @@ int survive_vive_usb_poll(SurviveContext *ctx, void *v) {
return 0;
#endif
#else
int r = libusb_handle_events(sv->usbctx);
// int r = libusb_handle_events(sv->usbctx);
struct timeval tv = {.tv_usec = 10 * 1000};
survive_release_ctx_lock(ctx);
int r = libusb_handle_events_timeout(sv->usbctx, &tv);
survive_get_ctx_lock(ctx);
if (r) {
SurviveContext *ctx = sv->ctx;
SV_ERROR(SURVIVE_ERROR_HARWARE_FAULT, "Libusb poll failed. %d (%s)", r, libusb_error_name(r));
}
#endif
@ -941,6 +946,12 @@ static int survive_get_config(char **config, SurviveViveData *sv, struct Survive
SV_INFO("Configuration length too long %s:%d (count: %d)", usbInfo->so->codename, iface, count);
return -4;
}
// Some (Tracker at least?) devices send a uint64_t before data; not sure what it means but skip it for now.
if (count == 0 && size >= 2 && cfgbuff[2] != 0x78) {
continue;
}
memcpy(&compressed_data[count], cfgbuff + 2, size);
count += size;
} while (1);
@ -958,7 +969,7 @@ static int survive_get_config(char **config, SurviveViveData *sv, struct Survive
return -5;
}
*config = malloc(len + 1);
*config = SV_MALLOC(len + 1);
memcpy(*config, uncompressed_data, len);
char fstname[128];
@ -1185,7 +1196,7 @@ struct sensorData {
uint8_t edgeCount;
};
static ssize_t read_light_data(SurviveObject *w, uint16_t time, uint8_t **readPtr, uint8_t *payloadEndPtr,
static int32_t read_light_data(SurviveObject *w, uint16_t time, uint8_t **readPtr, uint8_t *payloadEndPtr,
LightcapElement *output, int output_cnt) {
uint8_t *payloadPtr = *readPtr;
SurviveContext *ctx = w->ctx;
@ -1364,6 +1375,7 @@ static ssize_t read_light_data(SurviveObject *w, uint16_t time, uint8_t **readPt
timeDelta |= (*eventPtr & 0x7F);
if (((*(eventPtr--)) & 0x80) == 0x80)
break;
if (idsPtr + (timeIndex >> 1u) > eventPtr) {
eventPtr = eventPtrStart;
goto exit_while;
@ -1500,7 +1512,7 @@ static bool read_event(SurviveObject *w, uint16_t time, uint8_t **readPtr, uint8
SurviveContext *ctx = w->ctx;
// If we're looking at light data, return
if ((*payloadPtr & 0xE0) == 0)
if (!HAS_FLAG(*payloadPtr, 0xE0))
return true;
/*
@ -1979,7 +1991,32 @@ static void handle_watchman(SurviveObject *w, uint8_t *readdata) {
// Any remaining data after events (if any) have been read off is light data
if (payloadPtr < payloadEndPtr) {
parse_and_process_lightcap(w, time, payloadPtr, payloadEndPtr);
LightcapElement les[10] = {0};
int32_t cnt = read_light_data(w, time, &payloadPtr, payloadEndPtr, les, 10);
if (cnt < 0) {
SV_WARN("Read light data error %d [Time:%04hX] [Payload: %s]", (int)cnt, time,
packetToHex(payloadPtr, payloadEndPtr));
} else {
#ifdef VERIFY_LIGHTCAP
LightcapElement les_old[10] = {0};
int les_old_cnt = parse_watchman_lightcap(w->ctx, w->codename, time >> 8, w->activations.last_imu,
payloadPtr, payloadEndPtr - payloadPtr, les, 10);
assert(cnt == les_old_cnt);
#endif
for (int i = (int)cnt - 1; i >= 0; i--) {
#ifdef DEBUG_WATCHMAN
printf("%d: %u [%u]\n", les[i].sensor_id, les[i].length, les[i].timestamp);
#endif
#ifdef VERIFY_LIGHTCAP
assert(memcmp(&les[i], &les_old[i], sizeof(LightcapElement)) == 0);
#endif
handle_lightcap(w, &les[i]);
}
}
}
}
@ -2177,6 +2214,7 @@ static inline uint32_t read_buffer32(uint8_t *readdata, int idx) {
void survive_data_cb(SurviveUSBInterface *si) {
int size = si->actual_len;
SurviveContext *ctx = si->ctx;
survive_get_ctx_lock(ctx);
int iface = si->which_interface_am_i;
SurviveObject *obj = si->assoc_obj;
@ -2188,7 +2226,7 @@ void survive_data_cb(SurviveUSBInterface *si) {
}
if (iface == USB_IF_HMD_HEADSET_INFO && obj == 0)
return;
goto exit_fn;
int id = POP1;
// printf( "%16s Size: %2d ID: %d / %d\n", si->hname, size, id, iface );
@ -2299,6 +2337,9 @@ void survive_data_cb(SurviveUSBInterface *si) {
}
}
} else if (id == 39) { // LHv2
if (obj->ctx->lh_version == 0) {
goto exit_fn;
}
survive_notify_gen2(obj, "Report id 39");
// Implies that the user forced gen1
@ -2350,7 +2391,7 @@ void survive_data_cb(SurviveUSBInterface *si) {
uint32_t mask;
};
#pragma pack(pop)
uint32_t samples[4] = {}, masks[4] = {}, times[4] = {};
uint32_t samples[4] = {0}, masks[4] = {0}, times[4] = {0};
struct lh2_entry *entries = (struct lh2_entry *)readdata;
static uint32_t last_time = 0;
@ -2378,7 +2419,7 @@ void survive_data_cb(SurviveUSBInterface *si) {
fprintf(stderr, "\n");
}
uint32_t time_since_sync[4] = {};
uint32_t time_since_sync[4] = {0};
survive_channel chan = survive_decipher_channel(samples, masks, times, time_since_sync, 4);
fprintf(stderr, "Chan ootx: %d %d\n", chan / 2, chan & 1);
for (int i = 0; i < 4; i++) {
@ -2582,6 +2623,9 @@ void survive_data_cb(SurviveUSBInterface *si) {
int a = 0; // breakpoint here
}
}
exit_fn:
survive_release_ctx_lock(ctx);
}
///////////////////////////////////////////////////////////////////////////////
@ -2621,8 +2665,10 @@ int survive_vive_close(SurviveContext *ctx, void *driver) {
}
int DriverRegHTCVive(SurviveContext *ctx) {
SurviveViveData *sv = calloc(1, sizeof(SurviveViveData));
SurviveViveData *sv = SV_CALLOC(1, sizeof(SurviveViveData));
// Note: don't sleep for HTCVive, the handle_events call can block
ctx->poll_min_time_ms = 0;
survive_attach_configi(ctx, SECONDS_PER_HZ_OUTPUT_TAG, &sv->seconds_per_hz_output);
if(sv->seconds_per_hz_output > 0) {
SV_INFO("Reporting usb hz in %d second intervals", sv->seconds_per_hz_output);

2
src/driver_vive.h
View file

@ -28,7 +28,7 @@ enum USB_DEV_t {
#define MAX_INTERFACES_PER_DEVICE 8
enum USB_IF_t {
USB_IF_HMD_HEADSET_INFO = 0,
USB_IF_HMD_HEADSET_INFO = 1,
USB_IF_HMD_IMU,
USB_IF_WATCHMAN1,
USB_IF_WATCHMAN2,

13
src/epnp/epnp.c
View file

@ -29,6 +29,7 @@
#include "stdbool.h"
#include "stdio.h"
#include "stdlib.h"
#include <survive.h>
void print_mat(const CvMat *M) {
if (!M) {
@ -101,10 +102,10 @@ void epnp_set_maximum_number_of_correspondences(epnp *self, int n) {
free(self->object_pts_in_camera);
self->maximum_number_of_correspondences = n;
self->obj_pts = calloc(sizeof(double), 3 * self->maximum_number_of_correspondences);
self->meas = calloc(sizeof(double), 2 * self->maximum_number_of_correspondences);
self->alphas = calloc(sizeof(double), 4 * self->maximum_number_of_correspondences);
self->object_pts_in_camera = calloc(sizeof(double), 3 * self->maximum_number_of_correspondences);
self->obj_pts = SV_CALLOC(sizeof(double), 3 * self->maximum_number_of_correspondences);
self->meas = SV_CALLOC(sizeof(double), 2 * self->maximum_number_of_correspondences);
self->alphas = SV_CALLOC(sizeof(double), 4 * self->maximum_number_of_correspondences);
self->object_pts_in_camera = SV_CALLOC(sizeof(double), 3 * self->maximum_number_of_correspondences);
}
}
@ -321,8 +322,8 @@ void qr_solve(CvMat *A, CvMat *b, CvMat *X) {
}
if (max_nr < nr) {
max_nr = nr;
A1 = malloc(sizeof(double) * nr);
A2 = malloc(sizeof(double) * nr);
A1 = SV_MALLOC(sizeof(double) * nr);
A2 = SV_MALLOC(sizeof(double) * nr);
}
double *pA = A->data.db, *ppAkk = pA;

5
src/lfsr.c
View file

@ -2,6 +2,7 @@
#include <assert.h>
#include <malloc.h>
#include <stdbool.h>
#include <survive.h>
lfsr_state_t lsfr_iterate(lfsr_state_t state, lfsr_poly_t poly, uint32_t cnt) {
for (int i = 0; i < cnt; i++) {
@ -75,8 +76,8 @@ struct lfsr_lookup_t {
struct lfsr_lookup_t *lfsr_lookup_ctor(lfsr_poly_t p) {
uint32_t order = lfsr_order(p);
struct lfsr_lookup_t *lookup = malloc(sizeof(struct lfsr_lookup_t));
lookup->table = (uint32_t *)calloc(1 << order, sizeof(uint32_t));
struct lfsr_lookup_t *lookup = SV_MALLOC(sizeof(struct lfsr_lookup_t));
lookup->table = (uint32_t *)SV_CALLOC(1 << order, sizeof(uint32_t));
lookup->order = order;
uint32_t start = 1;
uint32_t state = start;

21
src/lfsr_lh2.c
View file

@ -1,5 +1,20 @@
#include "lfsr_lh2.h"
#ifndef _MSC_VER
#include "alloca.h"
#define clz(x) __builtin_clz(x)
#else
#include <intrin.h>
uint32_t inline clz(uint32_t value) {
uint32_t leading_zero = 0;
if (_BitScanReverse(&leading_zero, value)) {
return 31 - leading_zero;
} else {
// This is undefined, I better choose 32 than 0
return 32;
}
}
#endif
#include "stdio.h"
#include "string.h"
#include <malloc.h>
@ -45,7 +60,7 @@ lfsr_poly_t poly_pairs[32] = {
0x0001CB8D,
};
struct lfsr_lookup_t *poly_pair_lookups[32] = {};
struct lfsr_lookup_t *poly_pair_lookups[32] = {0};
static void init_lookups() {
if (poly_pair_lookups[0] == 0) {
for (int i = 0; i < 32; i++)
@ -96,7 +111,7 @@ survive_channel survive_decipher_channel(const uint32_t *sample, const uint32_t
uint32_t possible_polys = 0xFFFFFFFF;
uint32_t *timings = alloca(32 * sizeof(uint32_t) * count);
uint32_t *recon_samples = alloca(32 * sizeof(uint32_t) * count);
size_t known_solves[32] = {};
size_t known_solves[32] = {0};
memset(timings, 0, 32 * sizeof(uint32_t) * count);
memset(recon_samples, 0, 32 * sizeof(uint32_t) * count);
@ -163,7 +178,7 @@ survive_channel survive_decipher_channel(const uint32_t *sample, const uint32_t
}
if (popcnt(possible_polys) == 1) {
channel = 31 - __builtin_clz(possible_polys);
channel = 31 - clz(possible_polys);
}
if (channel != 255) {

4
src/lfsr_lh2.h
View file

@ -1,5 +1,5 @@
#include "lfsr.h"
#include "survive.h"
survive_channel survive_decipher_channel(const uint32_t *sample, const uint32_t *mask, const uint32_t *times,
uint32_t *output, size_t count);
SURVIVE_EXPORT survive_channel survive_decipher_channel(const uint32_t *sample, const uint32_t *mask,
const uint32_t *times, uint32_t *output, size_t count);

8
src/poser.c
View file

@ -128,9 +128,11 @@ void PoserData_lighthouse_pose_func(PoserData *poser_data, SurviveObject *so, ui
// Find the space with the same origin, but rotated so that gravity is up
SurvivePose lighthouse2objUp = {0}, object2objUp = {0};
if (quatmagnitude(so->activations.accel)) {
float accel_mag = quatmagnitude(so->activations.accel);
if (accel_mag != 0.0 && !isnan(accel_mag)) {
quatfrom2vectors(object2objUp.Rot, so->activations.accel, up);
} else {
SV_WARN("Calibration didn't have valid IMU data for %s; couldn't establish 'up' vector.", so->codename);
object2objUp.Rot[0] = 1.0;
}
@ -153,7 +155,7 @@ void PoserData_lighthouse_pose_func(PoserData *poser_data, SurviveObject *so, ui
sub3d(obj2world.Pos, obj2world.Pos, lighthouse2world.Pos);
lighthouse2world.Pos[0] = lighthouse2world.Pos[1] = lighthouse2world.Pos[2] = 0.0;
LinmathPoint3d camFwd = {0, 0, -1}, worldFwd = {};
LinmathPoint3d camFwd = {0, 0, -1}, worldFwd = {0};
ApplyPoseToPoint(worldFwd, &lighthouse2world, camFwd);
FLT ang = atan2(worldFwd[1], worldFwd[0]);
FLT euler[3] = {0, 0, M_PI / 2 - ang};
@ -201,7 +203,7 @@ void PoserData_lighthouse_poses_func(PoserData *poser_data, SurviveObject *so, S
bool worldEstablished = !quatiszero(object2World.Rot);
uint32_t lh_indices[NUM_GEN2_LIGHTHOUSES] = {};
uint32_t lh_indices[NUM_GEN2_LIGHTHOUSES] = {0};
uint32_t cnt = 0;
for (int lh = 0; lh < lighthouse_count; lh++) {
SurvivePose lh2object = lighthouse_pose[lh];

9
src/poser_barycentric_svd.c
View file

@ -1,6 +1,7 @@
#include "barycentric_svd/barycentric_svd.h"
#include "math.h"
#include "minimal_opencv.h"
#include <malloc.h>
#include <stdio.h>
#include <stdlib.h>
#include <survive.h>
@ -53,6 +54,10 @@ static void survive_fill_m(void *user, double *eq, int axis, FLT angle) {
break;
}
} break;
case 3: {
eq[0] = eq[1] = eq[2] = 0;
break;
}
default:
assert(false);
}
@ -66,7 +71,7 @@ static void PoserDataSVD_destroy(PoserDataSVD *dd) {
}
static PoserDataSVD *PoserDataSVD_new(SurviveObject *so) {
PoserDataSVD *rtn = calloc(sizeof(PoserDataSVD), 1);
PoserDataSVD *rtn = SV_CALLOC(sizeof(PoserDataSVD), 1);
rtn->so = so;
rtn->required_meas = survive_configi(so->ctx, "epnp-required-meas", SC_GET, 10);
@ -100,7 +105,7 @@ static SurvivePose solve_correspondence(PoserDataSVD *dd, bool cameraToWorld) {
// Super degenerate inputs will project us basically right in the camera. Detect and reject
if (err > 1 || magnitude3d(rtn.Pos) < 0.25 || magnitude3d(rtn.Pos) > 25) {
SV_WARN("pose is degenerate %d %f %f", (int)dd->bc.meas_cnt, err, magnitude3d(rtn.Pos));
// SV_WARN("pose is degenerate %d %f %f", (int)dd->bc.meas_cnt, err, magnitude3d(rtn.Pos));
return rtn;
}

2
src/poser_charlesrefine.c
View file

@ -129,7 +129,7 @@ int PoserCharlesRefine(SurviveObject *so, PoserData *pd) {
CharlesPoserData *dd = so->PoserData;
if (!dd)
{
so->PoserData = dd = calloc(sizeof(CharlesPoserData), 1);
so->PoserData = dd = SV_CALLOC(sizeof(CharlesPoserData), 1);
SurvivePose object_pose_out;
memcpy(&object_pose_out, &LinmathPose_Identity, sizeof(LinmathPose_Identity));
memcpy(&dd->InteralPoseUsedForCalc, &LinmathPose_Identity, sizeof(LinmathPose_Identity));

3
src/poser_charlesslow.c
View file

@ -24,7 +24,8 @@ int PoserCharlesSlow( SurviveObject * so, PoserData * pd )
SurviveContext * ctx = so->ctx;
DummyData * dd = so->PoserData;
if( !dd ) so->PoserData = dd = malloc( sizeof( DummyData ) );
if (!dd)
so->PoserData = dd = SV_MALLOC(sizeof(DummyData));
switch( pt )
{

2
src/poser_daveortho.c
View file

@ -34,7 +34,7 @@ int PoserDaveOrtho( SurviveObject * so, PoserData * pd )
DummyData * dd = so->PoserData;
if (!dd)
so->PoserData = dd = calloc(sizeof(DummyData), 1);
so->PoserData = dd = SV_CALLOC(sizeof(DummyData), 1);
switch( pt )
{

3
src/poser_dummy.c
View file

@ -14,7 +14,8 @@ int PoserDummy( SurviveObject * so, PoserData * pd )
SurviveContext * ctx = so->ctx;
DummyData * dd = so->PoserData;
if( !dd ) so->PoserData = dd = malloc( sizeof( DummyData ) );
if (!dd)
so->PoserData = dd = SV_MALLOC(sizeof(DummyData));
switch( pt )
{

1
src/poser_general_optimizer.c
View file

@ -149,6 +149,7 @@ bool general_optimizer_data_record_current_pose(GeneralOptimizerData *d, PoserDa
*soLocation = locations.pose;
}
d->failures_to_reset_cntr = d->failures_to_reset;
d->successes_to_reset_cntr = d->successes_to_reset;
} else if (seed_warning == false) {
seed_warning = true;

2
src/poser_imu.c
View file

@ -18,7 +18,7 @@ int PoserIMU(SurviveObject *so, PoserData *pd) {
struct PoserIMUData_t *dd = so->PoserData;
if (!dd) {
so->PoserData = dd = calloc(1, sizeof(struct PoserIMUData_t));
so->PoserData = dd = SV_CALLOC(1, sizeof(struct PoserIMUData_t));
survive_imu_tracker_init(&dd->tracker, so);
}

6
src/poser_mpfit.c
View file

@ -186,7 +186,7 @@ static void mpfit_set_cameras(SurviveObject *so, uint8_t lighthouse, SurvivePose
static inline void serialize_mpfit(MPFITData *d, survive_optimizer *mpfitctx) {
if (d->serialize_prefix) {
static int cnt = 0;
char path[1024] = {};
char path[1024] = {0};
snprintf(path, 1023, "%s_%s_%d.opt", d->serialize_prefix, d->opt.so->codename, cnt++);
survive_optimizer_serialize(mpfitctx, path);
}
@ -314,7 +314,7 @@ static double run_mpfit_find_3d_structure(MPFITData *d, PoserDataLight *pdl, Sur
*soLocation = (SurvivePose){ 0 };
SurvivePose *opt_cameras = survive_optimizer_get_camera(&mpfitctx);
SurvivePose cameras[NUM_GEN2_LIGHTHOUSES] = {};
SurvivePose cameras[NUM_GEN2_LIGHTHOUSES] = {0};
for (int i = 0; i < mpfitctx.cameraLength; i++) {
if (meas_for_lhs[i] > 0 && !quatiszero(opt_cameras[i].Rot)) {
cameras[i] = InvertPoseRtn(&opt_cameras[i]);
@ -433,7 +433,7 @@ static double run_mpfit_find_cameras(MPFITData *d, PoserDataFullScene *pdfs) {
int PoserMPFIT(SurviveObject *so, PoserData *pd) {
SurviveContext *ctx = so->ctx;
if (so->PoserData == 0) {
so->PoserData = calloc(1, sizeof(MPFITData));
so->PoserData = SV_CALLOC(1, sizeof(MPFITData));
MPFITData *d = so->PoserData;
general_optimizer_data_init(&d->opt, so);

6
src/poser_octavioradii.c
View file

@ -474,7 +474,7 @@ static void QuickPose(SurviveObject *so)
TrackedObject *to;
to = malloc(sizeof(TrackedObject) + (SENSORS_PER_OBJECT * sizeof(TrackedSensor)));
to = SV_MALLOC(sizeof(TrackedObject) + (SENSORS_PER_OBJECT * sizeof(TrackedSensor)));
{
int sensorCount = 0;
@ -544,7 +544,7 @@ int PoserOctavioRadii( SurviveObject * so, PoserData * pd )
if( !dd )
{
so->PoserData = dd = malloc( sizeof(OctavioRadiiData) );
so->PoserData = dd = SV_MALLOC(sizeof(OctavioRadiiData));
memset(dd, 0, sizeof(OctavioRadiiData));
}
@ -638,7 +638,7 @@ int PoserOctavioRadii( SurviveObject * so, PoserData * pd )
PoserDataFullScene * fs = (PoserDataFullScene*)pd;
to = malloc(sizeof(TrackedObject) + (SENSORS_PER_OBJECT * sizeof(TrackedSensor)));
to = SV_MALLOC(sizeof(TrackedObject) + (SENSORS_PER_OBJECT * sizeof(TrackedSensor)));
// FLT lengths[SENSORS_PER_OBJECT][NUM_GEN1_LIGHTHOUSES][2];
// FLT angles[SENSORS_PER_OBJECT][NUM_GEN1_LIGHTHOUSES][2]; //2 Axes (Angles in LH space)

2
src/poser_sba.c
View file

@ -345,7 +345,7 @@ static double run_sba(SBAData *d, PoserDataFullScene *pdfs, SurviveObject *so, i
int PoserSBA(SurviveObject *so, PoserData *pd) {
SurviveContext *ctx = so->ctx;
if (so->PoserData == 0) {
so->PoserData = calloc(1, sizeof(SBAData));
so->PoserData = SV_CALLOC(1, sizeof(SBAData));
SBAData *d = so->PoserData;
general_optimizer_data_init(&d->opt, so);

6
src/poser_turveytori.c
View file

@ -1568,7 +1568,7 @@ static void QuickPose(SurviveObject *so, PoserData *pd, SurvivePose *additionalT
TrackedObject *to;
to = malloc(sizeof(TrackedObject) + (SENSORS_PER_OBJECT * sizeof(TrackedSensor)));
to = SV_MALLOC(sizeof(TrackedObject) + (SENSORS_PER_OBJECT * sizeof(TrackedSensor)));
{
int sensorCount = 0;
@ -1666,7 +1666,7 @@ int PoserTurveyTori( SurviveObject * so, PoserData * poserData )
if (!td)
{
so->PoserData = td = malloc(sizeof(ToriData));
so->PoserData = td = SV_MALLOC(sizeof(ToriData));
memset(td, 0, sizeof(ToriData));
}
@ -1745,7 +1745,7 @@ int PoserTurveyTori( SurviveObject * so, PoserData * poserData )
PoserDataFullScene * fs = (PoserDataFullScene*)poserData;
to = malloc(sizeof(TrackedObject) + (SENSORS_PER_OBJECT * sizeof(TrackedSensor)));
to = SV_MALLOC(sizeof(TrackedObject) + (SENSORS_PER_OBJECT * sizeof(TrackedSensor)));
// if we rotate the internal reference frame of of the tracked object from having -z being arbitrary
// to being the down direction as defined by the accelerometer, then when we have come up

37
src/survive.c
View file

@ -240,11 +240,11 @@ SurviveContext *survive_init_internal(int argc, char *const *argv, void *userDat
}
#endif
SurviveContext *ctx = calloc(1, sizeof(SurviveContext));
SurviveContext *ctx = SV_CALLOC(1, sizeof(SurviveContext));
ctx->user_ptr = userData;
ctx->poll_min_time_ms = 10;
struct SurviveContext_private *pctx = ctx->private_members = calloc(1, sizeof(struct SurviveContext_private));
struct SurviveContext_private *pctx = ctx->private_members = SV_CALLOC(1, sizeof(struct SurviveContext_private));
pctx->poll_sema = OGCreateSema();
@ -260,9 +260,9 @@ SurviveContext *survive_init_internal(int argc, char *const *argv, void *userDat
survive_install_log_fn(ctx, log_func);
ctx->global_config_values = malloc(sizeof(config_group));
ctx->temporary_config_values = malloc(sizeof(config_group));
ctx->lh_config = malloc(sizeof(config_group) * NUM_GEN2_LIGHTHOUSES);
ctx->global_config_values = SV_MALLOC(sizeof(config_group));
ctx->temporary_config_values = SV_MALLOC(sizeof(config_group));
ctx->lh_config = SV_MALLOC(sizeof(config_group) * NUM_GEN2_LIGHTHOUSES);
// initdata
init_config_group(ctx->global_config_values, 30, ctx);
@ -594,7 +594,7 @@ void survive_default_new_object_process(SurviveObject *so) {}
int survive_add_object(SurviveContext *ctx, SurviveObject *obj) {
SV_INFO("Adding tracked object %s from %s", obj->codename, obj->drivername);
int oldct = ctx->objs_ct;
ctx->objs = realloc(ctx->objs, sizeof(SurviveObject *) * (oldct + 1));
ctx->objs = SV_REALLOC(ctx->objs, sizeof(SurviveObject *) * (oldct + 1));
ctx->objs[oldct] = obj;
ctx->objs_ct = oldct + 1;
@ -634,10 +634,10 @@ void survive_remove_object(SurviveContext *ctx, SurviveObject *obj) {
void survive_add_driver(SurviveContext *ctx, void *payload, DeviceDriverCb poll, DeviceDriverCb close,
DeviceDriverMagicCb magic) {
int oldct = ctx->driver_ct;
ctx->drivers = realloc(ctx->drivers, sizeof(void *) * (oldct + 1));
ctx->driverpolls = realloc(ctx->driverpolls, sizeof(DeviceDriverCb *) * (oldct + 1));
ctx->drivercloses = realloc(ctx->drivercloses, sizeof(DeviceDriverCb *) * (oldct + 1));
ctx->drivermagics = realloc(ctx->drivermagics, sizeof(DeviceDriverMagicCb *) * (oldct + 1));
ctx->drivers = SV_REALLOC(ctx->drivers, sizeof(void *) * (oldct + 1));
ctx->driverpolls = SV_REALLOC(ctx->driverpolls, sizeof(DeviceDriverCb *) * (oldct + 1));
ctx->drivercloses = SV_REALLOC(ctx->drivercloses, sizeof(DeviceDriverCb *) * (oldct + 1));
ctx->drivermagics = SV_REALLOC(ctx->drivermagics, sizeof(DeviceDriverMagicCb *) * (oldct + 1));
ctx->drivers[oldct] = payload;
ctx->driverpolls[oldct] = poll;
ctx->drivercloses[oldct] = close;
@ -675,10 +675,6 @@ void survive_close(SurviveContext *ctx) {
OGJoinThread(ctx->buttonservicethread);
OGDeleteSema(ctx->buttonQueue.buttonservicesem);
struct SurviveContext_private *pctx = ctx->private_members;
OGDeleteSema(pctx->poll_sema);
free(pctx);
while ((DriverName = GetDriverNameMatching("DriverUnreg", r++))) {
DeviceDriver dd = GetDriver(DriverName);
SV_INFO("De-registering driver %s (%p)", DriverName, dd);
@ -715,6 +711,9 @@ void survive_close(SurviveContext *ctx) {
}
survive_destroy_recording(ctx);
struct SurviveContext_private *pctx = ctx->private_members;
OGDeleteSema(pctx->poll_sema);
free(pctx);
free(ctx->objs);
free(ctx->drivers);
@ -759,10 +758,12 @@ int survive_poll(struct SurviveContext *ctx) {
}
survive_release_ctx_lock(ctx);
uint64_t timeNow = OGGetAbsoluteTimeMS();
if ((timeStart + ctx->poll_min_time_ms) > timeNow) {
uint64_t sleepTime = (timeStart + ctx->poll_min_time_ms) - timeNow;
OGUSleep(sleepTime * 1000);
if (ctx->poll_min_time_ms > 0) {
uint64_t timeNow = OGGetAbsoluteTimeMS();
if ((timeStart + ctx->poll_min_time_ms) > timeNow) {
uint64_t sleepTime = (timeStart + ctx->poll_min_time_ms) - timeNow;
OGUSleep(sleepTime * 1000);
}
}
survive_get_ctx_lock(ctx);

8
src/survive_api.c
View file

@ -101,7 +101,7 @@ static SurviveSimpleObject *find_or_create_external(SurviveSimpleContext *actx,
}
}
SurviveSimpleObject *so = calloc(1, sizeof(struct SurviveSimpleObject));
SurviveSimpleObject *so = SV_CALLOC(1, sizeof(struct SurviveSimpleObject));
so->type = SurviveSimpleObject_EXTERNAL;
so->actx = actx;
strncpy(so->name, name, 32);
@ -189,7 +189,7 @@ static void simple_log_fn(SurviveContext *ctx, SurviveLogLevel logLevel, const c
static void new_object_fn(SurviveObject *so) {
SurviveSimpleContext *actx = so->ctx->user_ptr;
SurviveSimpleObject *obj = calloc(1, sizeof(struct SurviveSimpleObject));
SurviveSimpleObject *obj = SV_CALLOC(1, sizeof(struct SurviveSimpleObject));
obj->data.so = so;
obj->type = SurviveSimpleObject_OBJECT;
obj->actx = actx;
@ -201,7 +201,7 @@ static void new_object_fn(SurviveObject *so) {
SURVIVE_EXPORT SurviveSimpleContext *survive_simple_init_with_logger(int argc, char *const *argv,
SurviveSimpleLogFn fn) {
SurviveSimpleContext *actx = calloc(1, sizeof(SurviveSimpleContext));
SurviveSimpleContext *actx = SV_CALLOC(1, sizeof(SurviveSimpleContext));
actx->log_fn = fn;
SurviveContext *ctx = survive_init_with_logger(argc, argv, actx, simple_log_fn);
@ -219,7 +219,7 @@ SURVIVE_EXPORT SurviveSimpleContext *survive_simple_init_with_logger(int argc, c
intptr_t i = 0;
for (i = 0; i < ctx->activeLighthouses; i++) {
SurviveSimpleObject *obj = calloc(1, sizeof(struct SurviveSimpleObject));
SurviveSimpleObject *obj = SV_CALLOC(1, sizeof(struct SurviveSimpleObject));
obj->data.lh.lighthouse = i;
obj->type = SurviveSimpleObject_LIGHTHOUSE;
obj->actx = actx;

2
src/survive_cal.c
View file

@ -134,7 +134,7 @@ void survive_cal_install( struct SurviveContext * ctx )
return;
int i;
struct SurviveCalData * cd = ctx->calptr = calloc( 1, sizeof( struct SurviveCalData ) );
struct SurviveCalData *cd = ctx->calptr = SV_CALLOC(1, sizeof(struct SurviveCalData));
if( ctx->state != SURVIVE_RUNNING )
{

12
src/survive_config.c
View file

@ -40,7 +40,7 @@ static struct static_conf_t *find_or_create_conf_t(const char *name) {
curr = curr->next;
}
curr = calloc(1, sizeof(struct static_conf_t));
curr = SV_CALLOC(1, sizeof(struct static_conf_t));
if (tail)
tail->next = curr;
if (head == 0)
@ -229,7 +229,7 @@ void init_config_group(config_group *cg, uint8_t count, SurviveContext * ctx) {
if (count == 0)
return;
cg->config_entries = malloc(count * sizeof(config_entry));
cg->config_entries = SV_MALLOC(count * sizeof(config_entry));
for (i = 0; i < count; ++i) {
init_config_entry(cg->config_entries + i);
@ -252,7 +252,7 @@ void resize_config_group(config_group *cg, uint16_t count) {
uint16_t i = 0;
if (count > cg->max_entries) {
config_entry *ptr = realloc(cg->config_entries, sizeof(config_entry) * count);
config_entry *ptr = SV_REALLOC(cg->config_entries, sizeof(config_entry) * count);
assert(ptr != NULL);
cg->config_entries = ptr;
@ -363,7 +363,7 @@ void sstrcpy(char **dest, const char *src) {
uint32_t len = (uint32_t)strlen(src) + 1;
assert(dest != NULL);
char *ptr = (char *)realloc(*dest, len); // acts like malloc if dest==NULL
char *ptr = (char *)SV_REALLOC(*dest, len); // acts like SV_MALLOC if dest==NULL
assert(ptr != NULL);
*dest = ptr;
@ -502,7 +502,7 @@ const FLT *config_set_float_a(config_group *cg, const char *tag, const FLT *valu
sstrcpy(&(cv->tag), tag);
char *ptr = (char *)realloc(cv->data, sizeof(FLT) * count);
char *ptr = (char *)SV_REALLOC(cv->data, sizeof(FLT) * count);
assert(ptr != NULL);
cv->data = ptr;
@ -878,7 +878,7 @@ static void survive_attach_config(SurviveContext *ctx, const char *tag, void * v
ul = &((*ul)->next);
}
update_list_t *t = *ul = malloc(sizeof(update_list_t));
update_list_t *t = *ul = SV_MALLOC(sizeof(update_list_t));
t->next = 0;
t->value = var;
}

6
src/survive_default_devices.c
View file

@ -12,7 +12,7 @@
SurviveObject *survive_create_device(SurviveContext *ctx, const char *driver_name, void *driver,
const char *device_name, haptic_func fn) {
SurviveObject *device = calloc(1, sizeof(SurviveObject));
SurviveObject *device = SV_CALLOC(1, sizeof(SurviveObject));
device->ctx = ctx;
device->driver = driver;
@ -76,7 +76,7 @@ static int ParsePoints(SurviveContext *ctx, SurviveObject *so, char *ct0conf, FL
so->sensor_ct = 0;
assert(*floats_out == 0);
*floats_out = malloc(sizeof(**floats_out) * 32 * 3);
*floats_out = SV_MALLOC(sizeof(**floats_out) * 32 * 3);
for (k = 0; k < pts; k++) {
tk = &t[2 + k * 4];
@ -209,7 +209,7 @@ static int process_jsonarray(scratch_space_t *scratch, char *ct0conf, stack_entr
int *values = NULL;
if (parse_int_array(ct0conf, tk + 2, &values, count)) {
int max_port = 32;
so->channel_map = malloc(sizeof(int) * max_port);
so->channel_map = SV_MALLOC(sizeof(int) * max_port);
for (int i = 0; i < max_port; i++)
so->channel_map[i] = -1;

26
src/survive_imu.c
View file

@ -126,14 +126,14 @@ void survive_imu_tracker_integrate_imu(SurviveIMUTracker *tracker, PoserDataIMU
// Wait til observation is in before reading IMU; gets rid of bad IMU data at the start
if (tracker->last_data.datamask == 0) {
tracker->imu_kalman_update = data->hdr.timecode;
tracker->obs_kalman_update = data->hdr.timecode;
tracker->last_kalman_update = tracker->obs_kalman_update = data->hdr.timecode;
return;
}
if (tracker->last_data.datamask == 1) {
tracker->last_data = *data;
tracker->imu_kalman_update = data->hdr.timecode;
tracker->obs_kalman_update = data->hdr.timecode;
tracker->last_kalman_update = tracker->obs_kalman_update = data->hdr.timecode;
return;
}
@ -144,12 +144,18 @@ void survive_imu_tracker_integrate_imu(SurviveIMUTracker *tracker, PoserDataIMU
// SV_INFO("%7f %7f", n, tracker->acc_bias);
FLT time_diff =
survive_timecode_difference(data->hdr.timecode, tracker->imu_kalman_update) / (FLT)tracker->so->timebase_hz;
survive_timecode_difference(data->hdr.timecode, tracker->last_kalman_update) / (FLT)tracker->so->timebase_hz;
FLT time_since_obs =
survive_timecode_difference(data->hdr.timecode, tracker->obs_kalman_update) / (FLT)tracker->so->timebase_hz;
// printf("i%u %f\n", data->timecode, time_diff);
LinmathQuat rot;
survive_kalman_predict_state(0, &tracker->rot, 0, rot);
assert(time_diff >= 0);
if (time_since_obs > .05) {
return;
}
if (time_diff > 0.5) {
SV_WARN("%s is probably dropping IMU packets; %f time reported between %u %u", tracker->so->codename, time_diff,
data->hdr.timecode, tracker->imu_kalman_update);
@ -184,14 +190,14 @@ void survive_imu_tracker_integrate_imu(SurviveIMUTracker *tracker, PoserDataIMU
survive_kalman_predict_update_state_extended(time_diff, &tracker->rot, rot_vel, Hr, update_rotation_from_rotvel,
Rv[1]);
tracker->imu_kalman_update = tracker->obs_kalman_update = data->hdr.timecode;
tracker->imu_kalman_update = tracker->last_kalman_update = data->hdr.timecode;
}
void survive_imu_tracker_predict(const SurviveIMUTracker *tracker, survive_timecode timecode, SurvivePose *out) {
if (tracker->position.info.P[0] > 100 || tracker->rot.info.P[0] > 100)
return;
FLT t = survive_timecode_difference(timecode, tracker->obs_kalman_update) / (FLT)tracker->so->timebase_hz;
FLT t = survive_timecode_difference(timecode, tracker->last_kalman_update) / (FLT)tracker->so->timebase_hz;
survive_kalman_predict_state(t, &tracker->position, 0, out->Pos);
@ -242,10 +248,10 @@ void survive_imu_tracker_integrate_observation(uint32_t timecode, SurviveIMUTrac
if (tracker->last_data.datamask == 0) {
tracker->last_data.datamask = 1;
tracker->imu_kalman_update = timecode;
tracker->obs_kalman_update = timecode;
tracker->last_kalman_update = tracker->obs_kalman_update = timecode;
}
FLT time_diff = survive_timecode_difference(timecode, tracker->obs_kalman_update) / (FLT)tracker->so->timebase_hz;
FLT time_diff = survive_timecode_difference(timecode, tracker->last_kalman_update) / (FLT)tracker->so->timebase_hz;
// assert(time_diff >= 0 && time_diff < 10);
// FLT H[] = {1., time_diff, time_diff * time_diff / 2.};
@ -264,7 +270,7 @@ void survive_imu_tracker_integrate_observation(uint32_t timecode, SurviveIMUTrac
// findnearestaxisanglemag(tracker->rot.state, tracker->rot.state, 0);
tracker->imu_kalman_update = tracker->obs_kalman_update = timecode;
tracker->last_kalman_update = tracker->obs_kalman_update = timecode;
}
STATIC_CONFIG_ITEM(POSE_POSITION_VARIANCE_SEC, "filter-pose-var-per-sec", 'f', "Position variance per second", 0.001);
@ -343,12 +349,12 @@ SurviveVelocity survive_imu_velocity(const SurviveIMUTracker *tracker) {
void survive_imu_tracker_integrate_velocity(SurviveIMUTracker *tracker, survive_timecode timecode, const FLT *Rv,
const SurviveVelocity *vel) {
const FLT H[] = {0, 1, 0};
FLT time_diff = survive_timecode_difference(timecode, tracker->obs_kalman_update) / (FLT)tracker->so->timebase_hz;
FLT time_diff = survive_timecode_difference(timecode, tracker->last_kalman_update) / (FLT)tracker->so->timebase_hz;
survive_kalman_predict_update_state(time_diff, &tracker->position, vel->Pos, H, Rv[0]);
survive_kalman_predict_update_state(time_diff, &tracker->rot, vel->AxisAngleRot, H, Rv[1]);
tracker->imu_kalman_update = tracker->obs_kalman_update = timecode;
tracker->last_kalman_update = tracker->obs_kalman_update = timecode;
}
void survive_imu_tracker_free(SurviveIMUTracker *tracker) {

1
src/survive_imu.h
View file

@ -65,6 +65,7 @@ typedef struct SurviveIMUTracker {
FLT mahony_variance;
survive_timecode last_kalman_update;
survive_timecode imu_kalman_update;
survive_timecode obs_kalman_update;
survive_kalman_state_t position;

4
src/survive_kalman.c
View file

@ -32,7 +32,7 @@ void survive_kalman_init(survive_kalman_t *k, size_t state_cnt, F_fn_t F, const
if (!k->P) {
k->P_is_heap = true;
k->P = calloc(1, sizeof(FLT) * state_cnt * state_cnt);
k->P = SV_CALLOC(1, sizeof(FLT) * state_cnt * state_cnt);
}
k->P[0] = 1e10;
@ -60,7 +60,7 @@ void survive_kalman_state_init(survive_kalman_state_t *k, size_t state_cnt, F_fn
if (!k->state) {
k->State_is_heap = true;
k->state = calloc(1, sizeof(FLT) * k->max_dim_cnt * k->info.state_cnt);
k->state = SV_CALLOC(1, sizeof(FLT) * k->max_dim_cnt * k->info.state_cnt);
}
}

10
src/survive_optimizer.c
View file

@ -2,7 +2,10 @@
#include <math.h>
#include <survive_optimizer.h>
#include <survive_reproject.h>
#ifndef NOZLIB
#include <zlib.h>
#endif
#include "survive_optimizer.h"
@ -264,6 +267,13 @@ void survive_optimizer_set_reproject_model(survive_optimizer *optimizer,
optimizer->reprojectModel = reprojectModel;
}
#ifdef NOZLIB
#define gzFile FILE *
#define gzopen fopen
#define gzprintf fprintf
#define gzclose fclose
#endif
void survive_optimizer_serialize(survive_optimizer *opt, const char *fn) {
gzFile f = gzopen(fn, "wT");

124
src/survive_playback.c
View file

@ -1,5 +1,8 @@
// All MIT/x11 Licensed Code in this file may be relicensed freely under the GPL
// or LGPL licenses.
#ifndef _GNU_SOURCE
#define _GNU_SOURCE
#endif
#include <stdio.h>
#include <stdlib.h>
@ -8,7 +11,26 @@
#include <assert.h>
#include <errno.h>
#include <string.h>
#ifdef NOZLIB
#define gzFile FILE *
#define gzopen fopen
#define gzprintf fprintf
#define gzclose fclose
#define gzvprintf vfprintf
#define gzerror_dropin ferror
#define gzwrite write
#define gzeof feof
#define gzseek fseek
#define gzgetc fgetc
#else
#include <zlib.h>
int gzerror_dropin(gzFile f) {
int rtn;
gzerror(f, &rtn);
return rtn;
}
#endif
#include "survive_config.h"
#include "survive_default_devices.h"
@ -23,16 +45,14 @@ typedef long ssize_t;
ssize_t getdelim(char **lineptr, size_t *n, int delimiter, FILE *stream);
ssize_t getline(char **lineptr, size_t *n, FILE *stream);
#define RESTRICT_KEYWORD
#else
#define RESTRICT_KEYWORD restrict
#endif
ssize_t gzgetdelim(char **restrict lineptr, size_t *restrict n, int delimiter, gzFile restrict stream);
ssize_t gzgetline(char **restrict lineptr, size_t *restrict n, gzFile restrict stream);
int gzerror_dropin(gzFile f) {
int rtn;
gzerror(f, &rtn);
return rtn;
}
ssize_t gzgetdelim(char **RESTRICT_KEYWORD lineptr, size_t *RESTRICT_KEYWORD n, int delimiter,
gzFile RESTRICT_KEYWORD stream);
ssize_t gzgetline(char **RESTRICT_KEYWORD lineptr, size_t *RESTRICT_KEYWORD n, gzFile RESTRICT_KEYWORD stream);
STATIC_CONFIG_ITEM(PLAYBACK_REPLAY_POSE, "playback-replay-pose", 'i', "Whether or not to output pose", 0);
STATIC_CONFIG_ITEM( RECORD, "record", 's', "File to record to if you wish to make a recording.", "" );
@ -53,11 +73,11 @@ typedef struct SurviveRecordingData {
gzFile output_file;
} SurviveRecordingData;
static double timestamp_in_us() {
static double start_time_us = 0;
if (start_time_us == 0.)
start_time_us = OGGetAbsoluteTime();
return OGGetAbsoluteTime() - start_time_us;
static double timestamp_in_s() {
static double start_time_s = 0;
if (start_time_s == 0.)
start_time_s = OGGetAbsoluteTime();
return OGGetAbsoluteTime() - start_time_s;
}
static void write_to_output_raw(SurviveRecordingData *recordingData, const char *string, int len) {
@ -75,7 +95,7 @@ static void write_to_output(SurviveRecordingData *recordingData, const char *for
return;
}
double ts = timestamp_in_us();
double ts = timestamp_in_s();
if (recordingData->output_file) {
va_list args;
@ -99,7 +119,7 @@ void survive_recording_config_process(SurviveObject *so, char *ct0conf, int len)
if (recordingData == 0)
return;
char *buffer = calloc(1, len + 1);
char *buffer = SV_CALLOC(1, len + 1);
memcpy(buffer, ct0conf, len);
for (int i = 0; i < len; i++)
if (buffer[i] == '\n')
@ -313,10 +333,14 @@ struct SurvivePlaybackData {
gzFile playback_file;
int lineno;
double next_time_us;
double next_time_s;
FLT playback_factor;
bool hasRawLight;
bool outputExternalPose;
uint32_t total_sleep_time;
bool keepRunning;
og_thread_t playback_thread;
};
typedef struct SurvivePlaybackData SurvivePlaybackData;
@ -541,7 +565,7 @@ static int parse_and_run_lightcode(const char *line, SurvivePlaybackData *driver
return 0;
}
static int playback_poll(struct SurviveContext *ctx, void *_driver) {
static int playback_pump_msg(struct SurviveContext *ctx, void *_driver) {
SurvivePlaybackData *driver = _driver;
gzFile f = driver->playback_file;
@ -549,14 +573,14 @@ static int playback_poll(struct SurviveContext *ctx, void *_driver) {
driver->lineno++;
char *line = 0;
if (driver->next_time_us == 0) {
if (driver->next_time_s == 0) {
size_t n = 0;
ssize_t r = gzgetdelim(&line, &n, ' ', f);
if (r <= 0) {
return 0;
}
if (sscanf(line, "%lf", &driver->next_time_us) != 1) {
if (sscanf(line, "%lf", &driver->next_time_s) != 1) {
free(line);
return 0;
}
@ -564,9 +588,9 @@ static int playback_poll(struct SurviveContext *ctx, void *_driver) {
line = 0;
}
if (driver->next_time_us * driver->playback_factor > timestamp_in_us())
if (driver->next_time_s * driver->playback_factor > timestamp_in_s())
return 0;
driver->next_time_us = 0;
driver->next_time_s = 0;
size_t n = 0;
ssize_t r = gzgetline(&line, &n, f);
@ -584,6 +608,7 @@ static int playback_poll(struct SurviveContext *ctx, void *_driver) {
return 0;
}
survive_get_ctx_lock(ctx);
switch (op[0]) {
case 'W':
if (op[1] == 0)
@ -629,6 +654,7 @@ static int playback_poll(struct SurviveContext *ctx, void *_driver) {
default:
SV_WARN("Playback doesn't understand '%s' op in '%s'", op, line);
}
survive_release_ctx_lock(ctx);
free(line);
} else {
@ -642,8 +668,41 @@ static int playback_poll(struct SurviveContext *ctx, void *_driver) {
return 0;
}
static void *playback_thread(void *_driver) {
SurvivePlaybackData *driver = _driver;
driver->keepRunning = true;
while (driver->keepRunning) {
double next_time_s_scaled = driver->next_time_s * driver->playback_factor;
double time_now = timestamp_in_s();
if (next_time_s_scaled == 0 || next_time_s_scaled < time_now) {
int rtnVal = playback_pump_msg(driver->ctx, driver);
if (rtnVal < 0)
driver->keepRunning = false;
} else {
int sleep_time_ms = 1 + (next_time_s_scaled - time_now) * 1000.;
int sr = OGUSleep(sleep_time_ms * 1000);
if (sr == 0)
driver->total_sleep_time += sleep_time_ms;
}
}
return 0;
}
static int playback_poll(struct SurviveContext *ctx, void *_driver) {
SurvivePlaybackData *driver = _driver;
if (driver->keepRunning == false)
return -1;
return 0;
}
static int playback_close(struct SurviveContext *ctx, void *_driver) {
SurvivePlaybackData *driver = _driver;
driver->keepRunning = false;
SV_VERBOSE(100, "Waiting on playback thread...");
survive_release_ctx_lock(ctx);
OGJoinThread(driver->playback_thread);
survive_get_ctx_lock(ctx);
SV_VERBOSE(100, "Playback thread slept for %ums", driver->total_sleep_time);
if (driver->playback_file)
gzclose(driver->playback_file);
driver->playback_file = 0;
@ -667,7 +726,7 @@ void survive_install_recording(SurviveContext *ctx) {
int record_to_stdout = survive_configi(ctx, "record-stdout", SC_GET, 0);
if (strlen(dataout_file) > 0 || record_to_stdout) {
ctx->recptr = calloc(1, sizeof(struct SurviveRecordingData));
ctx->recptr = SV_CALLOC(1, sizeof(struct SurviveRecordingData));
if (strlen(dataout_file) > 0) {
bool useCompression = strncmp(dataout_file + strlen(dataout_file) - 3, ".gz", 3) == 0;
@ -702,7 +761,7 @@ int DriverRegPlayback(SurviveContext *ctx) {
return -1;
}
SurvivePlaybackData *sp = calloc(1, sizeof(SurvivePlaybackData));
SurvivePlaybackData *sp = SV_CALLOC(1, sizeof(SurvivePlaybackData));
sp->ctx = ctx;
sp->playback_dir = playback_file;
@ -733,6 +792,12 @@ int DriverRegPlayback(SurviveContext *ctx) {
continue;
}
if (line[0] == 0x1f) {
SV_ERROR(SURVIVE_ERROR_INVALID_CONFIG, "Attempting to playback a gz compressed file without gz support.");
free(line);
return -1;
}
char dev[32];
char command[32];
@ -759,7 +824,7 @@ int DriverRegPlayback(SurviveContext *ctx) {
SurviveObject *so = survive_create_device(ctx, "replay", sp, dev, 0);
char *config = calloc(1, len + 1);
char *config = SV_CALLOC(1, len + 1);
memcpy(config, configStart, len);
if (ctx->configproc(so, config, len) == 0) {
@ -776,6 +841,8 @@ int DriverRegPlayback(SurviveContext *ctx) {
gzseek(sp->playback_file, 0, SEEK_SET); // same as rewind(f);
sp->playback_thread = OGCreateThread(playback_thread, sp);
OGNameThread(sp->playback_thread, "playback");
survive_add_driver(ctx, sp, playback_poll, playback_close, 0);
return 0;
}
@ -785,7 +852,8 @@ REGISTER_LINKTIME(DriverRegPlayback);
#define _GETDELIM_GROWBY 128 /* amount to grow line buffer by */
#define _GETDELIM_MINLEN 4 /* minimum line buffer size */
ssize_t gzgetdelim(char **restrict lineptr, size_t *restrict n, int delimiter, gzFile restrict stream) {
ssize_t gzgetdelim(char **RESTRICT_KEYWORD lineptr, size_t *RESTRICT_KEYWORD n, int delimiter,
gzFile RESTRICT_KEYWORD stream) {
char *buf, *pos;
int c;
ssize_t bytes;
@ -802,7 +870,7 @@ ssize_t gzgetdelim(char **restrict lineptr, size_t *restrict n, int delimiter, g
/* resize (or allocate) the line buffer if necessary */
buf = *lineptr;
if (buf == NULL || *n < _GETDELIM_MINLEN) {
buf = realloc(*lineptr, _GETDELIM_GROWBY);
buf = SV_REALLOC(*lineptr, _GETDELIM_GROWBY);
if (buf == NULL) {
/* ENOMEM */
return -1;
@ -822,7 +890,7 @@ ssize_t gzgetdelim(char **restrict lineptr, size_t *restrict n, int delimiter, g
}
bytes++;
if (bytes >= *n - 1) {
buf = realloc(*lineptr, *n + _GETDELIM_GROWBY);
buf = SV_REALLOC(*lineptr, *n + _GETDELIM_GROWBY);
if (buf == NULL) {
/* ENOMEM */
return -1;
@ -847,6 +915,6 @@ ssize_t gzgetdelim(char **restrict lineptr, size_t *restrict n, int delimiter, g
return bytes;
}
ssize_t gzgetline(char **restrict lineptr, size_t *restrict n, gzFile restrict stream) {
ssize_t gzgetline(char **RESTRICT_KEYWORD lineptr, size_t *RESTRICT_KEYWORD n, gzFile RESTRICT_KEYWORD stream) {
return gzgetdelim(lineptr, n, '\n', stream);
}

6
src/survive_plugins.c
View file

@ -6,6 +6,7 @@
#include <stdio.h>
#include <stdlib.h>
#include <string.h>
#include <survive.h>
#include "assert.h"
@ -50,8 +51,9 @@ typedef struct list_t {
} list_t;
static void list_add(list_t *list, const char *item) {
list->data = realloc(list->data, sizeof(item) * ++list->size);
list->data[list->size - 1] = malloc(strlen(item) + 1);
list->size++;
list->data = SV_REALLOC(list->data, sizeof(item) * list->size);
list->data[list->size - 1] = SV_MALLOC(strlen(item) + 1);
strcpy(list->data[list->size - 1], item);
}

2
src/survive_process.c
View file

@ -221,7 +221,7 @@ static void calibrate_gyro(SurviveObject *so, FLT *agm) {
}
void survive_default_raw_imu_process(SurviveObject *so, int mask, FLT *accelgyromag, uint32_t timecode, int id) {
FLT agm[9] = {};
FLT agm[9] = {0};
memcpy(agm, accelgyromag, sizeof(FLT) * 9);
calibrate_acc(so, agm);
calibrate_gyro(so, agm + 3);

2
src/survive_process_gen2.c
View file

@ -102,7 +102,7 @@ void survive_ootx_behavior(SurviveObject *so, int8_t bsd_idx, int8_t lh_version,
} else {
SV_INFO("OOTX not set for LH %d; attaching ootx decoder using device %s", bsd_idx, so->codename);
}
decoderContext = ctx->bsd[bsd_idx].ootx_data = calloc(1, sizeof(ootx_decoder_context));
decoderContext = ctx->bsd[bsd_idx].ootx_data = SV_CALLOC(1, sizeof(ootx_decoder_context));
ootx_init_decoder_context(decoderContext);
decoderContext->user1 = bsd_idx;
decoderContext->user = so;

1
src/survive_reproject_gen2.c
View file

@ -1,3 +1,4 @@
#define _USE_MATH_DEFINES
#include <assert.h>
#include <math.h>
#include <survive_reproject.h>

12
src/test_cases/CMakeLists.txt
View file

@ -1,4 +1,3 @@
add_executable(survive_tests
main.c
reproject.c
@ -15,9 +14,14 @@ add_executable(test_replays test_replays.c)
set_target_properties(test_replays PROPERTIES FOLDER "tests")
target_link_libraries(test_replays survive)
add_test(NAME lh1_test_cal COMMAND ./test_replays ${CMAKE_CURRENT_BINARY_DIR}/libsurvive-extras-data/tests/lh1_test_cal.rec.gz)
add_test(NAME lh2_test_cal COMMAND ./test_replays ${CMAKE_CURRENT_BINARY_DIR}/libsurvive-extras-data/tests/lh2_test_cal.rec.gz)
add_test(NAME lh2_test_cal_usb COMMAND ./test_replays ${CMAKE_CURRENT_BINARY_DIR}/libsurvive-extras-data/tests/lh2_test_cal.pcap.gz)
add_test(NAME lh1_test_cal COMMAND $<TARGET_FILE:test_replays> ${CMAKE_CURRENT_BINARY_DIR}/libsurvive-extras-data/tests/lh1_test_cal.rec.gz)
add_test(NAME lh2_test_cal COMMAND $<TARGET_FILE:test_replays> ${CMAKE_CURRENT_BINARY_DIR}/libsurvive-extras-data/tests/lh2_test_cal.rec.gz)
if(PCAP_LIBRARY)
add_test(NAME lh2_test_cal_usb COMMAND $<TARGET_FILE:test_replays> ${CMAKE_CURRENT_BINARY_DIR}/libsurvive-extras-data/tests/lh2_test_cal.pcap.gz)
add_test(NAME wm1_wand_test_cal_usb COMMAND $<TARGET_FILE:test_replays> ${CMAKE_CURRENT_BINARY_DIR}/libsurvive-extras-data/tests/WM1-wand.pcap.gz)
add_test(NAME wireless_tracker_test_cal_usb COMMAND $<TARGET_FILE:test_replays> ${CMAKE_CURRENT_BINARY_DIR}/libsurvive-extras-data/tests/wireless-tracker.pcap.gz)
endif()
include(ExternalProject)

2
src/test_cases/rotate_angvel.c
View file

@ -65,7 +65,7 @@ TEST(AngularVelocity, find) {
TEST(AngularVelocity, apply) {
LinmathQuat a = {1, 0, 0, 0};
SurviveAngularVelocity b = {M_PI, 0, 0};
SurviveAngularVelocity b = {LINMATHPI, 0, 0};
LinmathQuat c;
survive_apply_ang_velocity(c, b, 1, a);

34
src/test_cases/test_replays.c
View file

@ -3,23 +3,24 @@
#define SURVIVE_ENABLE_FULL_API
#include <complex.h>
#include <malloc.h>
#include <math.h>
#include <os_generic.h>
#include <survive.h>
#include <survive_api.h>
static double complex diff(const SurvivePose *a, const SurvivePose *b) {
static void diff(double *out, const SurvivePose *a, const SurvivePose *b) {
SurvivePose iB = InvertPoseRtn(b);
SurvivePose nearId;
ApplyPoseToPose(&nearId, a, &iB);
return (1 - fabs(nearId.Rot[0])) * I + norm3d(nearId.Pos);
out[0] = (1 - fabs(nearId.Rot[0]));
out[1] = norm3d(nearId.Pos);
}
static int test_path(const char *name, int main_argc, char **main_argv) {
int rtn = 0;
double max_pos_error = .005, max_rot_error = .001;
char configPath[FILENAME_MAX] = {};
double max_pos_error = .01, max_rot_error = .001;
char configPath[FILENAME_MAX] = {0};
sprintf(configPath, "%s.json", name);
char *playbackFlag = strstr(name, "pcap") ? "--usbmon-playback" : "--playback";
@ -42,7 +43,7 @@ static int test_path(const char *name, int main_argc, char **main_argv) {
ctx->bsd[0].PositionSet = false;
ctx->bsd[1].PositionSet = false;
SurvivePose originalLH[NUM_GEN2_LIGHTHOUSES] = {};
SurvivePose originalLH[NUM_GEN2_LIGHTHOUSES] = {0};
for (int i = 0; i < ctx->activeLighthouses; i++) {
SurvivePose pose = ctx->bsd[i].Pose;
@ -76,14 +77,14 @@ static int test_path(const char *name, int main_argc, char **main_argv) {
if (strcmp(name2, name + strlen("replay_")) == 0) {
SurvivePose pose2;
survive_simple_object_get_latest_pose(it2, &pose2);
double complex err = diff(&pose, &pose2);
double err[2] = {0};
diff(err, &pose, &pose2);
printf(" %s: " SurvivePose_format " %f\t%f\n", survive_simple_object_name(it2), pose2.Pos[0],
pose2.Pos[1], pose2.Pos[2], pose2.Rot[0], pose2.Rot[1], pose2.Rot[2], pose2.Rot[3],
crealf(err), cimagf(err));
if (crealf(err) > max_pos_error || cimagf(err) > max_rot_error) {
pose2.Pos[1], pose2.Pos[2], pose2.Rot[0], pose2.Rot[1], pose2.Rot[2], pose2.Rot[3], err[0],
err[1]);
if (err[1] > max_pos_error || err[0] > max_rot_error) {
fprintf(stderr, "TEST FAILED, %s deviates too much -- %f %f\n", survive_simple_object_name(it2),
crealf(err), cimagf(err));
err[0], err[1]);
rtn = -1;
}
}
@ -94,12 +95,13 @@ static int test_path(const char *name, int main_argc, char **main_argv) {
for (int i = 0; i < ctx->activeLighthouses; i++) {
SurvivePose pose = originalLH[i];
printf(SurvivePose_format "\n", SURVIVE_POSE_EXPAND(ctx->bsd[i].Pose));
double complex err = diff(&pose, &ctx->bsd[i].Pose);
double err[2] = {0};
diff(err, &pose, &ctx->bsd[i].Pose);
printf(SurvivePose_format " %f %f\n", pose.Pos[0], pose.Pos[1], pose.Pos[2], pose.Rot[0], pose.Rot[1],
pose.Rot[2], pose.Rot[3], crealf(err), cimagf(err));
pose.Rot[2], pose.Rot[3], err[0], err[1]);
if (crealf(err) > max_pos_error || cimagf(err) > max_rot_error) {
fprintf(stderr, "TEST FAILED, LH%d deviates too much -- %f %f\n", i, crealf(err), cimagf(err));
if (err[1] > max_pos_error || err[0] > max_rot_error) {
fprintf(stderr, "TEST FAILED, LH%d deviates too much -- %f %f\n", i, err[0], err[1]);
rtn = -1;
}
}

2
useful_files/git-hooks/pre-push
View file

@ -4,5 +4,5 @@ ROOT=`git rev-parse --show-toplevel`
cd $ROOT
mkdir -p bin
cd bin
cmake ..
cmake -DENABLE_TESTS=ON ..
make run_all_tests