From d3921d52e89235c5837664b87335eeabded60d8c Mon Sep 17 00:00:00 2001 From: Justin Berger Date: Mon, 9 Sep 2019 15:48:04 -0600 Subject: [PATCH 01/20] Fixs for windows --- api_example.c | 2 +- redist/CMakeLists.txt | 2 +- redist/lintest.c | 2 +- redist/minimal_opencvtest.c | 10 ++++----- redist/os_generic.h | 1 + sensors-readout.c | 6 +++--- src/barycentric_svd/barycentric_svd.c | 1 + src/driver_vive.c | 4 ++-- src/lfsr_lh2.c | 25 +++++++++++++++++++--- src/lfsr_lh2.h | 2 +- src/poser.c | 4 ++-- src/poser_barycentric_svd.c | 1 + src/poser_mpfit.c | 4 ++-- src/survive_optimizer.c | 10 +++++++++ src/survive_playback.c | 26 +++++++++++++++++++---- src/survive_process.c | 2 +- src/survive_reproject_gen2.c | 1 + src/test_cases/rotate_angvel.c | 2 +- src/test_cases/test_replays.c | 30 ++++++++++++++------------- 19 files changed, 94 insertions(+), 41 deletions(-) diff --git a/api_example.c b/api_example.c index d91a61a..b2b541c 100644 --- a/api_example.c +++ b/api_example.c @@ -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) { diff --git a/redist/CMakeLists.txt b/redist/CMakeLists.txt index 28b4d2a..6821321 100644 --- a/redist/CMakeLists.txt +++ b/redist/CMakeLists.txt @@ -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() diff --git a/redist/lintest.c b/redist/lintest.c index c2b82ee..510a127 100644 --- a/redist/lintest.c +++ b/redist/lintest.c @@ -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}}; diff --git a/redist/minimal_opencvtest.c b/redist/minimal_opencvtest.c index b6c22bc..3d1a213 100644 --- a/redist/minimal_opencvtest.c +++ b/redist/minimal_opencvtest.c @@ -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); diff --git a/redist/os_generic.h b/redist/os_generic.h index 76efb16..7b2da8c 100644 --- a/redist/os_generic.h +++ b/redist/os_generic.h @@ -75,6 +75,7 @@ extern "C" { #ifdef USE_WINDOWS #include +#include OSG_INLINE void OGSleep(int is) { Sleep(is * 1000); } diff --git a/sensors-readout.c b/sensors-readout.c index 54949d2..d778e8e 100644 --- a/sensors-readout.c +++ b/sensors-readout.c @@ -2,13 +2,13 @@ static volatile int keepRunning = 1; +#include "math.h" +#include +#include #ifdef __linux__ -#include "math.h" #include -#include #include -#include void intHandler(int dummy) { if (keepRunning == 0) diff --git a/src/barycentric_svd/barycentric_svd.c b/src/barycentric_svd/barycentric_svd.c index 01df880..db52cea 100644 --- a/src/barycentric_svd/barycentric_svd.c +++ b/src/barycentric_svd/barycentric_svd.c @@ -3,6 +3,7 @@ #include "stdbool.h" #include "stdio.h" #include "stdlib.h" +#include static void bc_svd_choose_control_points(bc_svd *self) { // Take C0 as the reference points centroid: diff --git a/src/driver_vive.c b/src/driver_vive.c index 44696d2..b729af7 100755 --- a/src/driver_vive.c +++ b/src/driver_vive.c @@ -2065,7 +2065,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; @@ -2093,7 +2093,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++) { diff --git a/src/lfsr_lh2.c b/src/lfsr_lh2.c index 7456290..7a880e2 100644 --- a/src/lfsr_lh2.c +++ b/src/lfsr_lh2.c @@ -1,5 +1,24 @@ #include "lfsr_lh2.h" +#ifndef _MSC_VER #include "alloca.h" +#define clz(x) __builtin_clz(x) +#else +#include +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 @@ -45,7 +64,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 +115,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 +182,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) { diff --git a/src/lfsr_lh2.h b/src/lfsr_lh2.h index 7ab89a5..172663b 100644 --- a/src/lfsr_lh2.h +++ b/src/lfsr_lh2.h @@ -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, +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); \ No newline at end of file diff --git a/src/poser.c b/src/poser.c index 6c42898..1b76d85 100644 --- a/src/poser.c +++ b/src/poser.c @@ -153,7 +153,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 +201,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]; diff --git a/src/poser_barycentric_svd.c b/src/poser_barycentric_svd.c index 3c8df6a..00b5433 100644 --- a/src/poser_barycentric_svd.c +++ b/src/poser_barycentric_svd.c @@ -5,6 +5,7 @@ #include #include #include +#include typedef struct { SurviveObject *so; diff --git a/src/poser_mpfit.c b/src/poser_mpfit.c index 7224ae7..a523c62 100644 --- a/src/poser_mpfit.c +++ b/src/poser_mpfit.c @@ -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]); diff --git a/src/survive_optimizer.c b/src/survive_optimizer.c index 82db7b8..90fe4d6 100644 --- a/src/survive_optimizer.c +++ b/src/survive_optimizer.c @@ -2,7 +2,10 @@ #include #include #include + +#ifndef NOZLIB #include +#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"); diff --git a/src/survive_playback.c b/src/survive_playback.c index d0d80e5..a992c97 100644 --- a/src/survive_playback.c +++ b/src/survive_playback.c @@ -8,7 +8,22 @@ #include #include #include + + +#ifdef NOZLIB +#define gzFile FILE* +#define gzopen fopen +#define gzprintf fprintf +#define gzclose fclose +#define gzvprintf vfprintf +#define gzerror ferror +#define gzwrite write +#define gzeof feof +#define gzseek fseek +#define gzgetc fgetc +#else #include +#endif #include "survive_config.h" #include "survive_default_devices.h" @@ -23,10 +38,13 @@ 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); +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); int gzerror_dropin(gzFile f) { int rtn; @@ -776,7 +794,7 @@ 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; @@ -838,6 +856,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); } diff --git a/src/survive_process.c b/src/survive_process.c index edd158b..fd24de3 100644 --- a/src/survive_process.c +++ b/src/survive_process.c @@ -219,7 +219,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); diff --git a/src/survive_reproject_gen2.c b/src/survive_reproject_gen2.c index d5b5bb6..6af751e 100644 --- a/src/survive_reproject_gen2.c +++ b/src/survive_reproject_gen2.c @@ -1,3 +1,4 @@ +#define _USE_MATH_DEFINES #include #include #include diff --git a/src/test_cases/rotate_angvel.c b/src/test_cases/rotate_angvel.c index aaccef0..9e37474 100644 --- a/src/test_cases/rotate_angvel.c +++ b/src/test_cases/rotate_angvel.c @@ -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); diff --git a/src/test_cases/test_replays.c b/src/test_cases/test_replays.c index a30c321..8d28d8b 100644 --- a/src/test_cases/test_replays.c +++ b/src/test_cases/test_replays.c @@ -2,24 +2,25 @@ #include #define SURVIVE_ENABLE_FULL_API +#include #include #include #include #include #include -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] = {}; + 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) { + 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; } } From 7888c030a9e25c16f1fa4294c4ddd21dd2f3c2ff Mon Sep 17 00:00:00 2001 From: Justin Berger Date: Mon, 9 Sep 2019 16:35:11 -0600 Subject: [PATCH 02/20] Fixed playback for nonlibz setup --- src/survive_playback.c | 12 ++++++------ 1 file changed, 6 insertions(+), 6 deletions(-) diff --git a/src/survive_playback.c b/src/survive_playback.c index a992c97..75ffe5e 100644 --- a/src/survive_playback.c +++ b/src/survive_playback.c @@ -16,13 +16,18 @@ #define gzprintf fprintf #define gzclose fclose #define gzvprintf vfprintf -#define gzerror ferror +#define gzerror_dropin ferror #define gzwrite write #define gzeof feof #define gzseek fseek #define gzgetc fgetc #else #include +int gzerror_dropin(gzFile f) { + int rtn; + gzerror(f, &rtn); + return rtn; +} #endif #include "survive_config.h" @@ -46,11 +51,6 @@ ssize_t getline(char **lineptr, size_t *n, FILE *stream); 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); -int gzerror_dropin(gzFile f) { - int rtn; - gzerror(f, &rtn); - return rtn; -} 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.", "" ); From 0af263ad5f98d0655951c62ebdf791e53bbad437 Mon Sep 17 00:00:00 2001 From: Justin Berger Date: Sun, 25 Aug 2019 10:45:06 -0600 Subject: [PATCH 03/20] Made more resilient to unknown RF payloads --- include/libsurvive/survive.h | 4 ++- src/driver_vive.c | 56 +++++++++++++++++++----------------- src/survive_disambiguator.c | 2 +- src/survive_process_gen2.c | 5 ++-- 4 files changed, 37 insertions(+), 30 deletions(-) diff --git a/include/libsurvive/survive.h b/include/libsurvive/survive.h index bba4dbd..f979c13 100644 --- a/include/libsurvive/survive.h +++ b/include/libsurvive/survive.h @@ -478,12 +478,14 @@ SURVIVE_EXPORT void handle_lightcap(SurviveObject *so, const LightcapElement *le SV_LOG_NULL_GUARD ctx->logproc(ctx, SURVIVE_LOG_LEVEL_INFO, stbuff); \ } -static inline void survive_notify_gen2(struct SurviveObject *so) { +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; } if (so->ctx->lh_version != 1) { + struct SurviveContext *ctx = so->ctx; + SV_VERBOSE(100, "Gen2 reason: %s %s", so->codename, msg); so->ctx->gen_detectedproc(so, 1); } } diff --git a/src/driver_vive.c b/src/driver_vive.c index b729af7..6d368a5 100755 --- a/src/driver_vive.c +++ b/src/driver_vive.c @@ -210,7 +210,7 @@ static void handle_transfer(struct libusb_transfer *transfer) { SurviveUSBInterface *iface = transfer->user_data; if (iface->assoc_obj == 0) { SurviveContext *ctx = iface->ctx; - SV_INFO("Cleaning up transfer on %d %s", iface->which_interface_am_i, iface->hname); + SV_VERBOSE(100, "Cleaning up transfer on %d %s", iface->which_interface_am_i, iface->hname); iface->ctx = 0; return; } @@ -1169,15 +1169,15 @@ struct sensorData { uint8_t edgeCount; }; -static size_t read_light_data(SurviveObject *w, uint16_t time, uint8_t **readPtr, uint8_t *payloadEndPtr, - LightcapElement *output, int output_cnt) { +static ssize_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; uint32_t reference_time = w->activations.last_imu; // DEBUG if ((*payloadPtr & 0xE0) == 0xE0) { - SV_INFO("Warn : Light contains probable non-light data : 0x%02hX [Time:%04hX] [Payload: %s]", *payloadPtr, time, + SV_WARN("Light contains probable non-light data : 0x%02hX [Time:%04hX] [Payload: %s]", *payloadPtr, time, packetToHex(payloadPtr, payloadEndPtr)); } @@ -1193,7 +1193,7 @@ static size_t read_light_data(SurviveObject *w, uint16_t time, uint8_t **readPtr * deltas between rising and falling of the sensor event. There are always two rising/falling events per * sensor though the ordering is not simple as new sensor events may start before others are finished. * - * The meaning and associated led with each 'event' is dermined by the edge count as encoded within the + * The meaning and associated led with each 'event' is determined by the edge count as encoded within the * sensor data (see below) * * The time deltas use variable length encoding, so we can't determine how many sensors are in the packet @@ -1329,7 +1329,7 @@ static size_t read_light_data(SurviveObject *w, uint16_t time, uint8_t **readPtr } times[0] = lastEventTime; - SV_VERBOSE(100, "Packet Start Time: %u", lastEventTime); + SV_VERBOSE(200, "Packet Start Time: %u", lastEventTime); while (idsPtr < eventPtr) { // There are two time deltas per 'event' @@ -1348,7 +1348,6 @@ static size_t read_light_data(SurviveObject *w, uint16_t time, uint8_t **readPtr if (((*(eventPtr--)) & 0x80) == 0x80) break; if (idsPtr > eventPtr) { - SV_WARN("Light data parse error 1"); return -1; } } @@ -1356,7 +1355,7 @@ static size_t read_light_data(SurviveObject *w, uint16_t time, uint8_t **readPtr // Store the event time times[++timeIndex] = lastEventTime; - SV_VERBOSE(100, "Time: [%zd] %u (%u)", timeIndex, lastEventTime, timeDelta); + SV_VERBOSE(200, "Time: [%zd] %u (%u)", timeIndex, lastEventTime, timeDelta); } // Step 2 - Convert events to pulses @@ -1371,23 +1370,22 @@ static size_t read_light_data(SurviveObject *w, uint16_t time, uint8_t **readPtr // Get the end time (Increment and find the next 'unused' time) while (times[++timeIndex] == 0) if (timeIndex + 1 >= maxTimeIndex) { - SV_WARN("Light data parse error 2"); return -2; } if (timeIndex >= maxTimeIndex) { - SV_WARN("Light data parse error 3"); return -3; } // Get the start time size_t startTimeIndex = timeIndex + (sensors[i].edgeCount + 1); if (startTimeIndex >= maxTimeIndex) { - SV_WARN("Light data parse error 4"); return -4; } // Store the start index so we can return in ascending time order - assert(reportOrder[startTimeIndex] == 0); + if (reportOrder[startTimeIndex] != 0) { + return -5; + } reportOrder[startTimeIndex] = i + 1; LightcapElement *le = &les[i]; @@ -1414,8 +1412,8 @@ static size_t read_light_data(SurviveObject *w, uint16_t time, uint8_t **readPtr *(output++) = *ol; output_cnt--; - // SV_INFO("Light Event [Ordered]: %i [%i] %li -> %li (%li)", i, ol->sensor_id, ol->timestamp, ol->timestamp - // + ol->length, ol->length); + SV_VERBOSE(500, "Light Event [Ordered]: %i [%2i] %u -> %u (%4hu)", i, ol->sensor_id, ol->timestamp, + ol->timestamp + ol->length, ol->length); } } return eventCount; @@ -1681,25 +1679,31 @@ 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) { LightcapElement les[10] = {0}; - size_t cnt = read_light_data(w, time, &payloadPtr, payloadEndPtr, les, 10); + ssize_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); + 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); + assert(cnt == les_old_cnt); #endif - for (int i = (int)cnt - 1; i >= 0; i--) { + 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); + 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); + assert(memcmp(&les[i], &les_old[i], sizeof(LightcapElement)) == 0); #endif - handle_lightcap(w, &les[i]); + handle_lightcap(w, &les[i]); + } } - } } @@ -2014,7 +2018,7 @@ void survive_data_cb(SurviveUSBInterface *si) { } } } else if (id == 39) { // LHv2 - survive_notify_gen2(obj); + survive_notify_gen2(obj, "Report id 39"); // Implies that the user forced gen1 if (obj->ctx->lh_version != 1) { @@ -2107,7 +2111,7 @@ void survive_data_cb(SurviveUSBInterface *si) { fprintf(stderr, "\n"); } } else if (id == 40) { - survive_notify_gen2(obj); + survive_notify_gen2(obj, "Report ID 40"); uint8_t *packet = readdata + 1; uint8_t length = readdata[0]; diff --git a/src/survive_disambiguator.c b/src/survive_disambiguator.c index 2bf33ca..57fe7f1 100644 --- a/src/survive_disambiguator.c +++ b/src/survive_disambiguator.c @@ -28,7 +28,7 @@ void handle_lightcap(SurviveObject *so, const LightcapElement *_le) { // without these gen2 packets we can just call it for gen1. if (so->ctx->lh_version == -1) { if (_le->length >= 0x8000) { - survive_notify_gen2(so); + survive_notify_gen2(so, "Lightcap length >= 0x8000"); } else if (_le->length >= 3000 && _le->length < 6500) { // Only look for the OOTX pulses; otherwise we get false hits and can potentially choose gen1 // on a gen2 system diff --git a/src/survive_process_gen2.c b/src/survive_process_gen2.c index c3b1c22..f71645c 100644 --- a/src/survive_process_gen2.c +++ b/src/survive_process_gen2.c @@ -154,6 +154,9 @@ SURVIVE_EXPORT void survive_default_sync_process(SurviveObject *so, survive_chan SURVIVE_EXPORT void survive_default_sweep_process(SurviveObject *so, survive_channel channel, int sensor_id, survive_timecode timecode, bool half_clock_flag) { struct SurviveContext *ctx = so->ctx; + + survive_notify_gen2(so, "sweep called"); + int8_t bsd_idx = survive_get_bsd_idx(ctx, channel); if (ctx->calptr) { @@ -195,8 +198,6 @@ SURVIVE_EXPORT void survive_default_sweep_process(SurviveObject *so, survive_cha SURVIVE_EXPORT void survive_default_sweep_angle_process(SurviveObject *so, survive_channel channel, int sensor_id, survive_timecode timecode, int8_t plane, FLT angle) { - survive_notify_gen2(so); - struct SurviveContext *ctx = so->ctx; // SV_INFO("Sensor ch%2d %2d %12f", channel, sensor_id, angle / M_PI * 180.); int8_t bsd_idx = survive_get_bsd_idx(ctx, channel); From f07bb30a4dedc1b1ab21f0c313775e94ded8bd93 Mon Sep 17 00:00:00 2001 From: Justin Berger Date: Mon, 9 Sep 2019 18:54:19 -0600 Subject: [PATCH 04/20] Put playback on its own thread --- redist/os_generic.h | 13 +++++--- src/survive.c | 8 ++--- src/survive_playback.c | 72 ++++++++++++++++++++++++++++++++---------- 3 files changed, 68 insertions(+), 25 deletions(-) diff --git a/redist/os_generic.h b/redist/os_generic.h index 7b2da8c..c79b79f 100644 --- a/redist/os_generic.h +++ b/redist/os_generic.h @@ -120,6 +120,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); } @@ -203,10 +205,6 @@ OSG_INLINE og_cv_t OGCreateConditionVariable() { #else -#ifndef _GNU_SOURCE -#define _GNU_SOURCE -#endif - #include #include #include @@ -220,7 +218,7 @@ 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 = {.tv_nsec = (ius % 1000000) * 1000, .tv_sec = ius / 1000000}; return nanosleep(&sleep, 0); } @@ -249,6 +247,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); diff --git a/src/survive.c b/src/survive.c index cc181c8..c08c84a 100644 --- a/src/survive.c +++ b/src/survive.c @@ -661,10 +661,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); @@ -700,6 +696,10 @@ void survive_close(SurviveContext *ctx) { survive_destroy_device(ctx->objs[i]); } + struct SurviveContext_private *pctx = ctx->private_members; + OGDeleteSema(pctx->poll_sema); + free(pctx); + free(ctx->objs); free(ctx->drivers); free(ctx->driverpolls); diff --git a/src/survive_playback.c b/src/survive_playback.c index 75ffe5e..1cf1c7d 100644 --- a/src/survive_playback.c +++ b/src/survive_playback.c @@ -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 #include @@ -9,7 +12,6 @@ #include #include - #ifdef NOZLIB #define gzFile FILE* #define gzopen fopen @@ -71,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) { @@ -93,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; @@ -331,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; @@ -559,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; @@ -567,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; } @@ -582,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); @@ -602,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) @@ -647,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 { @@ -660,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; @@ -728,9 +769,6 @@ int DriverRegPlayback(SurviveContext *ctx) { SV_INFO("Using playback file '%s' with timefactor of %f", playback_file, sp->playback_factor); - if (sp->playback_factor == 0.0) - ctx->poll_min_time_ms = 0; - FLT time; while (!gzeof(sp->playback_file) && !gzerror_dropin(sp->playback_file)) { char *line = 0; @@ -785,6 +823,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; } From 751e79cb373fddfee14d5b919ca412fadb478289 Mon Sep 17 00:00:00 2001 From: jdavidberger Date: Mon, 9 Sep 2019 18:58:07 -0600 Subject: [PATCH 05/20] Update README.md Fixed broke link in readme --- README.md | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/README.md b/README.md index 88489be..bfeaee3 100644 --- a/README.md +++ b/README.md @@ -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. From 2a7b197d331d1e7e5bbade8972c5acfbeec765b4 Mon Sep 17 00:00:00 2001 From: Justin Berger Date: Mon, 9 Sep 2019 21:43:26 -0600 Subject: [PATCH 06/20] Made IMU turn off after .05 seconds of no observation data --- src/disambiguator_statebased.c | 17 +++++++++++++++-- src/poser_barycentric_svd.c | 2 +- src/survive_imu.c | 27 +++++++++++++++++---------- src/survive_imu.h | 1 + 4 files changed, 34 insertions(+), 13 deletions(-) diff --git a/src/disambiguator_statebased.c b/src/disambiguator_statebased.c index 76baae5..e9cff62 100644 --- a/src/disambiguator_statebased.c +++ b/src/disambiguator_statebased.c @@ -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)); @@ -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) { @@ -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); diff --git a/src/poser_barycentric_svd.c b/src/poser_barycentric_svd.c index 00b5433..675625c 100644 --- a/src/poser_barycentric_svd.c +++ b/src/poser_barycentric_svd.c @@ -101,7 +101,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; } diff --git a/src/survive_imu.c b/src/survive_imu.c index 8a709e5..3c03911 100644 --- a/src/survive_imu.c +++ b/src/survive_imu.c @@ -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,7 +144,10 @@ 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); @@ -155,6 +158,10 @@ void survive_imu_tracker_integrate_imu(SurviveIMUTracker *tracker, PoserDataIMU data->hdr.timecode, tracker->imu_kalman_update); } + if (time_since_obs > .05) { + return; + } + if (tracker->mahony_variance >= 0) { LinmathQuat pose_rot; quatcopy(pose_rot, rot); @@ -184,14 +191,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 +249,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 +271,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 +350,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) { diff --git a/src/survive_imu.h b/src/survive_imu.h index 6ae7a27..edd571b 100644 --- a/src/survive_imu.h +++ b/src/survive_imu.h @@ -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; From 25572a8c62f750d7680d51f337aeeda30d7e9bfb Mon Sep 17 00:00:00 2001 From: Justin Berger Date: Mon, 9 Sep 2019 21:51:12 -0600 Subject: [PATCH 07/20] Slight relaxation on CI test threshold --- src/test_cases/test_replays.c | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/src/test_cases/test_replays.c b/src/test_cases/test_replays.c index 8d28d8b..ba98f97 100644 --- a/src/test_cases/test_replays.c +++ b/src/test_cases/test_replays.c @@ -19,7 +19,7 @@ static void diff(double* out, const SurvivePose *a, const SurvivePose *b) { 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; + double max_pos_error = .01, max_rot_error = .001; char configPath[FILENAME_MAX] = {0}; sprintf(configPath, "%s.json", name); From f6e4ccd4b60db0977703cbfcf8409f1b3b77b209 Mon Sep 17 00:00:00 2001 From: Justin Berger Date: Mon, 9 Sep 2019 22:12:43 -0600 Subject: [PATCH 08/20] Upped min light length needed to register --- src/disambiguator_statebased.c | 2 +- src/poser_general_optimizer.c | 1 + 2 files changed, 2 insertions(+), 1 deletion(-) diff --git a/src/disambiguator_statebased.c b/src/disambiguator_statebased.c index e9cff62..e499426 100644 --- a/src/disambiguator_statebased.c +++ b/src/disambiguator_statebased.c @@ -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++) { diff --git a/src/poser_general_optimizer.c b/src/poser_general_optimizer.c index 422d0d6..9785be7 100644 --- a/src/poser_general_optimizer.c +++ b/src/poser_general_optimizer.c @@ -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; From 8a351a794f78570bb24ad931fe84a077fb53adbc Mon Sep 17 00:00:00 2001 From: Justin Berger Date: Tue, 10 Sep 2019 13:05:17 -0600 Subject: [PATCH 09/20] Fixed os_generic to work in C++ context --- redist/os_generic.h | 18 +++++++++++++----- 1 file changed, 13 insertions(+), 5 deletions(-) diff --git a/redist/os_generic.h b/redist/os_generic.h index c79b79f..30edece 100644 --- a/redist/os_generic.h +++ b/redist/os_generic.h @@ -218,7 +218,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 % 1000000) * 1000, .tv_sec = ius / 1000000}; + struct timespec sleep = {0}; + sleep.tv_nsec = (ius % 1000000) * 1000; + sleep.tv_sec = ius / 1000000; return nanosleep(&sleep, 0); } @@ -342,12 +344,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); } From 97114c3f3f85da7620cf909e47eccd73d3faa602 Mon Sep 17 00:00:00 2001 From: Justin Berger Date: Tue, 10 Sep 2019 17:11:31 -0600 Subject: [PATCH 10/20] Apparently ID 39's can just show up; ignore them if LH version is already set --- src/driver_vive.c | 3 +++ src/poser_barycentric_svd.c | 4 ++++ src/survive_imu.c | 7 +++---- 3 files changed, 10 insertions(+), 4 deletions(-) diff --git a/src/driver_vive.c b/src/driver_vive.c index 6d368a5..bd88b05 100755 --- a/src/driver_vive.c +++ b/src/driver_vive.c @@ -2018,6 +2018,9 @@ void survive_data_cb(SurviveUSBInterface *si) { } } } else if (id == 39) { // LHv2 + if (obj->ctx->lh_version == 0) { + return; + } survive_notify_gen2(obj, "Report id 39"); // Implies that the user forced gen1 diff --git a/src/poser_barycentric_svd.c b/src/poser_barycentric_svd.c index 675625c..9318364 100644 --- a/src/poser_barycentric_svd.c +++ b/src/poser_barycentric_svd.c @@ -54,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); } diff --git a/src/survive_imu.c b/src/survive_imu.c index 3c03911..4edb469 100644 --- a/src/survive_imu.c +++ b/src/survive_imu.c @@ -153,15 +153,14 @@ void survive_imu_tracker_integrate_imu(SurviveIMUTracker *tracker, PoserDataIMU 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); } - if (time_since_obs > .05) { - return; - } - if (tracker->mahony_variance >= 0) { LinmathQuat pose_rot; quatcopy(pose_rot, rot); From 242388d080c2a7a5b67c7e1c61b8a0c05270bc74 Mon Sep 17 00:00:00 2001 From: Justin Berger Date: Fri, 20 Sep 2019 11:06:06 -0600 Subject: [PATCH 11/20] Protect test stuff behind cmake variable --- CMakeLists.txt | 13 ++++++++++--- redist/CMakeLists.txt | 17 ++++++++++------- redist/os_generic.h | 2 +- src/driver_vive.c | 6 ++++-- src/survive_playback.c | 6 ++++++ src/test_cases/CMakeLists.txt | 10 ++++++---- 6 files changed, 37 insertions(+), 17 deletions(-) diff --git a/CMakeLists.txt b/CMakeLists.txt index f8352ba..a453aeb 100644 --- a/CMakeLists.txt +++ b/CMakeLists.txt @@ -1,7 +1,6 @@ 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") ENDIF() @@ -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}/$) ENDIF() list (APPEND CMAKE_REQUIRED_DEFINITIONS -D_GNU_SOURCE) @@ -177,8 +183,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 diff --git a/redist/CMakeLists.txt b/redist/CMakeLists.txt index 6821321..e196adc 100644 --- a/redist/CMakeLists.txt +++ b/redist/CMakeLists.txt @@ -85,12 +85,15 @@ IF(USE_LAPACKE OR WIN32) # 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() \ No newline at end of file diff --git a/redist/os_generic.h b/redist/os_generic.h index 30edece..79981fe 100644 --- a/redist/os_generic.h +++ b/redist/os_generic.h @@ -79,7 +79,7 @@ extern "C" { 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; diff --git a/src/driver_vive.c b/src/driver_vive.c index bd88b05..3bad4a0 100755 --- a/src/driver_vive.c +++ b/src/driver_vive.c @@ -14,6 +14,8 @@ #include #include #include +#include +#include #include #include #include @@ -1169,7 +1171,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; @@ -1679,7 +1681,7 @@ 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) { LightcapElement les[10] = {0}; - ssize_t cnt = read_light_data(w, time, &payloadPtr, payloadEndPtr, les, 10); + 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, diff --git a/src/survive_playback.c b/src/survive_playback.c index 1cf1c7d..3560ecc 100644 --- a/src/survive_playback.c +++ b/src/survive_playback.c @@ -780,6 +780,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]; diff --git a/src/test_cases/CMakeLists.txt b/src/test_cases/CMakeLists.txt index d76f51c..3bb0d88 100644 --- a/src/test_cases/CMakeLists.txt +++ b/src/test_cases/CMakeLists.txt @@ -1,4 +1,3 @@ - add_executable(survive_tests main.c reproject.c @@ -15,9 +14,12 @@ 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 $ ${CMAKE_CURRENT_BINARY_DIR}/libsurvive-extras-data/tests/lh1_test_cal.rec.gz) +add_test(NAME lh2_test_cal COMMAND $ ${CMAKE_CURRENT_BINARY_DIR}/libsurvive-extras-data/tests/lh2_test_cal.rec.gz) + +if(PCAP_LIBRARY) + add_test(NAME lh2_test_cal_usb COMMAND $ ${CMAKE_CURRENT_BINARY_DIR}/libsurvive-extras-data/tests/lh2_test_cal.pcap.gz) +endif() include(ExternalProject) From 27f3d6bbba0710f2fa06f8660d4acae1b1d2d0f9 Mon Sep 17 00:00:00 2001 From: Justin Berger Date: Sat, 21 Sep 2019 14:55:16 -0600 Subject: [PATCH 12/20] Disabled pcap on windows --- CMakeLists.txt | 10 ++++++---- 1 file changed, 6 insertions(+), 4 deletions(-) diff --git a/CMakeLists.txt b/CMakeLists.txt index a453aeb..8873168 100644 --- a/CMakeLists.txt +++ b/CMakeLists.txt @@ -137,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) From 86b2c689534ae140848ac2217112bb56a1457150 Mon Sep 17 00:00:00 2001 From: Justin Berger Date: Thu, 17 Oct 2019 12:58:22 -0600 Subject: [PATCH 13/20] pre push test now enables tests --- useful_files/git-hooks/pre-push | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/useful_files/git-hooks/pre-push b/useful_files/git-hooks/pre-push index 1c352e2..47b6b89 100755 --- a/useful_files/git-hooks/pre-push +++ b/useful_files/git-hooks/pre-push @@ -4,5 +4,5 @@ ROOT=`git rev-parse --show-toplevel` cd $ROOT mkdir -p bin cd bin -cmake .. +cmake -DENABLE_TESTS .. make run_all_tests From 451bae65a45b85ec3b87821328e78b3f6959c9a9 Mon Sep 17 00:00:00 2001 From: Justin Berger Date: Thu, 17 Oct 2019 12:54:51 -0600 Subject: [PATCH 14/20] Added malloc/calloc/realloc check wrapper --- CMakeLists.txt | 2 +- include/libsurvive/survive.h | 13 +++++++++++++ src/barycentric_svd/barycentric_svd.c | 11 ++++++----- src/disambiguator_statebased.c | 4 ++-- src/disambiguator_turvey.c | 2 +- src/driver_dummy.c | 8 ++++---- src/driver_simulator.c | 4 ++-- src/driver_udp.c | 8 ++++---- src/driver_usbmon.c | 10 +++++----- src/driver_vive.c | 8 ++++---- src/epnp/epnp.c | 13 +++++++------ src/lfsr.c | 5 +++-- src/poser_barycentric_svd.c | 2 +- src/poser_charlesrefine.c | 2 +- src/poser_charlesslow.c | 3 ++- src/poser_daveortho.c | 2 +- src/poser_dummy.c | 3 ++- src/poser_imu.c | 2 +- src/poser_mpfit.c | 2 +- src/poser_octavioradii.c | 6 +++--- src/poser_sba.c | 2 +- src/poser_turveytori.c | 6 +++--- src/survive.c | 20 ++++++++++---------- src/survive_api.c | 8 ++++---- src/survive_cal.c | 2 +- src/survive_config.c | 12 ++++++------ src/survive_default_devices.c | 6 +++--- src/survive_kalman.c | 4 ++-- src/survive_playback.c | 12 ++++++------ src/survive_plugins.c | 6 ++++-- src/survive_process_gen2.c | 2 +- 31 files changed, 105 insertions(+), 85 deletions(-) diff --git a/CMakeLists.txt b/CMakeLists.txt index 8873168..476bb3f 100644 --- a/CMakeLists.txt +++ b/CMakeLists.txt @@ -2,7 +2,7 @@ project(libsurvive C) cmake_minimum_required(VERSION 3.5.1) 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) diff --git a/include/libsurvive/survive.h b/include/libsurvive/survive.h index f979c13..0f312d6 100644 --- a/include/libsurvive/survive.h +++ b/include/libsurvive/survive.h @@ -6,6 +6,7 @@ #include #include #include +#include #ifdef __cplusplus extern "C" { @@ -478,6 +479,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; diff --git a/src/barycentric_svd/barycentric_svd.c b/src/barycentric_svd/barycentric_svd.c index db52cea..48d982b 100644 --- a/src/barycentric_svd/barycentric_svd.c +++ b/src/barycentric_svd/barycentric_svd.c @@ -3,6 +3,7 @@ #include "stdbool.h" #include "stdio.h" #include "stdlib.h" +#include "survive.h" #include static void bc_svd_choose_control_points(bc_svd *self) { @@ -73,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); @@ -122,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}; @@ -269,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; diff --git a/src/disambiguator_statebased.c b/src/disambiguator_statebased.c index e499426..c937ec4 100644 --- a/src/disambiguator_statebased.c +++ b/src/disambiguator_statebased.c @@ -713,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; } diff --git a/src/disambiguator_turvey.c b/src/disambiguator_turvey.c index 65d225a..8e70f09 100644 --- a/src/disambiguator_turvey.c +++ b/src/disambiguator_turvey.c @@ -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)); } diff --git a/src/driver_dummy.c b/src/driver_dummy.c index a35c9bd..4ec1e70 100644 --- a/src/driver_dummy.c +++ b/src/driver_dummy.c @@ -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; diff --git a/src/driver_simulator.c b/src/driver_simulator.c index 8b1bcda..6666bd7 100644 --- a/src/driver_simulator.c +++ b/src/driver_simulator.c @@ -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; diff --git a/src/driver_udp.c b/src/driver_udp.c index 6f22931..1ccdcc5 100644 --- a/src/driver_udp.c +++ b/src/driver_udp.c @@ -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; diff --git a/src/driver_usbmon.c b/src/driver_usbmon.c index 04f9c29..4af7cbf 100644 --- a/src/driver_usbmon.c +++ b/src/driver_usbmon.c @@ -94,7 +94,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); @@ -185,7 +185,7 @@ static void ingest_config_request(vive_device_inst_t *dev, const struct _usb_hea 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, @@ -236,7 +236,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 +262,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]; @@ -527,7 +527,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) { diff --git a/src/driver_vive.c b/src/driver_vive.c index 3bad4a0..7591808 100755 --- a/src/driver_vive.c +++ b/src/driver_vive.c @@ -341,7 +341,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; @@ -434,7 +434,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; @@ -953,7 +953,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]; @@ -2343,7 +2343,7 @@ 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)); survive_attach_configi(ctx, SECONDS_PER_HZ_OUTPUT_TAG, &sv->seconds_per_hz_output); if(sv->seconds_per_hz_output > 0) { diff --git a/src/epnp/epnp.c b/src/epnp/epnp.c index f782bc8..429bc46 100644 --- a/src/epnp/epnp.c +++ b/src/epnp/epnp.c @@ -29,6 +29,7 @@ #include "stdbool.h" #include "stdio.h" #include "stdlib.h" +#include 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; diff --git a/src/lfsr.c b/src/lfsr.c index 7ff5fc6..c17804c 100644 --- a/src/lfsr.c +++ b/src/lfsr.c @@ -2,6 +2,7 @@ #include #include #include +#include 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; diff --git a/src/poser_barycentric_svd.c b/src/poser_barycentric_svd.c index 9318364..9ba7dba 100644 --- a/src/poser_barycentric_svd.c +++ b/src/poser_barycentric_svd.c @@ -71,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); diff --git a/src/poser_charlesrefine.c b/src/poser_charlesrefine.c index 2897da6..c400017 100644 --- a/src/poser_charlesrefine.c +++ b/src/poser_charlesrefine.c @@ -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)); diff --git a/src/poser_charlesslow.c b/src/poser_charlesslow.c index 5cac973..57c1cbb 100644 --- a/src/poser_charlesslow.c +++ b/src/poser_charlesslow.c @@ -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 ) { diff --git a/src/poser_daveortho.c b/src/poser_daveortho.c index 330e7e8..8d82aec 100644 --- a/src/poser_daveortho.c +++ b/src/poser_daveortho.c @@ -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 ) { diff --git a/src/poser_dummy.c b/src/poser_dummy.c index b5f1292..9727ab5 100644 --- a/src/poser_dummy.c +++ b/src/poser_dummy.c @@ -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 ) { diff --git a/src/poser_imu.c b/src/poser_imu.c index 455d47e..27fa092 100644 --- a/src/poser_imu.c +++ b/src/poser_imu.c @@ -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); } diff --git a/src/poser_mpfit.c b/src/poser_mpfit.c index a523c62..1638227 100644 --- a/src/poser_mpfit.c +++ b/src/poser_mpfit.c @@ -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); diff --git a/src/poser_octavioradii.c b/src/poser_octavioradii.c index f9bad84..1d11de8 100644 --- a/src/poser_octavioradii.c +++ b/src/poser_octavioradii.c @@ -473,7 +473,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; @@ -543,7 +543,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)); } @@ -640,7 +640,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_LIGHTHOUSES][2]; //FLT angles[SENSORS_PER_OBJECT][NUM_LIGHTHOUSES][2]; //2 Axes (Angles in LH space) diff --git a/src/poser_sba.c b/src/poser_sba.c index 884fb45..6407c0e 100644 --- a/src/poser_sba.c +++ b/src/poser_sba.c @@ -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); diff --git a/src/poser_turveytori.c b/src/poser_turveytori.c index 3791645..d35d4cb 100644 --- a/src/poser_turveytori.c +++ b/src/poser_turveytori.c @@ -1567,7 +1567,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; @@ -1665,7 +1665,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)); } @@ -1746,7 +1746,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 diff --git a/src/survive.c b/src/survive.c index c08c84a..c4607b2 100644 --- a/src/survive.c +++ b/src/survive.c @@ -226,11 +226,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(); @@ -246,9 +246,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); @@ -580,7 +580,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; @@ -620,10 +620,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; diff --git a/src/survive_api.c b/src/survive_api.c index a7001ef..7691f2f 100644 --- a/src/survive_api.c +++ b/src/survive_api.c @@ -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; diff --git a/src/survive_cal.c b/src/survive_cal.c index 888f4a1..2f9f836 100755 --- a/src/survive_cal.c +++ b/src/survive_cal.c @@ -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 ) { diff --git a/src/survive_config.c b/src/survive_config.c index c3040f1..3689404 100644 --- a/src/survive_config.c +++ b/src/survive_config.c @@ -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; } diff --git a/src/survive_default_devices.c b/src/survive_default_devices.c index 94b0814..efeccd0 100644 --- a/src/survive_default_devices.c +++ b/src/survive_default_devices.c @@ -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; diff --git a/src/survive_kalman.c b/src/survive_kalman.c index 49b6037..dec27cb 100644 --- a/src/survive_kalman.c +++ b/src/survive_kalman.c @@ -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); } } diff --git a/src/survive_playback.c b/src/survive_playback.c index 3560ecc..22123e4 100644 --- a/src/survive_playback.c +++ b/src/survive_playback.c @@ -119,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') @@ -718,7 +718,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; @@ -753,7 +753,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; @@ -812,7 +812,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) { @@ -857,7 +857,7 @@ ssize_t gzgetdelim(char **RESTRICT_KEYWORD lineptr, size_t *RESTRICT_KEYWORD n, /* 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; @@ -877,7 +877,7 @@ ssize_t gzgetdelim(char **RESTRICT_KEYWORD lineptr, size_t *RESTRICT_KEYWORD n, } bytes++; if (bytes >= *n - 1) { - buf = realloc(*lineptr, *n + _GETDELIM_GROWBY); + buf = SV_REALLOC(*lineptr, *n + _GETDELIM_GROWBY); if (buf == NULL) { /* ENOMEM */ return -1; diff --git a/src/survive_plugins.c b/src/survive_plugins.c index eeb45ee..4ed1691 100644 --- a/src/survive_plugins.c +++ b/src/survive_plugins.c @@ -6,6 +6,7 @@ #include #include #include +#include #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); } diff --git a/src/survive_process_gen2.c b/src/survive_process_gen2.c index f71645c..0ca2d2a 100644 --- a/src/survive_process_gen2.c +++ b/src/survive_process_gen2.c @@ -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; From f84bc0b529158144269e32305d9aec2b7162629e Mon Sep 17 00:00:00 2001 From: Christoph Haag Date: Thu, 31 Oct 2019 02:35:20 +0100 Subject: [PATCH 15/20] fix IO_off64_t datatype after glibc update --- src/driver_usbmon.c | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/src/driver_usbmon.c b/src/driver_usbmon.c index 4af7cbf..836ad54 100644 --- a/src/driver_usbmon.c +++ b/src/driver_usbmon.c @@ -507,7 +507,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}; From 1aac0155a36577ef348d7a14810096524d688022 Mon Sep 17 00:00:00 2001 From: Alibek Omarov Date: Mon, 11 Nov 2019 08:05:34 +0300 Subject: [PATCH 16/20] driver_vive: revert check introduced cffa6225, vive wands are working again --- src/driver_vive.c | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/src/driver_vive.c b/src/driver_vive.c index 7591808..4e90fd5 100755 --- a/src/driver_vive.c +++ b/src/driver_vive.c @@ -1450,7 +1450,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; /* From de9e8794f141933481a6a46201cd36aecd6ee203 Mon Sep 17 00:00:00 2001 From: Justin Berger Date: Fri, 22 Nov 2019 14:40:54 -0700 Subject: [PATCH 17/20] Fixed bad poll sleep --- src/driver_usbmon.c | 2 -- src/driver_vive.c | 17 +++++++++++++---- src/survive.c | 10 ++++++---- 3 files changed, 19 insertions(+), 10 deletions(-) diff --git a/src/driver_usbmon.c b/src/driver_usbmon.c index 836ad54..bf44b0a 100644 --- a/src/driver_usbmon.c +++ b/src/driver_usbmon.c @@ -470,9 +470,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); } } } diff --git a/src/driver_vive.c b/src/driver_vive.c index 7591808..4d09504 100755 --- a/src/driver_vive.c +++ b/src/driver_vive.c @@ -836,9 +836,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 @@ -1903,13 +1906,14 @@ 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; uint8_t *readdata = si->buffer; 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 ); @@ -2021,7 +2025,7 @@ void survive_data_cb(SurviveUSBInterface *si) { } } else if (id == 39) { // LHv2 if (obj->ctx->lh_version == 0) { - return; + goto exit_fn; } survive_notify_gen2(obj, "Report id 39"); @@ -2304,6 +2308,9 @@ void survive_data_cb(SurviveUSBInterface *si) { int a = 0; // breakpoint here } } + +exit_fn: + survive_release_ctx_lock(ctx); } /////////////////////////////////////////////////////////////////////////////// @@ -2345,6 +2352,8 @@ int survive_vive_close(SurviveContext *ctx, void *driver) { int DriverRegHTCVive(SurviveContext *ctx) { 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); diff --git a/src/survive.c b/src/survive.c index c4607b2..c93d43d 100644 --- a/src/survive.c +++ b/src/survive.c @@ -743,10 +743,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); From c4f76fb6cd34d249afcc7aff9cc318f858dfe459 Mon Sep 17 00:00:00 2001 From: Justin Berger Date: Sat, 23 Nov 2019 14:51:00 -0700 Subject: [PATCH 18/20] Fixed HMD IMU in usbmon driver --- src/driver_usbmon.c | 34 ++++++++++++++++++++++++---------- src/driver_vive.h | 2 +- src/poser.c | 4 +++- 3 files changed, 28 insertions(+), 12 deletions(-) diff --git a/src/driver_usbmon.c b/src/driver_usbmon.c index bf44b0a..687f8e2 100644 --- a/src/driver_usbmon.c +++ b/src/driver_usbmon.c @@ -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]; @@ -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: @@ -286,11 +288,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 +310,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 +347,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,16 +391,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++; - // 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) { if (is_config_start(usbp)) { @@ -549,6 +558,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); diff --git a/src/driver_vive.h b/src/driver_vive.h index a58c2a8..bc7034c 100644 --- a/src/driver_vive.h +++ b/src/driver_vive.h @@ -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, diff --git a/src/poser.c b/src/poser.c index 1b76d85..7473789 100644 --- a/src/poser.c +++ b/src/poser.c @@ -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; } From ac967098d148377e5c8a7b3cfbc2acf06a25acb6 Mon Sep 17 00:00:00 2001 From: Justin Berger Date: Sat, 23 Nov 2019 15:14:07 -0700 Subject: [PATCH 19/20] Added test case for WM1 wands --- src/test_cases/CMakeLists.txt | 1 + 1 file changed, 1 insertion(+) diff --git a/src/test_cases/CMakeLists.txt b/src/test_cases/CMakeLists.txt index 3bb0d88..a86da02 100644 --- a/src/test_cases/CMakeLists.txt +++ b/src/test_cases/CMakeLists.txt @@ -19,6 +19,7 @@ add_test(NAME lh2_test_cal COMMAND $ ${CMAKE_CURRE if(PCAP_LIBRARY) add_test(NAME lh2_test_cal_usb COMMAND $ ${CMAKE_CURRENT_BINARY_DIR}/libsurvive-extras-data/tests/lh2_test_cal.pcap.gz) + add_test(NAME wm1_wand_test_cal_usb COMMAND $ ${CMAKE_CURRENT_BINARY_DIR}/libsurvive-extras-data/tests/WM1-wand.pcap.gz) endif() include(ExternalProject) From bd6d0fc3775569dbebc99ee2c6caa3df4c03c193 Mon Sep 17 00:00:00 2001 From: Justin Berger Date: Sat, 23 Nov 2019 20:25:23 -0700 Subject: [PATCH 20/20] Fixed issue with wireless tracker not reading config --- src/driver_usbmon.c | 9 +++++++-- src/driver_vive.c | 5 +++++ src/test_cases/CMakeLists.txt | 1 + useful_files/git-hooks/pre-push | 2 +- 4 files changed, 14 insertions(+), 3 deletions(-) diff --git a/src/driver_usbmon.c b/src/driver_usbmon.c index 687f8e2..3495492 100644 --- a/src/driver_usbmon.c +++ b/src/driver_usbmon.c @@ -184,8 +184,13 @@ static void ingest_config_request(vive_device_inst_t *dev, const struct _usb_hea uint16_t cnt = pktData[1]; 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 = SV_MALLOC(65536); SurviveContext *ctx = dev->so->ctx; diff --git a/src/driver_vive.c b/src/driver_vive.c index c3098a2..f1dd2cb 100755 --- a/src/driver_vive.c +++ b/src/driver_vive.c @@ -939,6 +939,11 @@ static int survive_get_config(char **config, SurviveViveData *sv, struct Survive 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); diff --git a/src/test_cases/CMakeLists.txt b/src/test_cases/CMakeLists.txt index a86da02..531d1a1 100644 --- a/src/test_cases/CMakeLists.txt +++ b/src/test_cases/CMakeLists.txt @@ -20,6 +20,7 @@ add_test(NAME lh2_test_cal COMMAND $ ${CMAKE_CURRE if(PCAP_LIBRARY) add_test(NAME lh2_test_cal_usb COMMAND $ ${CMAKE_CURRENT_BINARY_DIR}/libsurvive-extras-data/tests/lh2_test_cal.pcap.gz) add_test(NAME wm1_wand_test_cal_usb COMMAND $ ${CMAKE_CURRENT_BINARY_DIR}/libsurvive-extras-data/tests/WM1-wand.pcap.gz) + add_test(NAME wireless_tracker_test_cal_usb COMMAND $ ${CMAKE_CURRENT_BINARY_DIR}/libsurvive-extras-data/tests/wireless-tracker.pcap.gz) endif() include(ExternalProject) diff --git a/useful_files/git-hooks/pre-push b/useful_files/git-hooks/pre-push index 47b6b89..0736906 100755 --- a/useful_files/git-hooks/pre-push +++ b/useful_files/git-hooks/pre-push @@ -4,5 +4,5 @@ ROOT=`git rev-parse --show-toplevel` cd $ROOT mkdir -p bin cd bin -cmake -DENABLE_TESTS .. +cmake -DENABLE_TESTS=ON .. make run_all_tests