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. --- 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 +++++++++++++++++++++--- 6 files changed, 97 insertions(+), 29 deletions(-) (limited to 'src') 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.2.3