diff options
| -rw-r--r-- | Makefile | 7 | ||||
| -rw-r--r-- | calibrate.c | 2 | ||||
| -rw-r--r-- | calibrate_client.c | 216 | ||||
| -rw-r--r-- | data_recorder.c | 4 | ||||
| -rw-r--r-- | include/survive.h | 2 | ||||
| -rw-r--r-- | src/survive.c | 3 | ||||
| -rw-r--r-- | src/survive_config.c | 174 | ||||
| -rw-r--r-- | src/survive_config.h | 45 | ||||
| -rw-r--r-- | test.c | 2 | ||||
| -rw-r--r-- | tools/data_server/data_server.c | 15 |
10 files changed, 459 insertions, 11 deletions
@@ -1,6 +1,6 @@ -all : lib data_recorder test calibrate +all : lib data_recorder test calibrate calibrate_client -CFLAGS:=-Iinclude -I. -fPIC -g -O0 -Iredist -flto -DUSE_DOUBLE +CFLAGS:=-Iinclude -I. -fPIC -g -O0 -Iredist -flto -DUSE_DOUBLE -std=gnu99 LDFLAGS:=-lpthread -lusb-1.0 -lz -lX11 -lm -flto -g @@ -17,6 +17,9 @@ data_recorder : data_recorder.c lib/libsurvive.so redist/os_generic.o redist/Dra calibrate : calibrate.c lib/libsurvive.so redist/os_generic.c redist/DrawFunctions.c redist/XDriver.c gcc -o $@ $^ $(LDFLAGS) $(CFLAGS) +calibrate_client : calibrate_client.c lib/libsurvive.so redist/os_generic.c redist/DrawFunctions.c redist/XDriver.c + gcc -o $@ $^ $(LDFLAGS) $(CFLAGS) + lib: mkdir lib diff --git a/calibrate.c b/calibrate.c index f50bad3..a1e150b 100644 --- a/calibrate.c +++ b/calibrate.c @@ -145,7 +145,7 @@ void * GuiThread( void * v ) int main() { - ctx = survive_init( ); + ctx = survive_init( 0 ); 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 new file mode 100644 index 0000000..4c59261 --- /dev/null +++ b/calibrate_client.c @@ -0,0 +1,216 @@ +//Totally hacky "client" for absorbing data from a network source of the vive data. + +#include <unistd.h> +#include <stdio.h> +#include <stdlib.h> +#include <stdint.h> +#include <survive.h> +#include <string.h> +#include <os_generic.h> +#include "src/survive_cal.h" +#include <DrawFunctions.h> + +struct SurviveContext * ctx; + +void HandleKey( int keycode, int bDown ) +{ + if( !bDown ) return; + + if( keycode == 'O' || keycode == 'o' ) + { + survive_send_magic(ctx,1,0,0); + } + if( keycode == 'F' || keycode == 'f' ) + { + survive_send_magic(ctx,0,0,0); + } +} + +void HandleButton( int x, int y, int button, int bDown ) +{ +} + +void HandleMotion( int x, int y, int mask ) +{ +} + +int bufferpts[32*2*3]; +char buffermts[32*128*3]; +int buffertimeto[32*3]; + +void my_light_process( struct SurviveObject * so, int sensor_id, int acode, int timeinsweep, uint32_t timecode, uint32_t length ) +{ + survive_default_light_process( so, sensor_id, acode, timeinsweep, timecode, length ); + + if( acode == -1 ) return; +//return; + int jumpoffset = sensor_id; + if( strcmp( so->codename, "WM0" ) == 0 ) jumpoffset += 32; + else if( strcmp( so->codename, "WM1" ) == 0 ) jumpoffset += 64; + + + if( acode == 0 || acode == 2 ) //data = 0 + { + bufferpts[jumpoffset*2+0] = (timeinsweep-100000)/500; + buffertimeto[jumpoffset] = 0; + } + if( acode == 1 || acode == 3 ) //data = 1 + { + bufferpts[jumpoffset*2+1] = (timeinsweep-100000)/500; + buffertimeto[jumpoffset] = 0; + } + + + if( acode == 4 || acode == 6 ) //data = 0 + { + bufferpts[jumpoffset*2+0] = (timeinsweep-100000)/500; + buffertimeto[jumpoffset] = 0; + } + if( acode == 5 || acode == 7 ) //data = 1 + { + bufferpts[jumpoffset*2+1] = (timeinsweep-100000)/500; + buffertimeto[jumpoffset] = 0; + } +} + +void my_imu_process( struct SurviveObject * so, int16_t * accelgyro, uint32_t timecode, int id ) +{ + survive_default_imu_process( so, 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 ); + } +} + + +void my_angle_process( struct SurviveObject * so, int sensor_id, int acode, uint32_t timecode, FLT length, FLT angle ) +{ + survive_default_angle_process( so, sensor_id, acode, timecode, length, angle ); +} + + +void * GuiThread( void * v ) +{ + short screenx, screeny; + while(1) + { + CNFGHandleInput(); + CNFGClearFrame(); + CNFGColor( 0xFFFFFF ); + CNFGGetDimensions( &screenx, &screeny ); + + int i; + for( i = 0; i < 32*3; i++ ) + { + if( buffertimeto[i] < 50 ) + { + uint32_t color = i * 3231349; + uint8_t r = color & 0xff; + uint8_t g = (color>>8) & 0xff; + uint8_t b = (color>>16) & 0xff; + r = (r * (5-buffertimeto[i])) / 5 ; + g = (g * (5-buffertimeto[i])) / 5 ; + b = (b * (5-buffertimeto[i])) / 5 ; + CNFGColor( (b<<16) | (g<<8) | r ); + CNFGTackRectangle( bufferpts[i*2+0], bufferpts[i*2+1], bufferpts[i*2+0] + 5, bufferpts[i*2+1] + 5 ); + CNFGPenX = bufferpts[i*2+0]; CNFGPenY = bufferpts[i*2+1]; + CNFGDrawText( buffermts, 2 ); + buffertimeto[i]++; + } + } + + CNFGColor( 0xffffff ); + char caldesc[256]; + survive_cal_get_status( ctx, caldesc, sizeof( caldesc ) ); + CNFGPenX = 3; + CNFGPenY = 3; + CNFGDrawText( caldesc, 4 ); + + + CNFGSwapBuffers(); + OGUSleep( 10000 ); + } +} + + + + +int main() +{ + ctx = survive_init( 1 ); + + survive_install_light_fn( ctx, my_light_process ); + survive_install_imu_fn( ctx, my_imu_process ); + survive_install_angle_fn( ctx, my_angle_process ); + + survive_cal_install( ctx ); + + struct SurviveObject * hmd = survive_get_so_by_name( ctx, "HMD" ); + struct SurviveObject * wm0 = survive_get_so_by_name( ctx, "WM0" ); + struct SurviveObject * wm1 = survive_get_so_by_name( ctx, "WM1" ); + + CNFGBGColor = 0x000000; + CNFGDialogColor = 0x444444; + CNFGSetup( "Survive GUI Debug", 640, 480 ); + OGCreateThread( GuiThread, 0 ); + + + if( !ctx ) + { + fprintf( stderr, "Fatal. Could not start\n" ); + return 1; + } + + while(survive_poll(ctx) == 0) + { + char * lineptr; + size_t n; + lineptr = 0; + n = 0; + ssize_t gl = getline( &lineptr, &n, stdin ); +// printf( "%d %s\n", gl, lineptr ); + + switch( lineptr[0] ) + { + case 'I': + //IMU data + //I WM0 -695533550 -321 1357 -3928 -16 -2 -2 0 + break; + case 'R': + case 'L': + { + //Light data + //R X HMD -878577652 -1 6 380498 6004 + char lhn[10]; + char beam[10]; + char dev[10]; + int timecode, sensor_id, acode, timeinsweep, length; + + sscanf( lineptr, "%9s %9s %9s %d %d %d %d %d\n", + lhn, beam, dev, &timecode, &sensor_id, &acode, &timeinsweep, &length ); + + struct SurviveObject * so = 0; + if( strcmp( dev, "HMD" ) == 0 ) + so = hmd; + if( strcmp( dev, "WM0" ) == 0 ) + so = wm0; + if( strcmp( dev, "WM1" ) == 0 ) + so = wm1; + + if( so ) + my_light_process( so, sensor_id, acode, timeinsweep, timecode, length ); + + break; + } + } + + free( lineptr ); + + //printf( "!!!\n" ); + //Do stuff. + } +} + diff --git a/data_recorder.c b/data_recorder.c index 516ac6c..951f234 100644 --- a/data_recorder.c +++ b/data_recorder.c @@ -81,7 +81,7 @@ void my_imu_process( struct SurviveObject * so, int16_t * accelgyro, uint32_t ti { survive_default_imu_process( so, accelgyro, timecode, id ); -return; +//return; //if( so->codename[0] == 'H' ) if( 1 ) { @@ -132,7 +132,7 @@ void * GuiThread( void * v ) int main() { - ctx = survive_init( ); + ctx = survive_init( 0 ); survive_install_light_fn( ctx, my_light_process ); survive_install_imu_fn( ctx, my_imu_process ); diff --git a/include/survive.h b/include/survive.h index 44f9926..51910a6 100644 --- a/include/survive.h +++ b/include/survive.h @@ -69,7 +69,7 @@ typedef void (*light_process_func)( struct SurviveObject * so, int sensor_id, in 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(); +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 ); diff --git a/src/survive.c b/src/survive.c index b472ccc..306dbe1 100644 --- a/src/survive.c +++ b/src/survive.c @@ -21,7 +21,7 @@ static void survivenote( struct SurviveContext * ctx, const char * fault ) } -struct SurviveContext * survive_init() +struct SurviveContext * survive_init( int headless ) { int r = 0; int i = 0; @@ -36,6 +36,7 @@ struct SurviveContext * survive_init() const char * DriverName; while( ( DriverName = GetDriverNameMatching( "DriverReg", i++ ) ) ) + { DeviceDriver dd = GetDriver( DriverName ); printf( "Loading driver %s (%p) (%d)\n", DriverName, dd, i ); diff --git a/src/survive_config.c b/src/survive_config.c new file mode 100644 index 0000000..ed3f6cd --- /dev/null +++ b/src/survive_config.c @@ -0,0 +1,174 @@ +#include <string.h> +#include <assert.h> +#include "survive_config.h" + +#define MAX_CONFIG_ENTRIES 100 + +config_val config_values[MAX_CONFIG_ENTRIES]; +static uint16_t used_entries = 0; + +static FILE *config_file = NULL; + +void config_init() { + uint16_t i = 0; + for (i=0;i<MAX_CONFIG_ENTRIES;++i) { + config_values[i].str = NULL; + config_values[i].tag = NULL; + } + + used_entries = 0; +} + +void write_float(char* tag, FLT x) { + fprintf(config_file, "\"%s\":\"%f\"\n", tag, x); +} + +void write_float_a(char* tag, FLT *x, uint8_t count) { + uint8_t i = 0; + char idx[4]; + for (i=0;i<count;++i) { + sprintf(idx,"%d",i); + fprintf(config_file, "\"%s%s\":\"%f\"\n", tag,idx, x); + } + fprintf(config_file, "\"%s\":\"%f\"\n", tag, x); +} + +void write_uint32(char* tag, uint32_t x) { + fprintf(config_file, "\"%s\":\"%d\"\n", tag, x); +} + +void config_open(const char* path, const char* mode) { + config_file = fopen(path, mode); +} + +void config_close() { + fclose(config_file); +} + +void config_write_lighthouse(struct BaseStationData* bsd, uint8_t length) { + uint8_t i = 0; + + for (i=0;i<length; ++i) { + write_uint32("id", bsd[i].BaseStationID); + write_float_a("position", bsd[i].Position, 3); + write_float_a("quaternion", bsd[i].Quaternion, 4); + write_float_a("quaternion", bsd[i].Quaternion, 4); + write_float_a("fcalphase", bsd[i].fcalphase, 2); + write_float_a("fcaltilt", bsd[i].fcaltilt,2); + write_float_a("fcalcurve", bsd[i].fcalcurve,2); + write_float_a("fcalgibpha", bsd[i].fcalgibpha,2); + write_float_a("fcalgibmag", bsd[i].fcalgibmag,2); + } +} + +void sstrcpy(char* dest, const char *src) { + uint32_t len = strlen(src)+1; + if (dest == NULL) { + dest = (char*)malloc(len); + } else { + dest = (char*)realloc(dest, len); + } + strcpy(dest,src); +} + +const char* config_read_str(const char *tag, const char *value, const char *def_str) { + uint16_t i = 0; + for (i=0;i<used_entries;++i) { + if ( strcmp(config_values[i].tag, tag) == 0 ) { + return config_values[i].str; + } + } + assert(used_entries<MAX_CONFIG_ENTRIES); + + i = used_entries++; + sstrcpy(config_values[i].tag, tag); + sstrcpy(config_values[i].str, def_str); + + return config_values[i].str; +} + +uint32_t config_read_uint32(const char *tag, const uint32_t value, const uint32_t def) { + uint16_t i = 0; + for (i=0;i<used_entries;++i) { + if ( strcmp(config_values[i].tag, tag) == 0 ) { + return config_values[i].numeric.i; + } + } + assert(used_entries<MAX_CONFIG_ENTRIES); + + i = used_entries++; + sstrcpy(config_values[i].tag, tag); + config_values[i].numeric.i = def; + + return config_values[i].numeric.i; +} + +FLT config_read_float(const char *tag, const FLT value, const FLT def) { + uint16_t i = 0; + for (i=0;i<used_entries;++i) { + if ( strcmp(config_values[i].tag, tag) == 0 ) { + return config_values[i].numeric.f; + } + } + assert(used_entries<MAX_CONFIG_ENTRIES); + + i = used_entries++; + sstrcpy(config_values[i].tag, tag); + config_values[i].numeric.f = def; + + return config_values[i].numeric.f; +} + +const char* config_set_str(const char *tag, const char* value) { + uint16_t i = 0; + + assert(used_entries<MAX_CONFIG_ENTRIES); + + i = used_entries++; + sstrcpy(config_values[i].tag, tag); + sstrcpy(config_values[i].str, value); + + return value; +} + +const uint32_t config_set_uint32(const char *tag, const uint32_t value) { + uint16_t i = 0; + + assert(used_entries<MAX_CONFIG_ENTRIES); + + i = used_entries++; + sstrcpy(config_values[i].tag, tag); + config_values[i].numeric.i = value; + + return value; +} + +const FLT config_set_float(const char *tag, const FLT value) { + uint16_t i = 0; + + assert(used_entries<MAX_CONFIG_ENTRIES); + + i = used_entries++; + sstrcpy(config_values[i].tag, tag); + config_values[i].numeric.f = value; + + return value; +} +void config_save(const char* path) { + uint16_t i = 0; + + FILE* f = fopen(path, "w"); + + 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); + } 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); + } + }; + + fclose(f); +} + diff --git a/src/survive_config.h b/src/survive_config.h new file mode 100644 index 0000000..24762cd --- /dev/null +++ b/src/survive_config.h @@ -0,0 +1,45 @@ +// (C) 2017 <>< Joshua Allen, Under MIT/x11 License. + + +#ifndef _SURVIVE_CONFIG_H +#define _SURVIVE_CONFIG_H + +#include "survive_internal.h" + +typedef enum { + CONFIG_UNKNOWN = 0, + CONFIG_FLOAT = 1, + CONFIG_UINT32 = 2, + CONFIG_STRING = 3 +} cval_type; +/* +typedef union { + uint32_t i; + FLT f; + } Numeric; +*/ +typedef struct { + char *tag; + cval_type type; + union { + uint32_t i; + FLT f; + } numeric; + char *str; +} config_val; + + +void config_open(const char* path, const char* mode); +void config_close(); +void config_write_lighthouse(struct BaseStationData* bsd, uint8_t length); + +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); + +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); + +#endif
\ No newline at end of file @@ -42,7 +42,7 @@ int main() int magicon = 0; double Start = OGGetAbsoluteTime(); - ctx = survive_init( ); + ctx = survive_init( 0 ); if( !ctx ) { diff --git a/tools/data_server/data_server.c b/tools/data_server/data_server.c index 29f5ead..5147d97 100644 --- a/tools/data_server/data_server.c +++ b/tools/data_server/data_server.c @@ -19,6 +19,7 @@ #define MAX_CONNS 32 int SocketList[MAX_CONNS]; +int droppedct[MAX_CONNS]; void error(char *msg) { perror(msg); @@ -44,10 +45,17 @@ void * SendThread( void * v ) int ss = send( sockc, buff, rd, MSG_DONTWAIT | MSG_NOSIGNAL ); if( ss < rd ) { - fprintf( stderr, "Dropped %d\n", i ); - close( SocketList[i] ); - SocketList[i] = 0; + if( droppedct[i]++ > 20 ) + { + fprintf( stderr, "Dropped %d\n", i ); + close( SocketList[i] ); + SocketList[i] = 0; + } } + else + { + droppedct[i] = 0; + } } } } @@ -142,6 +150,7 @@ int main( int argc, char ** argv ) if( SocketList[il] == 0 ) { SocketList[il] = childfd; + droppedct[il] = 0; printf("Conn %s At %d\n", hostaddrp, il); break; |