From e593db4c3a3575f826682d5eb9e402372aa1ba98 Mon Sep 17 00:00:00 2001 From: ultramn Date: Sun, 19 Feb 2017 10:56:20 -0800 Subject: Don't know why I have to commit this, it might make things worse, but it won't let me pull without it. --- tools/plot_lighthouse/Makefile | 6 +++--- 1 file changed, 3 insertions(+), 3 deletions(-) diff --git a/tools/plot_lighthouse/Makefile b/tools/plot_lighthouse/Makefile index d156bdb..07bdcb9 100644 --- a/tools/plot_lighthouse/Makefile +++ b/tools/plot_lighthouse/Makefile @@ -1,15 +1,15 @@ UNAME := $(shell uname) ifeq ($(UNAME), Linux) -CFLAGS:= -lGL -lGLU -lglut +CFLAGS:= -I../../redist -lGL -lGLU -lglut endif # Darwin is Mac OSX !! ifeq ($(UNAME), Darwin) -CFLAGS:= -w -framework OpenGL -framework GLUT +CFLAGS:= -I../../redist -w -framework OpenGL -framework GLUT endif all: - gcc -O3 -o plot_lighthouse main.c glutil.c fileutil.c ../../redist/os_generic.c $(CFLAGS) + gcc -O3 -o plot_lighthouse main.c glutil.c fileutil.c ../../redist/os_generic.c ../../redist/linmath.c $(CFLAGS) clean: rm -f plot_lighthouse -- cgit v1.2.3 From c2ee02ce28a7c7a9bebe9b69f214e8e441d60cc8 Mon Sep 17 00:00:00 2001 From: Joshua Allen Date: Sat, 25 Feb 2017 16:18:26 -0500 Subject: specify types --- src/survive_config.c | 9 +++++++++ 1 file changed, 9 insertions(+) diff --git a/src/survive_config.c b/src/survive_config.c index ed3f6cd..09153fa 100644 --- a/src/survive_config.c +++ b/src/survive_config.c @@ -1,3 +1,5 @@ +// (C) 2017 <>< Joshua Allen, Under MIT/x11 License. + #include #include #include "survive_config.h" @@ -14,6 +16,7 @@ void config_init() { for (i=0;i Date: Sat, 25 Feb 2017 23:53:33 -0500 Subject: some progress on json file writing --- src/survive_config.c | 214 +++++++++++++++++++++++++++++++++++---------------- src/survive_config.h | 9 ++- 2 files changed, 155 insertions(+), 68 deletions(-) diff --git a/src/survive_config.c b/src/survive_config.c index 09153fa..b18f083 100644 --- a/src/survive_config.c +++ b/src/survive_config.c @@ -10,13 +10,15 @@ config_val config_values[MAX_CONFIG_ENTRIES]; static uint16_t used_entries = 0; static FILE *config_file = NULL; +const FLT* config_set_float_a(const char *tag, const FLT* values, uint8_t count); void config_init() { uint16_t i = 0; for (i=0;iBaseStationID); + config_set_float_a("position", bsd->Position, 3); + config_set_float_a("quaternion", bsd->Quaternion, 4); + config_set_float_a("fcalphase", bsd->fcalphase, 2); + config_set_float_a("fcaltilt", bsd->fcaltilt,2); + config_set_float_a("fcalcurve", bsd->fcalcurve,2); + config_set_float_a("fcalgibpha", bsd->fcalgibpha,2); + config_set_float_a("fcalgibmag", bsd->fcalgibmag,2); +} -void sstrcpy(char* dest, const char *src) { +void sstrcpy(char** dest, const char *src) { uint32_t len = strlen(src)+1; - if (dest == NULL) { - dest = (char*)malloc(len); + if (*dest == NULL) { + *dest = (char*)malloc(len); } else { - dest = (char*)realloc(dest, len); + *dest = (char*)realloc(*dest, len); } - strcpy(dest,src); + strcpy(*dest,src); } -const char* config_read_str(const char *tag, const char *value, const char *def_str) { +config_val* find_config_entry(const char *tag) { uint16_t i = 0; for (i=0;idata; + assert(used_entriestag), tag); + sstrcpy(&(cv->data), def_str); + cv->type = CONFIG_STRING; - return config_values[i].str; + return cv->data; } uint32_t config_read_uint32(const char *tag, const uint32_t value, const uint32_t def) { - uint16_t i = 0; - for (i=0;inumeric.i; + assert(used_entriestag), tag); + cv->numeric.i = def; + cv->type = CONFIG_UINT32; - return config_values[i].numeric.i; + return cv->numeric.i; } FLT config_read_float(const char *tag, const FLT value, const FLT def) { - uint16_t i = 0; - for (i=0;inumeric.f; + assert(used_entriestag), tag); + cv->numeric.f = def; + cv->type = CONFIG_FLOAT; - return config_values[i].numeric.f; + return cv->numeric.f; } -const char* config_set_str(const char *tag, const char* value) { - uint16_t i = 0; - +config_val* next_unused_val() { + config_val *cv = config_values+used_entries; assert(used_entriestag), tag); + sstrcpy(&(cv->data), value); + cv->type = CONFIG_STRING; return value; } const uint32_t config_set_uint32(const char *tag, const uint32_t value) { - uint16_t i = 0; - - assert(used_entriestag), tag); + cv->numeric.i = value; + cv->type = CONFIG_UINT32; return value; } const FLT config_set_float(const char *tag, const FLT value) { - uint16_t i = 0; + config_val *cv = find_config_entry(tag); + if (cv == NULL) cv = next_unused_val(); - assert(used_entriestag), tag); + cv->numeric.f = value; + cv->type = CONFIG_FLOAT; return value; } + +const FLT* config_set_float_a(const char *tag, const FLT* values, uint8_t count) { + config_val *cv = find_config_entry(tag); + if (cv == NULL) cv = next_unused_val(); + + sstrcpy(&(cv->tag), tag); + + if (cv->data == NULL) { + cv->data = (char*)malloc(sizeof(FLT)*count); + } + else { + cv->data = (char*)realloc(cv->data, sizeof(FLT)*count); + } + printf("float array\n"); + + memcpy(cv->data,values,sizeof(FLT)*count); + cv->type = CONFIG_FLOAT_ARRAY; + cv->elements = count; + + return values; +} + +void write_float_array(FILE* f, char* tag, FLT* v, uint8_t count) { + uint8_t i = 0; + printf("save float array\n"); + + fprintf(f, "\"%s\":[", tag); + for (i=0;i0) { + fprintf(f, "\"%f\",", v[i]); +// } else { +// fprintf(f, "\"%f\"", v[i]); +// } + } + + fseek(f,-1,SEEK_CUR); + fprintf(f, "]\n"); + +} + void config_save(const char* path) { uint16_t i = 0; @@ -174,7 +256,9 @@ void config_save(const char* path) { } else if (config_values[i].type == CONFIG_UINT32) { fprintf(f, "\"%s\":\"%d\"\n", config_values[i].tag, config_values[i].numeric.i); } else if (config_values[i].type == CONFIG_STRING) { - fprintf(f, "\"%s\":\"%s\"\n", config_values[i].tag, config_values[i].str); + fprintf(f, "\"%s\":\"%s\"\n", config_values[i].tag, config_values[i].data); + } else if (config_values[i].type == CONFIG_FLOAT_ARRAY) { + write_float_array(f, config_values[i].tag, (FLT*)config_values[i].data, config_values[i].elements); } }; diff --git a/src/survive_config.h b/src/survive_config.h index 24762cd..3e62ec8 100644 --- a/src/survive_config.h +++ b/src/survive_config.h @@ -10,7 +10,8 @@ typedef enum { CONFIG_UNKNOWN = 0, CONFIG_FLOAT = 1, CONFIG_UINT32 = 2, - CONFIG_STRING = 3 + CONFIG_STRING = 3, + CONFIG_FLOAT_ARRAY = 4, } cval_type; /* typedef union { @@ -25,13 +26,15 @@ typedef struct { uint32_t i; FLT f; } numeric; - char *str; + char *data; + uint32_t elements; } config_val; - +void config_init(); void config_open(const char* path, const char* mode); void config_close(); void config_write_lighthouse(struct BaseStationData* bsd, uint8_t length); +void config_set_lighthouse(struct BaseStationData* bsd, uint8_t idx); void config_save(const char* path); const FLT config_set_float(const char *tag, const FLT value); -- cgit v1.2.3 From cab4ee6f89a661190b2bd80c30e42d5f45614f20 Mon Sep 17 00:00:00 2001 From: Joshua Allen Date: Sun, 26 Feb 2017 10:13:31 -0500 Subject: json helpers for writing --- Makefile | 2 +- redist/json_helpers.c | 63 +++++++++++++++++++++++++++++++++++++++++++++++++++ redist/json_helpers.h | 14 ++++++++++++ 3 files changed, 78 insertions(+), 1 deletion(-) create mode 100644 redist/json_helpers.c create mode 100644 redist/json_helpers.h diff --git a/Makefile b/Makefile index 693f6bc..1488e1f 100644 --- a/Makefile +++ b/Makefile @@ -23,7 +23,7 @@ calibrate_client : calibrate_client.c lib/libsurvive.so redist/os_generic.c red lib: mkdir lib -lib/libsurvive.so : src/survive.o src/survive_usb.o src/survive_data.o src/survive_process.o redist/jsmn.o src/ootx_decoder.o redist/linmath.o src/survive_config.o $(DEBUGSTUFF) $(CALS) +lib/libsurvive.so : src/survive.o src/survive_usb.o src/survive_data.o src/survive_process.o redist/jsmn.o src/ootx_decoder.o redist/linmath.o src/survive_config.o redist/json_helpers.o $(DEBUGSTUFF) $(CALS) gcc -o $@ $^ $(LDFLAGS) -shared clean : diff --git a/redist/json_helpers.c b/redist/json_helpers.c new file mode 100644 index 0000000..e96e873 --- /dev/null +++ b/redist/json_helpers.c @@ -0,0 +1,63 @@ +// (C) 2017 <>< Joshua Allen, Under MIT/x11 License. + +#define _GNU_SOURCE + +#include +#include +#include "json_helpers.h" + +void json_write_float_array(FILE* f, const char* tag, float* v, uint8_t count) { + uint8_t i = 0; + char * str1 = NULL; + char * str2 = NULL; + asprintf(&str1,"\"%s\":[", tag); + + for (i=0;i< Joshua Allen, Under MIT/x11 License. + +#ifndef JSON_HELPERS_H +#define JSON_HELPERS_H + +#include + +void json_write_float_array(FILE* f, const char* tag, float* v, uint8_t count); +void json_write_double_array(FILE* f, const char* tag, double* v, uint8_t count); +void json_write_uint32(FILE* f, const char* tag, uint32_t v); +void json_write_float(FILE* f, const char* tag, float v); +void json_write_str(FILE* f, const char* tag, const char* v); + +#endif \ No newline at end of file -- cgit v1.2.3 From f78983a584768a54503535be0366b07c97738299 Mon Sep 17 00:00:00 2001 From: Joshua Allen Date: Sun, 26 Feb 2017 10:20:48 -0500 Subject: use json helpers --- src/survive_config.c | 28 +++++++--------------------- 1 file changed, 7 insertions(+), 21 deletions(-) diff --git a/src/survive_config.c b/src/survive_config.c index b18f083..6afe3d1 100644 --- a/src/survive_config.c +++ b/src/survive_config.c @@ -1,8 +1,8 @@ // (C) 2017 <>< Joshua Allen, Under MIT/x11 License. - #include #include #include "survive_config.h" +#include #define MAX_CONFIG_ENTRIES 100 @@ -227,22 +227,8 @@ const FLT* config_set_float_a(const char *tag, const FLT* values, uint8_t count) return values; } -void write_float_array(FILE* f, char* tag, FLT* v, uint8_t count) { - uint8_t i = 0; - printf("save float array\n"); - - fprintf(f, "\"%s\":[", tag); - for (i=0;i0) { - fprintf(f, "\"%f\",", v[i]); -// } else { -// fprintf(f, "\"%f\"", v[i]); -// } - } - - fseek(f,-1,SEEK_CUR); - fprintf(f, "]\n"); - +void _json_write_float_array(FILE* f, const char* tag, FLT* v, uint8_t count) { + json_write_double_array(f,tag,v,count); } void config_save(const char* path) { @@ -252,13 +238,13 @@ void config_save(const char* path) { for (i=0;i<=used_entries;++i) { if (config_values[i].type == CONFIG_FLOAT) { - fprintf(f, "\"%s\":\"%F\"\n", config_values[i].tag, config_values[i].numeric.f); + json_write_float(f, config_values[i].tag, config_values[i].numeric.f); } else if (config_values[i].type == CONFIG_UINT32) { - fprintf(f, "\"%s\":\"%d\"\n", config_values[i].tag, config_values[i].numeric.i); + json_write_uint32(f, config_values[i].tag, config_values[i].numeric.i); } else if (config_values[i].type == CONFIG_STRING) { - fprintf(f, "\"%s\":\"%s\"\n", config_values[i].tag, config_values[i].data); + json_write_str(f, config_values[i].tag, config_values[i].data); } else if (config_values[i].type == CONFIG_FLOAT_ARRAY) { - write_float_array(f, config_values[i].tag, (FLT*)config_values[i].data, config_values[i].elements); + _json_write_float_array(f, config_values[i].tag, (FLT*)config_values[i].data, config_values[i].elements); } }; -- cgit v1.2.3 From e8d696e03128242be33eb0734addee645e894635 Mon Sep 17 00:00:00 2001 From: Joshua Allen Date: Mon, 27 Feb 2017 20:52:26 -0500 Subject: basic config can be written to file --- calibrate.c | 3 + calibrate_client.c | 8 ++ redist/json_helpers.c | 11 +-- src/survive_cal.c | 5 ++ src/survive_config.c | 230 +++++++++++++++++++++----------------------------- src/survive_config.h | 34 +++++--- 6 files changed, 136 insertions(+), 155 deletions(-) diff --git a/calibrate.c b/calibrate.c index 8eedc88..9c01e8a 100644 --- a/calibrate.c +++ b/calibrate.c @@ -10,6 +10,8 @@ #include "src/survive_cal.h" #include +#include "src/survive_config.h" + struct SurviveContext * ctx; void HandleKey( int keycode, int bDown ) @@ -141,6 +143,7 @@ void * GuiThread( void * v ) int main() { ctx = survive_init( 0 ); + config_init(); survive_install_light_fn( ctx, my_light_process ); survive_install_imu_fn( ctx, my_imu_process ); diff --git a/calibrate_client.c b/calibrate_client.c index 25b6303..897c9e0 100644 --- a/calibrate_client.c +++ b/calibrate_client.c @@ -10,6 +10,8 @@ #include "src/survive_cal.h" #include +#include "src/survive_config.h" + struct SurviveContext * ctx; void HandleKey( int keycode, int bDown ) @@ -152,6 +154,12 @@ int main() CNFGDialogColor = 0x444444; CNFGSetup( "Survive GUI Debug", 640, 480 ); OGCreateThread( GuiThread, 0 ); + + config_init(); + config_set_str(&global_config_values, "Hello","World!"); + const char *s = config_read_str(&global_config_values, "TestStr","This is a test."); + printf("%s\n", s); + config_save("config.json"); if( !ctx ) diff --git a/redist/json_helpers.c b/redist/json_helpers.c index e96e873..74028b2 100644 --- a/redist/json_helpers.c +++ b/redist/json_helpers.c @@ -3,6 +3,7 @@ #define _GNU_SOURCE #include +#include #include #include "json_helpers.h" @@ -22,7 +23,7 @@ void json_write_float_array(FILE* f, const char* tag, float* v, uint8_t count) { str1=str2; str2=NULL; } - asprintf(&str2, "%s]\n", str1,v[i]); + asprintf(&str2, "%s]", str1,v[i]); fputs(str2,f); free(str1); free(str2); @@ -44,20 +45,20 @@ void json_write_double_array(FILE* f, const char* tag, double* v, uint8_t count) str1=str2; str2=NULL; } - asprintf(&str2, "%s]\n", str1,v[i]); + asprintf(&str2, "%s]", str1,v[i]); fputs(str2,f); free(str1); free(str2); } void json_write_uint32(FILE* f, const char* tag, uint32_t v) { - fprintf(f, "\"%s\":\"%d\"\n", tag, v); + fprintf(f, "\"%s\":\"%d\"", tag, v); } void json_write_float(FILE* f, const char* tag, float v) { - fprintf(f, "\"%s\":\"%f\"\n", tag, v); + fprintf(f, "\"%s\":\"%f\"", tag, v); } void json_write_str(FILE* f, const char* tag, const char* v) { - fprintf(f, "\"%s\":\"%s\"\n", tag, v); + fprintf(f, "\"%s\":\"%s\"", tag, v); } \ No newline at end of file diff --git a/src/survive_cal.c b/src/survive_cal.c index 760692c..41d3fb1 100644 --- a/src/survive_cal.c +++ b/src/survive_cal.c @@ -15,6 +15,8 @@ #include #include +#include "survive_config.h" + #define PTS_BEFORE_COMMON 32 #define NEEDED_COMMON_POINTS 10 #define NEEDED_TIMES_OF_COMMON 5 @@ -49,6 +51,9 @@ void ootx_packet_clbk_d(ootx_decoder_context *ct, ootx_packet* packet) b->fcalgibmag[0] = v6.fcal_0_gibmag; b->fcalgibmag[1] = v6.fcal_1_gibmag; b->OOTXSet = 1; + + config_set_lighthouse(b,id); + config_save("config.json"); } int survive_cal_get_status( struct SurviveContext * ctx, char * description, int description_length ) diff --git a/src/survive_config.c b/src/survive_config.c index 6afe3d1..dfbd0e2 100644 --- a/src/survive_config.c +++ b/src/survive_config.c @@ -5,104 +5,65 @@ #include #define MAX_CONFIG_ENTRIES 100 +#define MAX_LIGHTHOUSES 2 -config_val config_values[MAX_CONFIG_ENTRIES]; -static uint16_t used_entries = 0; -static FILE *config_file = NULL; -const FLT* config_set_float_a(const char *tag, const FLT* values, uint8_t count); - -void config_init() { - uint16_t i = 0; - for (i=0;iconfig_entries = malloc(count*sizeof(config_entry)); + cg->used_entries = 0; + cg->max_entries = count; for (i=0;iconfig_entries[i].data = NULL; + cg->config_entries[i].tag = NULL; + cg->config_entries[i].type = CONFIG_UNKNOWN; + cg->config_entries[i].elements = 0; } } -*/ -void set_uint32(char* tag, uint32_t x) { -// fprintf(config_file, "\"%s\":\"%d\"\n", tag, x); +void config_init() { + uint16_t i = 0; + init_config_group(&global_config_values, MAX_CONFIG_ENTRIES); + for(i=0;iBaseStationID); - config_set_float_a("position", bsd->Position, 3); - config_set_float_a("quaternion", bsd->Quaternion, 4); - config_set_float_a("fcalphase", bsd->fcalphase, 2); - config_set_float_a("fcaltilt", bsd->fcaltilt,2); - config_set_float_a("fcalcurve", bsd->fcalcurve,2); - config_set_float_a("fcalgibpha", bsd->fcalgibpha,2); - config_set_float_a("fcalgibmag", bsd->fcalgibmag,2); + config_group *cg = lh_config+idx; + config_set_uint32(cg,"index", idx); + config_set_uint32(cg,"id", bsd->BaseStationID); + config_set_float_a(cg,"position", bsd->Position, 3); + config_set_float_a(cg,"quaternion", bsd->Quaternion, 4); + config_set_float_a(cg,"fcalphase", bsd->fcalphase, 2); + config_set_float_a(cg,"fcaltilt", bsd->fcaltilt,2); + config_set_float_a(cg,"fcalcurve", bsd->fcalcurve,2); + config_set_float_a(cg,"fcalgibpha", bsd->fcalgibpha,2); + config_set_float_a(cg,"fcalgibmag", bsd->fcalgibmag,2); } void sstrcpy(char** dest, const char *src) { uint32_t len = strlen(src)+1; + assert(dest!=NULL); + if (*dest == NULL) { *dest = (char*)malloc(len); } else { @@ -111,71 +72,50 @@ void sstrcpy(char** dest, const char *src) { strcpy(*dest,src); } -config_val* find_config_entry(const char *tag) { +config_entry* find_config_entry(config_group *cg, const char *tag) { uint16_t i = 0; - for (i=0;iused_entries;++i) { + if ( strcmp(cg->config_entries[i].tag, tag) == 0 ) { + return cg->config_entries+i; } } return NULL; } -const char* config_read_str(const char *tag, const char *value, const char *def_str) { - config_val *cv = find_config_entry(tag); +const char* config_read_str(config_group *cg, const char *tag, const char *def) { + config_entry *cv = find_config_entry(cg, tag); if (cv != NULL) return cv->data; - assert(used_entriestag), tag); - sstrcpy(&(cv->data), def_str); - cv->type = CONFIG_STRING; - - return cv->data; + return config_set_str(cg,tag,def); } -uint32_t config_read_uint32(const char *tag, const uint32_t value, const uint32_t def) { - config_val *cv = find_config_entry(tag); +uint32_t config_read_uint32(config_group *cg, const char *tag, const uint32_t def) { + config_entry *cv = find_config_entry(cg, tag); if (cv != NULL) return cv->numeric.i; - assert(used_entriestag), tag); - cv->numeric.i = def; - cv->type = CONFIG_UINT32; - - return cv->numeric.i; + return config_set_uint32(cg, tag, def); } -FLT config_read_float(const char *tag, const FLT value, const FLT def) { - config_val *cv = find_config_entry(tag); +FLT config_read_float(config_group *cg, const char *tag, const FLT def) { + config_entry *cv = find_config_entry(cg, tag); if (cv != NULL) return cv->numeric.f; - assert(used_entriestag), tag); - cv->numeric.f = def; - cv->type = CONFIG_FLOAT; - - return cv->numeric.f; + config_set_float(cg, tag, def); } -config_val* next_unused_val() { - config_val *cv = config_values+used_entries; - assert(used_entriesconfig_entries + cg->used_entries; + assert(cg->used_entries < cg->max_entries); + cg->used_entries++; return cv; } -const char* config_set_str(const char *tag, const char* value) { - config_val *cv = find_config_entry(tag); - if (cv == NULL) cv = next_unused_val(); +const char* config_set_str(config_group *cg, const char *tag, const char* value) { + config_entry *cv = find_config_entry(cg, tag); + if (cv == NULL) cv = next_unused_entry(cg); sstrcpy(&(cv->tag), tag); sstrcpy(&(cv->data), value); @@ -184,9 +124,9 @@ const char* config_set_str(const char *tag, const char* value) { return value; } -const uint32_t config_set_uint32(const char *tag, const uint32_t value) { - config_val *cv = find_config_entry(tag); - if (cv == NULL) cv = next_unused_val(); +const uint32_t config_set_uint32(config_group *cg, const char *tag, const uint32_t value) { + config_entry *cv = find_config_entry(cg, tag); + if (cv == NULL) cv = next_unused_entry(cg); sstrcpy(&(cv->tag), tag); cv->numeric.i = value; @@ -195,9 +135,9 @@ const uint32_t config_set_uint32(const char *tag, const uint32_t value) { return value; } -const FLT config_set_float(const char *tag, const FLT value) { - config_val *cv = find_config_entry(tag); - if (cv == NULL) cv = next_unused_val(); +const FLT config_set_float(config_group *cg, const char *tag, const FLT value) { + config_entry *cv = find_config_entry(cg, tag); + if (cv == NULL) cv = next_unused_entry(cg); sstrcpy(&(cv->tag), tag); cv->numeric.f = value; @@ -206,9 +146,9 @@ const FLT config_set_float(const char *tag, const FLT value) { return value; } -const FLT* config_set_float_a(const char *tag, const FLT* values, uint8_t count) { - config_val *cv = find_config_entry(tag); - if (cv == NULL) cv = next_unused_val(); +const FLT* config_set_float_a(config_group *cg, const char *tag, const FLT* values, uint8_t count) { + config_entry *cv = find_config_entry(cg, tag); + if (cv == NULL) cv = next_unused_entry(cg); sstrcpy(&(cv->tag), tag); @@ -231,23 +171,41 @@ void _json_write_float_array(FILE* f, const char* tag, FLT* v, uint8_t count) { json_write_double_array(f,tag,v,count); } -void config_save(const char* path) { +void write_config_group(FILE* f, config_group *cg, char *tag) { uint16_t i = 0; - FILE* f = fopen(path, "w"); + if (tag != NULL) { + fprintf(f, "\"%s\":{\n", tag); + } - for (i=0;i<=used_entries;++i) { - if (config_values[i].type == CONFIG_FLOAT) { - json_write_float(f, config_values[i].tag, config_values[i].numeric.f); - } else if (config_values[i].type == CONFIG_UINT32) { - json_write_uint32(f, config_values[i].tag, config_values[i].numeric.i); - } else if (config_values[i].type == CONFIG_STRING) { - json_write_str(f, config_values[i].tag, config_values[i].data); - } else if (config_values[i].type == CONFIG_FLOAT_ARRAY) { - _json_write_float_array(f, config_values[i].tag, (FLT*)config_values[i].data, config_values[i].elements); + for (i=0;i < cg->used_entries;++i) { + if (cg->config_entries[i].type == CONFIG_FLOAT) { + json_write_float(f, cg->config_entries[i].tag, cg->config_entries[i].numeric.f); + } else if (cg->config_entries[i].type == CONFIG_UINT32) { + json_write_uint32(f, cg->config_entries[i].tag, cg->config_entries[i].numeric.i); + } else if (cg->config_entries[i].type == CONFIG_STRING) { + json_write_str(f, cg->config_entries[i].tag, cg->config_entries[i].data); + } else if (cg->config_entries[i].type == CONFIG_FLOAT_ARRAY) { + _json_write_float_array(f, cg->config_entries[i].tag, (FLT*)cg->config_entries[i].data, cg->config_entries[i].elements); } + if ((i+1) < cg->used_entries) fprintf(f,","); + fprintf(f,"\n"); }; + if (tag != NULL) { + fprintf(f,"}\n"); + } +} + +void config_save(const char* path) { + uint16_t i = 0; + + FILE* f = fopen(path, "w"); + + write_config_group(f,&global_config_values, NULL); + write_config_group(f,lh_config, "lighthouse0"); + write_config_group(f,lh_config+1, "lighthouse1"); + fclose(f); } diff --git a/src/survive_config.h b/src/survive_config.h index 3e62ec8..14e2fc6 100644 --- a/src/survive_config.h +++ b/src/survive_config.h @@ -13,12 +13,8 @@ typedef enum { CONFIG_STRING = 3, CONFIG_FLOAT_ARRAY = 4, } cval_type; -/* -typedef union { - uint32_t i; - FLT f; - } Numeric; -*/ + + typedef struct { char *tag; cval_type type; @@ -28,21 +24,31 @@ typedef struct { } numeric; char *data; uint32_t elements; -} config_val; +} config_entry; + +typedef struct { + config_entry *config_entries; + uint16_t used_entries; + uint16_t max_entries; +} config_group; + +extern config_group global_config_values; +extern config_group lh_config[2]; //lighthouse configs + void config_init(); void config_open(const char* path, const char* mode); void config_close(); -void config_write_lighthouse(struct BaseStationData* bsd, uint8_t length); +//void config_write_lighthouse(struct BaseStationData* bsd, uint8_t length); void config_set_lighthouse(struct BaseStationData* bsd, uint8_t idx); void config_save(const char* path); -const FLT config_set_float(const char *tag, const FLT value); -const uint32_t config_set_uint32(const char *tag, const uint32_t value); -const char* config_set_str(const char *tag, const char* value); -FLT config_read_float(const char *tag, const FLT value, const FLT def); +const FLT config_set_float(config_group *cg, const char *tag, const FLT value); +const uint32_t config_set_uint32(config_group *cg, const char *tag, const uint32_t value); +const char* config_set_str(config_group *cg, const char *tag, const char* value); -uint32_t config_read_uint32(const char *tag, const uint32_t value, const uint32_t def); -const char* config_read_str(const char *tag, const char *value, const char *def_str); +FLT config_read_float(config_group *cg, const char *tag, const FLT def); +uint32_t config_read_uint32(config_group *cg, const char *tag, const uint32_t def); +const char* config_read_str(config_group *cg, const char *tag, const char *def); #endif \ No newline at end of file -- cgit v1.2.3 From 77644ff26ee7b8fcef3997d0ffe183d0a80c85e4 Mon Sep 17 00:00:00 2001 From: Joshua Allen Date: Mon, 27 Feb 2017 21:52:39 -0500 Subject: return set float value --- src/survive_config.c | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/src/survive_config.c b/src/survive_config.c index dfbd0e2..2ed265b 100644 --- a/src/survive_config.c +++ b/src/survive_config.c @@ -103,7 +103,7 @@ FLT config_read_float(config_group *cg, const char *tag, const FLT def) { if (cv != NULL) return cv->numeric.f; - config_set_float(cg, tag, def); + return config_set_float(cg, tag, def); } config_entry* next_unused_entry(config_group *cg) { -- cgit v1.2.3 From b0dc9c89146d2a5904a4ecc6b9c1f8d1c5fcfce0 Mon Sep 17 00:00:00 2001 From: Joshua Allen Date: Mon, 27 Feb 2017 22:00:35 -0500 Subject: check for correct float size --- src/survive_config.c | 6 +++++- 1 file changed, 5 insertions(+), 1 deletion(-) diff --git a/src/survive_config.c b/src/survive_config.c index 2ed265b..5531006 100644 --- a/src/survive_config.c +++ b/src/survive_config.c @@ -168,7 +168,11 @@ const FLT* config_set_float_a(config_group *cg, const char *tag, const FLT* valu } void _json_write_float_array(FILE* f, const char* tag, FLT* v, uint8_t count) { - json_write_double_array(f,tag,v,count); + if (sizeof(FLT) == sizeof(double)) { + json_write_double_array(f,tag,v,count); + } else if (sizeof(FLT) == sizeof(float)) { + json_write_double_array(f,tag,v,count); + } } void write_config_group(FILE* f, config_group *cg, char *tag) { -- cgit v1.2.3 From 6cc609204ba7ea1bd0a903cf918a2b7eea979b91 Mon Sep 17 00:00:00 2001 From: Joshua Allen Date: Mon, 27 Feb 2017 22:08:36 -0500 Subject: compile time check of float type --- src/survive_config.c | 8 ++++---- 1 file changed, 4 insertions(+), 4 deletions(-) diff --git a/src/survive_config.c b/src/survive_config.c index 5531006..c46e300 100644 --- a/src/survive_config.c +++ b/src/survive_config.c @@ -168,11 +168,11 @@ const FLT* config_set_float_a(config_group *cg, const char *tag, const FLT* valu } void _json_write_float_array(FILE* f, const char* tag, FLT* v, uint8_t count) { - if (sizeof(FLT) == sizeof(double)) { + #ifdef USE_DOUBLE json_write_double_array(f,tag,v,count); - } else if (sizeof(FLT) == sizeof(float)) { - json_write_double_array(f,tag,v,count); - } + #else + json_write_float_array(f,tag,v,count); + #endif } void write_config_group(FILE* f, config_group *cg, char *tag) { -- cgit v1.2.3 From 9d1b1d09ed51344c8ca7b4f0a94f5841ee2c509e Mon Sep 17 00:00:00 2001 From: cnlohr Date: Thu, 2 Mar 2017 22:20:07 -0500 Subject: cleanup, add some logging data to calinfo, open up restrictive rules about sensor positions, clean cleans redist/ --- Makefile | 2 +- src/survive_cal_lhfind.c | 2 +- src/survive_vive.c | 22 ++++++++++++++++++++++ 3 files changed, 24 insertions(+), 2 deletions(-) diff --git a/Makefile b/Makefile index 4938e28..75b5606 100644 --- a/Makefile +++ b/Makefile @@ -27,7 +27,7 @@ lib/libsurvive.so : src/survive.o src/survive_usb.o src/survive_data.o src/survi gcc -o $@ $^ $(LDFLAGS) -shared clean : - rm -rf *.o src/*.o *~ src/*~ test data_recorder lib/libsurvive.so + rm -rf *.o src/*.o *~ src/*~ test data_recorder lib/libsurvive.so redist/*.o redist/*~ diff --git a/src/survive_cal_lhfind.c b/src/survive_cal_lhfind.c index e1e5fc9..93d9dc0 100644 --- a/src/survive_cal_lhfind.c +++ b/src/survive_cal_lhfind.c @@ -129,7 +129,7 @@ int survive_cal_lhfind( struct SurviveCalData * cd ) fullrange *= 0.25; } - if( beste > 0.005 ) + if( beste > 0.01 ) { //Error too high SV_ERROR( "LH: %d / Best E %f Error too high\n", lh, beste ); diff --git a/src/survive_vive.c b/src/survive_vive.c index a402153..47886c9 100644 --- a/src/survive_vive.c +++ b/src/survive_vive.c @@ -20,6 +20,7 @@ #include //sleep if I ever use it. #include #include +#include struct SurviveViveData; @@ -976,6 +977,27 @@ static int LoadConfig( struct SurviveViveData * sv, struct SurviveObject * so, i //TODO: Cleanup any remaining USB stuff. return 1; } + + char fname[20]; + mkdir( "calinfo", 0755 ); + + sprintf( fname, "calinfo/%s_points.csv", so->codename ); + FILE * f = fopen( fname, "w" ); + int j; + for( j = 0; j < so->nr_locations; j++ ) + { + fprintf( f, "%f %f %f\n", so->sensor_locations[j*3+0], so->sensor_locations[j*3+1], so->sensor_locations[j*3+2] ); + } + fclose( f ); + + sprintf( fname, "calinfo/%s_normals.csv", so->codename ); + f = fopen( fname, "w" ); + for( j = 0; j < so->nr_locations; j++ ) + { + fprintf( f, "%f %f %f\n", so->sensor_normals[j*3+0], so->sensor_normals[j*3+1], so->sensor_normals[j*3+2] ); + } + fclose( f ); + return 0; } -- cgit v1.2.3 From 1c131c03fe8c5d5ab17193c9f6e7e79d81110d52 Mon Sep 17 00:00:00 2001 From: ultramn Date: Thu, 2 Mar 2017 19:55:33 -0800 Subject: added affinesolve --- dave/AffineSolve.c | 322 +++++++++++++++++++++++++++++++++++++++++++++++++++++ 1 file changed, 322 insertions(+) create mode 100644 dave/AffineSolve.c diff --git a/dave/AffineSolve.c b/dave/AffineSolve.c new file mode 100644 index 0000000..36587e6 --- /dev/null +++ b/dave/AffineSolve.c @@ -0,0 +1,322 @@ +// +// main.c +// AffineSolve +// +// Created by user on 3/2/17. +// Copyright © 2017 user. All rights reserved. +// + +#include +#include +#include +#include + +#define LH_ID 0 +#define NUM_HMD 32 + +float hmd_pos[NUM_HMD][3]; +void ReadHmdPoints() +{ + int i; + FILE *fin = fopen("HMD_points.csv","r"); + if (fin==NULL) { + printf("ERROR: could not open HMD_points.csv for reading\n"); + exit(1); + } + + for (i=0; i TOO_SMALL) { + float scaleRot = stepSizeRot / gradSizeRot; + for (j=0; j<3; j++) { + for (k=0; k<3; k++) { + T[j][k] += scaleRot * conj[j][k]; + } + } + stepSizeRot *= falloff; + } + + if (gradSizePos > TOO_SMALL) { + float scalePos = stepSizePos / gradSizePos; + for (j=0; j<3; j++) { + T[j][3] += scalePos * conj[j][3]; + } + stepSizePos *= falloff; + } + + // Constrain the gradient (such that scaling is one) + if (constrain) + { + // Measure the scales + float len[3] = {0.0, 0.0, 0.0}; + for (j=0; j<3; j++) { + double lenSq = 0.0; + for (k=0; k<3; k++) { lenSq += (double)T[j][k] * (double)T[j][k]; } + len[j] = sqrt(lenSq); + } + + // How far off is the scale? + float xzLen = 0.5 * (len[0] + len[2]); + if (xzLen > TOO_SMALL) { + float inv_xzLen = 1.0 / xzLen; + for (j=0; j<3; j++) { + T[3][j] *= inv_xzLen; + } + } + + // Rescale the thing + for (j=0; j<3; j++) + { + if (len[j] > TOO_SMALL) { + float inv_len = 1.0 / len[j]; + for (k=0; k<3; k++) { T[j][k] *= inv_len; } + } + } + } + } + +} + +int main() +{ + int i,j,k; + float Tcalc[4][4]; + float O[MAX_POINTS][4]; + float N[MAX_POINTS][3]; + float D[MAX_POINTS]; + int nPoints = 0; + + // Read the hmd points + ReadHmdPoints(); + + //------------------------- + // Read the lighthouse data + //------------------------- + FILE *fin = fopen("ptinfo.csv", "r"); + if (fin==NULL) { printf("ERROR: could not open ptinfo.csv for reading\n"); exit(1); } + while (!feof(fin)) + { + // Read the angle + int sen,lh,axis,count; + float angle, avglen, stddevang, stddevlen; + float max_outlier_length, max_outlier_angle; + int rt = fscanf( fin, "%d %d %d %d %f %f %f %f %f %f\n", + &sen, &lh, &axis, &count, + &angle, &avglen, &stddevang, &stddevlen, + &max_outlier_length, &max_outlier_angle); + if (rt != 10) { break; } + + if (lh == LH_ID && sen < NUM_HMD) { + // Set the offset + O[nPoints][0] = hmd_pos[sen][0]; + O[nPoints][1] = hmd_pos[sen][1]; + O[nPoints][2] = hmd_pos[sen][2]; + O[nPoints][3] = 1.0; + + // Calculate the plane equation + if (axis == 1) { // Horizontal + N[nPoints][0] = -cos(angle); + N[nPoints][1] = -sin(angle); + N[nPoints][2] = 0.0; + D[nPoints] = 0.0; + } else { // Vertical + N[nPoints][0] = 0.0; + N[nPoints][1] = -sin(angle); + N[nPoints][2] = cos(angle); + D[nPoints] = 0.0; + } + + printf("pt %d O %.3f %.3f %.3f %.3f N %.3f %.3f %.3f D %.3f\n", + nPoints, + O[nPoints][0], O[nPoints][1], O[nPoints][2], O[nPoints][3], + N[nPoints][0], N[nPoints][1], N[nPoints][2], + D[nPoints]); + + nPoints++; + } + } + fclose(fin); + + printf("nPoints %d\n", nPoints); + + // Run the calculation for Tcalc + int run; + //for (run=0; run<100; run++) { + + // Initialize Tcalc to the identity matrix + //memcpy(Tcalc, Torig, 4*4*sizeof(float)); + memset(Tcalc, 0, 4*4*sizeof(float)); + for (i=0; i<4; i++) { Tcalc[i][i] = 1.0f; } + + // Solve it! + AffineSolve( + Tcalc, // OUTPUT: transform + O, // INPUT: points, offsets + N, // INPUT: plane normals + D, // INPUT: plane offsets + nPoints, NITER, + STEP_SIZE_ROT, STEP_SIZE_POS, FALLOFF, + 1); + //} + + PRINT_MAT(Tcalc,4,4); + + + // insert code here... + printf("Hello, World!\n"); + return 0; +} -- cgit v1.2.3 From 4d14d795b070403acaada989e9fe07c5fbcd0e6a Mon Sep 17 00:00:00 2001 From: CNLohr Date: Thu, 2 Mar 2017 23:17:52 -0500 Subject: Update README.md --- README.md | 1 + 1 file changed, 1 insertion(+) diff --git a/README.md b/README.md index f90590f..582812d 100644 --- a/README.md +++ b/README.md @@ -3,6 +3,7 @@ **WARNING PROJECT NOT YET IN EXPERIMENTAL PHASE** Discord: https://discordapp.com/invite/7QbCAGS +Testing if git is linked to discord. ## Livestream collection | Note | Youtube URL | Run time | -- cgit v1.2.3 From b4734fc017c615387c4c2e00d8d2c1de9c8f5d89 Mon Sep 17 00:00:00 2001 From: CNLohr Date: Thu, 2 Mar 2017 23:27:38 -0500 Subject: Remove note about testing github linking. This is a description. --- README.md | 1 - 1 file changed, 1 deletion(-) diff --git a/README.md b/README.md index 582812d..f90590f 100644 --- a/README.md +++ b/README.md @@ -3,7 +3,6 @@ **WARNING PROJECT NOT YET IN EXPERIMENTAL PHASE** Discord: https://discordapp.com/invite/7QbCAGS -Testing if git is linked to discord. ## Livestream collection | Note | Youtube URL | Run time | -- cgit v1.2.3 From 76623e6a945cb8be50f8081f610b397e480dcf58 Mon Sep 17 00:00:00 2001 From: cnlohr Date: Fri, 3 Mar 2017 01:32:49 -0500 Subject: First thoughts on a resolver? --- include/survive.h | 20 ++++++++++++++++++-- src/survive_internal.h | 3 +-- 2 files changed, 19 insertions(+), 4 deletions(-) diff --git a/include/survive.h b/include/survive.h index 51910a6..0d43465 100644 --- a/include/survive.h +++ b/include/survive.h @@ -20,6 +20,11 @@ struct SurviveContext; //It will also require a recompile. TODO: revisit this and correct the comment once fixed. #define NUM_LIGHTHOUSES 2 +struct SurviveObject; + +//XXX TODO -> Probably should be one function to take multiple types of input? +typedef int (*ResolverCB)( struct SurviveObject * so ); + struct SurviveObject { struct SurviveContext * ctx; @@ -36,9 +41,16 @@ struct SurviveObject int8_t ison:1; int8_t additional_flags:6; + //Pose Information, also "resolver" field. + FLT PoseConfidence; //0..1 + FLT Position[3]; + FLT Rotation[4]; + void * Resolver; + ResolverCB * PreferredResolverFn; //XXX TODO + + int8_t nr_locations; FLT * sensor_locations; FLT * sensor_normals; - int8_t nr_locations; //Timing sensitive data (mostly for disambiguation) int32_t timebase_hz; //48,000,000 for normal vive hardware. (checked) @@ -51,7 +63,7 @@ struct SurviveObject int32_t pulse_synctime_slack; //5,000 for normal vive hardware. (guessed) //Flood info, for calculating which laser is currently sweeping. - int8_t oldcode; + int8_t oldcode; int8_t sync_set_number; //0 = master, 1 = slave, -1 = fault. int8_t did_handle_ootx; //If unset, will send lightcap data for sync pulses next time a sensor is hit. uint32_t last_time[NUM_LIGHTHOUSES]; @@ -60,10 +72,14 @@ struct SurviveObject uint32_t last_lighttime; //May be a 24- or 32- bit number depending on what device. + //Debug int tsl; }; + + + typedef void (*text_feedback_func)( struct SurviveContext * ctx, const char * fault ); typedef void (*light_process_func)( struct SurviveObject * so, int sensor_id, int acode, int timeinsweep, uint32_t timecode, uint32_t length ); typedef void (*imu_process_func)( struct SurviveObject * so, int16_t * accelgyro, uint32_t timecode, int id ); diff --git a/src/survive_internal.h b/src/survive_internal.h index 5962623..1fdef41 100644 --- a/src/survive_internal.h +++ b/src/survive_internal.h @@ -1,4 +1,4 @@ -//<>< (C) 2016 C. N. Lohr, MOSTLY Under MIT/x11 License. +//<>< (C) 2016-2017 C. N. Lohr, MOSTLY Under MIT/x11 License. // //Based off of https://github.com/collabora/OSVR-Vive-Libre // Originally Copyright 2016 Philipp Zabel @@ -36,7 +36,6 @@ struct SurviveUSBInterface; typedef int (*DeviceDriverCb)( struct SurviveContext * ctx, void * driver ); typedef int (*DeviceDriverMagicCb)( struct SurviveContext * ctx, void * driver, int magic_code, void * data, int datalen ); - //This is defined in survive.h struct SurviveObject; struct SurviveCalData; -- cgit v1.2.3 From 58b19da6848e64c9ca7aa47d8b3e0cd30db89d44 Mon Sep 17 00:00:00 2001 From: cnlohr Date: Fri, 3 Mar 2017 02:15:04 -0500 Subject: Split out into some more things in the include folder. Getting closer to trying to have posers. --- Makefile | 2 +- include/libsurvive/poser.h | 54 +++++++++++++++++ include/libsurvive/survive.h | 95 +++++++++++++++++++++++++++++ include/libsurvive/survive_types.h | 29 +++++++++ include/survive.h | 118 ------------------------------------- src/survive_internal.h | 2 - 6 files changed, 179 insertions(+), 121 deletions(-) create mode 100644 include/libsurvive/poser.h create mode 100644 include/libsurvive/survive.h create mode 100644 include/libsurvive/survive_types.h delete mode 100644 include/survive.h diff --git a/Makefile b/Makefile index 75b5606..470cbe3 100644 --- a/Makefile +++ b/Makefile @@ -1,6 +1,6 @@ all : lib data_recorder test calibrate calibrate_client -CFLAGS:=-Iinclude -I. -fPIC -g -O0 -Iredist -flto -DUSE_DOUBLE -std=gnu99 +CFLAGS:=-Iinclude/libsurvive -I. -fPIC -g -O0 -Iredist -flto -DUSE_DOUBLE -std=gnu99 LDFLAGS:=-lpthread -lusb-1.0 -lz -lX11 -lm -flto -g diff --git a/include/libsurvive/poser.h b/include/libsurvive/poser.h new file mode 100644 index 0000000..4894acf --- /dev/null +++ b/include/libsurvive/poser.h @@ -0,0 +1,54 @@ +#ifndef _LSPOSER_H +#define _LSPOSER_H + +#include "survive_types.h" + +typedef enum PoserType_t +{ + POSERDATA_NONE = 0, + POSERDATA_IMU, + POSERDATA_LIGHT, //Single lighting event. + POSERDATA_FULL_SCENE, //Full, statified X, Y sweep data for both lighthouses. +} PoserType; + +struct PoserData +{ + PoserType pt; + uint8_t data[0]; +}; + +struct PoserDataIMU +{ + PoserType pt; + uint8_t datamask; //0 = accel present, 1 = gyro present, 2 = mag present. + FLT accel[3]; + FLT gyro[3]; + FLT mag[3]; +}; + +struct PoserDataLight +{ + PoserType pt; + int sensor_id; + int acode; //OOTX Code associated with this sweep. base_station = acode >> 2; axis = acode & 1; + uint32_t timecode; //In object-local ticks. + FLT length; //In seconds + FLT angle; //In radians from center of lighthouse. +}; + +struct PoserDataFullScene +{ + PoserType pt; + + //If "lengths[...]" < 0, means not a valid piece of sweep information. + FLT lengths[SENSORS_PER_OBJECT][NUM_LIGHTHOUSES][2]; + FLT angles [SENSORS_PER_OBJECT][NUM_LIGHTHOUSES][2]; //2 Axes + + struct PoserDataIMU lastimu; +}; + +//When you register your posers using the internal system, +typedef int (*PoserCB)( struct SurviveObject * so, struct PoserData * pd ); + + +#endif diff --git a/include/libsurvive/survive.h b/include/libsurvive/survive.h new file mode 100644 index 0000000..536c7fb --- /dev/null +++ b/include/libsurvive/survive.h @@ -0,0 +1,95 @@ +#ifndef _SURVIVE_H +#define _SURVIVE_H + +#include +#include "poser.h" + +struct SurviveContext; + +//DANGER: This structure may be redefined. Note that it is logically split into 64-bit chunks +//for optimization on 32- and 64-bit systems. + +struct SurviveObject +{ + struct SurviveContext * ctx; + + char codename[4]; //3 letters, null-terminated. Currently HMD, WM0, WM1. + char drivername[4]; //3 letters for driver. Currently "HTC" + int16_t buttonmask; + int16_t axis1; + + int16_t axis2; + int16_t axis3; + int8_t charge; + int8_t charging:1; + int8_t ison:1; + int8_t additional_flags:6; + + //Pose Information, also "resolver" field. + FLT PoseConfidence; //0..1 + FLT Position[3]; + FLT Rotation[4]; + void * PoserData; //Initialized to zero, configured by poser, can be anything the poser wants. + PoserCB * PoserFn; + + //Device-specific information about the location of the sensors. This data will be used by the poser. + int8_t nr_locations; + FLT * sensor_locations; + FLT * sensor_normals; + + //Timing sensitive data (mostly for disambiguation) + int32_t timebase_hz; //48,000,000 for normal vive hardware. (checked) + int32_t timecenter_ticks; //200,000 for normal vive hardware. (checked) + int32_t pulsedist_max_ticks; //500,000 for normal vive hardware. (guessed) + int32_t pulselength_min_sync; //2,200 for normal vive hardware. (guessed) + int32_t pulse_in_clear_time; //35,000 for normal vive hardware. (guessed) + int32_t pulse_max_for_sweep; //1,800 for normal vive hardware. (guessed) + int32_t pulse_synctime_offset; //20,000 for normal vive hardware. (guessed) + int32_t pulse_synctime_slack; //5,000 for normal vive hardware. (guessed) + + //Flood info, for calculating which laser is currently sweeping. + int8_t oldcode; + int8_t sync_set_number; //0 = master, 1 = slave, -1 = fault. + int8_t did_handle_ootx; //If unset, will send lightcap data for sync pulses next time a sensor is hit. + uint32_t last_time[NUM_LIGHTHOUSES]; + uint32_t last_length[NUM_LIGHTHOUSES]; + uint32_t recent_sync_time; + + uint32_t last_lighttime; //May be a 24- or 32- bit number depending on what device. + + + //Debug + int tsl; +}; + +struct SurviveContext * survive_init( int headless ); + +//For any of these, you may pass in 0 for the function pointer to use default behavior. +void survive_install_info_fn( struct SurviveContext * ctx, text_feedback_func fbp ); +void survive_install_error_fn( struct SurviveContext * ctx, text_feedback_func fbp ); +void survive_install_light_fn( struct SurviveContext * ctx, light_process_func fbp ); +void survive_install_imu_fn( struct SurviveContext * ctx, imu_process_func fbp ); +void survive_install_angle_fn( struct SurviveContext * ctx, angle_process_func fbp ); + +void survive_close( struct SurviveContext * ctx ); +int survive_poll(); + +struct SurviveObject * survive_get_so_by_name( struct SurviveContext * ctx, const char * name ); + +//Utilitiy functions. +int survive_simple_inflate( struct SurviveContext * ctx, const char * input, int inlen, char * output, int outlen ); + +int survive_send_magic( struct SurviveContext * ctx, int magic_code, void * data, int datalen ); + +//Install the calibrator. +void survive_cal_install( struct SurviveContext * ctx ); + +//Call these from your callback if overridden. +//Accept higher-level data. +void survive_default_light_process( struct SurviveObject * so, int sensor_id, int acode, int timeinsweep, uint32_t timecode, uint32_t length ); +void survive_default_imu_process( struct SurviveObject * so, int16_t * accelgyro, uint32_t timecode, int id ); +void survive_default_angle_process( struct SurviveObject * so, int sensor_id, int acode, uint32_t timecode, FLT length, FLT angle ); + + +#endif + diff --git a/include/libsurvive/survive_types.h b/include/libsurvive/survive_types.h new file mode 100644 index 0000000..593819f --- /dev/null +++ b/include/libsurvive/survive_types.h @@ -0,0 +1,29 @@ +#ifndef _SURVIVE_TYPES_H +#define _SURVIVE_TYPES_H + +#ifndef FLT +#ifdef USE_DOUBLE +#define FLT double +#else +#define FLT float +#endif +#endif + + +//Careful with this, you can't just add another one right now, would take minor changes in survive_data.c and the cal tools. +//It will also require a recompile. TODO: revisit this and correct the comment once fixed. +#define NUM_LIGHTHOUSES 2 + +#define INTBUFFSIZE 64 +#define SENSORS_PER_OBJECT 32 + +struct SurviveObject; +struct SurviveContext; + +typedef void (*text_feedback_func)( struct SurviveContext * ctx, const char * fault ); +typedef void (*light_process_func)( struct SurviveObject * so, int sensor_id, int acode, int timeinsweep, uint32_t timecode, uint32_t length ); +typedef void (*imu_process_func)( struct SurviveObject * so, int16_t * accelgyro, uint32_t timecode, int id ); +typedef void (*angle_process_func)( struct SurviveObject * so, int sensor_id, int acode, uint32_t timecode, FLT length, FLT angle ); + +#endif + diff --git a/include/survive.h b/include/survive.h deleted file mode 100644 index 0d43465..0000000 --- a/include/survive.h +++ /dev/null @@ -1,118 +0,0 @@ -#ifndef _SURVIVE_H -#define _SURVIVE_H - -#include - -#ifndef FLT -#ifdef USE_DOUBLE -#define FLT double -#else -#define FLT float -#endif -#endif - -struct SurviveContext; - -//DANGER: This structure may be redefined. Note that it is logically split into 64-bit chunks -//for optimization on 32- and 64-bit systems. - -//Careful with this, you can't just add another one right now, would take minor changes in survive_data.c and the cal tools. -//It will also require a recompile. TODO: revisit this and correct the comment once fixed. -#define NUM_LIGHTHOUSES 2 - -struct SurviveObject; - -//XXX TODO -> Probably should be one function to take multiple types of input? -typedef int (*ResolverCB)( struct SurviveObject * so ); - -struct SurviveObject -{ - struct SurviveContext * ctx; - - char codename[4]; //3 letters, null-terminated. Currently HMD, WM0, WM1. - char drivername[4]; //3 letters for driver. Currently "HTC" - int16_t buttonmask; - int16_t axis1; - - int16_t axis2; - int16_t axis3; - int8_t charge; - int8_t charging:1; - int8_t ison:1; - int8_t additional_flags:6; - - //Pose Information, also "resolver" field. - FLT PoseConfidence; //0..1 - FLT Position[3]; - FLT Rotation[4]; - void * Resolver; - ResolverCB * PreferredResolverFn; //XXX TODO - - int8_t nr_locations; - FLT * sensor_locations; - FLT * sensor_normals; - - //Timing sensitive data (mostly for disambiguation) - int32_t timebase_hz; //48,000,000 for normal vive hardware. (checked) - int32_t timecenter_ticks; //200,000 for normal vive hardware. (checked) - int32_t pulsedist_max_ticks; //500,000 for normal vive hardware. (guessed) - int32_t pulselength_min_sync; //2,200 for normal vive hardware. (guessed) - int32_t pulse_in_clear_time; //35,000 for normal vive hardware. (guessed) - int32_t pulse_max_for_sweep; //1,800 for normal vive hardware. (guessed) - int32_t pulse_synctime_offset; //20,000 for normal vive hardware. (guessed) - int32_t pulse_synctime_slack; //5,000 for normal vive hardware. (guessed) - - //Flood info, for calculating which laser is currently sweeping. - int8_t oldcode; - int8_t sync_set_number; //0 = master, 1 = slave, -1 = fault. - int8_t did_handle_ootx; //If unset, will send lightcap data for sync pulses next time a sensor is hit. - uint32_t last_time[NUM_LIGHTHOUSES]; - uint32_t last_length[NUM_LIGHTHOUSES]; - uint32_t recent_sync_time; - - uint32_t last_lighttime; //May be a 24- or 32- bit number depending on what device. - - - //Debug - int tsl; -}; - - - - -typedef void (*text_feedback_func)( struct SurviveContext * ctx, const char * fault ); -typedef void (*light_process_func)( struct SurviveObject * so, int sensor_id, int acode, int timeinsweep, uint32_t timecode, uint32_t length ); -typedef void (*imu_process_func)( struct SurviveObject * so, int16_t * accelgyro, uint32_t timecode, int id ); -typedef void (*angle_process_func)( struct SurviveObject * so, int sensor_id, int acode, uint32_t timecode, FLT length, FLT angle ); - -struct SurviveContext * survive_init( int headless ); - -//For any of these, you may pass in 0 for the function pointer to use default behavior. -void survive_install_info_fn( struct SurviveContext * ctx, text_feedback_func fbp ); -void survive_install_error_fn( struct SurviveContext * ctx, text_feedback_func fbp ); -void survive_install_light_fn( struct SurviveContext * ctx, light_process_func fbp ); -void survive_install_imu_fn( struct SurviveContext * ctx, imu_process_func fbp ); -void survive_install_angle_fn( struct SurviveContext * ctx, angle_process_func fbp ); - -void survive_close( struct SurviveContext * ctx ); -int survive_poll(); - -struct SurviveObject * survive_get_so_by_name( struct SurviveContext * ctx, const char * name ); - -//Utilitiy functions. -int survive_simple_inflate( struct SurviveContext * ctx, const char * input, int inlen, char * output, int outlen ); - -int survive_send_magic( struct SurviveContext * ctx, int magic_code, void * data, int datalen ); - -//Install the calibrator. -void survive_cal_install( struct SurviveContext * ctx ); - -//Call these from your callback if overridden. -//Accept higher-level data. -void survive_default_light_process( struct SurviveObject * so, int sensor_id, int acode, int timeinsweep, uint32_t timecode, uint32_t length ); -void survive_default_imu_process( struct SurviveObject * so, int16_t * accelgyro, uint32_t timecode, int id ); -void survive_default_angle_process( struct SurviveObject * so, int sensor_id, int acode, uint32_t timecode, FLT length, FLT angle ); - - -#endif - diff --git a/src/survive_internal.h b/src/survive_internal.h index 1fdef41..9d04d93 100644 --- a/src/survive_internal.h +++ b/src/survive_internal.h @@ -27,8 +27,6 @@ //XXX TODO This one needs to be rewritten. #define SV_KILL() exit(0) -#define INTBUFFSIZE 64 -#define SENSORS_PER_OBJECT 32 struct SurviveContext; struct SurviveUSBInterface; -- cgit v1.2.3 From 294481ff114b210f3ccb436719e9595e7ffaa96b Mon Sep 17 00:00:00 2001 From: axlecrusher Date: Fri, 3 Mar 2017 06:33:26 -0500 Subject: Remove extra asprintf parameter --- redist/json_helpers.c | 6 +++--- 1 file changed, 3 insertions(+), 3 deletions(-) diff --git a/redist/json_helpers.c b/redist/json_helpers.c index 74028b2..fd04203 100644 --- a/redist/json_helpers.c +++ b/redist/json_helpers.c @@ -23,7 +23,7 @@ void json_write_float_array(FILE* f, const char* tag, float* v, uint8_t count) { str1=str2; str2=NULL; } - asprintf(&str2, "%s]", str1,v[i]); + asprintf(&str2, "%s]", str1); fputs(str2,f); free(str1); free(str2); @@ -45,7 +45,7 @@ void json_write_double_array(FILE* f, const char* tag, double* v, uint8_t count) str1=str2; str2=NULL; } - asprintf(&str2, "%s]", str1,v[i]); + asprintf(&str2, "%s]", str1); fputs(str2,f); free(str1); free(str2); @@ -61,4 +61,4 @@ void json_write_float(FILE* f, const char* tag, float v) { void json_write_str(FILE* f, const char* tag, const char* v) { fprintf(f, "\"%s\":\"%s\"", tag, v); -} \ No newline at end of file +} -- cgit v1.2.3 From 8967c5c82e6282bc1daeaca79619695da64b6dc5 Mon Sep 17 00:00:00 2001 From: axlecrusher Date: Fri, 3 Mar 2017 06:47:43 -0500 Subject: Add 7th livestream --- README.md | 1 + 1 file changed, 1 insertion(+) diff --git a/README.md b/README.md index f90590f..9428f39 100644 --- a/README.md +++ b/README.md @@ -14,6 +14,7 @@ Discord: https://discordapp.com/invite/7QbCAGS | Fourth livestream | https://www.youtube.com/watch?v=fces1O7kWGY | 4:50:33 | | Fifth livestream | https://www.youtube.com/watch?v=hHt3twW5_fI | 3:13:38 | | Sixth livestream | https://www.youtube.com/watch?v=JsfkNRFkFM4 | 3:44:49 | +| Seventh livestream | https://www.youtube.com/watch?v=EKSHvO3QSWY | 1:17:21 | Notes from second livestream trying to reverse engineer the watchman protocol: https://gist.github.com/cnlohr/581c433f36f4249f8bbc9c2b6450ef0e -- cgit v1.2.3 From 19d18fef008792e8918d4ded2dd7fa015bb2fedb Mon Sep 17 00:00:00 2001 From: cnlohr Date: Sun, 5 Mar 2017 09:45:38 -0500 Subject: Set max sensor # so it doesn't get confusd with -1 data. --- src/survive_data.c | 9 ++++++++- 1 file changed, 8 insertions(+), 1 deletion(-) diff --git a/src/survive_data.c b/src/survive_data.c index e55b640..ac640c6 100644 --- a/src/survive_data.c +++ b/src/survive_data.c @@ -11,7 +11,14 @@ void handle_lightcap( struct SurviveObject * so, struct LightcapElement * le ) struct SurviveContext * ctx = so->ctx; //int32_t deltat = (uint32_t)le->timestamp - (uint32_t)so->last_master_time; -// printf( "%s %d %d %d %d %d\n", so->codename, le->sensor_id, le->type, le->length, le->timestamp, le->timestamp-so->tsl ); + //if( so->codename[0] != 'H' ) + //printf( "*** %s %d %d %d %d %d\n", so->codename, le->sensor_id, le->type, le->length, le->timestamp, le->timestamp-so->tsl ); + + if( le->sensor_id > SENSORS_PER_OBJECT ) + { + return; + } + so->tsl = le->timestamp; if( le->length < 20 ) return; ///Assuming 20 is an okay value for here. -- cgit v1.2.3 From 0ebe84be5824202aecfd87ac58005afa76b36be9 Mon Sep 17 00:00:00 2001 From: Joshua Allen Date: Mon, 6 Mar 2017 22:05:02 -0500 Subject: Load strings from config files. I need to finish the loading of numeric values and arrays. --- calibrate_client.c | 18 ++++---- redist/json_helpers.c | 114 ++++++++++++++++++++++++++++++++++++++++++++++++-- redist/json_helpers.h | 6 +++ src/survive_cal.c | 2 +- src/survive_config.c | 57 +++++++++++++++++++++++++ src/survive_config.h | 3 +- 6 files changed, 188 insertions(+), 12 deletions(-) diff --git a/calibrate_client.c b/calibrate_client.c index 08f0715..927f40f 100644 --- a/calibrate_client.c +++ b/calibrate_client.c @@ -142,6 +142,17 @@ void * GuiThread( void * v ) int main() { + + config_init(); + config_read("config.json"); + config_set_str(&global_config_values, "Hello","World!"); + const char *s = config_read_str(&global_config_values, "test123","default"); + printf("%s\n", s); + + +// config_save("config.json"); + + ctx = survive_init( 1 ); survive_install_light_fn( ctx, my_light_process ); @@ -159,13 +170,6 @@ int main() CNFGSetup( "Survive GUI Debug", 640, 480 ); OGCreateThread( GuiThread, 0 ); - config_init(); - config_set_str(&global_config_values, "Hello","World!"); - const char *s = config_read_str(&global_config_values, "TestStr","This is a test."); - printf("%s\n", s); - config_save("config.json"); - - if( !ctx ) { fprintf( stderr, "Fatal. Could not start\n" ); diff --git a/redist/json_helpers.c b/redist/json_helpers.c index fd04203..b7ccb40 100644 --- a/redist/json_helpers.c +++ b/redist/json_helpers.c @@ -6,6 +6,8 @@ #include #include #include "json_helpers.h" +#include + void json_write_float_array(FILE* f, const char* tag, float* v, uint8_t count) { uint8_t i = 0; @@ -14,10 +16,10 @@ void json_write_float_array(FILE* f, const char* tag, float* v, uint8_t count) { asprintf(&str1,"\"%s\":[", tag); for (i=0;i=npos) return NULL; + + char* x = malloc(l); + memcpy(x,str+start,l); + x[l-1] = '\0'; + return x; +} + +static uint16_t json_load_array(const char* JSON_STRING, jsmntok_t* tokens, uint16_t size, char* tag) { + jsmntok_t* t = tokens; + uint16_t i = 0; + + char* values[size]; + + for (i=0;istart, t->end, JSON_STRING_LEN); + } + + if (json_tag_value != NULL) json_tag_value(tag, values, i); + + for (i=0;istart, tag_t->end, JSON_STRING_LEN); + char* value = substr(JSON_STRING, value_t->start, value_t->end, JSON_STRING_LEN); + + printf("%d %d c:%d %d %s \n", tag_t->start, tag_t->end, tag_t->size, tag_t->type, tag); + + + if (value_t->type == JSMN_ARRAY) { + i += json_load_array(JSON_STRING, tokens+i+2,value_t->size, tag); //look at array children + } else if (value_t->type == JSMN_OBJECT) { + printf("Begin Object\n"); + if (json_begin_object != NULL) json_begin_object(tag); + children = value_t->size +1; //+1 to account for this loop where we are not yed parsing children +// i += decode_jsmn_object(JSON_STRING, tokens+i+2,value_t->size); + } + else { + if (json_tag_value != NULL) json_tag_value(tag, &value, 1); + } + + if (children>=0) children--; + if (children == 0) { + children = -1; + printf("End Object\n"); + if (json_end_object!=NULL) json_end_object(); + } + +// printf("%d %s \n", value_t->type, tag); + + free(tag); + free(value); + } + + free(JSON_STRING); +} + diff --git a/redist/json_helpers.h b/redist/json_helpers.h index 8d6eb64..1670058 100644 --- a/redist/json_helpers.h +++ b/redist/json_helpers.h @@ -11,4 +11,10 @@ void json_write_uint32(FILE* f, const char* tag, uint32_t v); void json_write_float(FILE* f, const char* tag, float v); void json_write_str(FILE* f, const char* tag, const char* v); +void json_load_file(const char* path); +extern void (*json_begin_object)(char* tag); +extern void (*json_end_object)(); +extern void (*json_tag_value)(char* tag, char** values, uint16_t count); + + #endif \ No newline at end of file diff --git a/src/survive_cal.c b/src/survive_cal.c index f372309..33d1faa 100644 --- a/src/survive_cal.c +++ b/src/survive_cal.c @@ -53,7 +53,7 @@ void ootx_packet_clbk_d(ootx_decoder_context *ct, ootx_packet* packet) b->OOTXSet = 1; config_set_lighthouse(b,id); - config_save("config.json"); +// config_save("config.json"); } int survive_cal_get_status( struct SurviveContext * ctx, char * description, int description_length ) diff --git a/src/survive_config.c b/src/survive_config.c index c46e300..d91596f 100644 --- a/src/survive_config.c +++ b/src/survive_config.c @@ -109,6 +109,14 @@ FLT config_read_float(config_group *cg, const char *tag, const FLT def) { config_entry* next_unused_entry(config_group *cg) { config_entry *cv = cg->config_entries + cg->used_entries; assert(cg->used_entries < cg->max_entries); + +/* + if (cg->used_entries >= cg->max_entries) { + cg->max_entries+=10; + cg->config_entries = realloc(cg->config_entries, sizeof(config_entry)*cg->max_entries); + } +*/ + cg->used_entries++; return cv; } @@ -213,3 +221,52 @@ void config_save(const char* path) { fclose(f); } +void print_json_value(char* tag, char** values, uint16_t count) { + uint16_t i = 0; + for (i=0;i1) config_set_str +} + +void config_read(const char* path) { + json_begin_object = handle_config_group; + json_end_object = pop_config_group; + json_tag_value = handle_tag_value; + + cg_stack[0] = &global_config_values; + + json_load_file(path); + + json_begin_object = NULL; + json_end_object = NULL; + json_tag_value = NULL; +} + diff --git a/src/survive_config.h b/src/survive_config.h index 14e2fc6..2610e2e 100644 --- a/src/survive_config.h +++ b/src/survive_config.h @@ -37,7 +37,8 @@ extern config_group lh_config[2]; //lighthouse configs void config_init(); -void config_open(const char* path, const char* mode); +//void config_open(const char* path, const char* mode); +void config_read(const char* path); void config_close(); //void config_write_lighthouse(struct BaseStationData* bsd, uint8_t length); void config_set_lighthouse(struct BaseStationData* bsd, uint8_t idx); -- cgit v1.2.3 From a7b423ba1f0b28d0e6f3f375475cf93d66b922c9 Mon Sep 17 00:00:00 2001 From: Joshua Allen Date: Mon, 6 Mar 2017 22:21:15 -0500 Subject: remove unused function --- src/survive_config.c | 9 +-------- src/survive_config.h | 1 - 2 files changed, 1 insertion(+), 9 deletions(-) diff --git a/src/survive_config.c b/src/survive_config.c index d91596f..83951fb 100644 --- a/src/survive_config.c +++ b/src/survive_config.c @@ -14,7 +14,7 @@ config_group lh_config[MAX_LIGHTHOUSES]; //lighthouse configs //static uint16_t used_entries = 0; -static FILE *config_file = NULL; +//static FILE *config_file = NULL; const FLT* config_set_float_a(config_group *cg, const char *tag, const FLT* values, uint8_t count); void init_config_group(config_group *cg, uint16_t count) { @@ -39,13 +39,6 @@ void config_init() { } } -void config_load(const char* path) { - config_file = fopen(path, "r"); -} - -void config_close() { - fclose(config_file); -} void config_set_lighthouse(struct BaseStationData* bsd, uint8_t idx) { config_group *cg = lh_config+idx; diff --git a/src/survive_config.h b/src/survive_config.h index 2610e2e..b14f928 100644 --- a/src/survive_config.h +++ b/src/survive_config.h @@ -39,7 +39,6 @@ extern config_group lh_config[2]; //lighthouse configs void config_init(); //void config_open(const char* path, const char* mode); void config_read(const char* path); -void config_close(); //void config_write_lighthouse(struct BaseStationData* bsd, uint8_t length); void config_set_lighthouse(struct BaseStationData* bsd, uint8_t idx); -- cgit v1.2.3 From 90ed96e7b59e60a39f1e1f33223c6ffb0ee833f7 Mon Sep 17 00:00:00 2001 From: Joshua Allen Date: Tue, 7 Mar 2017 20:33:19 -0500 Subject: parse out integers and floats --- calibrate_client.c | 6 ++++ src/survive_config.c | 81 ++++++++++++++++++++++++++++++++++++++++++++++++++-- src/survive_config.h | 1 + 3 files changed, 86 insertions(+), 2 deletions(-) diff --git a/calibrate_client.c b/calibrate_client.c index 927f40f..9867d67 100644 --- a/calibrate_client.c +++ b/calibrate_client.c @@ -149,6 +149,12 @@ int main() const char *s = config_read_str(&global_config_values, "test123","default"); printf("%s\n", s); + FLT *f; + + uint16_t fs = config_read_float_array(lh_config+1, "fcalgibpha", &f, NULL, 0); + printf("%d\n", fs); + printf("===> %f %f\n", f[0], f[1]); + // config_save("config.json"); diff --git a/src/survive_config.c b/src/survive_config.c index 83951fb..d8bad17 100644 --- a/src/survive_config.c +++ b/src/survive_config.c @@ -4,6 +4,8 @@ #include "survive_config.h" #include +#include + #define MAX_CONFIG_ENTRIES 100 #define MAX_LIGHTHOUSES 2 @@ -99,6 +101,21 @@ FLT config_read_float(config_group *cg, const char *tag, const FLT def) { return config_set_float(cg, tag, def); } +uint16_t config_read_float_array(config_group *cg, const char *tag, FLT** values, const FLT* def, uint16_t count) { + config_entry *cv = find_config_entry(cg, tag); + + if (cv != NULL) { + *values = (FLT*)cv->data; + return cv->elements; + } + + if (def == NULL) return 0; + + config_set_float_a(cg, tag, def, count); + *values = def; + return count; +} + config_entry* next_unused_entry(config_group *cg) { config_entry *cv = cg->config_entries + cg->used_entries; assert(cg->used_entries < cg->max_entries); @@ -239,13 +256,73 @@ void pop_config_group() { cg_stack_head--; } + +int parse_floats(char* tag, char** values, uint16_t count) { + uint16_t i = 0; + FLT f[count]; + char* end = NULL; + config_group* cg = cg_stack[cg_stack_head]; + + for(i=0;i1) { + config_set_float_a(cg, tag, f, count); + } + else { + config_set_float(cg, tag, f[0]); + } + + return 1; +} + +int parse_uint32(char* tag, char** values, uint16_t count) { + uint16_t i = 0; + uint32_t l[count]; + char* end = NULL; + config_group* cg = cg_stack[cg_stack_head]; + +/* + //look for non numeric values + for(end=values[0];*end!='\0';++end) { + if ((*end<48) || (*end>57)) return 0; + } + + end=NULL; +*/ + for(i=0;i1) +// config_set_uint32_array(cg, tag, f, count); +// else + config_set_uint32(cg, tag, l[0]); + + return 1; +} + void handle_tag_value(char* tag, char** values, uint16_t count) { print_json_value(tag,values,count); config_group* cg = cg_stack[cg_stack_head]; - //parse out numeric values + if (parse_uint32(tag,values,count) > 0) return; //parse integers first, stricter rules + + if (parse_floats(tag,values,count) > 0) return; - if (count == 1) config_set_str(cg,tag,values[0]); + //should probably also handle string arrays + config_set_str(cg,tag,values[0]); // else if (count>1) config_set_str } diff --git a/src/survive_config.h b/src/survive_config.h index b14f928..234db68 100644 --- a/src/survive_config.h +++ b/src/survive_config.h @@ -48,6 +48,7 @@ const uint32_t config_set_uint32(config_group *cg, const char *tag, const uint32 const char* config_set_str(config_group *cg, const char *tag, const char* value); FLT config_read_float(config_group *cg, const char *tag, const FLT def); +uint16_t config_read_float_array(config_group *cg, const char *tag, FLT** values, const FLT* def, uint16_t count); uint32_t config_read_uint32(config_group *cg, const char *tag, const uint32_t def); const char* config_read_str(config_group *cg, const char *tag, const char *def); -- cgit v1.2.3 From 6613cdc174401379b88f253385e8f82304de42f5 Mon Sep 17 00:00:00 2001 From: Joshua Allen Date: Tue, 7 Mar 2017 21:20:39 -0500 Subject: simplify use of realloc --- src/survive_config.c | 19 ++++++++----------- 1 file changed, 8 insertions(+), 11 deletions(-) diff --git a/src/survive_config.c b/src/survive_config.c index d8bad17..23d7ef1 100644 --- a/src/survive_config.c +++ b/src/survive_config.c @@ -59,11 +59,10 @@ void sstrcpy(char** dest, const char *src) { uint32_t len = strlen(src)+1; assert(dest!=NULL); - if (*dest == NULL) { - *dest = (char*)malloc(len); - } else { - *dest = (char*)realloc(*dest, len); - } + char* ptr = (char*)realloc(*dest, len); //acts like mallos if dest==NULL + assert(ptr!=NULL); + *dest = ptr; + strcpy(*dest,src); } @@ -170,12 +169,10 @@ const FLT* config_set_float_a(config_group *cg, const char *tag, const FLT* valu sstrcpy(&(cv->tag), tag); - if (cv->data == NULL) { - cv->data = (char*)malloc(sizeof(FLT)*count); - } - else { - cv->data = (char*)realloc(cv->data, sizeof(FLT)*count); - } + char* ptr = (char*)realloc(cv->data, sizeof(FLT)*count); + assert(ptr!=NULL); + cv->data = ptr; + printf("float array\n"); memcpy(cv->data,values,sizeof(FLT)*count); -- cgit v1.2.3 From 73405169f773cb4acc807102f3c0ba166214cbdf Mon Sep 17 00:00:00 2001 From: Joshua Allen Date: Tue, 7 Mar 2017 21:21:16 -0500 Subject: dynamically expand config_entries in config_group --- src/survive_config.c | 33 +++++++++++++++++++++++++-------- 1 file changed, 25 insertions(+), 8 deletions(-) diff --git a/src/survive_config.c b/src/survive_config.c index 23d7ef1..b27563c 100644 --- a/src/survive_config.c +++ b/src/survive_config.c @@ -33,6 +33,26 @@ void init_config_group(config_group *cg, uint16_t count) { } } +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); + assert(ptr!=NULL); + + cg->config_entries = ptr; + + for (i=cg->max_entries;iconfig_entries[i].data = NULL; + cg->config_entries[i].tag = NULL; + cg->config_entries[i].type = CONFIG_UNKNOWN; + cg->config_entries[i].elements = 0; + } + + cg->max_entries = count; + } +} + void config_init() { uint16_t i = 0; init_config_group(&global_config_values, MAX_CONFIG_ENTRIES); @@ -116,15 +136,12 @@ uint16_t config_read_float_array(config_group *cg, const char *tag, FLT** values } config_entry* next_unused_entry(config_group *cg) { - config_entry *cv = cg->config_entries + cg->used_entries; - assert(cg->used_entries < cg->max_entries); + config_entry *cv = NULL; +// assert(cg->used_entries < cg->max_entries); -/* - if (cg->used_entries >= cg->max_entries) { - cg->max_entries+=10; - cg->config_entries = realloc(cg->config_entries, sizeof(config_entry)*cg->max_entries); - } -*/ + if (cg->used_entries >= cg->max_entries) resize_config_group(cg, cg->max_entries + 10); + + cv = cg->config_entries + cg->used_entries; cg->used_entries++; return cv; -- cgit v1.2.3 From be3b6becc78ca68287e2570eea292535fc3327ec Mon Sep 17 00:00:00 2001 From: Joshua Allen Date: Tue, 7 Mar 2017 21:24:32 -0500 Subject: dynamically expand config_entries in config_group --- src/survive_config.c | 35 ++++++++++++++++++++++++++--------- 1 file changed, 26 insertions(+), 9 deletions(-) diff --git a/src/survive_config.c b/src/survive_config.c index 23d7ef1..b59bd85 100644 --- a/src/survive_config.c +++ b/src/survive_config.c @@ -33,6 +33,26 @@ void init_config_group(config_group *cg, uint16_t count) { } } +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); + assert(ptr!=NULL); + + cg->config_entries = ptr; + + for (i=cg->max_entries;iconfig_entries[i].data = NULL; + cg->config_entries[i].tag = NULL; + cg->config_entries[i].type = CONFIG_UNKNOWN; + cg->config_entries[i].elements = 0; + } + + cg->max_entries = count; + } +} + void config_init() { uint16_t i = 0; init_config_group(&global_config_values, MAX_CONFIG_ENTRIES); @@ -59,7 +79,7 @@ void sstrcpy(char** dest, const char *src) { uint32_t len = strlen(src)+1; assert(dest!=NULL); - char* ptr = (char*)realloc(*dest, len); //acts like mallos if dest==NULL + char* ptr = (char*)realloc(*dest, len); //acts like malloc if dest==NULL assert(ptr!=NULL); *dest = ptr; @@ -116,15 +136,12 @@ uint16_t config_read_float_array(config_group *cg, const char *tag, FLT** values } config_entry* next_unused_entry(config_group *cg) { - config_entry *cv = cg->config_entries + cg->used_entries; - assert(cg->used_entries < cg->max_entries); + config_entry *cv = NULL; +// assert(cg->used_entries < cg->max_entries); -/* - if (cg->used_entries >= cg->max_entries) { - cg->max_entries+=10; - cg->config_entries = realloc(cg->config_entries, sizeof(config_entry)*cg->max_entries); - } -*/ + if (cg->used_entries >= cg->max_entries) resize_config_group(cg, cg->max_entries + 10); + + cv = cg->config_entries + cg->used_entries; cg->used_entries++; return cv; -- cgit v1.2.3 From 57e82519b4844620851784e7682a2c562cb06d47 Mon Sep 17 00:00:00 2001 From: Joshua Allen Date: Tue, 7 Mar 2017 21:39:40 -0500 Subject: add init and destroy functions for config_entry --- src/survive_config.c | 24 ++++++++++++++---------- 1 file changed, 14 insertions(+), 10 deletions(-) diff --git a/src/survive_config.c b/src/survive_config.c index b59bd85..f30bf87 100644 --- a/src/survive_config.c +++ b/src/survive_config.c @@ -9,8 +9,6 @@ #define MAX_CONFIG_ENTRIES 100 #define MAX_LIGHTHOUSES 2 - - config_group global_config_values; config_group lh_config[MAX_LIGHTHOUSES]; //lighthouse configs @@ -19,6 +17,18 @@ config_group lh_config[MAX_LIGHTHOUSES]; //lighthouse configs //static FILE *config_file = NULL; const FLT* config_set_float_a(config_group *cg, const char *tag, const FLT* values, uint8_t count); +void init_config_entry(config_entry* ce) { + ce->data = NULL; + ce->tag = NULL; + ce->type = CONFIG_UNKNOWN; + ce->elements = 0; +} + +void destroy_config_entry(config_entry* ce) { + if (ce->tag!=NULL) free(ce->tag); + if (ce->data!=NULL) free(ce->data); +} + void init_config_group(config_group *cg, uint16_t count) { uint16_t i = 0; cg->config_entries = malloc(count*sizeof(config_entry)); @@ -26,10 +36,7 @@ void init_config_group(config_group *cg, uint16_t count) { cg->max_entries = count; for (i=0;iconfig_entries[i].data = NULL; - cg->config_entries[i].tag = NULL; - cg->config_entries[i].type = CONFIG_UNKNOWN; - cg->config_entries[i].elements = 0; + init_config_entry(cg->config_entries+i); } } @@ -43,10 +50,7 @@ void resize_config_group(config_group *cg, uint16_t count) { cg->config_entries = ptr; for (i=cg->max_entries;iconfig_entries[i].data = NULL; - cg->config_entries[i].tag = NULL; - cg->config_entries[i].type = CONFIG_UNKNOWN; - cg->config_entries[i].elements = 0; + init_config_entry(cg->config_entries+i); } cg->max_entries = count; -- cgit v1.2.3 From 394cbc465e776137834eea830038b43ea98f6268 Mon Sep 17 00:00:00 2001 From: cnlohr Date: Wed, 8 Mar 2017 00:30:48 -0500 Subject: Switch types over to avoiding extra struct keyword. Switch poses to "SurvivePose" type. --- include/libsurvive/poser.h | 22 ++++++------ include/libsurvive/survive.h | 8 ++--- include/libsurvive/survive_types.h | 17 +++++---- src/survive_cal.c | 6 ++-- src/survive_cal.h | 14 ++++---- src/survive_cal_lhfind.c | 14 ++++---- src/survive_config.c | 5 ++- src/survive_config.h | 4 +-- src/survive_data.c | 4 +-- src/survive_internal.h | 34 ++++++++---------- src/survive_process.c | 12 +++---- src/survive_vive.c | 74 ++++++++++++++++++++------------------ 12 files changed, 109 insertions(+), 105 deletions(-) diff --git a/include/libsurvive/poser.h b/include/libsurvive/poser.h index 4894acf..c514953 100644 --- a/include/libsurvive/poser.h +++ b/include/libsurvive/poser.h @@ -11,22 +11,22 @@ typedef enum PoserType_t POSERDATA_FULL_SCENE, //Full, statified X, Y sweep data for both lighthouses. } PoserType; -struct PoserData +typedef struct { PoserType pt; uint8_t data[0]; -}; +} PoserData; -struct PoserDataIMU +typedef struct { PoserType pt; uint8_t datamask; //0 = accel present, 1 = gyro present, 2 = mag present. FLT accel[3]; FLT gyro[3]; FLT mag[3]; -}; +} PoserDataIMU; -struct PoserDataLight +typedef struct { PoserType pt; int sensor_id; @@ -34,21 +34,21 @@ struct PoserDataLight uint32_t timecode; //In object-local ticks. FLT length; //In seconds FLT angle; //In radians from center of lighthouse. -}; +} PoserDataLight; -struct PoserDataFullScene +typedef struct { PoserType pt; //If "lengths[...]" < 0, means not a valid piece of sweep information. FLT lengths[SENSORS_PER_OBJECT][NUM_LIGHTHOUSES][2]; - FLT angles [SENSORS_PER_OBJECT][NUM_LIGHTHOUSES][2]; //2 Axes + FLT angles [SENSORS_PER_OBJECT][NUM_LIGHTHOUSES][2]; //2 Axes (Angles in LH space) - struct PoserDataIMU lastimu; -}; + PoserDataIMU lastimu; +} PoserDataFullScene; //When you register your posers using the internal system, -typedef int (*PoserCB)( struct SurviveObject * so, struct PoserData * pd ); +typedef int (*PoserCB)( SurviveObject * so, PoserData * pd ); #endif diff --git a/include/libsurvive/survive.h b/include/libsurvive/survive.h index 536c7fb..20e50ca 100644 --- a/include/libsurvive/survive.h +++ b/include/libsurvive/survive.h @@ -4,14 +4,14 @@ #include #include "poser.h" -struct SurviveContext; +typedef struct SurviveContext SurviveContext; //DANGER: This structure may be redefined. Note that it is logically split into 64-bit chunks //for optimization on 32- and 64-bit systems. struct SurviveObject { - struct SurviveContext * ctx; + SurviveContext * ctx; char codename[4]; //3 letters, null-terminated. Currently HMD, WM0, WM1. char drivername[4]; //3 letters for driver. Currently "HTC" @@ -27,8 +27,8 @@ struct SurviveObject //Pose Information, also "resolver" field. FLT PoseConfidence; //0..1 - FLT Position[3]; - FLT Rotation[4]; + SurvivePose OutPose; + SurvivePose FromLHPose[NUM_LIGHTHOUSES]; //Optionally filled out by poser, contains computed position from each lighthouse. void * PoserData; //Initialized to zero, configured by poser, can be anything the poser wants. PoserCB * PoserFn; diff --git a/include/libsurvive/survive_types.h b/include/libsurvive/survive_types.h index 593819f..6137774 100644 --- a/include/libsurvive/survive_types.h +++ b/include/libsurvive/survive_types.h @@ -9,6 +9,11 @@ #endif #endif +typedef struct SurvivePose +{ + FLT Pos[3]; + FLT Rot[4]; +} SurvivePose; //Careful with this, you can't just add another one right now, would take minor changes in survive_data.c and the cal tools. //It will also require a recompile. TODO: revisit this and correct the comment once fixed. @@ -17,13 +22,13 @@ #define INTBUFFSIZE 64 #define SENSORS_PER_OBJECT 32 -struct SurviveObject; -struct SurviveContext; +typedef struct SurviveObject SurviveObject; +typedef struct SurviveContext SurviveContext; -typedef void (*text_feedback_func)( struct SurviveContext * ctx, const char * fault ); -typedef void (*light_process_func)( struct SurviveObject * so, int sensor_id, int acode, int timeinsweep, uint32_t timecode, uint32_t length ); -typedef void (*imu_process_func)( struct SurviveObject * so, int16_t * accelgyro, uint32_t timecode, int id ); -typedef void (*angle_process_func)( struct SurviveObject * so, int sensor_id, int acode, uint32_t timecode, FLT length, FLT angle ); +typedef void (*text_feedback_func)( SurviveContext * ctx, const char * fault ); +typedef void (*light_process_func)( SurviveObject * so, int sensor_id, int acode, int timeinsweep, uint32_t timecode, uint32_t length ); +typedef void (*imu_process_func)( SurviveObject * so, int16_t * accelgyro, uint32_t timecode, int id ); +typedef void (*angle_process_func)( SurviveObject * so, int sensor_id, int acode, uint32_t timecode, FLT length, FLT angle ); #endif diff --git a/src/survive_cal.c b/src/survive_cal.c index 33d1faa..2acf51c 100644 --- a/src/survive_cal.c +++ b/src/survive_cal.c @@ -27,8 +27,8 @@ static void reset_calibration( struct SurviveCalData * cd ); void ootx_packet_clbk_d(ootx_decoder_context *ct, ootx_packet* packet) { - struct SurviveContext * ctx = (struct SurviveContext*)(ct->user); - struct SurviveCalData * cd = ctx->calptr; + SurviveContext * ctx = (SurviveContext*)(ct->user); + SurviveCalData * cd = ctx->calptr; int id = ct->user1; SV_INFO( "Got OOTX packet %d %p", id, cd ); @@ -36,7 +36,7 @@ void ootx_packet_clbk_d(ootx_decoder_context *ct, ootx_packet* packet) lighthouse_info_v6 v6; init_lighthouse_info_v6(&v6, packet->data); - struct BaseStationData * b = &ctx->bsd[id]; + BaseStationData * b = &ctx->bsd[id]; //print_lighthouse_info_v6(&v6); b->BaseStationID = v6.id; diff --git a/src/survive_cal.h b/src/survive_cal.h index bb4eb32..a5e372e 100644 --- a/src/survive_cal.h +++ b/src/survive_cal.h @@ -21,21 +21,21 @@ #include "ootx_decoder.h" #include "survive_internal.h" -void survive_cal_install( struct SurviveContext * ctx ); -int survive_cal_get_status( struct SurviveContext * ctx, char * description, int description_length ); +void survive_cal_install( SurviveContext * ctx ); +int survive_cal_get_status( SurviveContext * ctx, char * description, int description_length ); //void survive_cal_teardown( struct SurviveContext * ctx ); //Called from survive_default_light_process -void survive_cal_light( struct SurviveObject * so, int sensor_id, int acode, int timeinsweep, uint32_t timecode, uint32_t length ); -void survive_cal_angle( struct SurviveObject * so, int sensor_id, int acode, uint32_t timecode, FLT length, FLT angle ); +void survive_cal_light( SurviveObject * so, int sensor_id, int acode, int timeinsweep, uint32_t timecode, uint32_t length ); +void survive_cal_angle( SurviveObject * so, int sensor_id, int acode, uint32_t timecode, FLT length, FLT angle ); #define MAX_SENSORS_TO_CAL 96 #define DRPTS 1024 #define MAX_CAL_PT_DAT (MAX_SENSORS_TO_CAL*NUM_LIGHTHOUSES*2) struct SurviveCalData { - struct SurviveContext * ctx; + SurviveContext * ctx; //OOTX Data is sync'd off of the sync pulses coming from the lighthouses. ootx_decoder_context ootx_decoders[NUM_LIGHTHOUSES]; @@ -57,7 +57,7 @@ struct SurviveCalData int senid_of_checkpt; //This is a point on a watchman that can be used to check the lh solution. - struct SurviveObject * hmd; + SurviveObject * hmd; //Stage: // 0: Idle @@ -68,7 +68,7 @@ struct SurviveCalData //The following function is not included in the core survive_cal and must be compiled from a camfind file. //It should use data for stage 4 and report if it found the -int survive_cal_lhfind( struct SurviveCalData * cd ); +int survive_cal_lhfind( SurviveCalData * cd ); #endif diff --git a/src/survive_cal_lhfind.c b/src/survive_cal_lhfind.c index 93d9dc0..a1bb2cc 100644 --- a/src/survive_cal_lhfind.c +++ b/src/survive_cal_lhfind.c @@ -7,13 +7,13 @@ #define MIN_HITS_FOR_VALID 10 -FLT static RunOpti( struct SurviveCalData * cd, int lh, int print, FLT * LighthousePos, FLT * LighthouseQuat ); +FLT static RunOpti( SurviveCalData * cd, int lh, int print, FLT * LighthousePos, FLT * LighthouseQuat ); //Values used for RunTest() -int survive_cal_lhfind( struct SurviveCalData * cd ) +int survive_cal_lhfind( SurviveCalData * cd ) { - struct SurviveContext * ctx = cd->ctx; + SurviveContext * ctx = cd->ctx; int cycle, i; int lh = 0; FLT dx, dy, dz; @@ -137,8 +137,8 @@ int survive_cal_lhfind( struct SurviveCalData * cd ) } cd->ctx->bsd[lh].PositionSet = 1; - copy3d( cd->ctx->bsd[lh].Position, LighthousePos ); - quatcopy( cd->ctx->bsd[lh].Quaternion, LighthouseQuat ); + copy3d( cd->ctx->bsd[lh].Pose.Pos, LighthousePos ); + quatcopy( cd->ctx->bsd[lh].Pose.Rot, LighthouseQuat ); } return 0; //Return 0 if success. @@ -149,14 +149,14 @@ int survive_cal_lhfind( struct SurviveCalData * cd ) -static FLT RunOpti( struct SurviveCalData * cd, int lh, int print, FLT * LighthousePos, FLT * LighthouseQuat ) +static FLT RunOpti( SurviveCalData * cd, int lh, int print, FLT * LighthousePos, FLT * LighthouseQuat ) { int i, p; FLT UsToTarget[3]; FLT LastUsToTarget[3]; FLT mux = .9; quatsetnone( LighthouseQuat ); - struct SurviveObject * hmd = cd->hmd; + SurviveObject * hmd = cd->hmd; FLT * hmd_points = hmd->sensor_locations; FLT * hmd_normals = hmd->sensor_normals; diff --git a/src/survive_config.c b/src/survive_config.c index f30bf87..352a15b 100644 --- a/src/survive_config.c +++ b/src/survive_config.c @@ -66,12 +66,11 @@ void config_init() { } -void config_set_lighthouse(struct BaseStationData* bsd, uint8_t idx) { +void config_set_lighthouse(BaseStationData* bsd, uint8_t idx) { config_group *cg = lh_config+idx; config_set_uint32(cg,"index", idx); config_set_uint32(cg,"id", bsd->BaseStationID); - config_set_float_a(cg,"position", bsd->Position, 3); - config_set_float_a(cg,"quaternion", bsd->Quaternion, 4); + config_set_float_a(cg,"pose", &bsd->Pose.Pos[0], 7); config_set_float_a(cg,"fcalphase", bsd->fcalphase, 2); config_set_float_a(cg,"fcaltilt", bsd->fcaltilt,2); config_set_float_a(cg,"fcalcurve", bsd->fcalcurve,2); diff --git a/src/survive_config.h b/src/survive_config.h index 234db68..cd16c76 100644 --- a/src/survive_config.h +++ b/src/survive_config.h @@ -40,7 +40,7 @@ void config_init(); //void config_open(const char* path, const char* mode); void config_read(const char* path); //void config_write_lighthouse(struct BaseStationData* bsd, uint8_t length); -void config_set_lighthouse(struct BaseStationData* bsd, uint8_t idx); +void config_set_lighthouse(BaseStationData* bsd, uint8_t idx); void config_save(const char* path); const FLT config_set_float(config_group *cg, const char *tag, const FLT value); @@ -52,4 +52,4 @@ uint16_t config_read_float_array(config_group *cg, const char *tag, FLT** values uint32_t config_read_uint32(config_group *cg, const char *tag, const uint32_t def); const char* config_read_str(config_group *cg, const char *tag, const char *def); -#endif \ No newline at end of file +#endif diff --git a/src/survive_data.c b/src/survive_data.c index ac640c6..63cc5c2 100644 --- a/src/survive_data.c +++ b/src/survive_data.c @@ -6,9 +6,9 @@ #include //This is the disambiguator function, for taking light timing and figuring out place-in-sweep for a given photodiode. -void handle_lightcap( struct SurviveObject * so, struct LightcapElement * le ) +void handle_lightcap( SurviveObject * so, LightcapElement * le ) { - struct SurviveContext * ctx = so->ctx; + SurviveContext * ctx = so->ctx; //int32_t deltat = (uint32_t)le->timestamp - (uint32_t)so->last_master_time; //if( so->codename[0] != 'H' ) diff --git a/src/survive_internal.h b/src/survive_internal.h index 9d04d93..83a429c 100644 --- a/src/survive_internal.h +++ b/src/survive_internal.h @@ -29,20 +29,20 @@ struct SurviveContext; -struct SurviveUSBInterface; +struct SurvivecalData; typedef int (*DeviceDriverCb)( struct SurviveContext * ctx, void * driver ); typedef int (*DeviceDriverMagicCb)( struct SurviveContext * ctx, void * driver, int magic_code, void * data, int datalen ); //This is defined in survive.h -struct SurviveObject; -struct SurviveCalData; +typedef struct SurviveObject SurviveObject; +typedef struct SurviveCalData SurviveCalData; -struct BaseStationData +typedef struct { uint8_t PositionSet:1; - FLT Position[3]; - FLT Quaternion[4]; + + SurvivePose Pose; uint8_t OOTXSet:1; uint32_t BaseStationID; @@ -51,7 +51,7 @@ struct BaseStationData FLT fcalcurve[2]; FLT fcalgibpha[2]; FLT fcalgibmag[2]; -}; +} BaseStationData; struct SurviveContext { @@ -62,11 +62,11 @@ struct SurviveContext angle_process_func angleproc; //Calibration data: - struct BaseStationData bsd[NUM_LIGHTHOUSES]; + BaseStationData bsd[NUM_LIGHTHOUSES]; - struct SurviveCalData * calptr; //If and only if the calibration subsystem is attached. + SurviveCalData * calptr; //If and only if the calibration subsystem is attached. - struct SurviveObject ** objs; + SurviveObject ** objs; int objs_ct; void ** drivers; @@ -76,25 +76,21 @@ struct SurviveContext int driver_ct; }; -int survive_add_object( struct SurviveContext * ctx, struct SurviveObject * obj ); +int survive_add_object( SurviveContext * ctx, SurviveObject * obj ); -void survive_add_driver( struct SurviveContext * ctx, void * payload, DeviceDriverCb poll, DeviceDriverCb close, DeviceDriverMagicCb magic ); +void survive_add_driver( SurviveContext * ctx, void * payload, DeviceDriverCb poll, DeviceDriverCb close, DeviceDriverMagicCb magic ); //For lightcap, etc. Don't change this structure at all. Regular vive is dependent on it being exactly as-is. -struct LightcapElement +typedef struct { uint8_t sensor_id; uint8_t type; uint16_t length; uint32_t timestamp; -} __attribute__((packed)); +} __attribute__((packed)) LightcapElement; //This is the disambiguator function, for taking light timing and figuring out place-in-sweep for a given photodiode. -void handle_lightcap( struct SurviveObject * so, struct LightcapElement * le ); - - -//Accept Data from backend. -void survive_data_cb( struct SurviveUSBInterface * si ); +void handle_lightcap( SurviveObject * so, LightcapElement * le ); #endif diff --git a/src/survive_process.c b/src/survive_process.c index 75453da..edabfff 100644 --- a/src/survive_process.c +++ b/src/survive_process.c @@ -6,9 +6,9 @@ //XXX TODO: Once data is avialble in the context, use the stuff here to handle converting from time codes to //proper angles, then from there perform the rest of the solution. -void survive_default_light_process( struct SurviveObject * so, int sensor_id, int acode, int timeinsweep, uint32_t timecode, uint32_t length ) +void survive_default_light_process( SurviveObject * so, int sensor_id, int acode, int timeinsweep, uint32_t timecode, uint32_t length ) { - struct SurviveContext * ctx = so->ctx; + SurviveContext * ctx = so->ctx; int base_station = acode >> 2; int axis = acode & 1; @@ -25,7 +25,7 @@ void survive_default_light_process( struct SurviveObject * so, int sensor_id, in //Need to now do angle correction. #if 1 - struct BaseStationData * bsd = &ctx->bsd[base_station]; + BaseStationData * bsd = &ctx->bsd[base_station]; //XXX TODO: This seriously needs to be worked on. See: https://github.com/cnlohr/libsurvive/issues/18 angle += bsd->fcalphase[axis]; @@ -39,9 +39,9 @@ void survive_default_light_process( struct SurviveObject * so, int sensor_id, in } -void survive_default_angle_process( struct SurviveObject * so, int sensor_id, int acode, uint32_t timecode, FLT length, FLT angle ) +void survive_default_angle_process( SurviveObject * so, int sensor_id, int acode, uint32_t timecode, FLT length, FLT angle ) { - struct SurviveContext * ctx = so->ctx; + SurviveContext * ctx = so->ctx; if( ctx->calptr ) { survive_cal_angle( so, sensor_id, acode, timecode, length, angle ); @@ -51,7 +51,7 @@ void survive_default_angle_process( struct SurviveObject * so, int sensor_id, in } -void survive_default_imu_process( struct SurviveObject * so, int16_t * accelgyro, uint32_t timecode, int id ) +void survive_default_imu_process( SurviveObject * so, int16_t * accelgyro, uint32_t timecode, int id ) { //TODO: Writeme! } diff --git a/src/survive_vive.c b/src/survive_vive.c index 47886c9..6be5114 100644 --- a/src/survive_vive.c +++ b/src/survive_vive.c @@ -24,13 +24,6 @@ struct SurviveViveData; -//USB Subsystem -void survive_usb_close( struct SurviveContext * t ); -int survive_usb_init( struct SurviveViveData * sv, struct SurviveObject * hmd, struct SurviveObject *wm0, struct SurviveObject * wm1 ); -int survive_usb_poll( struct SurviveContext * ctx ); -int survive_get_config( char ** config, struct SurviveViveData * ctx, int devno, int interface, int send_extra_magic ); - - const short vidpids[] = { 0x0bb4, 0x2c87, 0, //The main HTC HMD device 0x28de, 0x2000, 0, //Valve lighthouse @@ -60,15 +53,18 @@ const char * devnames[] = { #define USB_IF_LIGHTCAP 4 #define MAX_INTERFACES 5 -typedef void (*usb_callback)( struct SurviveUSBInterface * ti ); +typedef struct SurviveUSBInterface SurviveUSBInterface; +typedef struct SurviveViveData SurviveViveData; + +typedef void (*usb_callback)( SurviveUSBInterface * ti ); struct SurviveUSBInterface { - struct SurviveViveData * sv; - struct SurviveContext * ctx; + SurviveViveData * sv; + SurviveContext * ctx; struct libusb_transfer * transfer; - struct SurviveObject * assoc_obj; + SurviveObject * assoc_obj; int actual_len; uint8_t buffer[INTBUFFSIZE]; usb_callback cb; @@ -78,16 +74,24 @@ struct SurviveUSBInterface struct SurviveViveData { - struct SurviveContext * ctx; + SurviveContext * ctx; //XXX TODO: UN-STATICIFY THIS. - struct SurviveUSBInterface uiface[MAX_INTERFACES]; + SurviveUSBInterface uiface[MAX_INTERFACES]; //USB Subsystem struct libusb_context* usbctx; struct libusb_device_handle * udev[MAX_USB_DEVS]; }; +void survive_data_cb( SurviveUSBInterface * si ); + +//USB Subsystem +void survive_usb_close( SurviveContext * t ); +int survive_usb_init( SurviveViveData * sv, SurviveObject * hmd, SurviveObject *wm0, SurviveObject * wm1 ); +int survive_usb_poll( SurviveContext * ctx ); +int survive_get_config( char ** config, SurviveViveData * ctx, int devno, int interface, int send_extra_magic ); + @@ -115,10 +119,10 @@ static void handle_transfer(struct libusb_transfer* transfer) -static int AttachInterface( struct SurviveViveData * sv, struct SurviveObject * assocobj, int which_interface_am_i, libusb_device_handle * devh, int endpoint, usb_callback cb, const char * hname ) +static int AttachInterface( SurviveViveData * sv, SurviveObject * assocobj, int which_interface_am_i, libusb_device_handle * devh, int endpoint, usb_callback cb, const char * hname ) { - struct SurviveContext * ctx = sv->ctx; - struct SurviveUSBInterface * iface = &sv->uiface[which_interface_am_i]; + SurviveContext * ctx = sv->ctx; + SurviveUSBInterface * iface = &sv->uiface[which_interface_am_i]; iface->ctx = ctx; iface->sv = sv; iface->which_interface_am_i = which_interface_am_i; @@ -680,7 +684,7 @@ static void handle_watchman( struct SurviveObject * w, uint8_t * readdata ) } - struct LightcapElement les[10]; + LightcapElement les[10]; int lese = 0; //les's end //Second, go through all LEDs and extract the lightevent from them. @@ -711,7 +715,7 @@ static void handle_watchman( struct SurviveObject * w, uint8_t * readdata ) //reverse sorted, but that is to minimize operations. To read it //in sorted order simply read it back backwards. //Use insertion sort, since we should most of the time, be in order. - struct LightcapElement * le = &les[lese++]; + LightcapElement * le = &les[lese++]; le->sensor_id = led; le->type = 0xfe; @@ -725,7 +729,7 @@ static void handle_watchman( struct SurviveObject * w, uint8_t * readdata ) int swap = lese-2; while( swap >= 0 && les[swap].timestamp < les[swap+1].timestamp ) { - struct LightcapElement l; + LightcapElement l; memcpy( &l, &les[swap], sizeof( l ) ); memcpy( &les[swap], &les[swap+1], sizeof( l ) ); memcpy( &les[swap+1], &l, sizeof( l ) ); @@ -744,17 +748,17 @@ static void handle_watchman( struct SurviveObject * w, uint8_t * readdata ) return; end: { - struct SurviveContext * ctx = w->ctx; + SurviveContext * ctx = w->ctx; SV_INFO( "Light decoding fault: %d\n", fault ); } } } -void survive_data_cb( struct SurviveUSBInterface * si ) +void survive_data_cb( SurviveUSBInterface * si ) { int size = si->actual_len; - struct SurviveContext * ctx = si->ctx; + SurviveContext * ctx = si->ctx; #if 0 int i; printf( "%16s: %d: ", si->hname, len ); @@ -767,7 +771,7 @@ void survive_data_cb( struct SurviveUSBInterface * si ) #endif int iface = si->which_interface_am_i; - struct SurviveObject * obj = si->assoc_obj; + SurviveObject * obj = si->assoc_obj; uint8_t * readdata = si->buffer; int id = POP1; @@ -818,7 +822,7 @@ void survive_data_cb( struct SurviveUSBInterface * si ) case USB_IF_WATCHMAN1: case USB_IF_WATCHMAN2: { - struct SurviveObject * w = obj; + SurviveObject * w = obj; if( id == 35 ) { handle_watchman( w, readdata); @@ -844,7 +848,7 @@ void survive_data_cb( struct SurviveUSBInterface * si ) int i; for( i = 0; i < 7; i++ ) { - handle_lightcap( obj, (struct LightcapElement*)&readdata[i*8] ); + handle_lightcap( obj, (LightcapElement*)&readdata[i*8] ); } break; } @@ -875,7 +879,7 @@ static int jsoneq(const char *json, jsmntok_t *tok, const char *s) { } -static int ParsePoints( struct SurviveContext * ctx, struct SurviveObject * so, char * ct0conf, FLT ** floats_out, jsmntok_t * t, int i ) +static int ParsePoints( SurviveContext * ctx, SurviveObject * so, char * ct0conf, FLT ** floats_out, jsmntok_t * t, int i ) { int k; int pts = t[i+1].size; @@ -914,9 +918,9 @@ static int ParsePoints( struct SurviveContext * ctx, struct SurviveObject * so, return 0; } -static int LoadConfig( struct SurviveViveData * sv, struct SurviveObject * so, int devno, int iface, int extra_magic ) +static int LoadConfig( SurviveViveData * sv, SurviveObject * so, int devno, int iface, int extra_magic ) { - struct SurviveContext * ctx = sv->ctx; + SurviveContext * ctx = sv->ctx; char * ct0conf = 0; int len = survive_get_config( &ct0conf, sv, devno, iface, extra_magic ); @@ -1002,21 +1006,21 @@ static int LoadConfig( struct SurviveViveData * sv, struct SurviveObject * so, i } -int survive_vive_close( struct SurviveContext * ctx, void * driver ) +int survive_vive_close( SurviveContext * ctx, void * driver ) { - struct SurviveViveData * sv = driver; + SurviveViveData * sv = driver; survive_vive_usb_close( sv ); } -int DriverRegHTCVive( struct SurviveContext * ctx ) +int DriverRegHTCVive( SurviveContext * ctx ) { int i, r; - struct SurviveObject * hmd = calloc( 1, sizeof( struct SurviveObject ) ); - struct SurviveObject * wm0 = calloc( 1, sizeof( struct SurviveObject ) ); - struct SurviveObject * wm1 = calloc( 1, sizeof( struct SurviveObject ) ); - struct SurviveViveData * sv = calloc( 1, sizeof( struct SurviveViveData ) ); + SurviveObject * hmd = calloc( 1, sizeof( SurviveObject ) ); + SurviveObject * wm0 = calloc( 1, sizeof( SurviveObject ) ); + SurviveObject * wm1 = calloc( 1, sizeof( SurviveObject ) ); + SurviveViveData * sv = calloc( 1, sizeof( SurviveViveData ) ); sv->ctx = ctx; -- cgit v1.2.3 From b960e4d04b49bee0f60a5b1d1199c256cfbc17d6 Mon Sep 17 00:00:00 2001 From: cnlohr Date: Wed, 8 Mar 2017 00:34:00 -0500 Subject: apparently quat code had been wrong for quite some time. Luckily it was code that would rarely execute. --- redist/linmath.c | 7 +------ 1 file changed, 1 insertion(+), 6 deletions(-) diff --git a/redist/linmath.c b/redist/linmath.c index 157141f..1c5c25b 100644 --- a/redist/linmath.c +++ b/redist/linmath.c @@ -93,11 +93,6 @@ void quatsetnone(FLT * q) q[0] = 1; q[1] = 0; q[2] = 0; q[3] = 0; } -void quatsetidentity(FLT * q) -{ - q[0] = 1; q[1] = 0; q[2] = 0; q[3] = 1; -} - void quatcopy(FLT * qout, const FLT * qin) { qout[0] = qin[0]; @@ -447,7 +442,7 @@ void quatfrom2vectors(FLT *q, const FLT *src, const FLT *dest) // If dot == 1, vectors are the same if (d >= 1.0f) { - quatsetidentity(q); + quatsetnone(q); return; } if (d < (1e-6f - 1.0f)) -- cgit v1.2.3 From a087915c5dd8c356911453a019723ebaa9904a36 Mon Sep 17 00:00:00 2001 From: cnlohr Date: Wed, 8 Mar 2017 00:53:28 -0500 Subject: Move most of the libsurvive stuff into a common area. --- include/libsurvive/survive.h | 101 ++++++++++++++++++++++++++++++------- include/libsurvive/survive_types.h | 10 ++++ src/survive_driverman.h | 24 ++------- src/survive_internal.h | 75 --------------------------- 4 files changed, 98 insertions(+), 112 deletions(-) diff --git a/include/libsurvive/survive.h b/include/libsurvive/survive.h index 20e50ca..01b5e2b 100644 --- a/include/libsurvive/survive.h +++ b/include/libsurvive/survive.h @@ -2,10 +2,9 @@ #define _SURVIVE_H #include +#include "survive_types.h" #include "poser.h" -typedef struct SurviveContext SurviveContext; - //DANGER: This structure may be redefined. Note that it is logically split into 64-bit chunks //for optimization on 32- and 64-bit systems. @@ -25,7 +24,7 @@ struct SurviveObject int8_t ison:1; int8_t additional_flags:6; - //Pose Information, also "resolver" field. + //Pose Information, also "poser" field. FLT PoseConfidence; //0..1 SurvivePose OutPose; SurvivePose FromLHPose[NUM_LIGHTHOUSES]; //Optionally filled out by poser, contains computed position from each lighthouse. @@ -62,34 +61,102 @@ struct SurviveObject int tsl; }; -struct SurviveContext * survive_init( int headless ); + +struct BaseStationData +{ + uint8_t PositionSet:1; + + SurvivePose Pose; + + uint8_t OOTXSet:1; + uint32_t BaseStationID; + FLT fcalphase[2]; + FLT fcaltilt[2]; + FLT fcalcurve[2]; + FLT fcalgibpha[2]; + FLT fcalgibmag[2]; +}; + +struct SurviveContext +{ + text_feedback_func faultfunction; + text_feedback_func notefunction; + light_process_func lightproc; + imu_process_func imuproc; + angle_process_func angleproc; + + //Calibration data: + BaseStationData bsd[NUM_LIGHTHOUSES]; + + SurviveCalData * calptr; //If and only if the calibration subsystem is attached. + + SurviveObject ** objs; + int objs_ct; + + void ** drivers; + DeviceDriverCb * driverpolls; + DeviceDriverCb * drivercloses; + DeviceDriverMagicCb * drivermagics; + int driver_ct; +}; + +SurviveContext * survive_init( int headless ); //For any of these, you may pass in 0 for the function pointer to use default behavior. -void survive_install_info_fn( struct SurviveContext * ctx, text_feedback_func fbp ); -void survive_install_error_fn( struct SurviveContext * ctx, text_feedback_func fbp ); -void survive_install_light_fn( struct SurviveContext * ctx, light_process_func fbp ); -void survive_install_imu_fn( struct SurviveContext * ctx, imu_process_func fbp ); -void survive_install_angle_fn( struct SurviveContext * ctx, angle_process_func fbp ); +void survive_install_info_fn( SurviveContext * ctx, text_feedback_func fbp ); +void survive_install_error_fn( SurviveContext * ctx, text_feedback_func fbp ); +void survive_install_light_fn( SurviveContext * ctx, light_process_func fbp ); +void survive_install_imu_fn( SurviveContext * ctx, imu_process_func fbp ); +void survive_install_angle_fn( SurviveContext * ctx, angle_process_func fbp ); -void survive_close( struct SurviveContext * ctx ); +void survive_close( SurviveContext * ctx ); int survive_poll(); -struct SurviveObject * survive_get_so_by_name( struct SurviveContext * ctx, const char * name ); +SurviveObject * survive_get_so_by_name( SurviveContext * ctx, const char * name ); //Utilitiy functions. -int survive_simple_inflate( struct SurviveContext * ctx, const char * input, int inlen, char * output, int outlen ); +int survive_simple_inflate( SurviveContext * ctx, const char * input, int inlen, char * output, int outlen ); -int survive_send_magic( struct SurviveContext * ctx, int magic_code, void * data, int datalen ); +int survive_send_magic( SurviveContext * ctx, int magic_code, void * data, int datalen ); //Install the calibrator. -void survive_cal_install( struct SurviveContext * ctx ); +void survive_cal_install( SurviveContext * ctx ); //Call these from your callback if overridden. //Accept higher-level data. -void survive_default_light_process( struct SurviveObject * so, int sensor_id, int acode, int timeinsweep, uint32_t timecode, uint32_t length ); -void survive_default_imu_process( struct SurviveObject * so, int16_t * accelgyro, uint32_t timecode, int id ); -void survive_default_angle_process( struct SurviveObject * so, int sensor_id, int acode, uint32_t timecode, FLT length, FLT angle ); +void survive_default_light_process( SurviveObject * so, int sensor_id, int acode, int timeinsweep, uint32_t timecode, uint32_t length ); +void survive_default_imu_process( SurviveObject * so, int16_t * accelgyro, uint32_t timecode, int id ); +void survive_default_angle_process( SurviveObject * so, int sensor_id, int acode, uint32_t timecode, FLT length, FLT angle ); + + +////////////////////// Survive Drivers //////////////////////////// + +void RegisterDriver( const char * name, void * data ); +void * GetDriver( const char * name ); +const char * GetDriverNameMatching( const char * prefix, int place ); +void ListDrivers(); +#define REGISTER_LINKTIME( func ) \ + void __attribute__((constructor)) Register##func() { RegisterDriver( #func, &func ); } + +int survive_add_object( SurviveContext * ctx, SurviveObject * obj ); +void survive_add_driver( SurviveContext * ctx, void * payload, DeviceDriverCb poll, DeviceDriverCb close, DeviceDriverMagicCb magic ); + + +////////////////////// Lightcap driver data /////////////////////// + +//For lightcap, etc. Don't change this structure at all. Regular vive is dependent on it being exactly as-is. +//When you write drivers, you can use this to send survive lightcap data. +typedef struct +{ + uint8_t sensor_id; + uint8_t type; //Mostly unused. Set to 255 to ignore it. + uint16_t length; + uint32_t timestamp; +} __attribute__((packed)) LightcapElement; + +//This is the disambiguator function, for taking light timing and figuring out place-in-sweep for a given photodiode. +void handle_lightcap( SurviveObject * so, LightcapElement * le ); #endif diff --git a/include/libsurvive/survive_types.h b/include/libsurvive/survive_types.h index 6137774..f235330 100644 --- a/include/libsurvive/survive_types.h +++ b/include/libsurvive/survive_types.h @@ -24,11 +24,21 @@ typedef struct SurvivePose typedef struct SurviveObject SurviveObject; typedef struct SurviveContext SurviveContext; +typedef struct BaseStationData BaseStationData; +typedef struct SurviveCalData SurviveCalData; //XXX Warning: This may be removed. Check at a later time for its defunctness. typedef void (*text_feedback_func)( SurviveContext * ctx, const char * fault ); typedef void (*light_process_func)( SurviveObject * so, int sensor_id, int acode, int timeinsweep, uint32_t timecode, uint32_t length ); typedef void (*imu_process_func)( SurviveObject * so, int16_t * accelgyro, uint32_t timecode, int id ); typedef void (*angle_process_func)( SurviveObject * so, int sensor_id, int acode, uint32_t timecode, FLT length, FLT angle ); + +//Device drivers (prefix your drivers with "DriverReg") i.e. +// REGISTER_LINKTIME( DriverRegHTCVive ); +typedef int (*DeviceDriver)( SurviveContext * ctx ); +typedef int (*DeviceDriverCb)( struct SurviveContext * ctx, void * driver ); +typedef int (*DeviceDriverMagicCb)( struct SurviveContext * ctx, void * driver, int magic_code, void * data, int datalen ); + + #endif diff --git a/src/survive_driverman.h b/src/survive_driverman.h index 5e13caf..216333a 100644 --- a/src/survive_driverman.h +++ b/src/survive_driverman.h @@ -10,29 +10,13 @@ #ifndef SURVIVE_DRIVERMAN_H #define SURVIVE_DRIVERMAN_H -//Driver registration -#define MAX_DRIVERS 32 - -void RegisterDriver( const char * name, void * data ); -void * GetDriver( const char * name ); -const char * GetDriverNameMatching( const char * prefix, int place ); -void ListDrivers(); - -#define REGISTER_LINKTIME( func ) \ - void __attribute__((constructor)) Register##func() { RegisterDriver( #func, &func ); } - +#include "survive.h" -// -// Specific types of drivers. -// - -struct SurviveContext; +//Very little here. Mostly included in survive.h. -//Device drivers (prefix your drivers with "DriverReg") i.e. -// REGISTER_LINKTIME( DriverRegHTCVive ); -typedef int (*DeviceDriver)( struct SurviveContext * ctx ); +//Driver registration +#define MAX_DRIVERS 32 -//more driver types here? i.e. posefinders, etc. #endif diff --git a/src/survive_internal.h b/src/survive_internal.h index 83a429c..ff156ad 100644 --- a/src/survive_internal.h +++ b/src/survive_internal.h @@ -1,15 +1,5 @@ //<>< (C) 2016-2017 C. N. Lohr, MOSTLY Under MIT/x11 License. // -//Based off of https://github.com/collabora/OSVR-Vive-Libre -// Originally Copyright 2016 Philipp Zabel -// Originally Copyright 2016 Lubosz Sarnecki -// Originally Copyright (C) 2013 Fredrik Hultin -// Originally Copyright (C) 2013 Jakob Bornecrantz -// -//But, re-written as best as I can to get it put under an open souce license instead of a forced-source license. -//If there are portions of the code too similar to the original, I would like to know so they can be re-written. -//All MIT/x11 Licensed Code in this file may be relicensed freely under the GPL or LGPL licenses. - #ifndef _SURVIVE_INTERNAL_H #define _SURVIVE_INTERNAL_H @@ -28,71 +18,6 @@ #define SV_KILL() exit(0) -struct SurviveContext; -struct SurvivecalData; - -typedef int (*DeviceDriverCb)( struct SurviveContext * ctx, void * driver ); -typedef int (*DeviceDriverMagicCb)( struct SurviveContext * ctx, void * driver, int magic_code, void * data, int datalen ); - -//This is defined in survive.h -typedef struct SurviveObject SurviveObject; -typedef struct SurviveCalData SurviveCalData; - -typedef struct -{ - uint8_t PositionSet:1; - - SurvivePose Pose; - - uint8_t OOTXSet:1; - uint32_t BaseStationID; - FLT fcalphase[2]; - FLT fcaltilt[2]; - FLT fcalcurve[2]; - FLT fcalgibpha[2]; - FLT fcalgibmag[2]; -} BaseStationData; - -struct SurviveContext -{ - text_feedback_func faultfunction; - text_feedback_func notefunction; - light_process_func lightproc; - imu_process_func imuproc; - angle_process_func angleproc; - - //Calibration data: - BaseStationData bsd[NUM_LIGHTHOUSES]; - - SurviveCalData * calptr; //If and only if the calibration subsystem is attached. - - SurviveObject ** objs; - int objs_ct; - - void ** drivers; - DeviceDriverCb * driverpolls; - DeviceDriverCb * drivercloses; - DeviceDriverMagicCb * drivermagics; - int driver_ct; -}; - -int survive_add_object( SurviveContext * ctx, SurviveObject * obj ); - -void survive_add_driver( SurviveContext * ctx, void * payload, DeviceDriverCb poll, DeviceDriverCb close, DeviceDriverMagicCb magic ); - -//For lightcap, etc. Don't change this structure at all. Regular vive is dependent on it being exactly as-is. -typedef struct -{ - uint8_t sensor_id; - uint8_t type; - uint16_t length; - uint32_t timestamp; -} __attribute__((packed)) LightcapElement; - -//This is the disambiguator function, for taking light timing and figuring out place-in-sweep for a given photodiode. -void handle_lightcap( SurviveObject * so, LightcapElement * le ); - - #endif -- cgit v1.2.3 From 2a4a803b2f162692ca11e700b32da0a77049bfd2 Mon Sep 17 00:00:00 2001 From: cnlohr Date: Wed, 8 Mar 2017 01:01:42 -0500 Subject: cleaning up more header stuff --- include/libsurvive/survive.h | 18 +++++++++++------- src/survive.c | 1 - src/survive_driverman.c | 2 +- src/survive_driverman.h | 22 ---------------------- src/survive_internal.h | 10 +++++----- src/survive_vive.c | 8 +++----- 6 files changed, 20 insertions(+), 41 deletions(-) delete mode 100644 src/survive_driverman.h diff --git a/include/libsurvive/survive.h b/include/libsurvive/survive.h index 01b5e2b..c35e43c 100644 --- a/include/libsurvive/survive.h +++ b/include/libsurvive/survive.h @@ -103,6 +103,7 @@ struct SurviveContext SurviveContext * survive_init( int headless ); //For any of these, you may pass in 0 for the function pointer to use default behavior. +//In general unless you are doing wacky things like recording or playing back data, you won't need to use this. void survive_install_info_fn( SurviveContext * ctx, text_feedback_func fbp ); void survive_install_error_fn( SurviveContext * ctx, text_feedback_func fbp ); void survive_install_light_fn( SurviveContext * ctx, light_process_func fbp ); @@ -120,7 +121,7 @@ int survive_simple_inflate( SurviveContext * ctx, const char * input, int inlen, int survive_send_magic( SurviveContext * ctx, int magic_code, void * data, int datalen ); //Install the calibrator. -void survive_cal_install( SurviveContext * ctx ); +void survive_cal_install( SurviveContext * ctx ); //XXX This will be removed if not already done so. //Call these from your callback if overridden. //Accept higher-level data. @@ -132,18 +133,17 @@ void survive_default_angle_process( SurviveObject * so, int sensor_id, int acode ////////////////////// Survive Drivers //////////////////////////// void RegisterDriver( const char * name, void * data ); -void * GetDriver( const char * name ); -const char * GetDriverNameMatching( const char * prefix, int place ); -void ListDrivers(); #define REGISTER_LINKTIME( func ) \ void __attribute__((constructor)) Register##func() { RegisterDriver( #func, &func ); } -int survive_add_object( SurviveContext * ctx, SurviveObject * obj ); -void survive_add_driver( SurviveContext * ctx, void * payload, DeviceDriverCb poll, DeviceDriverCb close, DeviceDriverMagicCb magic ); -////////////////////// Lightcap driver data /////////////////////// +///////////////////////// General stuff for writing drivers /////// + +//For device drivers to call. This actually attaches them. +int survive_add_object( SurviveContext * ctx, SurviveObject * obj ); +void survive_add_driver( SurviveContext * ctx, void * payload, DeviceDriverCb poll, DeviceDriverCb close, DeviceDriverMagicCb magic ); //For lightcap, etc. Don't change this structure at all. Regular vive is dependent on it being exactly as-is. //When you write drivers, you can use this to send survive lightcap data. @@ -158,5 +158,9 @@ typedef struct //This is the disambiguator function, for taking light timing and figuring out place-in-sweep for a given photodiode. void handle_lightcap( SurviveObject * so, LightcapElement * le ); +#define SV_INFO( x... ) { char stbuff[1024]; sprintf( stbuff, x ); ctx->notefunction( ctx, stbuff ); } +#define SV_ERROR( x... ) { char stbuff[1024]; sprintf( stbuff, x ); ctx->faultfunction( ctx, stbuff ); } +#define SV_KILL() exit(0) //XXX This should likely be re-defined. + #endif diff --git a/src/survive.c b/src/survive.c index 9bc1a2c..e1ab943 100644 --- a/src/survive.c +++ b/src/survive.c @@ -3,7 +3,6 @@ #include #include "survive_internal.h" -#include "survive_driverman.h" #include #include #include diff --git a/src/survive_driverman.c b/src/survive_driverman.c index 8cdfb71..d694e64 100644 --- a/src/survive_driverman.c +++ b/src/survive_driverman.c @@ -3,7 +3,7 @@ // See notice in survive_driverman.h // -#include "survive_driverman.h" +#include "survive_internal.h" #include #include diff --git a/src/survive_driverman.h b/src/survive_driverman.h deleted file mode 100644 index 216333a..0000000 --- a/src/survive_driverman.h +++ /dev/null @@ -1,22 +0,0 @@ -// (C) 2017 <>< C. N. Lohr, Under MIT/x11 License. -// -// This file is intended to be used for self-registering functions. By using -// this it means that you do not need to have complicated switch statements or -// #defines for dfferent inclusion of drivers/other code. You can simply -// register your function and it will be put into a list. -// -// - -#ifndef SURVIVE_DRIVERMAN_H -#define SURVIVE_DRIVERMAN_H - -#include "survive.h" - -//Very little here. Mostly included in survive.h. - -//Driver registration -#define MAX_DRIVERS 32 - - -#endif - diff --git a/src/survive_internal.h b/src/survive_internal.h index ff156ad..392104a 100644 --- a/src/survive_internal.h +++ b/src/survive_internal.h @@ -7,16 +7,16 @@ #include #include #include -#include "survive_driverman.h" #include #include -#define SV_INFO( x... ) { char stbuff[1024]; sprintf( stbuff, x ); ctx->notefunction( ctx, stbuff ); } -#define SV_ERROR( x... ) { char stbuff[1024]; sprintf( stbuff, x ); ctx->faultfunction( ctx, stbuff ); } -//XXX TODO This one needs to be rewritten. -#define SV_KILL() exit(0) +//Driver registration +#define MAX_DRIVERS 32 +void * GetDriver( const char * name ); +const char * GetDriverNameMatching( const char * prefix, int place ); +void ListDrivers(); #endif diff --git a/src/survive_vive.c b/src/survive_vive.c index 6be5114..61716d3 100644 --- a/src/survive_vive.c +++ b/src/survive_vive.c @@ -10,14 +10,12 @@ //If there are portions of the code too similar to the original, I would like to know so they can be re-written. //All MIT/x11 Licensed Code in this file may be relicensed freely under the GPL or LGPL licenses. -#include "survive_internal.h" -#include "survive_driverman.h" +#include #include - -#include "survive_internal.h" #include #include -#include //sleep if I ever use it. +#include +#include #include #include #include -- cgit v1.2.3 From c65498054c77192b2a12fdb5ef44439a14110292 Mon Sep 17 00:00:00 2001 From: cnlohr Date: Wed, 8 Mar 2017 01:43:56 -0500 Subject: Architect the way the posers receive data. --- Makefile | 2 +- calibrate.c | 9 +++--- calibrate_client.c | 9 +++--- data_recorder.c | 6 ++-- include/libsurvive/poser.h | 4 ++- include/libsurvive/survive.h | 4 +-- include/libsurvive/survive_types.h | 2 +- src/survive.c | 57 +++++++++++++++++++++++++++++--------- src/survive_cal.h | 2 ++ src/survive_process.c | 29 ++++++++++++++++--- src/survive_vive.c | 16 +++++++++-- 11 files changed, 103 insertions(+), 37 deletions(-) diff --git a/Makefile b/Makefile index 470cbe3..3c8431b 100644 --- a/Makefile +++ b/Makefile @@ -23,7 +23,7 @@ calibrate_client : calibrate_client.c lib/libsurvive.so redist/os_generic.c red lib: mkdir lib -lib/libsurvive.so : src/survive.o src/survive_usb.o src/survive_data.o src/survive_process.o redist/jsmn.o src/ootx_decoder.o redist/linmath.o src/survive_driverman.o src/survive_vive.o src/survive_config.o redist/json_helpers.o $(DEBUGSTUFF) $(CALS) +lib/libsurvive.so : src/survive.o src/survive_usb.o src/survive_data.o src/survive_process.o redist/jsmn.o src/ootx_decoder.o redist/linmath.o src/survive_driverman.o src/survive_vive.o src/survive_config.o redist/json_helpers.o src/PoserDummy.o $(DEBUGSTUFF) $(CALS) gcc -o $@ $^ $(LDFLAGS) -shared clean : diff --git a/calibrate.c b/calibrate.c index a04c269..c557251 100644 --- a/calibrate.c +++ b/calibrate.c @@ -76,15 +76,14 @@ void my_light_process( struct SurviveObject * so, int sensor_id, int acode, int } } -void my_imu_process( struct SurviveObject * so, int16_t * accelgyro, uint32_t timecode, int id ) +void my_imu_process( struct SurviveObject * so, int mask, FLT * accelgyro, uint32_t timecode, int id ) { - survive_default_imu_process( so, accelgyro, timecode, id ); + survive_default_imu_process( so, mask, accelgyro, timecode, id ); -return; //if( so->codename[0] == 'H' ) - if( 1 ) + if( 0 ) { - printf( "I %s %d %d %d %d %d %d %d %d\n", so->codename, timecode, accelgyro[0], accelgyro[1], accelgyro[2], accelgyro[3], accelgyro[4], accelgyro[5], id ); + printf( "I %s %d %f %f %f %f %f %f %d\n", so->codename, timecode, accelgyro[0], accelgyro[1], accelgyro[2], accelgyro[3], accelgyro[4], accelgyro[5], id ); } } diff --git a/calibrate_client.c b/calibrate_client.c index 9867d67..b15b9db 100644 --- a/calibrate_client.c +++ b/calibrate_client.c @@ -75,15 +75,14 @@ void my_light_process( struct SurviveObject * so, int sensor_id, int acode, int } } -void my_imu_process( struct SurviveObject * so, int16_t * accelgyro, uint32_t timecode, int id ) +void my_imu_process( struct SurviveObject * so, int mask, FLT * accelgyro, uint32_t timecode, int id ) { - survive_default_imu_process( so, accelgyro, timecode, id ); + survive_default_imu_process( so, mask, accelgyro, timecode, id ); -return; //if( so->codename[0] == 'H' ) - if( 1 ) + if( 0 ) { - printf( "I %s %d %d %d %d %d %d %d %d\n", so->codename, timecode, accelgyro[0], accelgyro[1], accelgyro[2], accelgyro[3], accelgyro[4], accelgyro[5], id ); + printf( "I %s %d %f %f %f %f %f %f %d\n", so->codename, timecode, accelgyro[0], accelgyro[1], accelgyro[2], accelgyro[3], accelgyro[4], accelgyro[5], id ); } } diff --git a/data_recorder.c b/data_recorder.c index 951f234..5504d42 100644 --- a/data_recorder.c +++ b/data_recorder.c @@ -77,15 +77,15 @@ void my_light_process( struct SurviveObject * so, int sensor_id, int acode, int } -void my_imu_process( struct SurviveObject * so, int16_t * accelgyro, uint32_t timecode, int id ) +void my_imu_process( struct SurviveObject * so, int mask, FLT * accelgyro, uint32_t timecode, int id ) { - survive_default_imu_process( so, accelgyro, timecode, id ); + survive_default_imu_process( so, mask, accelgyro, timecode, id ); //return; //if( so->codename[0] == 'H' ) if( 1 ) { - printf( "I %s %d %d %d %d %d %d %d %d\n", so->codename, timecode, accelgyro[0], accelgyro[1], accelgyro[2], accelgyro[3], accelgyro[4], accelgyro[5], id ); + printf( "I %s %d %f %f %f %f %f %f %d\n", so->codename, timecode, accelgyro[0], accelgyro[1], accelgyro[2], accelgyro[3], accelgyro[4], accelgyro[5], id ); } } diff --git a/include/libsurvive/poser.h b/include/libsurvive/poser.h index c514953..98c926e 100644 --- a/include/libsurvive/poser.h +++ b/include/libsurvive/poser.h @@ -9,6 +9,7 @@ typedef enum PoserType_t POSERDATA_IMU, POSERDATA_LIGHT, //Single lighting event. POSERDATA_FULL_SCENE, //Full, statified X, Y sweep data for both lighthouses. + POSERDATA_DISASSOCIATE, //If you get this, it doesn't contain data. It just tells you to please disassociate from the current SurviveObject and delete your poserdata. } PoserType; typedef struct @@ -24,6 +25,7 @@ typedef struct FLT accel[3]; FLT gyro[3]; FLT mag[3]; + uint32_t timecode; //In object-local ticks. } PoserDataIMU; typedef struct @@ -47,7 +49,7 @@ typedef struct PoserDataIMU lastimu; } PoserDataFullScene; -//When you register your posers using the internal system, +//When you write your posers, use the following definition, and register with REGISTER_LINKTIME. typedef int (*PoserCB)( SurviveObject * so, PoserData * pd ); diff --git a/include/libsurvive/survive.h b/include/libsurvive/survive.h index c35e43c..7fd6046 100644 --- a/include/libsurvive/survive.h +++ b/include/libsurvive/survive.h @@ -29,7 +29,7 @@ struct SurviveObject SurvivePose OutPose; SurvivePose FromLHPose[NUM_LIGHTHOUSES]; //Optionally filled out by poser, contains computed position from each lighthouse. void * PoserData; //Initialized to zero, configured by poser, can be anything the poser wants. - PoserCB * PoserFn; + PoserCB PoserFn; //Device-specific information about the location of the sensors. This data will be used by the poser. int8_t nr_locations; @@ -126,7 +126,7 @@ void survive_cal_install( SurviveContext * ctx ); //XXX This will be removed if //Call these from your callback if overridden. //Accept higher-level data. void survive_default_light_process( SurviveObject * so, int sensor_id, int acode, int timeinsweep, uint32_t timecode, uint32_t length ); -void survive_default_imu_process( SurviveObject * so, int16_t * accelgyro, uint32_t timecode, int id ); +void survive_default_imu_process( SurviveObject * so, int mode, FLT * accelgyro, uint32_t timecode, int id ); void survive_default_angle_process( SurviveObject * so, int sensor_id, int acode, uint32_t timecode, FLT length, FLT angle ); diff --git a/include/libsurvive/survive_types.h b/include/libsurvive/survive_types.h index f235330..1600e11 100644 --- a/include/libsurvive/survive_types.h +++ b/include/libsurvive/survive_types.h @@ -29,7 +29,7 @@ typedef struct SurviveCalData SurviveCalData; //XXX Warning: This may be remov typedef void (*text_feedback_func)( SurviveContext * ctx, const char * fault ); typedef void (*light_process_func)( SurviveObject * so, int sensor_id, int acode, int timeinsweep, uint32_t timecode, uint32_t length ); -typedef void (*imu_process_func)( SurviveObject * so, int16_t * accelgyro, uint32_t timecode, int id ); +typedef void (*imu_process_func)( SurviveObject * so, int mask, FLT * accelgyro, uint32_t timecode, int id ); typedef void (*angle_process_func)( SurviveObject * so, int sensor_id, int acode, uint32_t timecode, FLT length, FLT angle ); diff --git a/src/survive.c b/src/survive.c index e1ab943..efa5d82 100644 --- a/src/survive.c +++ b/src/survive.c @@ -20,11 +20,11 @@ static void survivenote( struct SurviveContext * ctx, const char * fault ) } -struct SurviveContext * survive_init( int headless ) +SurviveContext * survive_init( int headless ) { int r = 0; int i = 0; - struct SurviveContext * ctx = calloc( 1, sizeof( struct SurviveContext ) ); + SurviveContext * ctx = calloc( 1, sizeof( SurviveContext ) ); ctx->faultfunction = survivefault; ctx->notefunction = survivenote; @@ -35,7 +35,6 @@ struct SurviveContext * survive_init( int headless ) const char * DriverName; while( ( DriverName = GetDriverNameMatching( "DriverReg", i++ ) ) ) - { DeviceDriver dd = GetDriver( DriverName ); printf( "Loading driver %s (%p) (%d)\n", DriverName, dd, i ); @@ -43,10 +42,34 @@ struct SurviveContext * survive_init( int headless ) printf( "Driver %s reports status %d\n", DriverName, r ); } + i = 0; + const char * PreferredPoser = "PoserDummy"; //config_read_str( cg, "DefualtPoser", "PoserDummy" ); /XXX Axlecrusher, can you add config stuff for this? + PoserCB PreferredPoserCB = 0; + const char * FirstPoser = 0; + printf( "Available posers:\n" ); + while( ( DriverName = GetDriverNameMatching( "Poser", i++ ) ) ) + { + PoserCB p = GetDriver( DriverName ); + if( !PreferredPoserCB ) PreferredPoserCB = p; + int ThisPoser = strcmp( DriverName, PreferredPoser ) == 0; + printf( "\t%c%s\n", ThisPoser?'*':' ', DriverName ); + if( ThisPoser ) PreferredPoserCB = p; + } + printf( "Totals %d posers. Using selected poser (or first!).\n", i-1 ); + if( !PreferredPoserCB ) + { + SV_ERROR( "Error. Cannot find any valid poser." ); + } + + for( i = 0; i < ctx->objs_ct; i++ ) + { + ctx->objs[i]->PoserFn = PreferredPoserCB; + } + return ctx; } -void survive_install_info_fn( struct SurviveContext * ctx, text_feedback_func fbp ) +void survive_install_info_fn( SurviveContext * ctx, text_feedback_func fbp ) { if( fbp ) ctx->notefunction = fbp; @@ -54,7 +77,7 @@ void survive_install_info_fn( struct SurviveContext * ctx, text_feedback_func f ctx->notefunction = survivenote; } -void survive_install_error_fn( struct SurviveContext * ctx, text_feedback_func fbp ) +void survive_install_error_fn( SurviveContext * ctx, text_feedback_func fbp ) { if( fbp ) ctx->faultfunction = fbp; @@ -62,7 +85,7 @@ void survive_install_error_fn( struct SurviveContext * ctx, text_feedback_func ctx->faultfunction = survivefault; } -void survive_install_light_fn( struct SurviveContext * ctx, light_process_func fbp ) +void survive_install_light_fn( SurviveContext * ctx, light_process_func fbp ) { if( fbp ) ctx->lightproc = fbp; @@ -70,7 +93,7 @@ void survive_install_light_fn( struct SurviveContext * ctx, light_process_func f ctx->lightproc = survive_default_light_process; } -void survive_install_imu_fn( struct SurviveContext * ctx, imu_process_func fbp ) +void survive_install_imu_fn( SurviveContext * ctx, imu_process_func fbp ) { if( fbp ) ctx->imuproc = fbp; @@ -79,7 +102,7 @@ void survive_install_imu_fn( struct SurviveContext * ctx, imu_process_func fbp } -void survive_install_angle_fn( struct SurviveContext * ctx, angle_process_func fbp ) +void survive_install_angle_fn( SurviveContext * ctx, angle_process_func fbp ) { if( fbp ) ctx->angleproc = fbp; @@ -87,15 +110,15 @@ void survive_install_angle_fn( struct SurviveContext * ctx, angle_process_func ctx->angleproc = survive_default_angle_process; } -int survive_add_object( struct SurviveContext * ctx, struct SurviveObject * obj ) +int survive_add_object( SurviveContext * ctx, SurviveObject * obj ) { int oldct = ctx->objs_ct; - ctx->objs = realloc( ctx->objs, sizeof( struct SurviveObject * ) * (oldct+1) ); + ctx->objs = realloc( ctx->objs, sizeof( SurviveObject * ) * (oldct+1) ); ctx->objs[oldct] = obj; ctx->objs_ct = oldct+1; } -void survive_add_driver( struct SurviveContext * ctx, void * payload, DeviceDriverCb poll, DeviceDriverCb close, DeviceDriverMagicCb magic ) +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) ); @@ -109,7 +132,7 @@ void survive_add_driver( struct SurviveContext * ctx, void * payload, DeviceDriv ctx->driver_ct = oldct+1; } -int survive_send_magic( struct SurviveContext * ctx, int magic_code, void * data, int datalen ) +int survive_send_magic( SurviveContext * ctx, int magic_code, void * data, int datalen ) { int oldct = ctx->driver_ct; int i; @@ -119,7 +142,7 @@ int survive_send_magic( struct SurviveContext * ctx, int magic_code, void * data } } -void survive_close( struct SurviveContext * ctx ) +void survive_close( SurviveContext * ctx ) { const char * DriverName; int r = 0; @@ -133,6 +156,14 @@ void survive_close( struct SurviveContext * ctx ) int oldct = ctx->driver_ct; int i; + + for( i = 0; i < ctx->objs_ct; i++ ) + { + PoserData pd; + pd.pt = POSERDATA_DISASSOCIATE; + if( ctx->objs[i]->PoserFn ) ctx->objs[i]->PoserFn( ctx->objs[i], &pd ); + } + for( i = 0; i < oldct; i++ ) { ctx->drivercloses[i]( ctx, ctx->drivers[i] ); diff --git a/src/survive_cal.h b/src/survive_cal.h index a5e372e..dd2a1e2 100644 --- a/src/survive_cal.h +++ b/src/survive_cal.h @@ -2,6 +2,8 @@ // All OOTX code was written by J. Allen. Rest of the code is probably mostly CNLohr. +//XXX XXX XXX Warning: This subsystem will likely be mostly re-written. + #ifndef _SURVIVE_CAL_H #define _SURVIVE_CAL_H diff --git a/src/survive_process.c b/src/survive_process.c index edabfff..2fea99d 100644 --- a/src/survive_process.c +++ b/src/survive_process.c @@ -46,13 +46,34 @@ void survive_default_angle_process( SurviveObject * so, int sensor_id, int acode { survive_cal_angle( so, sensor_id, acode, timecode, length, angle ); } - - //TODO: Writeme! + if( so->PoserFn ) + { + PoserDataLight l = { + .pt = POSERDATA_LIGHT, + .sensor_id = sensor_id, + .acode = acode, + .timecode = timecode, + .length = length, + .angle = angle, + }; + so->PoserFn( so, (PoserData *)&l ); + } } -void survive_default_imu_process( SurviveObject * so, int16_t * accelgyro, uint32_t timecode, int id ) +void survive_default_imu_process( SurviveObject * so, int mask, FLT * accelgyromag, uint32_t timecode, int id ) { - //TODO: Writeme! + if( so->PoserFn ) + { + PoserDataIMU imu = { + .pt = POSERDATA_IMU, + .datamask = mask, + .accel = { accelgyromag[0], accelgyromag[1], accelgyromag[2] }, + .gyro = { accelgyromag[3], accelgyromag[4], accelgyromag[5] }, + .mag = { accelgyromag[6], accelgyromag[7], accelgyromag[8] }, + .timecode = timecode, + }; + so->PoserFn( so, (PoserData *)&imu ); + } } diff --git a/src/survive_vive.c b/src/survive_vive.c index 61716d3..7da2897 100644 --- a/src/survive_vive.c +++ b/src/survive_vive.c @@ -595,7 +595,13 @@ static void handle_watchman( struct SurviveObject * w, uint8_t * readdata ) if( ( ( type & 0xe8 ) == 0xe8 ) || doimu ) //Hmm, this looks kind of yucky... we can get e8's that are accelgyro's but, cleared by first propset. { propset |= 2; - w->ctx->imuproc( w, (int16_t *)&readdata[1], (time1<<24)|(time2<<16)|readdata[0], 0 ); + //XXX XXX BIG TODO!!! Actually recal gyro data. + FLT agm[9] = { readdata[1], readdata[2], readdata[3], + readdata[4], readdata[5], readdata[6], + 0,0,0 }; + + w->ctx->imuproc( w, 3, agm, (time1<<24)|(time2<<16)|readdata[0], 0 ); + int16_t * k = (int16_t *)readdata+1; //printf( "Match8 %d %d %d %d %d %3d %3d\n", qty, k[0], k[1], k[2], k[3], k[4], k[5] ); readdata += 13; qty -= 13; @@ -810,7 +816,13 @@ void survive_data_cb( SurviveUSBInterface * si ) if( cd > 0 ) { obj->oldcode = code; - ctx->imuproc( obj, acceldata, timecode, code ); + + //XXX XXX BIG TODO!!! Actually recal gyro data. + FLT agm[9] = { acceldata[0], acceldata[1], acceldata[2], + acceldata[3], acceldata[4], acceldata[5], + 0,0,0 }; + + ctx->imuproc( obj, 3, agm, timecode, code ); } } -- cgit v1.2.3