From 35e08aaa24bd01e6ace453f89ddb73a6bb0508b0 Mon Sep 17 00:00:00 2001 From: cnlohr Date: Tue, 14 Feb 2017 01:00:33 -0500 Subject: Still not working, but getting closer to dynamic OOTX decoding. --- calibrate.c | 124 ++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++ 1 file changed, 124 insertions(+) create mode 100644 calibrate.c (limited to 'calibrate.c') diff --git a/calibrate.c b/calibrate.c new file mode 100644 index 0000000..4db2ed0 --- /dev/null +++ b/calibrate.c @@ -0,0 +1,124 @@ +//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*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 ); +} + +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 * 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]++; + } + } + + + CNFGSwapBuffers(); + OGUSleep( 10000 ); + } +} + + + +int main() +{ + ctx = survive_init( ); + + survive_install_light_fn( ctx, my_light_process ); + survive_install_imu_fn( ctx, my_imu_process ); + + survive_cal_install( ctx ); + + 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. + } +} + -- cgit v1.3.1 From 2e5d0355da2376f27dcbe0cc6d04b737145ac853 Mon Sep 17 00:00:00 2001 From: cnlohr Date: Thu, 16 Feb 2017 15:47:46 -0500 Subject: update with more integration stuff. --- calibrate.c | 30 ++++++++++++++++++++++++++++++ src/survive_cal.c | 27 ++++++++++++++++++++++++--- src/survive_process.c | 20 ++++++++++++++++++-- 3 files changed, 72 insertions(+), 5 deletions(-) (limited to 'calibrate.c') diff --git a/calibrate.c b/calibrate.c index 4db2ed0..ce50b67 100644 --- a/calibrate.c +++ b/calibrate.c @@ -40,6 +40,36 @@ 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 ) diff --git a/src/survive_cal.c b/src/survive_cal.c index adcb7bc..cdc7783 100644 --- a/src/survive_cal.c +++ b/src/survive_cal.c @@ -12,11 +12,26 @@ void ootx_packet_clbk_d(ootx_decoder_context *ct, ootx_packet* packet) struct SurviveCalData * cd = ctx->calptr; int id = ct->user1; - printf( "Got OOTX packet %d %p\n", id, cd ); + SV_INFO( "Got OOTX packet %d %p\n", id, cd ); lighthouse_info_v6 v6; init_lighthouse_info_v6(&v6, packet->data); - print_lighthouse_info_v6(&v6); + + struct BaseStationData * b = &ctx->bsd[id]; + //print_lighthouse_info_v6(&v6); + + b->BaseStationID = v6.id; + b->fcalphase[0] = v6.fcal_0_phase; + b->fcalphase[1] = v6.fcal_1_phase; + b->fcaltilt[0] = v6.fcal_0_tilt; + b->fcaltilt[1] = v6.fcal_1_tilt; + b->fcalcurve[0] = v6.fcal_0_curve; + b->fcalcurve[1] = v6.fcal_1_curve; + b->fcalgibpha[0] = v6.fcal_0_gibphase; + b->fcalgibpha[1] = v6.fcal_1_gibphase; + b->fcalgibmag[0] = v6.fcal_0_gibmag; + b->fcalgibmag[1] = v6.fcal_1_gibmag; + b->OOTXSet = 1; } @@ -64,8 +79,14 @@ void survive_cal_light( struct SurviveObject * so, int sensor_id, int acode, int //printf( "%s %d %d %d\n", so->codename, lhid, acode, dbit ); ootx_pump_bit( &cd->ootx_decoders[lhid], dbit ); } + int i; + for( i = 0; i < NUM_LIGHTHOUSES; i++ ) + if( ctx->bsd[i].OOTXSet == 0 ) break; + if( i == NUM_LIGHTHOUSES ) cd->stage = 2; //If all lighthouses have their OOTX set, move on. } - + break; + + case 2: break; } } diff --git a/src/survive_process.c b/src/survive_process.c index 184532b..cb53588 100644 --- a/src/survive_process.c +++ b/src/survive_process.c @@ -8,13 +8,29 @@ void survive_default_light_process( struct SurviveObject * so, int sensor_id, int acode, int timeinsweep, uint32_t timecode, uint32_t length ) { + struct SurviveContext * ctx = so->ctx; + int base_station = acode >> 2; + int axis = acode & 1; - if( so->ctx->calptr ) + if( ctx->calptr ) { survive_cal_light( so, sensor_id, acode, timeinsweep, timecode, length ); } - //TODO: Writeme! + if( base_station > NUM_LIGHTHOUSES ) return; + + //No loner need sync information past this point. + if( sensor_id < 0 ) return; + + float angle = (timeinsweep - 200000) * (1./200000. * 3.14159265359/2.0); + + //Need to now do angle correction. +#if 1 + struct BaseStationData * bsd = &ctx->bsd[base_station]; + + //TODO!!! +#endif + } void survive_default_imu_process( struct SurviveObject * so, int16_t * accelgyro, uint32_t timecode, int id ) -- cgit v1.3.1 From 32fbccbd7d90f1e456d1e477eab2128aaf88df93 Mon Sep 17 00:00:00 2001 From: cnlohr Date: Thu, 16 Feb 2017 17:16:31 -0500 Subject: Move to having an angle callback. --- Makefile | 2 +- calibrate.c | 6 ++++++ include/survive.h | 38 ++++++++++++++++++++++++++------------ src/survive.c | 34 +++++++++++++++++++++++++++++----- src/survive_cal.c | 26 +++++++++++++++++++++++--- src/survive_cal.h | 1 + src/survive_data.c | 32 +++++++++++++++++++------------- src/survive_internal.h | 9 ++++----- src/survive_process.c | 24 +++++++++++++++++++++--- 9 files changed, 130 insertions(+), 42 deletions(-) (limited to 'calibrate.c') diff --git a/Makefile b/Makefile index 797c257..ed51e30 100644 --- a/Makefile +++ b/Makefile @@ -1,7 +1,7 @@ all : lib data_recorder test calibrate CFLAGS:=-Iinclude -fPIC -g -Os -Iredist -flto -LDFLAGS:=-lpthread -lusb-1.0 -lz -lX11 -flto +LDFLAGS:=-lpthread -lusb-1.0 -lz -lX11 -lm -flto # unused: redist/crc32.c diff --git a/calibrate.c b/calibrate.c index ce50b67..6bd3e77 100644 --- a/calibrate.c +++ b/calibrate.c @@ -85,6 +85,10 @@ return; } +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 ) @@ -125,12 +129,14 @@ void * GuiThread( void * v ) + int main() { ctx = survive_init( ); 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 ); diff --git a/include/survive.h b/include/survive.h index 4a95a2f..3e8dc35 100644 --- a/include/survive.h +++ b/include/survive.h @@ -3,19 +3,19 @@ #include -#define SV_FLOAT double +#ifndef FLT +#define FLT double +#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 { struct SurviveContext * ctx; @@ -31,17 +31,26 @@ struct SurviveObject int8_t ison:1; int8_t additional_flags:6; - SV_FLOAT * sensor_locations; - SV_FLOAT * sensor_normals; + 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) int8_t nr_locations; //Flood info, for calculating which laser is currently sweeping. int8_t oldcode; - - uint32_t last_time[NUM_LIGHTHOUSES]; - uint32_t last_length[NUM_LIGHTHOUSES]; 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. @@ -50,17 +59,19 @@ struct SurviveObject int tsl; }; -typedef void (*text_feedback_fnptr)( struct SurviveContext * ctx, const char * fault ); +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(); //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_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(); @@ -77,8 +88,11 @@ int survive_usb_send_magic( struct SurviveContext * ctx, int on ); void survive_cal_install( struct SurviveContext * ctx ); //Call these from your callback if overridden. -void survive_default_light_process( struct SurviveObject * so, int sensor_id, int acode, int timeinsweep, uint32_t timecode, uint32_t length ); +//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.c b/src/survive.c index ed18da2..aab2ae6 100644 --- a/src/survive.c +++ b/src/survive.c @@ -30,7 +30,7 @@ static void survivenote( struct SurviveContext * ctx, const char * fault ) fprintf( stderr, "Info: %s\n", fault ); } -static int ParsePoints( struct SurviveContext * ctx, struct SurviveObject * so, char * ct0conf, SV_FLOAT ** floats_out, jsmntok_t * t, int i ) +static int ParsePoints( struct SurviveContext * ctx, struct SurviveObject * so, char * ct0conf, FLT ** floats_out, jsmntok_t * t, int i ) { int k; int pts = t[i+1].size; @@ -43,7 +43,7 @@ static int ParsePoints( struct SurviveContext * ctx, struct SurviveObject * so, { tk = &t[i+2+k*4]; - float vals[3]; + FLT vals[3]; int m; for( m = 0; m < 3; m++ ) { @@ -60,7 +60,7 @@ static int ParsePoints( struct SurviveContext * ctx, struct SurviveObject * so, memcpy( ctt, ct0conf + tk->start, elemlen ); ctt[elemlen] = 0; - float f = atof( ctt ); + FLT f = atof( ctt ); int id = so->nr_locations*3+m; (*floats_out)[id] = f; } @@ -144,6 +144,7 @@ struct SurviveContext * survive_init() ctx->lightproc = survive_default_light_process; ctx->imuproc = survive_default_imu_process; + ctx->angleproc = survive_default_angle_process; ctx->headset.ctx = ctx; memcpy( ctx->headset.codename, "HMD", 4 ); @@ -164,6 +165,18 @@ struct SurviveContext * survive_init() if( LoadConfig( ctx, &ctx->watchman[0], 2, 0, 1 ) ) { SV_INFO( "Watchman 0 config issue." ); } if( LoadConfig( ctx, &ctx->watchman[1], 3, 0, 1 ) ) { SV_INFO( "Watchman 1 config issue." ); } + ctx->headset.timebase_hz = ctx->watchman[0].timebase_hz = ctx->watchman[1].timebase_hz = 48000000; + ctx->headset.pulsedist_max_ticks = ctx->watchman[0].pulsedist_max_ticks = ctx->watchman[1].pulsedist_max_ticks = 500000; + ctx->headset.pulselength_min_sync = ctx->watchman[0].pulselength_min_sync = ctx->watchman[1].pulselength_min_sync = 2200; + ctx->headset.pulse_in_clear_time = ctx->watchman[0].pulse_in_clear_time = ctx->watchman[1].pulse_in_clear_time = 35000; + ctx->headset.pulse_max_for_sweep = ctx->watchman[0].pulse_max_for_sweep = ctx->watchman[1].pulse_max_for_sweep = 1800; + + ctx->headset.pulse_synctime_offset = ctx->watchman[0].pulse_synctime_offset = ctx->watchman[1].pulse_synctime_offset = 20000; + ctx->headset.pulse_synctime_slack = ctx->watchman[0].pulse_synctime_slack = ctx->watchman[1].pulse_synctime_slack = 5000; + + ctx->headset.timecenter_ticks = ctx->headset.timebase_hz / 240; + ctx->watchman[0].timecenter_ticks = ctx->watchman[0].timebase_hz / 240; + ctx->watchman[1].timecenter_ticks = ctx->watchman[1].timebase_hz / 240; /* int i; int locs = ctx->headset.nr_locations; @@ -195,7 +208,7 @@ fail_gracefully: return 0; } -void survive_install_info_fn( struct SurviveContext * ctx, text_feedback_fnptr fbp ) +void survive_install_info_fn( struct SurviveContext * ctx, text_feedback_func fbp ) { if( fbp ) ctx->notefunction = fbp; @@ -203,7 +216,7 @@ void survive_install_info_fn( struct SurviveContext * ctx, text_feedback_fnptr ctx->notefunction = survivenote; } -void survive_install_error_fn( struct SurviveContext * ctx, text_feedback_fnptr fbp ) +void survive_install_error_fn( struct SurviveContext * ctx, text_feedback_func fbp ) { if( fbp ) ctx->faultfunction = fbp; @@ -227,6 +240,17 @@ void survive_install_imu_fn( struct SurviveContext * ctx, imu_process_func fbp ctx->imuproc = survive_default_imu_process; } + +void survive_install_angle_fn( struct SurviveContext * ctx, angle_process_func fbp ) +{ + if( fbp ) + ctx->angleproc = fbp; + else + ctx->angleproc = survive_default_angle_process; +} + + + void survive_close( struct SurviveContext * ctx ) { survive_usb_close( ctx ); diff --git a/src/survive_cal.c b/src/survive_cal.c index cdc7783..6bb8173 100644 --- a/src/survive_cal.c +++ b/src/survive_cal.c @@ -5,6 +5,7 @@ #include "survive_cal.h" #include "survive_internal.h" +#include void ootx_packet_clbk_d(ootx_decoder_context *ct, ootx_packet* packet) { @@ -23,8 +24,8 @@ void ootx_packet_clbk_d(ootx_decoder_context *ct, ootx_packet* packet) b->BaseStationID = v6.id; b->fcalphase[0] = v6.fcal_0_phase; b->fcalphase[1] = v6.fcal_1_phase; - b->fcaltilt[0] = v6.fcal_0_tilt; - b->fcaltilt[1] = v6.fcal_1_tilt; + b->fcaltilt[0] = tan(v6.fcal_0_tilt); + b->fcaltilt[1] = tan(v6.fcal_1_tilt); //XXX??? Is this right? See https://github.com/cnlohr/libsurvive/issues/18 b->fcalcurve[0] = v6.fcal_0_curve; b->fcalcurve[1] = v6.fcal_1_curve; b->fcalgibpha[0] = v6.fcal_0_gibphase; @@ -76,7 +77,6 @@ void survive_cal_light( struct SurviveObject * so, int sensor_id, int acode, int if( lhid < NUM_LIGHTHOUSES && so->codename[0] == 'H' ) { uint8_t dbit = (acode & 2)>>1; - //printf( "%s %d %d %d\n", so->codename, lhid, acode, dbit ); ootx_pump_bit( &cd->ootx_decoders[lhid], dbit ); } int i; @@ -85,9 +85,29 @@ void survive_cal_light( struct SurviveObject * so, int sensor_id, int acode, int if( i == NUM_LIGHTHOUSES ) cd->stage = 2; //If all lighthouses have their OOTX set, move on. } break; + case 2: //Taking in angle data. + break; + } +} + +void survive_cal_angle( struct SurviveObject * so, int sensor_id, int acode, uint32_t timecode, FLT length, FLT angle ) +{ + struct SurviveContext * ctx = so->ctx; + struct SurviveCalData * cd = ctx->calptr; + + if( !cd ) return; + switch( cd->stage ) + { + default: + case 1: //Collecting OOTX data. (Don't do anything here, yet.) + case 0: //Default, inactive. + break; case 2: + //if( sensor_id == 0 && so->codename[0] == 'H' ) printf( "%d %f %f\n", acode, length, angle ); break; } } + + diff --git a/src/survive_cal.h b/src/survive_cal.h index a74bf95..6653a84 100644 --- a/src/survive_cal.h +++ b/src/survive_cal.h @@ -28,6 +28,7 @@ int survive_cal_get_status( struct SurviveContext * ctx, char * description, int //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 ); struct SurviveCalData { diff --git a/src/survive_data.c b/src/survive_data.c index c557951..ad834cf 100644 --- a/src/survive_data.c +++ b/src/survive_data.c @@ -37,36 +37,34 @@ static void handle_lightcap( struct SurviveObject * so, struct LightcapElement * // printf( "%s %d %d %d %d %d\n", so->codename, le->sensor_id, le->type, le->length, le->timestamp, le->timestamp-so->tsl ); so->tsl = le->timestamp; - if( le->length < 20 ) return; + if( le->length < 20 ) return; ///Assuming 20 is an okay value for here. //The sync pulse finder is taking Charles's old disambiguator code and mixing it with a more linear //version of Julian Picht's disambiguator, available in 488c5e9. Removed afterwards into this //unified driver. - - int ssn = so->sync_set_number; if( ssn < 0 ) ssn = 0; int last_sync_time = so->last_time [ssn]; int last_sync_length = so->last_length[ssn]; int32_t delta = le->timestamp - last_sync_time; //Handle time wrapping (be sure to be int32) - if( delta < -500000 || delta > 500000 ) + if( delta < -so->pulsedist_max_ticks || delta > so->pulsedist_max_ticks ) { //Reset pulse, etc. so->sync_set_number = -1; - delta = 500000; + delta = so->pulsedist_max_ticks; } - if( le->length > 2200 ) //Pulse longer indicates a sync pulse. + if( le->length > so->pulselength_min_sync ) //Pulse longer indicates a sync pulse. { - int is_new_pulse = delta > 1500 + last_sync_length; + int is_new_pulse = delta > so->pulselength_min_sync /*1500*/ + last_sync_length; so->did_handle_ootx = 0; if( is_new_pulse ) { - int is_master_sync_pulse = delta > 40000; + int is_master_sync_pulse = delta > so->pulse_in_clear_time /*40000*/; if( is_master_sync_pulse ) { @@ -110,7 +108,7 @@ static void handle_lightcap( struct SurviveObject * so, struct LightcapElement * //See if this is a valid actual pulse. - else if( le->length < 1800 && le->length > 40 && delta > 30000 && ssn >= 0 ) + else if( le->length < so->pulse_max_for_sweep && delta > so->pulse_in_clear_time && ssn >= 0 ) { int32_t dl = so->last_time[0]; int32_t tpco = so->last_length[0]; @@ -123,10 +121,13 @@ static void handle_lightcap( struct SurviveObject * so, struct LightcapElement * //Adding length //Long pulse-code from IR flood. //Make sure it fits nicely into a divisible-by-500 time. + + int32_t main_divisor = so->timebase_hz / 384000; //125 @ 48 MHz. + int32_t acode_array[2] = { - (so->last_length[0]+125+50)/250, - (so->last_length[1]+125+50)/250, + (so->last_length[0]+main_divisor+50)/(main_divisor*2), //+50 adds a small offset and seems to help always get it right. + (so->last_length[1]+main_divisor+50)/(main_divisor*2), //Check the +50 in the future to see how well this works on a variety of hardware. }; //XXX: TODO: Capture error count here. @@ -150,14 +151,19 @@ static void handle_lightcap( struct SurviveObject * so, struct LightcapElement * so->recent_sync_time = so->last_time[1]; //Throw out everything if our sync pulses look like they're bad. - if( delta1 < 375000 || delta1 > 385000 ) + + int32_t center_1 = so->timecenter_ticks*2 - so->pulse_synctime_offset; + int32_t center_2 = so->pulse_synctime_offset; + int32_t slack = so->pulse_synctime_slack; + + if( delta1 < center_1 - slack || delta1 > center_1 + slack ) { //XXX: TODO: Count faults. so->sync_set_number = -1; return; } - if( delta2 < 15000 || delta2 > 25000 ) + if( delta2 < center_2 - slack || delta2 > center_2 + slack ) { //XXX: TODO: Count faults. so->sync_set_number = -1; diff --git a/src/survive_internal.h b/src/survive_internal.h index 11c9d89..0d0b8a4 100644 --- a/src/survive_internal.h +++ b/src/survive_internal.h @@ -86,10 +86,12 @@ struct SurviveContext struct libusb_device_handle * udev[MAX_USB_DEVS]; struct SurviveUSBInterface uiface[MAX_INTERFACES]; - text_feedback_fnptr faultfunction; - text_feedback_fnptr notefunction; + text_feedback_func faultfunction; + text_feedback_func notefunction; light_process_func lightproc; imu_process_func imuproc; + angle_process_func angleproc; + //Calibration data: struct BaseStationData bsd[NUM_LIGHTHOUSES]; @@ -112,9 +114,6 @@ int survive_get_config( char ** config, struct SurviveContext * ctx, int devno, //Accept Data from backend. void survive_data_cb( struct SurviveUSBInterface * si ); -//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 ); #endif diff --git a/src/survive_process.c b/src/survive_process.c index cb53588..75453da 100644 --- a/src/survive_process.c +++ b/src/survive_process.c @@ -21,18 +21,36 @@ void survive_default_light_process( struct SurviveObject * so, int sensor_id, in //No loner need sync information past this point. if( sensor_id < 0 ) return; - - float angle = (timeinsweep - 200000) * (1./200000. * 3.14159265359/2.0); + FLT angle = (timeinsweep - so->timecenter_ticks) * (1./so->timecenter_ticks * 3.14159265359/2.0); //Need to now do angle correction. #if 1 struct 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]; +// angle += bsd->fcaltilt[axis] * predicted_angle(axis1); + //TODO!!! #endif - + + FLT length_sec = length / (FLT)so->timebase_hz; + ctx->angleproc( so, sensor_id, acode, timecode, length_sec, angle ); } + +void survive_default_angle_process( struct SurviveObject * so, int sensor_id, int acode, uint32_t timecode, FLT length, FLT angle ) +{ + struct SurviveContext * ctx = so->ctx; + if( ctx->calptr ) + { + survive_cal_angle( so, sensor_id, acode, timecode, length, angle ); + } + + //TODO: Writeme! +} + + void survive_default_imu_process( struct SurviveObject * so, int16_t * accelgyro, uint32_t timecode, int id ) { //TODO: Writeme! -- cgit v1.3.1 From 9bba1f9e7888f512a587f76179b9dd8f389c7ae8 Mon Sep 17 00:00:00 2001 From: cnlohr Date: Thu, 16 Feb 2017 18:09:19 -0500 Subject: Start collecting data. Getting closer to having a full cal stack. --- calibrate.c | 7 +++++++ src/survive_cal.c | 47 ++++++++++++++++++++++++++++++++++++++++++++++- src/survive_cal.h | 15 +++++++++++++-- 3 files changed, 66 insertions(+), 3 deletions(-) (limited to 'calibrate.c') diff --git a/calibrate.c b/calibrate.c index 6bd3e77..2db544a 100644 --- a/calibrate.c +++ b/calibrate.c @@ -121,6 +121,13 @@ void * GuiThread( void * v ) } } + CNFGColor( 0xffffff ); + char caldesc[256]; + survive_cal_get_status( ctx, caldesc, sizeof( caldesc ) ); + CNFGPenX = 3; + CNFGPenY = 3; + CNFGDrawText( caldesc, 2 ); + CNFGSwapBuffers(); OGUSleep( 10000 ); diff --git a/src/survive_cal.c b/src/survive_cal.c index 6bb8173..8069154 100644 --- a/src/survive_cal.c +++ b/src/survive_cal.c @@ -6,6 +6,9 @@ #include "survive_cal.h" #include "survive_internal.h" #include +#include + +static void handle_calibration( struct SurviveCalData *cd ); void ootx_packet_clbk_d(ootx_decoder_context *ct, ootx_packet* packet) { @@ -35,6 +38,22 @@ void ootx_packet_clbk_d(ootx_decoder_context *ct, ootx_packet* packet) b->OOTXSet = 1; } +int survive_cal_get_status( struct SurviveContext * ctx, char * description, int description_length ) +{ + struct SurviveCalData * cd = ctx->calptr; + + switch( cd->stage ) + { + case 0: + return snprintf( description, description_length, "Not calibrating" ); + case 1: + return snprintf( description, description_length, "Collecting OOTX Data (%d:%d)", cd->ootx_decoders[0].buf_offset, cd->ootx_decoders[1].buf_offset ); + case 2: + return snprintf( description, description_length, "Collecting Sweep Data %d/%d", cd->peak_counts, DRPTS ); + default: + return snprintf( description, description_length, "Unkown calibration state" ); + } +} void survive_cal_install( struct SurviveContext * ctx ) { @@ -104,10 +123,36 @@ void survive_cal_angle( struct SurviveObject * so, int sensor_id, int acode, uin case 0: //Default, inactive. break; case 2: - //if( sensor_id == 0 && so->codename[0] == 'H' ) printf( "%d %f %f\n", acode, length, angle ); + { + int sensid = sensor_id; + if( strcmp( so->codename, "WM0" ) == 0 ) + sensid += 32; + if( strcmp( so->codename, "WM1" ) == 1 ) + sensid += 64; + + int lighthouse = acode>>2; + int axis = acode & 1; + int ct = cd->all_counts[sensid][lighthouse][axis]++; + cd->all_lengths[sensid][lighthouse][axis][ct] = length; + cd->all_angles[sensid][lighthouse][axis][ct] = angle; + if( ct > cd->peak_counts ) + { + cd->peak_counts = ct; + if( ct >= DRPTS ) + handle_calibration( cd ); //This will also reset all cals. + } break; } + } } +static void handle_calibration( struct SurviveCalData *cd ) +{ + //Do stuff. + memset( cd->all_lengths, 0, sizeof( cd->all_lengths ) ); + memset( cd->all_angles, 0, sizeof( cd->all_angles ) ); + memset( cd->all_counts, 0, sizeof( cd->all_counts ) ); + cd->peak_counts = 0; +} diff --git a/src/survive_cal.h b/src/survive_cal.h index 6653a84..b9d4b11 100644 --- a/src/survive_cal.h +++ b/src/survive_cal.h @@ -22,7 +22,7 @@ #include "survive_internal.h" void survive_cal_install( struct SurviveContext * ctx ); -int survive_cal_get_status( struct SurviveContext * ctx, char * description, int max_data ); +int survive_cal_get_status( struct SurviveContext * ctx, char * description, int description_length ); //void survive_cal_teardown( struct SurviveContext * ctx ); @@ -30,6 +30,9 @@ int survive_cal_get_status( struct SurviveContext * ctx, char * description, int 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 ); +#define MAX_TO_CAL 96 +#define DRPTS 512 + struct SurviveCalData { //Stage: @@ -37,9 +40,17 @@ struct SurviveCalData // 1: Collecting OOTX data. int stage; - //OOTX Data is sync'd off of + //OOTX Data is sync'd off of the sync pulses coming from the lighthouses. ootx_decoder_context ootx_decoders[NUM_LIGHTHOUSES]; + + //For statistics-gathering phase. + FLT all_lengths[MAX_TO_CAL][NUM_LIGHTHOUSES][2][DRPTS]; + FLT all_angles[MAX_TO_CAL][NUM_LIGHTHOUSES][2][DRPTS]; + int16_t all_counts[MAX_TO_CAL][NUM_LIGHTHOUSES][2]; + int peak_counts; }; + + #endif -- cgit v1.3.1 From bd89d46cb01f7069166e85f017f169e07acc7094 Mon Sep 17 00:00:00 2001 From: cnlohr Date: Thu, 16 Feb 2017 18:28:54 -0500 Subject: closing in on finding common cal points. --- calibrate.c | 2 +- src/survive_cal.c | 47 ++++++++++++++++++++++++++++++++++++++++++----- src/survive_cal.h | 13 +++++++------ 3 files changed, 50 insertions(+), 12 deletions(-) (limited to 'calibrate.c') diff --git a/calibrate.c b/calibrate.c index 2db544a..60f4316 100644 --- a/calibrate.c +++ b/calibrate.c @@ -126,7 +126,7 @@ void * GuiThread( void * v ) survive_cal_get_status( ctx, caldesc, sizeof( caldesc ) ); CNFGPenX = 3; CNFGPenY = 3; - CNFGDrawText( caldesc, 2 ); + CNFGDrawText( caldesc, 4 ); CNFGSwapBuffers(); diff --git a/src/survive_cal.c b/src/survive_cal.c index 8069154..7b0a824 100644 --- a/src/survive_cal.c +++ b/src/survive_cal.c @@ -8,7 +8,11 @@ #include #include +#define PTS_BEFORE_COMMON 32 +#define NEEDED_COMMON_POINTS 20 + static void handle_calibration( struct SurviveCalData *cd ); +static void reset_calibration( struct SurviveCalData * cd ); void ootx_packet_clbk_d(ootx_decoder_context *ct, ootx_packet* packet) { @@ -49,7 +53,14 @@ int survive_cal_get_status( struct SurviveContext * ctx, char * description, int case 1: return snprintf( description, description_length, "Collecting OOTX Data (%d:%d)", cd->ootx_decoders[0].buf_offset, cd->ootx_decoders[1].buf_offset ); case 2: - return snprintf( description, description_length, "Collecting Sweep Data %d/%d", cd->peak_counts, DRPTS ); + if( cd->found_common ) + { + return snprintf( description, description_length, "Collecting Sweep Data %d/%d", cd->peak_counts, DRPTS ); + } + else + { + return snprintf( description, description_length, "Searching for common watchman cal %d/%d", cd->peak_counts, PTS_BEFORE_COMMON ); + } default: return snprintf( description, description_length, "Unkown calibration state" ); } @@ -141,18 +152,44 @@ void survive_cal_angle( struct SurviveObject * so, int sensor_id, int acode, uin if( ct >= DRPTS ) handle_calibration( cd ); //This will also reset all cals. } + + //TODO: Determine if there is a sensor on a watchman visible from both lighthouses. + if( sensid >= 32 && !cd->found_common ) + { + int k; + int ok = 1; + for( k = 0; k < NUM_LIGHTHOUSES; k++ ) + { + + if( cd->all_counts[sensid][k][0] < NEEDED_COMMON_POINTS || cd->all_counts[sensid][k][1] < NEEDED_COMMON_POINTS ) + { + ok = 0; + break; + } + } + if( ok ) cd->found_common = 1; + } + + if( cd->peak_counts > PTS_BEFORE_COMMON && !cd->found_common ) + { + reset_calibration( cd ); + } + break; } } } +static void reset_calibration( struct SurviveCalData * cd ) +{ + memset( cd->all_counts, 0, sizeof( cd->all_counts ) ); + cd->peak_counts = 0; + cd->found_common = 0; +} static void handle_calibration( struct SurviveCalData *cd ) { //Do stuff. - memset( cd->all_lengths, 0, sizeof( cd->all_lengths ) ); - memset( cd->all_angles, 0, sizeof( cd->all_angles ) ); - memset( cd->all_counts, 0, sizeof( cd->all_counts ) ); - cd->peak_counts = 0; + reset_calibration( cd ); } diff --git a/src/survive_cal.h b/src/survive_cal.h index b9d4b11..42ff1ee 100644 --- a/src/survive_cal.h +++ b/src/survive_cal.h @@ -35,11 +35,6 @@ void survive_cal_angle( struct SurviveObject * so, int sensor_id, int acode, uin struct SurviveCalData { - //Stage: - // 0: Idle - // 1: Collecting OOTX data. - int stage; - //OOTX Data is sync'd off of the sync pulses coming from the lighthouses. ootx_decoder_context ootx_decoders[NUM_LIGHTHOUSES]; @@ -47,7 +42,13 @@ struct SurviveCalData FLT all_lengths[MAX_TO_CAL][NUM_LIGHTHOUSES][2][DRPTS]; FLT all_angles[MAX_TO_CAL][NUM_LIGHTHOUSES][2][DRPTS]; int16_t all_counts[MAX_TO_CAL][NUM_LIGHTHOUSES][2]; - int peak_counts; + int16_t peak_counts; + int8_t found_common; + + //Stage: + // 0: Idle + // 1: Collecting OOTX data. + int8_t stage; }; -- cgit v1.3.1