From 898a9a5f242a1691e1c34062208b48cb0682b5d9 Mon Sep 17 00:00:00 2001 From: cnlohr Date: Fri, 16 Dec 2016 00:41:28 -0500 Subject: Remove dependency on Xinerama Improve camfinder a little Add data_recorder.c Closes #1 making libsurvive much more pluggable. --- Makefile | 10 +-- data_recorder.c | 160 +++++++++++++++++++++++++++++++++++++++++++++ include/survive.h | 42 +++++++++++- redist/XDriver.c | 2 +- src/survive.c | 58 ++++++++++++++-- src/survive_data.c | 29 ++------ src/survive_internal.h | 37 +++-------- src/survive_process.c | 54 ++------------- test.c | 106 +++--------------------------- tools/planetest2/camfind.c | 27 ++------ 10 files changed, 293 insertions(+), 232 deletions(-) create mode 100644 data_recorder.c diff --git a/Makefile b/Makefile index 29c05a1..d688287 100644 --- a/Makefile +++ b/Makefile @@ -1,10 +1,12 @@ -all : test +all : data_recorder test CFLAGS:=-Iinclude -fPIC -g -Os -Iredist -LDFLAGS:=-lpthread -lusb-1.0 -lz -lX11 -lXinerama -DEBUGSTUFF:=redist/os_generic.o redist/DrawFunctions.o redist/XDriver.o +LDFLAGS:=-lpthread -lusb-1.0 -lz -lX11 -test : test.c lib/libsurvive.so +test : test.c lib/libsurvive.so redist/os_generic.o + gcc -o $@ $^ $(LDFLAGS) $(CFLAGS) + +data_recorder : data_recorder.c lib/libsurvive.so redist/os_generic.o redist/DrawFunctions.o redist/XDriver.o gcc -o $@ $^ $(LDFLAGS) $(CFLAGS) lib/libsurvive.so : src/survive.o src/survive_usb.o src/survive_data.o src/survive_process.o redist/jsmn.o $(DEBUGSTUFF) diff --git a/data_recorder.c b/data_recorder.c new file mode 100644 index 0000000..24c8bc1 --- /dev/null +++ b/data_recorder.c @@ -0,0 +1,160 @@ +//Data recorder mod with GUI showing light positions. + +#include +#include +#include +#include +#include +#include +#include +#include + +struct SurviveContext * ctx; + +void HandleKey( int keycode, int bDown ) +{ + if( !bDown ) return; + + if( keycode == 'O' || keycode == 'o' ) + { + survive_usb_send_magic(ctx,1); + } + if( keycode == 'F' || keycode == 'f' ) + { + survive_usb_send_magic(ctx,0); + } +} + +void HandleButton( int x, int y, int button, int bDown ) +{ +} + +void HandleMotion( int x, int y, int mask ) +{ +} + +int bufferpts[32*2]; +char buffermts[32*128]; +int buffertimeto[32]; + +void my_light_process( struct SurviveObject * so, int sensor_id, int acode, int timeinsweep, uint32_t timecode, uint32_t length ) +{ + if( acode == -1 ) return; + + if( acode == 0 || acode == 2 ) //data = 0 + { + printf( "L X %s %d %d %d %d %d\n", so->codename, timecode, sensor_id, acode, timeinsweep, length ); + if( strcmp( so->codename, "HED" ) == 0 ) + { + bufferpts[sensor_id*2+0] = (timeinsweep-100000)/500; + buffertimeto[sensor_id] = 0; + } + } + if( acode == 1 || acode == 3 ) //data = 1 + { + printf( "L Y %s %d %d %d %d %d\n", so->codename, timecode, sensor_id, acode, timeinsweep, length ); + if( strcmp( so->codename, "HED" ) == 0 ) + { + bufferpts[sensor_id*2+1] = (timeinsweep-100000)/500; + buffertimeto[sensor_id] = 0; + } + } + + + if( acode == 4 || acode == 6 ) //data = 0 + { + printf( "R X %s %d %d %d %d %d\n", so->codename, timecode, sensor_id, acode, timeinsweep, length ); + if( strcmp( so->codename, "HED" ) == 0 ) + { + bufferpts[sensor_id*2+0] = (timeinsweep-100000)/500; + buffertimeto[sensor_id] = 0; + } + } + if( acode == 5 || acode == 7 ) //data = 1 + { + printf( "R Y %s %d %d %d %d %d\n", so->codename, timecode, sensor_id, acode, timeinsweep, length ); + if( strcmp( so->codename, "HED" ) == 0 ) + { + bufferpts[sensor_id*2+1] = (timeinsweep-100000)/500; + buffertimeto[sensor_id] = 0; + } + } + +} + +void my_imu_process( struct SurviveObject * so, int16_t * accelgyro, uint32_t timecode, int id ) +{ + //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 * GuiThread( void * v ) +{ + short screenx, screeny; + while(1) + { + CNFGHandleInput(); + CNFGClearFrame(); + CNFGColor( 0xFFFFFF ); + CNFGGetDimensions( &screenx, &screeny ); + + int i; + for( i = 0; i < 32; i++ ) + { + if( buffertimeto[i] < 5 ) + { + 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]++; + } + } + + + CNFGSwapBuffers(); + OGUSleep( 10000 ); + } +} + + + +int main() +{ + ctx = survive_init( ); + + survive_install_light_fn( ctx, my_light_process ); + survive_install_imu_fn( ctx, my_imu_process ); + + + 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) + { + //Do stuff. + } +} + diff --git a/include/survive.h b/include/survive.h index 637bd09..61a6620 100644 --- a/include/survive.h +++ b/include/survive.h @@ -1,17 +1,53 @@ #ifndef _SURVIVE_H #define _SURVIVE_H +#include + +#define SV_FLOAT double + struct SurviveContext; -struct SurviveContext * survive_init( void(*faultfunction)( struct SurviveContext * ctx, const char * fault ), - void(*notefunction)( struct SurviveContext * ctx, const char * note ) ); +//DANGER: This structure may be redefined + +struct SurviveObject +{ + struct SurviveContext * ctx; + char codename[4]; + int16_t buttonmask; + int16_t axis1; + int16_t axis2; + int16_t axis3; + int8_t charge; + int8_t charging:1; + int8_t ison:1; + int sensors; + + int nr_locations; + SV_FLOAT * sensor_locations; +}; + +typedef void (*text_feedback_fnptr)( 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 ); + +struct SurviveContext * survive_init(); + +//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_fnptr fbp ); +void survive_install_error_fn( struct SurviveContext * ctx, text_feedback_fnptr 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_close( struct SurviveContext * ctx ); int survive_poll(); + +//Utilitiy functions. int survive_simple_inflate( struct SurviveContext * ctx, const char * input, int inlen, char * output, int outlen ); -int survive_usb_send_magic(struct SurviveContext * ctx, int on ); + +//TODO: Need to make this do haptic responses for hands. +int survive_usb_send_magic( struct SurviveContext * ctx, int on ); #endif diff --git a/redist/XDriver.c b/redist/XDriver.c index 942fe0f..507ca95 100644 --- a/redist/XDriver.c +++ b/redist/XDriver.c @@ -2,7 +2,7 @@ //portions from //http://www.xmission.com/~georgeps/documentation/tutorials/Xlib_Beginner.html -#define HAS_XINERAMA +//#define HAS_XINERAMA #include "DrawFunctions.h" diff --git a/src/survive.c b/src/survive.c index edcbb86..773f02a 100644 --- a/src/survive.c +++ b/src/survive.c @@ -17,13 +17,29 @@ static int jsoneq(const char *json, jsmntok_t *tok, const char *s) { return -1; } -struct SurviveContext * survive_init( void(*ff)( struct SurviveContext * ctx, const char * fault ), void(*notefunction)( struct SurviveContext * ctx, const char * note ) ) + +static void survivefault( struct SurviveContext * ctx, const char * fault ) +{ + fprintf( stderr, "Error: %s\n", fault ); + exit( -1 ); +} + +static void survivenote( struct SurviveContext * ctx, const char * fault ) +{ + fprintf( stderr, "Info: %s\n", fault ); +} + + +struct SurviveContext * survive_init() { int r = 0; - struct SurviveContext * ctx = calloc( 1, sizeof( struct SurviveContext ) ); + struct SurviveContext * ctx = calloc( 1, sizeof( struct SurviveContext ) ); + + ctx->faultfunction = survivefault; + ctx->notefunction = survivenote; - ctx->faultfunction = ff; - ctx->notefunction = notefunction; + ctx->lightproc = survive_default_light_process; + ctx->imuproc = survive_default_imu_process; ctx->headset.sensors = 32; ctx->headset.ctx = ctx; @@ -44,8 +60,6 @@ struct SurviveContext * survive_init( void(*ff)( struct SurviveContext * ctx, co return 0; } - -#if 1 //Next, pull out the config stuff. { char * ct0conf = 0; @@ -123,12 +137,42 @@ struct SurviveContext * survive_init( void(*ff)( struct SurviveContext * ctx, co } } -#endif //ctx->headset->photos = malloc( ctx->headset->sensors * sizeof(struct SurvivePhoto) ); return ctx; } +void survive_install_info_fn( struct SurviveContext * ctx, text_feedback_fnptr fbp ) +{ + if( fbp ) + ctx->notefunction = fbp; + else + ctx->notefunction = survivenote; +} + +void survive_install_error_fn( struct SurviveContext * ctx, text_feedback_fnptr fbp ) +{ + if( fbp ) + ctx->faultfunction = fbp; + else + ctx->faultfunction = survivefault; +} + +void survive_install_light_fn( struct SurviveContext * ctx, light_process_func fbp ) +{ + if( fbp ) + ctx->lightproc = fbp; + else + ctx->lightproc = survive_default_light_process; +} + +void survive_install_imu_fn( struct SurviveContext * ctx, imu_process_func fbp ) +{ + if( fbp ) + ctx->imuproc = fbp; + else + ctx->imuproc = survive_default_imu_process; +} void survive_close( struct SurviveContext * ctx ) { diff --git a/src/survive_data.c b/src/survive_data.c index c84ab09..88c9e7d 100644 --- a/src/survive_data.c +++ b/src/survive_data.c @@ -47,7 +47,7 @@ static void handle_lightcap( struct SurviveObject * so, struct LightcapElement * ct->total_photo_time = 0; ct->total_photos = 0; ct->total_pulsecode_time = 0; - survive_light_process( so, le->sensor_id, -1, 0, le->timestamp, deltat ); + ct->lightproc( so, le->sensor_id, -1, 0, le->timestamp, deltat ); } } else @@ -73,7 +73,7 @@ static void handle_lightcap( struct SurviveObject * so, struct LightcapElement * if( offset_from < 380000 ) { - survive_light_process( so, le->sensor_id, acode, offset_from, le->timestamp, le->length ); + ct->lightproc( so, le->sensor_id, acode, offset_from, le->timestamp, le->length ); } } else @@ -158,7 +158,7 @@ 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; - survive_imu_process( w, (int16_t *)&readdata[1], (time1<<24)|(time2<<16)|readdata[0], 0 ); + w->ctx->imuproc( w, (int16_t *)&readdata[1], (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; @@ -178,16 +178,6 @@ static void handle_watchman( struct SurviveObject * w, uint8_t * readdata ) readdata--; *readdata = type; -#if 1 - // good good maybe? probably wrong - printf( "POST %d: %4d (%02x%02x) - ", propset, qty, time1, time2 ); - for( i = 0; i < qty + 4; i++ ) - { - printf( "%02x ", readdata[i] ); - } - printf("\n"); -#endif - uint8_t * end = &readdata[qty]; uint32_t mytime = (end[1] << 0)|(end[2] << 8)|(end[3] << 16); @@ -289,7 +279,8 @@ static void handle_watchman( struct SurviveObject * w, uint8_t * readdata ) int i; for( i = lese-1; i >= 0; i-- ) { - printf( "%d: %d [%d]\n", les[i].sensor_id, les[i].length, les[i].timestamp ); + //printf( "%d: %d [%d]\n", les[i].sensor_id, les[i].length, les[i].timestamp ); + handle_lightcap( w, &les[i] ); } return; @@ -345,14 +336,6 @@ void survive_data_cb( struct SurviveUSBInterface * si ) //printf( "%d -> ", size ); for( i = 0; i < 3; i++ ) { -/* - for( i = 0; i < 17; i++ ) - { - printf( "%02x ", readdata[i] ); - } - printf( "\n" ); -*/ - //handle_lightdata( (struct LightpulseStructure *)readdata ); int16_t * acceldata = (int16_t*)readdata; readdata += 12; uint32_t timecode = POP4; @@ -363,7 +346,7 @@ void survive_data_cb( struct SurviveUSBInterface * si ) if( cd > 0 ) { ctx->oldcode = code; - survive_imu_process( &ctx->headset, acceldata, timecode, code ); + ctx->imuproc( &ctx->headset, acceldata, timecode, code ); } } diff --git a/src/survive_internal.h b/src/survive_internal.h index 40659e8..ac378a2 100644 --- a/src/survive_internal.h +++ b/src/survive_internal.h @@ -19,6 +19,7 @@ #include #include #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 ); } @@ -26,8 +27,6 @@ //XXX TODO This one needs to be rewritten. #define SV_KILL() exit(0) -#define SV_FLOAT double - #define USB_DEV_HMD 0 #define USB_DEV_LIGHTHOUSE 1 #define USB_DEV_WATCHMAN1 2 @@ -61,33 +60,20 @@ struct SurviveUSBInterface const char * hname; //human-readable names }; - -struct SurviveObject -{ - struct SurviveContext * ctx; - char codename[4]; - int16_t buttonmask; - int16_t axis1; - int16_t axis2; - int16_t axis3; - int8_t charge; - int8_t charging:1; - int8_t ison:1; - int sensors; - - int nr_locations; - SV_FLOAT * sensor_locations; -}; +//This is defined in survive.h +struct SurviveObject; struct SurviveContext { //USB Subsystem struct libusb_context* usbctx; - void(*faultfunction)( struct SurviveContext * ctx, const char * fault ); - void(*notefunction)( struct SurviveContext * ctx, const char * fault ); struct libusb_device_handle * udev[MAX_USB_DEVS]; struct SurviveUSBInterface uiface[MAX_INTERFACES]; + text_feedback_fnptr faultfunction; + text_feedback_fnptr notefunction; + light_process_func lightproc; + imu_process_func imuproc; //Flood info, for calculating which laser is currently sweeping. int8_t oldcode; @@ -113,13 +99,8 @@ int survive_get_config( char ** config, struct SurviveContext * ctx, int devno, void survive_data_cb( struct SurviveUSBInterface * si ); //Accept higher-level data. -void survive_light_process( struct SurviveObject * so, int sensor_id, int acode, int timeinsweep, uint32_t timecode, uint32_t length ); -void survive_imu_process( struct SurviveObject * so, int16_t * accelgyro, uint32_t timecode, int id ); - - -//Util -int survive_simple_inflate( struct SurviveContext * ctx, const char * input, int inlen, char * output, int outlen ); - +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 ); #endif diff --git a/src/survive_process.c b/src/survive_process.c index 046971f..d3a8c4a 100644 --- a/src/survive_process.c +++ b/src/survive_process.c @@ -8,56 +8,14 @@ int bufferpts[32*2]; char buffermts[32*128]; int buffertimeto[32]; -void survive_light_process( struct SurviveObject * so, int sensor_id, int acode, int timeinsweep, uint32_t timecode, uint32_t length ) +void survive_default_light_process( struct SurviveObject * so, int sensor_id, int acode, int timeinsweep, uint32_t timecode, uint32_t length ) { - if( acode == -1 ) return; - - if( acode == 0 || acode == 2 ) //data = 0 - { - printf( "L X %s %d %d %d %d %d\n", so->codename, timecode, sensor_id, acode, timeinsweep, length ); - bufferpts[sensor_id*2+0] = (timeinsweep-100000)/500; - buffertimeto[sensor_id] = 0; - //printf( "X: %d\n",bufferpts[sensor_id*2+0] ); - //480-(timeinsweep)/1000; // Full scan - } - if( acode == 1 || acode == 3 ) //data = 1 - { - printf( "L Y %s %d %d %d %d %d\n", so->codename, timecode, sensor_id, acode, timeinsweep, length ); - bufferpts[sensor_id*2+1] = (timeinsweep-100000)/500; - //printf( "Y: %d\n",bufferpts[sensor_id*2+1] ); - buffertimeto[sensor_id] = 0; - - //480-(timeinsweep)/1000; //Full scan - } - - - if( acode == 4 || acode == 6 ) //data = 0 - { - printf( "R X %s %d %d %d %d %d\n", so->codename, timecode, sensor_id, acode, timeinsweep, length ); - bufferpts[sensor_id*2+0] = (timeinsweep-100000)/500; - buffertimeto[sensor_id] = 0; - //printf( "X: %d\n",bufferpts[sensor_id*2+0] ); - //480-(timeinsweep)/1000; // Full scan - } - if( acode == 5 || acode == 7 ) //data = 1 - { - printf( "R Y %s %d %d %d %d %d\n", so->codename, timecode, sensor_id, acode, timeinsweep, length ); - bufferpts[sensor_id*2+1] = (timeinsweep-100000)/500; - //printf( "Y: %d\n",bufferpts[sensor_id*2+1] ); - buffertimeto[sensor_id] = 0; - - //480-(timeinsweep)/1000; //Full scan - } - - - //timeinsweep = 200,000 1/48,000,000ths of a second is "center-of-image" + //TODO: Writeme! } -void survive_imu_process( struct SurviveObject * so, int16_t * accelgyro, uint32_t timecode, int id ) +void survive_default_imu_process( struct SurviveObject * so, int16_t * accelgyro, uint32_t timecode, int id ) { - //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 ); - } + //TODO: Writeme! } + + diff --git a/test.c b/test.c index 1f6778a..537b2e5 100644 --- a/test.c +++ b/test.c @@ -9,106 +9,12 @@ struct SurviveContext * ctx; -void survivefault( struct SurviveContext * ctx, const char * fault ) -{ - fprintf( stderr, "Error: %s\n", fault ); - exit( -1 ); -} - -void survivenote( struct SurviveContext * ctx, const char * fault ) -{ - fprintf( stderr, "Info: %s\n", fault ); -} - - -void HandleKey( int keycode, int bDown ) -{ - if( !bDown ) return; - - if( keycode == 'O' || keycode == 'o' ) - { - survive_usb_send_magic(ctx,1); - } - if( keycode == 'F' || keycode == 'f' ) - { - survive_usb_send_magic(ctx,0); - } -} - -void HandleButton( int x, int y, int button, int bDown ) -{ -} - -void HandleMotion( int x, int y, int mask ) -{ -} - -extern int bufferpts[32*2]; -extern char buffermts[32*128]; -extern int buffertimeto[32]; - -void * GuiThread( void * v ) -{ - short screenx, screeny; - while(1) - { - CNFGHandleInput(); - CNFGClearFrame(); - CNFGColor( 0xFFFFFF ); - CNFGGetDimensions( &screenx, &screeny ); - - int i; - for( i = 0; i < 32; i++ ) - { - if( buffertimeto[i] < 5 ) - { - 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]++; - } - } - - - CNFGSwapBuffers(); - OGUSleep( 10000 ); - } -} - - - int main() { -/* int i; - uint8_t input[] = { 0x7a, 0x01, 0x48, 0xc4, 0x1e, 0x1a, 0xfe, 0x6f, 0x6a, 0xf7, 0x25, 0x34 }; -// uint8_t input[] = { 0x1f, 0x8b, 0x08, 0x00, 0xc2, 0x45, 0x43, 0x58, 0x00, 0x03, 0xcb, 0xc8, 0xe4, 0x02, 0x00, 0x7a, 0x7a, 0x6f, 0xed, 0x03, 0x00, 0x00, 0x00 }; -// uint8_t input[] = { 0x78, 0xda, 0xcb, 0xc8, 0x04, 0x00, 0x01, 0x3b, 0x00, 0xd2 }; - - uint8_t output[1024]; - - int r = survive_simple_inflate( 0, input, sizeof( input ), output, sizeof(output) ); + int magicon = 0; + double Start = OGGetAbsoluteTime(); - printf( "%d: ", r ); - for( i = 0 ;i < r; i++ ) - { - printf( "%02x ", output[i] ); - } - return 0;*/ - ctx = survive_init( &survivefault, &survivenote ); - - CNFGBGColor = 0x000000; - CNFGDialogColor = 0x444444; - CNFGSetup( "Survive GUI Debug", 640, 480 ); - OGCreateThread( GuiThread, 0 ); - + ctx = survive_init( ); if( !ctx ) { @@ -118,6 +24,12 @@ int main() while(survive_poll(ctx) == 0) { + double Now = OGGetAbsoluteTime(); + if( Now > (Start+1) && !magicon ) + { + survive_usb_send_magic(ctx,1); + magicon = 1; + } //Do stuff. } } diff --git a/tools/planetest2/camfind.c b/tools/planetest2/camfind.c index 6caee1a..c987e46 100644 --- a/tools/planetest2/camfind.c +++ b/tools/planetest2/camfind.c @@ -5,7 +5,7 @@ #include #define PTS 32 -#define MAX_CHECKS 20000 +#define MAX_CHECKS 30000 #define MIN_HITS_FOR_VALID 10 FLT hmd_points[PTS*3]; @@ -28,7 +28,7 @@ int main() int i; //Load either 'L' (LH1) or 'R' (LH2) data. - if( LoadData( 'R' ) ) return 5; + if( LoadData( 'L' ) ) return 5; int opti = 0; int cycle = 0; @@ -39,17 +39,6 @@ int main() FLT bestxyz[3]; memcpy( bestxyz, LighthousePos, sizeof( LighthousePos ) ); - if( 0 ) - { - LighthousePos[0] = .0531311; - LighthousePos[1] = 1.2911; - LighthousePos[2] = 2.902; - RunOpti(1); - FLT ft = RunTest(1); - printf( "Final RMS: %f\n", ft ); - return 0; - } - //STAGE1 1: Detemine vectoral position from lighthouse to target. Does not determine lighthouse-target distance. //This also is constantly optimizing the lighthouse quaternion for optimal spotting. FLT fullrange = 5; //Maximum search space for positions. (Relative to HMD) @@ -231,7 +220,7 @@ FLT RunOpti( int print ) cross3d( xproduct, UsToTarget, RayShootOut ); FLT dist = magnitude3d( xproduct ); errorsq += dist*dist; - if( print ) printf( "%f (%d) ", dist, p ); + if( print ) printf( "%f (%d(%d/%d))\n", dist, p, hmd_point_counts[p*2+0], hmd_point_counts[p*2+1] ); } if( print ) printf( " = %f\n", sqrt( errorsq ) ); return sqrt(errorsq); @@ -270,11 +259,6 @@ FLT RunTest( int print ) //plane_normal is our normal / LighthousePos is our point. FLT w0[] = { hmd_points[pt*3+0], hmd_points[pt*3+1], hmd_points[pt*3+2] }; - -//May be able to remove this. -// FLT w0[] = { hmd_points[pt*3+0], hmd_points[pt*3+2],-hmd_points[pt*3+1] }; //XXX WRONG Why does this produce the right answers? - - //FLT w0[] = { 0, 0, 0 }; FLT d = -(plane_normal[0] * LighthousePos[0] + plane_normal[1] * LighthousePos[1] + plane_normal[2] * LighthousePos[2]); FLT D = plane_normal[0] * w0[0] + plane_normal[1] * w0[1] + plane_normal[2] * w0[2] + d; //Point line distance assuming ||normal|| = 1. @@ -291,8 +275,9 @@ FLT RunTest( int print ) if( print ) { int p; - printf( "Imagespace comparison:\n" ); + printf( "POS: %f %f %f %f\n", PFFOUR(LighthousePos ) ); printf( "QUAT: %f %f %f %f\n", PFFOUR(LighthouseQuat ) ); + printf( "Imagespace comparison:\n" ); for( p = 0; p < 32; p++ ) { if( hmd_point_counts[p*2+0] < MIN_HITS_FOR_VALID || hmd_point_counts[p*2+1] < MIN_HITS_FOR_VALID ) continue; @@ -381,7 +366,7 @@ int LoadData( char Camera ) int xck = 0; - f = fopen( "third_test_with_time_lengths.csv", "r" ); + f = fopen( "testfive.csv", "r" ); if( !f ) { fprintf( stderr, "Error: can't open two lighthouses test data.\n" ); return -11; } while( !feof(f) && !ferror(f) ) { -- cgit v1.2.3