From 4e3ac12016cc489c818709412acd293cac54cb78 Mon Sep 17 00:00:00 2001 From: Joshua Allen Date: Fri, 17 Mar 2017 11:37:30 -0400 Subject: reusable function for decoding acode --- src/survive_data.c | 25 +++++++++++++++++-------- 1 file changed, 17 insertions(+), 8 deletions(-) (limited to 'src') diff --git a/src/survive_data.c b/src/survive_data.c index 4a2cfb6..d658e18 100644 --- a/src/survive_data.c +++ b/src/survive_data.c @@ -5,6 +5,19 @@ #include #include + +int32_t decode_acode(uint32_t length, int32_t main_divisor) { + //+50 adds a small offset and seems to help always get it right. + //Check the +50 in the future to see how well this works on a variety of hardware. + + int32_t acode = (length+main_divisor+50)/(main_divisor*2); + if( acode & 1 ) return -1; + + return (acode>>1) - 6; +} + +} + //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 ) { @@ -112,17 +125,13 @@ void handle_lightcap( SurviveObject * so, LightcapElement * le ) int32_t acode_array[2] = { - (so->last_sync_length[0]+main_divisor+50)/(main_divisor*2), //+50 adds a small offset and seems to help always get it right. - (so->last_sync_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. + decode_acode(so->last_sync_length[0],main_divisor), + decode_acode(so->last_sync_length[1],main_divisor) }; //XXX: TODO: Capture error count here. - if( acode_array[0] & 1 ) return; - if( acode_array[1] & 1 ) return; - - acode_array[0] = (acode_array[0]>>1) - 6; - acode_array[1] = (acode_array[1]>>1) - 6; - + if( acode_array[0] < 0 ) return; + if( acode_array[1] < 0 ) return; int acode = acode_array[0]; -- cgit v1.2.3 From 94be8ccdbfb2f44c9bc569428537444030ba8eeb Mon Sep 17 00:00:00 2001 From: mwturvey Date: Fri, 17 Mar 2017 13:46:34 -0700 Subject: Wired Watchman Now Emit Light & IMU Data --- src/survive_vive.c | 101 +++++++++++++++++++++++++++++++++++++++++++++-------- 1 file changed, 86 insertions(+), 15 deletions(-) (limited to 'src') diff --git a/src/survive_vive.c b/src/survive_vive.c index 1929b1a..d4a853d 100755 --- a/src/survive_vive.c +++ b/src/survive_vive.c @@ -39,13 +39,15 @@ struct SurviveViveData; const short vidpids[] = { - 0x0bb4, 0x2c87, 0, //The main HTC HMD device - 0x28de, 0x2000, 0, //Valve lighthouse + 0x0bb4, 0x2c87, 0, //Valve HMD Button and face proximity sensor + 0x28de, 0x2000, 0, //Valve HMD IMU & Lighthouse Sensors 0x28de, 0x2101, 0, //Valve Watchman 0x28de, 0x2101, 1, //Valve Watchman 0x28de, 0x2022, 0, //HTC Tracker + 0x28de, 0x2012, 0, //Valve Watchman, USB connected #ifdef HIDAPI - 0x28de, 0x2000, 1, //Valve lighthouse(B) (only used on HIDAPI, for lightcap) + 0x28de, 0x2000, 1, //Valve HMD lighthouse(B) (only used on HIDAPI, for lightcap) + 0x28de, 0x2012, 1, //Valve Watchman, USB connected (only used on HIDAPI, for lightcap) #endif }; //length MAX_USB_INTERFACES*2 @@ -55,8 +57,10 @@ const char * devnames[] = { "Watchman 1", "Watchman 2", "Tracker 0", + "Wired Watchman 1", #ifdef HIDAPI - "Lightcap", + "HMD Lightcap", + "Wired Watchman 1 Lightcap", #endif }; //length MAX_USB_INTERFACES @@ -66,12 +70,14 @@ const char * devnames[] = { #define USB_DEV_WATCHMAN1 2 #define USB_DEV_WATCHMAN2 3 #define USB_DEV_TRACKER0 4 +#define USB_DEV_W_WATCHMAN1 5 // Wired Watchman attached via USB #ifdef HIDAPI -#define USB_DEV_LIGHTHOUSEB 5 -#define MAX_USB_DEVS 6 +#define USB_DEV_LIGHTHOUSEB 6 +#define USB_DEV_W_WATCHMAN1_LIGHTCAP 7 +#define MAX_USB_DEVS 8 #else -#define MAX_USB_DEVS 5 +#define MAX_USB_DEVS 6 #endif #define USB_IF_HMD 0 @@ -79,8 +85,10 @@ const char * devnames[] = { #define USB_IF_WATCHMAN1 2 #define USB_IF_WATCHMAN2 3 #define USB_IF_TRACKER0 4 -#define USB_IF_LIGHTCAP 5 -#define MAX_INTERFACES 6 +#define USB_IF_W_WATCHMAN1 5 +#define USB_IF_LIGHTCAP 6 +#define USB_IF_W_WATCHMAN1_LIGHTCAP 7 +#define MAX_INTERFACES 8 typedef struct SurviveUSBInterface SurviveUSBInterface; typedef struct SurviveViveData SurviveViveData; @@ -299,7 +307,7 @@ static inline int hid_get_feature_report_timeout(USBHANDLE device, uint16_t ifac return -1; } -int survive_usb_init( struct SurviveViveData * sv, struct SurviveObject * hmd, struct SurviveObject *wm0, struct SurviveObject * wm1, struct SurviveObject * tr0 ) +int survive_usb_init( struct SurviveViveData * sv, struct SurviveObject * hmd, struct SurviveObject *wm0, struct SurviveObject * wm1, struct SurviveObject * tr0 , struct SurviveObject * ww0 ) { struct SurviveContext * ctx = sv->ctx; #ifdef HIDAPI @@ -463,11 +471,14 @@ int survive_usb_init( struct SurviveViveData * sv, struct SurviveObject * hmd, s if( sv->udev[USB_DEV_WATCHMAN1] && AttachInterface( sv, wm0, USB_IF_WATCHMAN1, sv->udev[USB_DEV_WATCHMAN1], 0x81, survive_data_cb, "Watchman 1" ) ) { return -8; } if( sv->udev[USB_DEV_WATCHMAN2] && AttachInterface( sv, wm1, USB_IF_WATCHMAN2, sv->udev[USB_DEV_WATCHMAN2], 0x81, survive_data_cb, "Watchman 2")) { return -9; } if( sv->udev[USB_DEV_TRACKER0] && AttachInterface( sv, tr0, USB_IF_TRACKER0, sv->udev[USB_DEV_TRACKER0], 0x81, survive_data_cb, "Tracker 1")) { return -10; } + if( sv->udev[USB_DEV_W_WATCHMAN1] && AttachInterface( sv, ww0, USB_IF_W_WATCHMAN1, sv->udev[USB_DEV_W_WATCHMAN1], 0x81, survive_data_cb, "Wired Watchman 1")) { return -11; } #ifdef HIDAPI //Tricky: use other interface for actual lightcap. XXX THIS IS NOT YET RIGHT!!! if( sv->udev[USB_DEV_LIGHTHOUSEB] && AttachInterface( sv, hmd, USB_IF_LIGHTCAP, sv->udev[USB_DEV_LIGHTHOUSEB], 0x82, survive_data_cb, "Lightcap")) { return -12; } + if( sv->udev[USB_DEV_W_WATCHMAN1_LIGHTCAP] && AttachInterface( sv, ww0, USB_IF_W_WATCHMAN1_LIGHTCAP, sv->udev[USB_DEV_W_WATCHMAN1_LIGHTCAP], 0x82, survive_data_cb, "Wired Watchman 1 Lightcap")) { return -13; } #else if( sv->udev[USB_DEV_LIGHTHOUSE] && AttachInterface( sv, hmd, USB_IF_LIGHTCAP, sv->udev[USB_DEV_LIGHTHOUSE], 0x82, survive_data_cb, "Lightcap")) { return -12; } + if( sv->udev[USB_DEV_W_WATCHMAN1] && AttachInterface( sv, ww0, USB_IF_W_WATCHMAN1_LIGHTCAP, sv->udev[USB_DEV_W_WATCHMAN1], 0x82, survive_data_cb, "Wired Watchman 1 Lightcap")) { return -13; } #endif SV_INFO( "All enumerated devices attached." ); @@ -508,8 +519,19 @@ int survive_vive_send_magic(struct SurviveContext * ctx, void * drv, int magic_c if( r != sizeof( vive_magic_enable_lighthouse2 ) ) return 5; } + if (sv->udev[USB_DEV_W_WATCHMAN1]) + { + static uint8_t vive_magic_enable_lighthouse[5] = { 0x04 }; + r = update_feature_report( sv->udev[USB_DEV_W_WATCHMAN1], 0, vive_magic_enable_lighthouse, sizeof( vive_magic_enable_lighthouse ) ); + if( r != sizeof( vive_magic_enable_lighthouse ) ) return 5; + + static uint8_t vive_magic_enable_lighthouse2[5] = { 0x07, 0x02 }; //Switch to 0x25 mode (able to get more light updates) + r = update_feature_report( sv->udev[USB_DEV_W_WATCHMAN1], 0, vive_magic_enable_lighthouse2, sizeof( vive_magic_enable_lighthouse2 ) ); + if( r != sizeof( vive_magic_enable_lighthouse2 ) ) return 5; + } + #if 0 - for( i = 0; i < 256; i++ ) + for( int i = 0; i < 256; i++ ) { static uint8_t vive_controller_haptic_pulse[64] = { 0xff, 0x8f, 0xff, 0, 0, 0, 0, 0, 0, 0 }; r = update_feature_report( sv->udev[USB_DEV_WATCHMAN1], 0, vive_controller_haptic_pulse, sizeof( vive_controller_haptic_pulse ) ); @@ -518,6 +540,14 @@ int survive_vive_send_magic(struct SurviveContext * ctx, void * drv, int magic_c OGUSleep( 1000 ); } #endif + + if (sv->udev[USB_DEV_TRACKER0]) + { + static uint8_t vive_magic_power_on[64] = { 0x04, 0x78, 0x29, 0x38 }; + r = update_feature_report( sv->udev[USB_DEV_TRACKER0], 0, vive_magic_power_on, sizeof( vive_magic_power_on ) ); + if( r != sizeof( vive_magic_power_on ) ) return 5; + } + SV_INFO( "Powered unit on." ); } else @@ -550,7 +580,7 @@ int survive_vive_send_magic(struct SurviveContext * ctx, void * drv, int magic_c if( r != sizeof( vive_magic_power_off2 ) ) return 5; } } - + return 0; } void survive_vive_usb_close( struct SurviveViveData * sv ) @@ -584,6 +614,7 @@ int survive_vive_usb_poll( struct SurviveContext * ctx, void * v ) OGUnlockMutex( GlobalRXUSBMutx ); OGUSleep( 100 ); OGUnlockMutex( GlobalRXUSBMutx ); + return 0; #else struct SurviveViveData * sv = v; int r = libusb_handle_events( sv->usbctx ); @@ -642,6 +673,7 @@ int survive_get_config( char ** config, struct SurviveViveData * sv, int devno, OGUSleep(1000); } + // Send Report 16 to prepare the device for reading config info memset( cfgbuff, 0, sizeof( cfgbuff ) ); cfgbuff[0] = 0x10; if( ( ret = hid_get_feature_report_timeout( dev, iface, cfgbuff, sizeof( cfgbuff ) ) ) < 0 ) @@ -651,6 +683,7 @@ int survive_get_config( char ** config, struct SurviveViveData * sv, int devno, } + // Now do a bunch of Report 17 until there are no bytes left cfgbuff[1] = 0xaa; cfgbuff[0] = 0x11; do @@ -996,6 +1029,7 @@ void survive_data_cb( SurviveUSBInterface * si ) break; } case USB_IF_LIGHTHOUSE: + case USB_IF_W_WATCHMAN1: { int i; //printf( "%d -> ", size ); @@ -1055,7 +1089,7 @@ void survive_data_cb( SurviveUSBInterface * si ) // TODO: Looks like this will need to be handle_tracker, since // it appears the interface is sufficiently different. // More work needd to reverse engineer it. - handle_watchman( w, readdata); + //handle_wired_watchman( w, readdata, size); } else { @@ -1077,6 +1111,20 @@ void survive_data_cb( SurviveUSBInterface * si ) } break; } + case USB_IF_W_WATCHMAN1_LIGHTCAP: + { + int i; + for( i = 0; i < 7; i++ ) + { + LightcapElement le; + le.sensor_id = POP2; + le.length = POP2; + le.timestamp = POP4; + if( le.sensor_id == 0xff ) break; + handle_lightcap( obj, &le ); + } + break; + } } } @@ -1234,6 +1282,8 @@ int survive_vive_close( SurviveContext * ctx, void * driver ) SurviveViveData * sv = driver; survive_vive_usb_close( sv ); + + return 0; } @@ -1244,6 +1294,7 @@ int DriverRegHTCVive( SurviveContext * ctx ) SurviveObject * wm0 = calloc( 1, sizeof( SurviveObject ) ); SurviveObject * wm1 = calloc( 1, sizeof( SurviveObject ) ); SurviveObject * tr0 = calloc( 1, sizeof( SurviveObject ) ); + SurviveObject * ww0 = calloc( 1, sizeof( SurviveObject ) ); SurviveViveData * sv = calloc( 1, sizeof( SurviveViveData ) ); sv->ctx = ctx; @@ -1269,9 +1320,12 @@ int DriverRegHTCVive( SurviveContext * ctx ) tr0->ctx = ctx; memcpy( tr0->codename, "TR0", 4 ); memcpy( tr0->drivername, "HTC", 4 ); + ww0->ctx = ctx; + memcpy( ww0->codename, "WW0", 4 ); + memcpy( ww0->drivername, "HTC", 4 ); //USB must happen last. - if( r = survive_usb_init( sv, hmd, wm0, wm1, tr0) ) + if( r = survive_usb_init( sv, hmd, wm0, wm1, tr0, ww0) ) { //TODO: Cleanup any libUSB stuff sitting around. goto fail_gracefully; @@ -1281,20 +1335,35 @@ int DriverRegHTCVive( SurviveContext * ctx ) if( sv->udev[USB_DEV_HMD] && LoadConfig( sv, hmd, 1, 0, 0 )) { SV_INFO( "HMD config issue." ); } if( sv->udev[USB_DEV_WATCHMAN1] && LoadConfig( sv, wm0, 2, 0, 1 )) { SV_INFO( "Watchman 0 config issue." ); } if( sv->udev[USB_DEV_WATCHMAN2] && LoadConfig( sv, wm1, 3, 0, 1 )) { SV_INFO( "Watchman 1 config issue." ); } - if( sv->udev[USB_DEV_TRACKER0] && LoadConfig( sv, tr0, 4, 0, 1 )) { SV_INFO( "Tracker 0 config issue." ); } + if( sv->udev[USB_DEV_TRACKER0] && LoadConfig( sv, tr0, 4, 0, 0 )) { SV_INFO( "Tracker 0 config issue." ); } + if( sv->udev[USB_DEV_W_WATCHMAN1] && LoadConfig( sv, ww0, 5, 0, 0 )) { SV_INFO( "Wired Watchman 0 config issue." ); } hmd->timebase_hz = wm0->timebase_hz = wm1->timebase_hz = 48000000; + tr0->timebase_hz = ww0->timebase_hz = hmd->timebase_hz; + hmd->pulsedist_max_ticks = wm0->pulsedist_max_ticks = wm1->pulsedist_max_ticks = 500000; + tr0->pulsedist_max_ticks = ww0->pulsedist_max_ticks = hmd->pulsedist_max_ticks; + hmd->pulselength_min_sync = wm0->pulselength_min_sync = wm1->pulselength_min_sync = 2200; + tr0->pulselength_min_sync = ww0->pulselength_min_sync = hmd->pulselength_min_sync; + hmd->pulse_in_clear_time = wm0->pulse_in_clear_time = wm1->pulse_in_clear_time = 35000; + tr0->pulse_in_clear_time = ww0->pulse_in_clear_time = hmd->pulse_in_clear_time; + hmd->pulse_max_for_sweep = wm0->pulse_max_for_sweep = wm1->pulse_max_for_sweep = 1800; + tr0->pulse_max_for_sweep = ww0->pulse_max_for_sweep = hmd->pulse_max_for_sweep; hmd->pulse_synctime_offset = wm0->pulse_synctime_offset = wm1->pulse_synctime_offset = 20000; + tr0->pulse_synctime_offset = ww0->pulse_synctime_offset = hmd->pulse_synctime_offset; + hmd->pulse_synctime_slack = wm0->pulse_synctime_slack = wm1->pulse_synctime_slack = 5000; + tr0->pulse_synctime_slack = ww0->pulse_synctime_slack = hmd->pulse_synctime_slack; hmd->timecenter_ticks = hmd->timebase_hz / 240; wm0->timecenter_ticks = wm0->timebase_hz / 240; wm1->timecenter_ticks = wm1->timebase_hz / 240; + tr0->timecenter_ticks = tr0->timebase_hz / 240; + ww0->timecenter_ticks = ww0->timebase_hz / 240; /* int i; int locs = hmd->nr_locations; @@ -1322,6 +1391,7 @@ int DriverRegHTCVive( SurviveContext * ctx ) if( sv->udev[USB_DEV_WATCHMAN1] ) { survive_add_object( ctx, wm0 ); } if( sv->udev[USB_DEV_WATCHMAN2] ) { survive_add_object( ctx, wm1 ); } if( sv->udev[USB_DEV_TRACKER0] ) { survive_add_object( ctx, tr0 ); } + if( sv->udev[USB_DEV_W_WATCHMAN1] ) { survive_add_object( ctx, ww0 ); } survive_add_driver( ctx, sv, survive_vive_usb_poll, survive_vive_close, survive_vive_send_magic ); @@ -1331,6 +1401,7 @@ fail_gracefully: free( wm0 ); free( wm1 ); free( tr0 ); + free( ww0 ); survive_vive_usb_close( sv ); free( sv ); return -1; -- cgit v1.2.3 From 64e3b67aa0d6a970e1e1988881e77d04c4fb5b07 Mon Sep 17 00:00:00 2001 From: mwturvey Date: Fri, 17 Mar 2017 14:51:00 -0700 Subject: Remove extra debug print --- src/survive_data.c | 1 - 1 file changed, 1 deletion(-) (limited to 'src') diff --git a/src/survive_data.c b/src/survive_data.c index b9099ac..5c5a5e9 100644 --- a/src/survive_data.c +++ b/src/survive_data.c @@ -39,7 +39,6 @@ void handle_lightcap( SurviveObject * so, LightcapElement * le ) // return; //if we don't know what lighthouse this is we don't care to do much else } - printf("m sync %d %d %d %d\n", le->sensor_id, so->last_sync_time[ssn], le->timestamp, delta); if( le->length > so->pulselength_min_sync ) //Pulse longer indicates a sync pulse. { -- cgit v1.2.3 From e1818d84e1ab0ae217d8035f8a12398f06508059 Mon Sep 17 00:00:00 2001 From: mwturvey Date: Fri, 17 Mar 2017 16:32:18 -0700 Subject: Alternate disambiguator and calibration updates --- src/survive_cal.c | 50 ++++++++++++++++++++++++++-------- src/survive_cal.h | 6 +++-- src/survive_data.c | 78 ++++++++++++++++++++++++++++++++++++++++++++++++++++++ 3 files changed, 121 insertions(+), 13 deletions(-) (limited to 'src') diff --git a/src/survive_cal.c b/src/survive_cal.c index 0eb9446..c36a48a 100755 --- a/src/survive_cal.c +++ b/src/survive_cal.c @@ -30,6 +30,11 @@ int mkdir(const char *); #define DRPTS_NEEDED_FOR_AVG ((int)(DRPTS*3/4)) + //at stage 1, is when it branches to stage two or stage 7 + //stage 2 checks for the presence of two watchmen and an HMD visible to both lighthouses. + //Stage 3 collects a bunch of data for statistical averageing + //stage 4 does the calculation for the poses (NOT DONE!) + //Stage 5 = System Calibrate.d static void handle_calibration( struct SurviveCalData *cd ); @@ -117,17 +122,36 @@ void survive_cal_install( struct SurviveContext * ctx ) cd->stage = 1; cd->ctx = ctx; - cd->poseobjects[0] = survive_get_so_by_name( ctx, "HMD" ); - cd->poseobjects[1] = survive_get_so_by_name( ctx, "WM0" ); - cd->poseobjects[2] = survive_get_so_by_name( ctx, "WM1" ); + cd->numPoseObjects = 0; - if( cd->poseobjects[0] == 0 || cd->poseobjects[1] == 0 || cd->poseobjects[2] == 0 ) + for (int i=0; i < ctx->objs_ct; i++) { - SV_ERROR( "Error: cannot find all devices needed for calibration." ); - free( cd ); - return; + // This would be a place where we could conditionally decide if we + // want to include a certain device in the calibration routine. + if (1) + { + cd->poseobjects[i] = ctx->objs[i]; + cd->numPoseObjects++; + + SV_INFO("Calibration is using %s", cd->poseobjects[i]->codename); + } } + // If we want to mandate that certain devices have been found + + + + //cd->poseobjects[0] = survive_get_so_by_name( ctx, "HMD" ); + //cd->poseobjects[1] = survive_get_so_by_name( ctx, "WM0" ); + //cd->poseobjects[2] = survive_get_so_by_name( ctx, "WM1" ); + + //if( cd->poseobjects[0] == 0 || cd->poseobjects[1] == 0 || cd->poseobjects[2] == 0 ) + //{ + // SV_ERROR( "Error: cannot find all devices needed for calibration." ); + // free( cd ); + // return; + //} + //XXX TODO MWTourney, work on your code here. /* if( !cd->hmd ) @@ -185,7 +209,9 @@ void survive_cal_light( struct SurviveObject * so, int sensor_id, int acode, int if( sensor_id < 0 ) { int lhid = -sensor_id-1; - if( lhid < NUM_LIGHTHOUSES && so->codename[0] == 'H' ) + // Take the OOTX data from the first device. + // TODO: Improve this so we'll watch all devices looking for OOTX data. Why limit ourselves to just one? + if( lhid < NUM_LIGHTHOUSES && so == cd->poseobjects[0] ) { uint8_t dbit = (acode & 2)>>1; ootx_pump_bit( &cd->ootx_decoders[lhid], dbit ); @@ -193,7 +219,9 @@ void survive_cal_light( struct SurviveObject * so, int sensor_id, int acode, int 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. + //if( i == NUM_LIGHTHOUSES ) cd->stage = 2; //If all lighthouses have their OOTX set, move on. <------- Revert This!!!!! + if( i == 1 ) + cd->stage = 2; //If all lighthouses have their OOTX set, move on. } break; } @@ -498,11 +526,11 @@ static void handle_calibration( struct SurviveCalData *cd ) int obj; //Poses of lighthouses relative to objects. - SurvivePose objphl[POSE_OBJECTS][NUM_LIGHTHOUSES]; + SurvivePose objphl[MAX_POSE_OBJECTS][NUM_LIGHTHOUSES]; FILE * fobjp = fopen( "calinfo/objposes.csv", "w" ); - for( obj = 0; obj < POSE_OBJECTS; obj++ ) + for( obj = 0; obj < cd->numPoseObjects; obj++ ) { int i, j; PoserDataFullScene fsd; diff --git a/src/survive_cal.h b/src/survive_cal.h index 3d62051..8c488bd 100644 --- a/src/survive_cal.h +++ b/src/survive_cal.h @@ -37,7 +37,7 @@ void survive_cal_angle( SurviveObject * so, int sensor_id, int acode, uint32_t t #define MIN_PTS_BEFORE_CAL 24 #define DRPTS 128 -#define POSE_OBJECTS 3 +#define MAX_POSE_OBJECTS 10 #define MAX_CAL_PT_DAT (MAX_SENSORS_TO_CAL*NUM_LIGHTHOUSES*2) struct SurviveCalData @@ -64,7 +64,9 @@ struct SurviveCalData int senid_of_checkpt; //This is a point on a watchman that can be used to check the lh solution. - SurviveObject * poseobjects[POSE_OBJECTS]; + SurviveObject * poseobjects[MAX_POSE_OBJECTS]; + + size_t numPoseObjects; PoserCB ConfigPoserFn; diff --git a/src/survive_data.c b/src/survive_data.c index 5c5a5e9..e6538d1 100644 --- a/src/survive_data.c +++ b/src/survive_data.c @@ -5,9 +5,87 @@ #include #include +int getAcodeFromSyncPulse(int pulseLen) +{ + if (pulseLen < 3125) return 0; + if (pulseLen < 3625) return 1; + if (pulseLen < 4125) return 2; + if (pulseLen < 4625) return 3; + if (pulseLen < 5125) return 4; + if (pulseLen < 5625) return 5; + if (pulseLen < 6125) return 6; + return 7; +} +void handle_lightcap2_sync(SurviveObject * so, LightcapElement * le ) +{ + fprintf(stderr, "%d\n", le->length); + + if (le->timestamp - so->recent_sync_time < 24000) + { + // I do believe we are lighthouse B + so->last_sync_time[1] = so->recent_sync_time = le->timestamp; + so->last_sync_length[1] = le->length; + if (le->length < 4625) // max non-skip pulse + 1/4 the difference in time between pulses. + { + so->sync_set_number = 1; + so->ctx->lightproc(so, -2, getAcodeFromSyncPulse(le->length), le->length, le->timestamp, le->length); // Don't think I got this quite right. + } + } + else + { + // looks like this is the first sync pulse. Cool! + if (le->length < 4625) // max non-skip pulse + 1/4 the difference in time between pulses. + { + so->sync_set_number = 1; + so->ctx->lightproc(so, -1, getAcodeFromSyncPulse(le->length), le->length, le->timestamp, le->length); // Don't think I got this quite right. + } + } + + +// ctx->lightproc( so, -2, acode_array[1], delta2, so->last_sync_time[1], so->last_sync_length[1] ); + + +// typedef void (*light_process_func)( SurviveObject * so, int sensor_id, int acode, int timeinsweep, uint32_t timecode, uint32_t length ); + +} + +void handle_lightcap2_sweep(SurviveObject * so, LightcapElement * le ) +{ + +} + +void handle_lightcap2( SurviveObject * so, LightcapElement * le ) +{ + SurviveContext * ctx = so->ctx; + + if( le->sensor_id > SENSORS_PER_OBJECT ) + { + return; + } + + if (le->length > 6750) + { + // Should never get a reading so high. Odd. + return; + } + if (le->length >= 2750) + { + // Looks like a sync pulse, process it! + handle_lightcap2_sync(so, le); + } + + // must be a sweep pulse, process it! + handle_lightcap2_sweep(so, le); + +} + + //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 ) { + handle_lightcap2(so,le); + return; + SurviveContext * ctx = so->ctx; //int32_t deltat = (uint32_t)le->timestamp - (uint32_t)so->last_master_time; -- cgit v1.2.3 From 129677f041dc87689e73d85ef737501be59b58ca Mon Sep 17 00:00:00 2001 From: cnlohr Date: Sat, 18 Mar 2017 01:01:49 -0400 Subject: Fix mixed usage of "struct" and typedef'd struct, and fix compile error on gcc/Linux. --- src/survive_vive.c | 32 ++++++++++++++++---------------- 1 file changed, 16 insertions(+), 16 deletions(-) (limited to 'src') diff --git a/src/survive_vive.c b/src/survive_vive.c index a9b295f..91efc2c 100755 --- a/src/survive_vive.c +++ b/src/survive_vive.c @@ -141,10 +141,10 @@ 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, SurviveObject * tr0 ); +int survive_usb_init( SurviveViveData * sv, SurviveObject * hmd, SurviveObject *wm0, SurviveObject * wm1, SurviveObject * tr0 , SurviveObject * ww0 ); int survive_usb_poll( SurviveContext * ctx ); int survive_get_config( char ** config, SurviveViveData * ctx, int devno, int iface, int send_extra_magic ); -int survive_vive_send_magic(struct SurviveContext * ctx, void * drv, int magic_code, void * data, int datalen ); +int survive_vive_send_magic(SurviveContext * ctx, void * drv, int magic_code, void * data, int datalen ); #ifdef HIDAPI void * HAPIReceiver( void * v ) @@ -177,8 +177,8 @@ void * HAPIReceiver( void * v ) #else static void handle_transfer(struct libusb_transfer* transfer) { - struct SurviveUSBInterface * iface = transfer->user_data; - struct SurviveContext * ctx = iface->ctx; + SurviveUSBInterface * iface = transfer->user_data; + SurviveContext * ctx = iface->ctx; if( transfer->status != LIBUSB_TRANSFER_COMPLETED ) { @@ -307,9 +307,9 @@ static inline int hid_get_feature_report_timeout(USBHANDLE device, uint16_t ifac return -1; } -int survive_usb_init( struct SurviveViveData * sv, struct SurviveObject * hmd, struct SurviveObject *wm0, struct SurviveObject * wm1, struct SurviveObject * tr0 , struct SurviveObject * ww0 ) +int survive_usb_init( SurviveViveData * sv, SurviveObject * hmd, SurviveObject *wm0, SurviveObject * wm1, SurviveObject * tr0 , SurviveObject * ww0 ) { - struct SurviveContext * ctx = sv->ctx; + SurviveContext * ctx = sv->ctx; #ifdef HIDAPI if( !GlobalRXUSBMutx ) { @@ -488,10 +488,10 @@ int survive_usb_init( struct SurviveViveData * sv, struct SurviveObject * hmd, s return 0; } -int survive_vive_send_magic(struct SurviveContext * ctx, void * drv, int magic_code, void * data, int datalen ) +int survive_vive_send_magic(SurviveContext * ctx, void * drv, int magic_code, void * data, int datalen ) { int r; - struct SurviveViveData * sv = drv; + SurviveViveData * sv = drv; printf( "*CALLING %p %p\n", ctx, sv ); //XXX TODO: Handle haptics, etc. @@ -583,7 +583,7 @@ int survive_vive_send_magic(struct SurviveContext * ctx, void * drv, int magic_c return 0; } -void survive_vive_usb_close( struct SurviveViveData * sv ) +void survive_vive_usb_close( SurviveViveData * sv ) { int i; #ifdef HIDAPI @@ -608,7 +608,7 @@ void survive_vive_usb_close( struct SurviveViveData * sv ) #endif } -int survive_vive_usb_poll( struct SurviveContext * ctx, void * v ) +int survive_vive_usb_poll( SurviveContext * ctx, void * v ) { #ifdef HIDAPI OGUnlockMutex( GlobalRXUSBMutx ); @@ -616,11 +616,11 @@ int survive_vive_usb_poll( struct SurviveContext * ctx, void * v ) OGUnlockMutex( GlobalRXUSBMutx ); return 0; #else - struct SurviveViveData * sv = v; + SurviveViveData * sv = v; int r = libusb_handle_events( sv->usbctx ); if( r ) { - struct SurviveContext * ctx = sv->ctx; + SurviveContext * ctx = sv->ctx; SV_ERROR( "Libusb poll failed." ); } return r; @@ -629,9 +629,9 @@ int survive_vive_usb_poll( struct SurviveContext * ctx, void * v ) } -int survive_get_config( char ** config, struct SurviveViveData * sv, int devno, int iface, int send_extra_magic ) +int survive_get_config( char ** config, SurviveViveData * sv, int devno, int iface, int send_extra_magic ) { - struct SurviveContext * ctx = sv->ctx; + SurviveContext * ctx = sv->ctx; int ret, count = 0, size = 0; uint8_t cfgbuff[64]; uint8_t compressed_data[8192]; @@ -749,7 +749,7 @@ int survive_get_config( char ** config, struct SurviveViveData * sv, int devno, #define POP2 (*(((uint16_t*)((readdata+=2)-2)))) #define POP4 (*(((uint32_t*)((readdata+=4)-4)))) -static void handle_watchman( struct SurviveObject * w, uint8_t * readdata ) +static void handle_watchman( SurviveObject * w, uint8_t * readdata ) { uint8_t startread[29]; @@ -1015,7 +1015,7 @@ void survive_data_cb( SurviveUSBInterface * si ) { case USB_IF_HMD: { - struct SurviveObject * headset = obj; + SurviveObject * headset = obj; readdata+=2; headset->buttonmask = POP1; //Lens headset->axis2 = POP2; //Lens Separation -- cgit v1.2.3 From af81a4dbe0e58e94b61b11c14b21a2decc15d823 Mon Sep 17 00:00:00 2001 From: cnlohr Date: Sat, 18 Mar 2017 13:15:22 -0400 Subject: Fix unititialized array. --- src/survive_vive.c | 5 +++-- 1 file changed, 3 insertions(+), 2 deletions(-) (limited to 'src') diff --git a/src/survive_vive.c b/src/survive_vive.c index 91efc2c..c8e2b55 100755 --- a/src/survive_vive.c +++ b/src/survive_vive.c @@ -920,7 +920,7 @@ static void handle_watchman( SurviveObject * w, uint8_t * readdata ) { uint8_t *marked; marked = alloca(nrtime); - memset( marked, 0, sizeof( marked ) ); + memset( marked, 0, sizeof( nrtime ) ); int i, parpl = 0; timecount--; int timepl = 0; @@ -935,6 +935,7 @@ static void handle_watchman( SurviveObject * w, uint8_t * readdata ) while( marked[timepl] ) timepl++; if( timepl > timecount ) { fault = 3; goto end; } //Ran off max of list. uint32_t endtime = times[timepl++]; + int end = timepl + adv; if( end > timecount ) { fault = 4; goto end; } //end referencing off list if( marked[end] > 0 ) { fault = 5; goto end; } //Already marked trying to be used. @@ -978,7 +979,7 @@ static void handle_watchman( SurviveObject * w, uint8_t * readdata ) end: { SurviveContext * ctx = w->ctx; - SV_INFO( "Light decoding fault: %d\n", fault ); + SV_INFO( "Light decoding fault: %d", fault ); } } } -- cgit v1.2.3 From f976a532970afecce640ddfba88a6f1b04ab34b8 Mon Sep 17 00:00:00 2001 From: cnlohr Date: Sat, 18 Mar 2017 13:21:13 -0400 Subject: Wow... Actually initilize to right size. --- src/survive_vive.c | 16 ++++++++++++++-- 1 file changed, 14 insertions(+), 2 deletions(-) (limited to 'src') diff --git a/src/survive_vive.c b/src/survive_vive.c index c8e2b55..f6465b2 100755 --- a/src/survive_vive.c +++ b/src/survive_vive.c @@ -771,7 +771,7 @@ static void handle_watchman( SurviveObject * w, uint8_t * readdata ) qty-=2; int propset = 0; int doimu = 0; - + int i; if( (type & 0xf0) == 0xf0 ) { @@ -916,11 +916,12 @@ static void handle_watchman( SurviveObject * w, uint8_t * readdata ) LightcapElement les[10]; int lese = 0; //les's end + //Second, go through all LEDs and extract the lightevent from them. { uint8_t *marked; marked = alloca(nrtime); - memset( marked, 0, sizeof( nrtime ) ); + memset( marked, 0, nrtime ); int i, parpl = 0; timecount--; int timepl = 0; @@ -933,6 +934,17 @@ static void handle_watchman( SurviveObject * w, uint8_t * readdata ) led >>= 3; while( marked[timepl] ) timepl++; + +#ifdef DEBUG_WATCHMAN + int i; + printf( "TP %d TC: %d : ", timepl, timecount ); + for( i = 0; i < nrtime; i++ ) + { + printf( "%d", marked[i] ); + } + printf( "\n" ); +#endif + if( timepl > timecount ) { fault = 3; goto end; } //Ran off max of list. uint32_t endtime = times[timepl++]; -- cgit v1.2.3 From 91a6bb98e0531bda74efc682223de14a1787fede Mon Sep 17 00:00:00 2001 From: cnlohr Date: Sat, 18 Mar 2017 15:20:30 -0400 Subject: Fix histograms --- src/survive_cal.c | 3 ++- 1 file changed, 2 insertions(+), 1 deletion(-) (limited to 'src') diff --git a/src/survive_cal.c b/src/survive_cal.c index 0eb9446..9dc3af2 100755 --- a/src/survive_cal.c +++ b/src/survive_cal.c @@ -272,6 +272,7 @@ void survive_cal_angle( struct SurviveObject * so, int sensor_id, int acode, uin if( sensors_visible < MIN_SENSORS_VISIBLE_PER_LH_FOR_CAL ) { //printf( "Dev %d, LH %d not enough visible points found.\n", i, j ); + reset_calibration( cd ); cd->found_common = 0; return; } @@ -440,7 +441,7 @@ static void handle_calibration( struct SurviveCalData *cd ) FLT stddevlen = 0; #define HISTOGRAMSIZE 31 - #define HISTOGRAMBINANG 0.00001 //TODO: Tune + #define HISTOGRAMBINANG ((3.14159)/400000.0) //TODO: Tune int histo[HISTOGRAMSIZE]; memset( histo, 0, sizeof( histo ) ); -- cgit v1.2.3 From 67ccb935ef12d9ab25f8223dea5208b96ae5052f Mon Sep 17 00:00:00 2001 From: cnlohr Date: Sat, 18 Mar 2017 15:20:50 -0400 Subject: increase time for integrating points. --- src/survive_cal.h | 4 +++- 1 file changed, 3 insertions(+), 1 deletion(-) (limited to 'src') diff --git a/src/survive_cal.h b/src/survive_cal.h index 3d62051..701dc60 100644 --- a/src/survive_cal.h +++ b/src/survive_cal.h @@ -35,7 +35,9 @@ void survive_cal_angle( SurviveObject * so, int sensor_id, int acode, uint32_t t #define MAX_SENSORS_TO_CAL 96 #define MIN_PTS_BEFORE_CAL 24 -#define DRPTS 128 + + +#define DRPTS 1024 //Number of samples required in collection phase. #define POSE_OBJECTS 3 -- cgit v1.2.3 From 0f94e39d96fbc2744e61b85d2884250d0783eb11 Mon Sep 17 00:00:00 2001 From: cnlohr Date: Sat, 18 Mar 2017 15:21:24 -0400 Subject: Add more possible debugging if turned on. --- src/survive_data.c | 22 ++++++++++++++++++---- 1 file changed, 18 insertions(+), 4 deletions(-) (limited to 'src') diff --git a/src/survive_data.c b/src/survive_data.c index 5c5a5e9..0789203 100644 --- a/src/survive_data.c +++ b/src/survive_data.c @@ -8,17 +8,31 @@ //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 ) { - 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' ) - - if( le->sensor_id > SENSORS_PER_OBJECT ) { return; } +#if 0 + if( so->codename[0] == 'H' ) + { + static int lt; + static int last; + if( le->length > 1000 ) + { + int dl = le->timestamp - lt; + lt = le->timestamp; + if( dl > 10000 || dl < -10000 ) + printf( "+++%s %3d %5d %9d ", so->codename, le->sensor_id, le->length, dl ); + if( dl > 100000 ) printf(" \n" ); + } + last=le->length; + } +#endif + so->tsl = le->timestamp; if( le->length < 20 ) return; ///Assuming 20 is an okay value for here. -- cgit v1.2.3 From d4e7bc4037f9e0b4af3f11940b53520eb9f82ff6 Mon Sep 17 00:00:00 2001 From: Joshua Allen Date: Sat, 18 Mar 2017 20:25:06 -0400 Subject: fix compile --- src/survive_data.c | 2 -- 1 file changed, 2 deletions(-) (limited to 'src') diff --git a/src/survive_data.c b/src/survive_data.c index 16a22e9..ffeacff 100644 --- a/src/survive_data.c +++ b/src/survive_data.c @@ -16,8 +16,6 @@ int32_t decode_acode(uint32_t length, int32_t main_divisor) { return (acode>>1) - 6; } -} - //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 ) { -- cgit v1.2.3 From 8253e3c3adbb54ebc3d3c8e02c017e900a83edb0 Mon Sep 17 00:00:00 2001 From: cnlohr Date: Mon, 20 Mar 2017 01:42:32 -0400 Subject: log synctimes. --- src/survive_cal.c | 56 ++++++++++++++++++++++++++++++++++++++++++++++++++- src/survive_cal.h | 3 +++ src/survive_data.c | 10 ++++++++- src/survive_process.c | 3 +++ 4 files changed, 70 insertions(+), 2 deletions(-) (limited to 'src') diff --git a/src/survive_cal.c b/src/survive_cal.c index 9dc3af2..04931cc 100755 --- a/src/survive_cal.c +++ b/src/survive_cal.c @@ -196,6 +196,21 @@ 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 3: //Look for light sync lengths. + { + if( acode >= -2 ) break; + else if( acode < -4 ) break; + int lh = (-acode) - 3; + + if( strcmp( so->codename, "WM0" ) == 0 ) + sensor_id += 32; + if( strcmp( so->codename, "WM1" ) == 0 ) + sensor_id += 64; + + cd->all_sync_times[sensor_id][lh][cd->all_sync_counts[sensor_id][lh]++] = length; + break; + } + } } @@ -333,6 +348,8 @@ static void reset_calibration( struct SurviveCalData * cd ) cd->found_common = 0; cd->times_found_common = 0; cd->stage = 2; + + memset( cd->all_sync_counts, 0, sizeof( cd->all_sync_counts ) ); } static void handle_calibration( struct SurviveCalData *cd ) @@ -358,9 +375,45 @@ static void handle_calibration( struct SurviveCalData *cd ) #else mkdir( "calinfo", 0755 ); #endif + int sen, axis, lh; + FLT temp_syncs[SENSORS_PER_OBJECT][NUM_LIGHTHOUSES]; + + //Just to get it out of the way early, we'll calculate the sync-pulse-lengths here. + FILE * sync_time_info = fopen( "calinfo/synctime.csv", "w" ); + + for( sen = 0; sen < MAX_SENSORS_TO_CAL; sen++ ) + for( lh = 0; lh < NUM_LIGHTHOUSES; lh++ ) + { + int count = cd->all_sync_counts[sen][lh]; + int i; + double totaltime; + + totaltime = 0; + + if( count < 20 ) continue; + for( i = 0; i < count; i++ ) + { + totaltime += cd->all_sync_times[sen][lh][i]; + } + FLT avg = totaltime/count; + + double stddev = 0.0; + for( i = 0; i < count; i++ ) + { + stddev += (cd->all_sync_times[sen][lh][i] - avg)*(cd->all_sync_times[sen][lh][i] - avg); + } + stddev /= count; + + fprintf( sync_time_info, "%d %d %f %d %f\n", sen, lh, totaltime/count, count, stddev ); + } + + fclose( sync_time_info ); + + + + FILE * hists = fopen( "calinfo/histograms.csv", "w" ); FILE * ptinfo = fopen( "calinfo/ptinfo.csv", "w" ); - int sen, axis, lh; for( sen = 0; sen < MAX_SENSORS_TO_CAL; sen++ ) for( lh = 0; lh < NUM_LIGHTHOUSES; lh++ ) for( axis = 0; axis < 2; axis++ ) @@ -526,6 +579,7 @@ static void handle_calibration( struct SurviveCalData *cd ) fsd.lengths[i][j][1] = cd->avglens[dataindex+1]; fsd.angles[i][j][0] = cd->avgsweeps[dataindex+0]; fsd.angles[i][j][1] = cd->avgsweeps[dataindex+1]; + fsd.synctimes[i][j] = temp_syncs[i][j]; } int r = cd->ConfigPoserFn( cd->poseobjects[obj], (PoserData*)&fsd ); diff --git a/src/survive_cal.h b/src/survive_cal.h index 701dc60..c64b8f9 100644 --- a/src/survive_cal.h +++ b/src/survive_cal.h @@ -56,6 +56,9 @@ struct SurviveCalData int8_t found_common; int8_t times_found_common; + FLT all_sync_times[MAX_SENSORS_TO_CAL][NUM_LIGHTHOUSES][DRPTS]; + int16_t all_sync_counts[MAX_SENSORS_TO_CAL][NUM_LIGHTHOUSES]; + //For camfind (4+) //Index is calculated with: int dataindex = sen*(2*NUM_LIGHTHOUSES)+lh*2+axis; FLT avgsweeps[MAX_CAL_PT_DAT]; diff --git a/src/survive_data.c b/src/survive_data.c index ffeacff..8c5c646 100644 --- a/src/survive_data.c +++ b/src/survive_data.c @@ -69,7 +69,7 @@ void handle_lightcap( SurviveObject * so, LightcapElement * le ) { int is_new_pulse = delta > so->pulselength_min_sync /*1500*/ + last_sync_length; - //printf("m sync %d %d %d %d\n", le->sensor_id, so->last_sync_time[ssn], le->timestamp, delta); + so->did_handle_ootx = 0; @@ -114,6 +114,14 @@ void handle_lightcap( SurviveObject * so, LightcapElement * le ) } } } + + //Extra tidbit for storing length-of-sync-pulses. + { + int32_t main_divisor = so->timebase_hz / 384000; //125 @ 48 MHz. + int base_station = is_new_pulse; + printf( "%s %d %d %d\n", so->codename, le->sensor_id, so->sync_set_number, le->length ); + ctx->lightproc( so, le->sensor_id, -3 - so->sync_set_number, 0, le->timestamp, le->length ); + } } diff --git a/src/survive_process.c b/src/survive_process.c index 8dc849a..9295638 100644 --- a/src/survive_process.c +++ b/src/survive_process.c @@ -16,6 +16,9 @@ void survive_default_light_process( SurviveObject * so, int sensor_id, int acode survive_cal_light( so, sensor_id, acode, timeinsweep, timecode, length ); } + //We don't use sync times, yet. + if( acode < -1 ) return; + if( base_station > NUM_LIGHTHOUSES ) return; //No loner need sync information past this point. -- cgit v1.2.3 From 39ef5af74702c8825a82f65cf68e6af875a814ee Mon Sep 17 00:00:00 2001 From: ultramn Date: Thu, 23 Mar 2017 00:41:22 -0400 Subject: Added a dynamic plotting tool for OrthoSolve. Charles added orthogonalization to the rotation matrix. --- src/poser_daveortho.c | 85 +++++++++++++++++++++++++++++++++++++++++++++------ src/survive_cal.h | 2 +- src/survive_data.c | 2 +- 3 files changed, 77 insertions(+), 12 deletions(-) (limited to 'src') diff --git a/src/poser_daveortho.c b/src/poser_daveortho.c index e81e154..80f65a9 100644 --- a/src/poser_daveortho.c +++ b/src/poser_daveortho.c @@ -265,7 +265,8 @@ printf("rhat %f %f (len %f)\n", rhat[0][0], rhat[1][0], rhat_len); // FLT ydist1 = 1.0 / uhat_len; //0.25*PI / uhat_len; // FLT ydist2 = 1.0 / rhat_len; //0.25*PI / rhat_len; FLT ydist = 1.0 / urhat_len; - //printf("ydist1 %f ydist2 %f ydist %f\n", ydist1, ydist2, ydist); + printf("ydist %f\n", ydist); +// printf("ydist1 %f ydist2 %f ydist %f\n", ydist1, ydist2, ydist); //-------------------- // Rescale the axies to be of the proper length @@ -282,7 +283,7 @@ printf("rhat %f %f (len %f)\n", rhat[0][0], rhat[1][0], rhat_len); if( x_y != x_y ) x_y = 0; if( y_y != y_y ) y_y = 0; if( z_y != z_y ) z_y = 0; -/* + // Exhaustively flip the minus sign of the z axis until we find the right one . . . FLT bestErr = 9999.0; FLT xy_dot2 = x[0][0]*y[0][0] + x[2][0]*y[2][0]; @@ -302,7 +303,7 @@ printf("rhat %f %f (len %f)\n", rhat[0][0], rhat[1][0], rhat_len); FLT cx,cy,cz; CrossProduct(cx,cy,cz,x[0][0],x_y,x[2][0],y[0][0],y_y,y[2][0]); FLT hand = cx*z[0][0] + cy*z_y + cz*z[2][0]; - printf("err %f hand %f\n", err, hand); +// printf("err %f hand %f\n", err, hand); // If we are the best right-handed frame so far //if (hand > 0 && err < bestErr) { x[1][0]=x_y; y[1][0]=y_y; z[1][0]=z_y; bestErr=err; } @@ -313,9 +314,9 @@ printf("rhat %f %f (len %f)\n", rhat[0][0], rhat[1][0], rhat_len); } x_y = -x_y; } - printf("bestErr %f\n", bestErr); -*/ +// printf("bestErr %f\n", bestErr); +/* //------------------------- // A test version of the rescaling to the proper length //------------------------- @@ -338,7 +339,7 @@ printf("rhat %f %f (len %f)\n", rhat[0][0], rhat[1][0], rhat_len); if( y_y != y_y ) y_y = 0; if( z_y != z_y ) z_y = 0; - printf( "---> %f %f %f\n", x_y, y_y, z_y ); +// printf( "---> %f %f %f\n", x_y, y_y, z_y ); // Exhaustively flip the minus sign of the z axis until we find the right one . . . FLT bestErr = 9999.0; @@ -359,7 +360,7 @@ printf("rhat %f %f (len %f)\n", rhat[0][0], rhat[1][0], rhat_len); FLT cx,cy,cz; CrossProduct(cx,cy,cz,x2[0][0],x_y,x2[2][0],y2[0][0],y_y,y2[2][0]); FLT hand = cx*z2[0][0] + cy*z_y + cz*z2[2][0]; - printf("err %f hand %f\n", err, hand); + //printf("err %f hand %f\n", err, hand); // If we are the best right-handed frame so far if (hand > 0 && err < bestErr) { x2[1][0]=x_y; y2[1][0]=y_y; z2[1][0]=z_y; bestErr=err; } @@ -380,7 +381,7 @@ printf("rhat %f %f (len %f)\n", rhat[0][0], rhat[1][0], rhat_len); } } ydist = bestYdist; - +*/ /* for (i=0; itimebase_hz / 384000; //125 @ 48 MHz. int base_station = is_new_pulse; - printf( "%s %d %d %d\n", so->codename, le->sensor_id, so->sync_set_number, le->length ); + //printf( "%s %d %d %d\n", so->codename, le->sensor_id, so->sync_set_number, le->length ); ctx->lightproc( so, le->sensor_id, -3 - so->sync_set_number, 0, le->timestamp, le->length ); } } -- cgit v1.2.3 From 472d05c2a356ec6d71669d67fb599e401a6f4a76 Mon Sep 17 00:00:00 2001 From: Mike Turvey Date: Thu, 23 Mar 2017 00:25:25 -0700 Subject: Updated disambiguator. Solid OOTX --- src/survive_cal.c | 9 +-- src/survive_data.c | 211 ++++++++++++++++++++++++++++++++++++++++++++++---- src/survive_process.c | 6 +- 3 files changed, 202 insertions(+), 24 deletions(-) (limited to 'src') diff --git a/src/survive_cal.c b/src/survive_cal.c index c36a48a..51d12dd 100755 --- a/src/survive_cal.c +++ b/src/survive_cal.c @@ -190,7 +190,7 @@ void survive_cal_install( struct SurviveContext * ctx ) } -void survive_cal_light( struct SurviveObject * so, int sensor_id, int acode, int timeinsweep, uint32_t timecode, uint32_t length ) +void survive_cal_light( struct SurviveObject * so, int sensor_id, int acode, int timeinsweep, uint32_t timecode, uint32_t length, int lh) { struct SurviveContext * ctx = so->ctx; struct SurviveCalData * cd = ctx->calptr; @@ -210,7 +210,6 @@ void survive_cal_light( struct SurviveObject * so, int sensor_id, int acode, int { int lhid = -sensor_id-1; // Take the OOTX data from the first device. - // TODO: Improve this so we'll watch all devices looking for OOTX data. Why limit ourselves to just one? if( lhid < NUM_LIGHTHOUSES && so == cd->poseobjects[0] ) { uint8_t dbit = (acode & 2)>>1; @@ -219,9 +218,9 @@ void survive_cal_light( struct SurviveObject * so, int sensor_id, int acode, int 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. <------- Revert This!!!!! - if( i == 1 ) - cd->stage = 2; //If all lighthouses have their OOTX set, move on. + if( i == NUM_LIGHTHOUSES ) cd->stage = 2; //If all lighthouses have their OOTX set, move on. <------- Revert This!!!!! + //if( i == 1 ) + //cd->stage = 2; //If all lighthouses have their OOTX set, move on. } break; } diff --git a/src/survive_data.c b/src/survive_data.c index e6538d1..2ef4940 100644 --- a/src/survive_data.c +++ b/src/survive_data.c @@ -5,7 +5,32 @@ #include #include -int getAcodeFromSyncPulse(int pulseLen) +typedef struct +{ + unsigned int sweep_time[SENSORS_PER_OBJECT]; + unsigned int sweep_len[SENSORS_PER_OBJECT]; +} lightcaps_sweep_data; +typedef struct +{ + lightcaps_sweep_data sweep; +} lightcap2_data; + +typedef struct +{ + int recent_sync_time; + int activeLighthouse; + int activeSweepStartTime; + int activeAcode; + + int lh_pulse_len[NUM_LIGHTHOUSES]; + int lh_start_time[NUM_LIGHTHOUSES]; + int current_lh; // used knowing which sync pulse we're looking at. + +} lightcap2_global_data; + +static lightcap2_global_data lcgd = { 0 }; + +int handle_lightcap2_getAcodeFromSyncPulse(int pulseLen) { if (pulseLen < 3125) return 0; if (pulseLen < 3625) return 1; @@ -16,48 +41,201 @@ int getAcodeFromSyncPulse(int pulseLen) if (pulseLen < 6125) return 6; return 7; } +void handle_lightcap2_process_sweep_data(SurviveObject *so) +{ + lightcap2_data *lcd = so->disambiguator_data; + + // look at all of the sensors we found, and process the ones that were hit. + // TODO: find the sensor(s) with the longest pulse length, and assume + // those are the "highest quality". Then, reject any pulses that are sufficiently + // different from those values, assuming that they are reflections. + { + unsigned int longest_pulse = 0; + unsigned int timestamp_of_longest_pulse = 0; + for (int i = 0; i < SENSORS_PER_OBJECT; i++) + { + if (lcd->sweep.sweep_len[i] > longest_pulse) + { + longest_pulse = lcd->sweep.sweep_len[i]; + timestamp_of_longest_pulse = lcd->sweep.sweep_time[i]; + } + } + + for (int i = 0; i < SENSORS_PER_OBJECT; i++) + { + if (lcd->sweep.sweep_len[i] != 0) // if the sensor was hit, process it + { + int offset_from = lcd->sweep.sweep_time[i] - lcgd.activeSweepStartTime + lcd->sweep.sweep_len[i] / 2; + + if (offset_from < 380000 && offset_from > 70000) + { + int timeDelta = abs(timestamp_of_longest_pulse - lcd->sweep.sweep_time[i]); + if (timeDelta < 15000) // if this sweep point is within ~7 degrees of the point with the longest pulse. + { + so->ctx->lightproc(so, i, lcgd.activeAcode, offset_from, lcd->sweep.sweep_time[i], lcd->sweep.sweep_len[i], lcgd.activeLighthouse); + } + } + } + } + } + // clear out sweep data (could probably limit this to only after a "first" sync. + // this is slightly more robust, so doing it here for now. + memset(&(((lightcap2_data*)so->disambiguator_data)->sweep), 0, sizeof(lightcaps_sweep_data)); +} void handle_lightcap2_sync(SurviveObject * so, LightcapElement * le ) { - fprintf(stderr, "%d\n", le->length); + //fprintf(stderr, "%6.6d %4.4d \n", le->timestamp - so->recent_sync_time, le->length); + lightcap2_data *lcd = so->disambiguator_data; - if (le->timestamp - so->recent_sync_time < 24000) + //static unsigned int recent_sync_time = 0; + //static unsigned int recent_sync_count = -1; + //static unsigned int activeSweepStartTime; + + + // Process any sweep data we have + handle_lightcap2_process_sweep_data(so); + + int time_since_last_sync = (le->timestamp - lcgd.recent_sync_time); + + fprintf(stderr, " %2d %8d %d\n", le->sensor_id, time_since_last_sync, le->length); + // need to store up sync pulses, so we can take the earliest starting time for all sensors. + if (time_since_last_sync < 2400) { + lcgd.recent_sync_time = le->timestamp; + // it's the same sync pulse; + so->sync_set_number = 1; + //so->ctx->lightproc(so, recent_sync_count, getAcodeFromSyncPulse(le->length), le->length, le->timestamp, le->length); // Don't think I got this quite right. + so->recent_sync_time = le->timestamp; + + lcgd.lh_pulse_len[lcgd.current_lh] = le->length; + lcgd.lh_start_time[lcgd.current_lh] = le->timestamp; + + int acode = handle_lightcap2_getAcodeFromSyncPulse(le->length); + if (!(acode >> 2 & 1)) // if the skip bit is not set + { + lcgd.activeLighthouse = lcgd.current_lh; + lcgd.activeSweepStartTime = le->timestamp; + lcgd.activeAcode = acode; + } + else + { + lcgd.activeLighthouse = -1; + lcgd.activeSweepStartTime = 0; + lcgd.activeAcode = 0; + } + } + else if (time_since_last_sync < 24000) + { + //recent_sync_count--; + lcgd.recent_sync_time = le->timestamp; // I do believe we are lighthouse B - so->last_sync_time[1] = so->recent_sync_time = le->timestamp; - so->last_sync_length[1] = le->length; - if (le->length < 4625) // max non-skip pulse + 1/4 the difference in time between pulses. + lcgd.current_lh = 1; + lcgd.lh_pulse_len[lcgd.current_lh] = le->length; + lcgd.lh_start_time[lcgd.current_lh] = le->timestamp; + + int acode = handle_lightcap2_getAcodeFromSyncPulse(le->length); + + //{ + //fprintf(stderr, "2"); + //so->ctx->lightproc(so, -2, acode, le->length, le->timestamp, le->length); // Don't think I got this quite right. + + + //} + if (!(acode >> 2 & 1)) // if the skip bit is not set { - so->sync_set_number = 1; - so->ctx->lightproc(so, -2, getAcodeFromSyncPulse(le->length), le->length, le->timestamp, le->length); // Don't think I got this quite right. + if (lcgd.activeLighthouse != -1) + { + // hmm, it appears we got two non-skip pulses at the same time. That should never happen + fprintf(stderr, "WARNING: Two non-skip pulses received on the same cycle!\n"); + } + lcgd.activeLighthouse = 1; + lcgd.activeSweepStartTime = le->timestamp; + lcgd.activeAcode = acode; } + } - else + else if (time_since_last_sync > 370000) { // looks like this is the first sync pulse. Cool! - if (le->length < 4625) // max non-skip pulse + 1/4 the difference in time between pulses. + + // first, send out the sync pulse data for the last round (for OOTX decoding { - so->sync_set_number = 1; - so->ctx->lightproc(so, -1, getAcodeFromSyncPulse(le->length), le->length, le->timestamp, le->length); // Don't think I got this quite right. - } - } + if (lcgd.lh_pulse_len[0] != 0) + { + so->ctx->lightproc( + so, + -1, + handle_lightcap2_getAcodeFromSyncPulse(lcgd.lh_pulse_len[0]), + lcgd.lh_pulse_len[0], + lcgd.lh_start_time[0], + 0, + 0); + } + if (lcgd.lh_pulse_len[1] != 0) + { + so->ctx->lightproc( + so, + -2, + handle_lightcap2_getAcodeFromSyncPulse(lcgd.lh_pulse_len[1]), + lcgd.lh_pulse_len[1], + lcgd.lh_start_time[1], + 0, + 1); + } + } + // initialize here. + memset(&lcgd, 0, sizeof(lcgd)); + lcgd.activeLighthouse = -1; -// ctx->lightproc( so, -2, acode_array[1], delta2, so->last_sync_time[1], so->last_sync_length[1] ); -// typedef void (*light_process_func)( SurviveObject * so, int sensor_id, int acode, int timeinsweep, uint32_t timecode, uint32_t length ); + lcgd.recent_sync_time = le->timestamp; + // I do believe we are lighthouse A + lcgd.current_lh = 0; + lcgd.lh_pulse_len[lcgd.current_lh] = le->length; + lcgd.lh_start_time[lcgd.current_lh] = le->timestamp; + + int acode = handle_lightcap2_getAcodeFromSyncPulse(le->length); + //{ + // so->ctx->lightproc(so, -1, acode, le->length, le->timestamp, le->length); // Don't think I got this quite right. + //} + + if (!(acode >> 2 & 1)) // if the skip bit is not set + { + lcgd.activeLighthouse = 0; + lcgd.activeSweepStartTime = le->timestamp; + lcgd.activeAcode = acode; + } + + } } void handle_lightcap2_sweep(SurviveObject * so, LightcapElement * le ) { + lightcap2_data *lcd = so->disambiguator_data; + // If we see multiple "hits" on the sweep for a given sensor, + // assume that the longest (i.e. strongest signal) is most likely + // the non-reflected signal. + if (lcd->sweep.sweep_len[le->sensor_id] < le->length) + { + lcd->sweep.sweep_len[le->sensor_id] = le->length; + lcd->sweep.sweep_time[le->sensor_id] = le->timestamp; + } } void handle_lightcap2( SurviveObject * so, LightcapElement * le ) { SurviveContext * ctx = so->ctx; + if (so->disambiguator_data == NULL) + { + so->disambiguator_data = malloc(sizeof(lightcap2_data)); + memset(so->disambiguator_data, 0, sizeof(lightcap2_data)); + } + if( le->sensor_id > SENSORS_PER_OBJECT ) { return; @@ -72,6 +250,7 @@ void handle_lightcap2( SurviveObject * so, LightcapElement * le ) { // Looks like a sync pulse, process it! handle_lightcap2_sync(so, le); + return; } // must be a sweep pulse, process it! diff --git a/src/survive_process.c b/src/survive_process.c index 8dc849a..6735f10 100644 --- a/src/survive_process.c +++ b/src/survive_process.c @@ -6,14 +6,14 @@ //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( 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, int lh) { SurviveContext * ctx = so->ctx; - int base_station = acode >> 2; + int base_station = lh; int axis = acode & 1; if( ctx->calptr ) { - survive_cal_light( so, sensor_id, acode, timeinsweep, timecode, length ); + survive_cal_light( so, sensor_id, acode, timeinsweep, timecode, length, lh); } if( base_station > NUM_LIGHTHOUSES ) return; -- cgit v1.2.3 From 7701d14440d5d005b1ec3513ff8646e2f6b101ef Mon Sep 17 00:00:00 2001 From: Mike Turvey Date: Thu, 23 Mar 2017 08:11:36 -0700 Subject: Cleanup --- src/survive_data.c | 27 +++++++++++---------------- 1 file changed, 11 insertions(+), 16 deletions(-) (limited to 'src') diff --git a/src/survive_data.c b/src/survive_data.c index 2ef4940..d92db36 100644 --- a/src/survive_data.c +++ b/src/survive_data.c @@ -69,8 +69,7 @@ void handle_lightcap2_process_sweep_data(SurviveObject *so) if (offset_from < 380000 && offset_from > 70000) { - int timeDelta = abs(timestamp_of_longest_pulse - lcd->sweep.sweep_time[i]); - if (timeDelta < 15000) // if this sweep point is within ~7 degrees of the point with the longest pulse. + if (longest_pulse *10 / 8 < -lcd->sweep.sweep_len[i]) { so->ctx->lightproc(so, i, lcgd.activeAcode, offset_from, lcd->sweep.sweep_time[i], lcd->sweep.sweep_len[i], lcgd.activeLighthouse); } @@ -97,14 +96,13 @@ void handle_lightcap2_sync(SurviveObject * so, LightcapElement * le ) int time_since_last_sync = (le->timestamp - lcgd.recent_sync_time); - fprintf(stderr, " %2d %8d %d\n", le->sensor_id, time_since_last_sync, le->length); + //fprintf(stderr, " %2d %8d %d\n", le->sensor_id, time_since_last_sync, le->length); // need to store up sync pulses, so we can take the earliest starting time for all sensors. if (time_since_last_sync < 2400) { lcgd.recent_sync_time = le->timestamp; // it's the same sync pulse; so->sync_set_number = 1; - //so->ctx->lightproc(so, recent_sync_count, getAcodeFromSyncPulse(le->length), le->length, le->timestamp, le->length); // Don't think I got this quite right. so->recent_sync_time = le->timestamp; lcgd.lh_pulse_len[lcgd.current_lh] = le->length; @@ -126,7 +124,6 @@ void handle_lightcap2_sync(SurviveObject * so, LightcapElement * le ) } else if (time_since_last_sync < 24000) { - //recent_sync_count--; lcgd.recent_sync_time = le->timestamp; // I do believe we are lighthouse B lcgd.current_lh = 1; @@ -135,12 +132,6 @@ void handle_lightcap2_sync(SurviveObject * so, LightcapElement * le ) int acode = handle_lightcap2_getAcodeFromSyncPulse(le->length); - //{ - //fprintf(stderr, "2"); - //so->ctx->lightproc(so, -2, acode, le->length, le->timestamp, le->length); // Don't think I got this quite right. - - - //} if (!(acode >> 2 & 1)) // if the skip bit is not set { if (lcgd.activeLighthouse != -1) @@ -197,9 +188,6 @@ void handle_lightcap2_sync(SurviveObject * so, LightcapElement * le ) lcgd.lh_start_time[lcgd.current_lh] = le->timestamp; int acode = handle_lightcap2_getAcodeFromSyncPulse(le->length); - //{ - // so->ctx->lightproc(so, -1, acode, le->length, le->timestamp, le->length); // Don't think I got this quite right. - //} if (!(acode >> 2 & 1)) // if the skip bit is not set { @@ -207,9 +195,7 @@ void handle_lightcap2_sync(SurviveObject * so, LightcapElement * le ) lcgd.activeSweepStartTime = le->timestamp; lcgd.activeAcode = acode; } - } - } void handle_lightcap2_sweep(SurviveObject * so, LightcapElement * le ) @@ -219,6 +205,15 @@ void handle_lightcap2_sweep(SurviveObject * so, LightcapElement * le ) // If we see multiple "hits" on the sweep for a given sensor, // assume that the longest (i.e. strongest signal) is most likely // the non-reflected signal. + + if (le->length < 80) + { + // this is a low-quality read. Better to throw it out than to use it. + //fprintf(stderr, "%2d %d\n", le->sensor_id, le->length); + return; + } + fprintf(stderr, "%2d %d\n", le->sensor_id, le->length); + if (lcd->sweep.sweep_len[le->sensor_id] < le->length) { lcd->sweep.sweep_len[le->sensor_id] = le->length; -- cgit v1.2.3 From 280a6599fea76a7d2c16cfe0fcc5c8f37fde66de Mon Sep 17 00:00:00 2001 From: mwturvey Date: Thu, 23 Mar 2017 09:47:38 -0700 Subject: More cleanup --- src/survive_cal.c | 2 +- src/survive_cal.h | 2 +- src/survive_data.c | 96 ++++++++++++++++++++++++++------------------------- src/survive_process.c | 2 +- src/survive_vive.c | 2 +- 5 files changed, 53 insertions(+), 51 deletions(-) (limited to 'src') diff --git a/src/survive_cal.c b/src/survive_cal.c index 51d12dd..dfa2e85 100755 --- a/src/survive_cal.c +++ b/src/survive_cal.c @@ -190,7 +190,7 @@ void survive_cal_install( struct SurviveContext * ctx ) } -void survive_cal_light( struct SurviveObject * so, int sensor_id, int acode, int timeinsweep, uint32_t timecode, uint32_t length, int lh) +void survive_cal_light( struct SurviveObject * so, int sensor_id, int acode, int timeinsweep, uint32_t timecode, uint32_t length, uint32_t lh) { struct SurviveContext * ctx = so->ctx; struct SurviveCalData * cd = ctx->calptr; diff --git a/src/survive_cal.h b/src/survive_cal.h index 8c488bd..9d7b3a9 100644 --- a/src/survive_cal.h +++ b/src/survive_cal.h @@ -29,7 +29,7 @@ int survive_cal_get_status( SurviveContext * ctx, char * description, int descri //void survive_cal_teardown( struct SurviveContext * ctx ); //Called from survive_default_light_process -void survive_cal_light( SurviveObject * so, int sensor_id, int acode, int timeinsweep, uint32_t timecode, uint32_t length ); +void survive_cal_light( SurviveObject * so, int sensor_id, int acode, int timeinsweep, uint32_t timecode, uint32_t length, uint32_t lighthouse); void survive_cal_angle( SurviveObject * so, int sensor_id, int acode, uint32_t timecode, FLT length, FLT angle ); #define MAX_SENSORS_TO_CAL 96 diff --git a/src/survive_data.c b/src/survive_data.c index d92db36..00e66f0 100644 --- a/src/survive_data.c +++ b/src/survive_data.c @@ -10,11 +10,6 @@ typedef struct unsigned int sweep_time[SENSORS_PER_OBJECT]; unsigned int sweep_len[SENSORS_PER_OBJECT]; } lightcaps_sweep_data; -typedef struct -{ - lightcaps_sweep_data sweep; -} lightcap2_data; - typedef struct { int recent_sync_time; @@ -28,7 +23,14 @@ typedef struct } lightcap2_global_data; -static lightcap2_global_data lcgd = { 0 }; +typedef struct +{ + lightcaps_sweep_data sweep; + lightcap2_global_data global; +} lightcap2_data; + + +//static lightcap2_global_data lcgd = { 0 }; int handle_lightcap2_getAcodeFromSyncPulse(int pulseLen) { @@ -65,13 +67,13 @@ void handle_lightcap2_process_sweep_data(SurviveObject *so) { if (lcd->sweep.sweep_len[i] != 0) // if the sensor was hit, process it { - int offset_from = lcd->sweep.sweep_time[i] - lcgd.activeSweepStartTime + lcd->sweep.sweep_len[i] / 2; + int offset_from = lcd->sweep.sweep_time[i] - lcd->global.activeSweepStartTime + lcd->sweep.sweep_len[i] / 2; if (offset_from < 380000 && offset_from > 70000) { - if (longest_pulse *10 / 8 < -lcd->sweep.sweep_len[i]) + if (longest_pulse *10 / 8 < lcd->sweep.sweep_len[i]) { - so->ctx->lightproc(so, i, lcgd.activeAcode, offset_from, lcd->sweep.sweep_time[i], lcd->sweep.sweep_len[i], lcgd.activeLighthouse); + so->ctx->lightproc(so, i, lcd->global.activeAcode, offset_from, lcd->sweep.sweep_time[i], lcd->sweep.sweep_len[i], lcd->global.activeLighthouse); } } } @@ -94,54 +96,54 @@ void handle_lightcap2_sync(SurviveObject * so, LightcapElement * le ) // Process any sweep data we have handle_lightcap2_process_sweep_data(so); - int time_since_last_sync = (le->timestamp - lcgd.recent_sync_time); + int time_since_last_sync = (le->timestamp - lcd->global.recent_sync_time); //fprintf(stderr, " %2d %8d %d\n", le->sensor_id, time_since_last_sync, le->length); // need to store up sync pulses, so we can take the earliest starting time for all sensors. if (time_since_last_sync < 2400) { - lcgd.recent_sync_time = le->timestamp; + lcd->global.recent_sync_time = le->timestamp; // it's the same sync pulse; so->sync_set_number = 1; so->recent_sync_time = le->timestamp; - lcgd.lh_pulse_len[lcgd.current_lh] = le->length; - lcgd.lh_start_time[lcgd.current_lh] = le->timestamp; + lcd->global.lh_pulse_len[lcd->global.current_lh] = le->length; + lcd->global.lh_start_time[lcd->global.current_lh] = le->timestamp; int acode = handle_lightcap2_getAcodeFromSyncPulse(le->length); if (!(acode >> 2 & 1)) // if the skip bit is not set { - lcgd.activeLighthouse = lcgd.current_lh; - lcgd.activeSweepStartTime = le->timestamp; - lcgd.activeAcode = acode; + lcd->global.activeLighthouse = lcd->global.current_lh; + lcd->global.activeSweepStartTime = le->timestamp; + lcd->global.activeAcode = acode; } else { - lcgd.activeLighthouse = -1; - lcgd.activeSweepStartTime = 0; - lcgd.activeAcode = 0; + lcd->global.activeLighthouse = -1; + lcd->global.activeSweepStartTime = 0; + lcd->global.activeAcode = 0; } } else if (time_since_last_sync < 24000) { - lcgd.recent_sync_time = le->timestamp; + lcd->global.recent_sync_time = le->timestamp; // I do believe we are lighthouse B - lcgd.current_lh = 1; - lcgd.lh_pulse_len[lcgd.current_lh] = le->length; - lcgd.lh_start_time[lcgd.current_lh] = le->timestamp; + lcd->global.current_lh = 1; + lcd->global.lh_pulse_len[lcd->global.current_lh] = le->length; + lcd->global.lh_start_time[lcd->global.current_lh] = le->timestamp; int acode = handle_lightcap2_getAcodeFromSyncPulse(le->length); if (!(acode >> 2 & 1)) // if the skip bit is not set { - if (lcgd.activeLighthouse != -1) + if (lcd->global.activeLighthouse != -1) { // hmm, it appears we got two non-skip pulses at the same time. That should never happen fprintf(stderr, "WARNING: Two non-skip pulses received on the same cycle!\n"); } - lcgd.activeLighthouse = 1; - lcgd.activeSweepStartTime = le->timestamp; - lcgd.activeAcode = acode; + lcd->global.activeLighthouse = 1; + lcd->global.activeSweepStartTime = le->timestamp; + lcd->global.activeAcode = acode; } } @@ -151,49 +153,49 @@ void handle_lightcap2_sync(SurviveObject * so, LightcapElement * le ) // first, send out the sync pulse data for the last round (for OOTX decoding { - if (lcgd.lh_pulse_len[0] != 0) + if (lcd->global.lh_pulse_len[0] != 0) { so->ctx->lightproc( so, -1, - handle_lightcap2_getAcodeFromSyncPulse(lcgd.lh_pulse_len[0]), - lcgd.lh_pulse_len[0], - lcgd.lh_start_time[0], + handle_lightcap2_getAcodeFromSyncPulse(lcd->global.lh_pulse_len[0]), + lcd->global.lh_pulse_len[0], + lcd->global.lh_start_time[0], 0, 0); } - if (lcgd.lh_pulse_len[1] != 0) + if (lcd->global.lh_pulse_len[1] != 0) { so->ctx->lightproc( so, -2, - handle_lightcap2_getAcodeFromSyncPulse(lcgd.lh_pulse_len[1]), - lcgd.lh_pulse_len[1], - lcgd.lh_start_time[1], + handle_lightcap2_getAcodeFromSyncPulse(lcd->global.lh_pulse_len[1]), + lcd->global.lh_pulse_len[1], + lcd->global.lh_start_time[1], 0, 1); } } // initialize here. - memset(&lcgd, 0, sizeof(lcgd)); - lcgd.activeLighthouse = -1; + memset(&lcd->global, 0, sizeof(lcd->global)); + lcd->global.activeLighthouse = -1; - lcgd.recent_sync_time = le->timestamp; + lcd->global.recent_sync_time = le->timestamp; // I do believe we are lighthouse A - lcgd.current_lh = 0; - lcgd.lh_pulse_len[lcgd.current_lh] = le->length; - lcgd.lh_start_time[lcgd.current_lh] = le->timestamp; + lcd->global.current_lh = 0; + lcd->global.lh_pulse_len[lcd->global.current_lh] = le->length; + lcd->global.lh_start_time[lcd->global.current_lh] = le->timestamp; int acode = handle_lightcap2_getAcodeFromSyncPulse(le->length); if (!(acode >> 2 & 1)) // if the skip bit is not set { - lcgd.activeLighthouse = 0; - lcgd.activeSweepStartTime = le->timestamp; - lcgd.activeAcode = acode; + lcd->global.activeLighthouse = 0; + lcd->global.activeSweepStartTime = le->timestamp; + lcd->global.activeAcode = acode; } } } @@ -383,8 +385,8 @@ void handle_lightcap( SurviveObject * so, LightcapElement * le ) int32_t delta1 = so->last_sync_time[0] - so->recent_sync_time; int32_t delta2 = so->last_sync_time[1] - so->last_sync_time[0]; - ctx->lightproc( so, -1, acode_array[0], delta1, so->last_sync_time[0], so->last_sync_length[0] ); - ctx->lightproc( so, -2, acode_array[1], delta2, so->last_sync_time[1], so->last_sync_length[1] ); + ctx->lightproc( so, -1, acode_array[0], delta1, so->last_sync_time[0], so->last_sync_length[0], 0 ); + ctx->lightproc( so, -2, acode_array[1], delta2, so->last_sync_time[1], so->last_sync_length[1], 1 ); so->recent_sync_time = so->last_sync_time[1]; @@ -427,7 +429,7 @@ void handle_lightcap( SurviveObject * so, LightcapElement * le ) //Make sure pulse is in valid window if( offset_from < 380000 && offset_from > 70000 ) { - ctx->lightproc( so, le->sensor_id, acode, offset_from, le->timestamp, le->length ); + ctx->lightproc( so, le->sensor_id, acode, offset_from, le->timestamp, le->length, so->sync_set_number ); } } else diff --git a/src/survive_process.c b/src/survive_process.c index 6735f10..463481a 100644 --- a/src/survive_process.c +++ b/src/survive_process.c @@ -6,7 +6,7 @@ //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( SurviveObject * so, int sensor_id, int acode, int timeinsweep, uint32_t timecode, uint32_t length, int lh) +void survive_default_light_process( SurviveObject * so, int sensor_id, int acode, int timeinsweep, uint32_t timecode, uint32_t length, uint32_t lh) { SurviveContext * ctx = so->ctx; int base_station = lh; diff --git a/src/survive_vive.c b/src/survive_vive.c index a9b295f..c04fc03 100755 --- a/src/survive_vive.c +++ b/src/survive_vive.c @@ -141,7 +141,7 @@ 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, SurviveObject * tr0 ); +int survive_usb_init( SurviveViveData * sv, SurviveObject * hmd, SurviveObject *wm0, SurviveObject * wm1, SurviveObject * tr0, struct SurviveObject * ww0 ); int survive_usb_poll( SurviveContext * ctx ); int survive_get_config( char ** config, SurviveViveData * ctx, int devno, int iface, int send_extra_magic ); int survive_vive_send_magic(struct SurviveContext * ctx, void * drv, int magic_code, void * data, int datalen ); -- cgit v1.2.3 From f33018188bf5bdf6f6b8af0d646e3f8c519d9d71 Mon Sep 17 00:00:00 2001 From: mwturvey Date: Thu, 23 Mar 2017 12:13:44 -0700 Subject: Added support for empty string in config.json & other cleanup. --- src/survive_cal.c | 50 +++++++++++++++++++++----------------------------- src/survive_config.c | 10 +++++++++- src/survive_data.c | 2 +- 3 files changed, 31 insertions(+), 31 deletions(-) (limited to 'src') diff --git a/src/survive_cal.c b/src/survive_cal.c index 19eb3ca..f9ec595 100755 --- a/src/survive_cal.c +++ b/src/survive_cal.c @@ -124,50 +124,42 @@ void survive_cal_install( struct SurviveContext * ctx ) cd->numPoseObjects = 0; - for (int i=0; i < ctx->objs_ct; i++) + const char * RequiredTrackersForCal = config_read_str( ctx->global_config_values, "RequiredTrackersForCal", "HMD,WM0,WM1" ); + const uint32_t AllowAllTrackersForCal = config_read_uint32( ctx->global_config_values, "AllowAllTrackersForCal", 0 ); + size_t requiredTrackersFound = 0; + + for (int j=0; j < ctx->objs_ct; j++) { - // This would be a place where we could conditionally decide if we - // want to include a certain device in the calibration routine. - if (1) + // Add the tracker if we allow all trackers for calibration, or if it's in the list + // of required trackers. + int isRequiredTracker = strstr(RequiredTrackersForCal, ctx->objs[j]->codename) != NULL; + + if (isRequiredTracker) { - cd->poseobjects[i] = ctx->objs[i]; + requiredTrackersFound++; + } + + if (AllowAllTrackersForCal || isRequiredTracker) + { + cd->poseobjects[j] = ctx->objs[j]; cd->numPoseObjects++; - SV_INFO("Calibration is using %s", cd->poseobjects[i]->codename); + SV_INFO("Calibration is using %s", cd->poseobjects[j]->codename); } + } // If we want to mandate that certain devices have been found - - - //cd->poseobjects[0] = survive_get_so_by_name( ctx, "HMD" ); - //cd->poseobjects[1] = survive_get_so_by_name( ctx, "WM0" ); - //cd->poseobjects[2] = survive_get_so_by_name( ctx, "WM1" ); - - //if( cd->poseobjects[0] == 0 || cd->poseobjects[1] == 0 || cd->poseobjects[2] == 0 ) - //{ - // SV_ERROR( "Error: cannot find all devices needed for calibration." ); - // free( cd ); - // return; - //} - -//XXX TODO MWTourney, work on your code here. -/* - if( !cd->hmd ) + if (strlen(RequiredTrackersForCal) > 0) { - cd->hmd = survive_get_so_by_name( ctx, "TR0" ); - - if( !cd->hmd ) + if (requiredTrackersFound != ((strlen(RequiredTrackersForCal) + 1) / 4)) { - SV_ERROR( "Error: cannot find any devices labeled HMD. Required for calibration" ); + SV_ERROR( "Error: cannot find all devices needed for calibration." ); free( cd ); return; } - SV_INFO( "HMD not found, calibrating using Tracker" ); } -*/ - const char * DriverName; const char * PreferredPoser = config_read_str( ctx->global_config_values, "ConfigPoser", "PoserCharlesSlow" ); diff --git a/src/survive_config.c b/src/survive_config.c index 0810280..005cfaf 100644 --- a/src/survive_config.c +++ b/src/survive_config.c @@ -175,7 +175,13 @@ const char* config_set_str(config_group *cg, const char *tag, const char* value) if (cv == NULL) cv = next_unused_entry(cg); sstrcpy(&(cv->tag), tag); - sstrcpy(&(cv->data), value); + + if (NULL != value){ + sstrcpy(&(cv->data), value); + } + else { + sstrcpy(&(cv->data), ""); + } cv->type = CONFIG_STRING; return value; @@ -357,9 +363,11 @@ void handle_tag_value(char* tag, char** values, uint8_t count) { print_json_value(tag,values,count); config_group* cg = cg_stack[cg_stack_head]; + if (NULL != *values){ if (parse_uint32(tag,values,count) > 0) return; //parse integers first, stricter rules if (parse_floats(tag,values,count) > 0) return; + } //should probably also handle string arrays config_set_str(cg,tag,values[0]); diff --git a/src/survive_data.c b/src/survive_data.c index ee180b1..da4e0a2 100644 --- a/src/survive_data.c +++ b/src/survive_data.c @@ -372,7 +372,7 @@ void handle_lightcap( SurviveObject * so, LightcapElement * le ) int32_t main_divisor = so->timebase_hz / 384000; //125 @ 48 MHz. int base_station = is_new_pulse; //printf( "%s %d %d %d\n", so->codename, le->sensor_id, so->sync_set_number, le->length ); - ctx->lightproc( so, le->sensor_id, -3 - so->sync_set_number, 0, le->timestamp, le->length ); + ctx->lightproc( so, le->sensor_id, -3 - so->sync_set_number, 0, le->timestamp, le->length, base_station); } } -- cgit v1.2.3 From 3d86dda66d50d1e1955d4e0cd5f374e0aed1789f Mon Sep 17 00:00:00 2001 From: mwturvey Date: Thu, 23 Mar 2017 16:26:27 -0700 Subject: Fixed Windows USB Interface Enumeration --- src/survive_cal.c | 4 ++-- src/survive_data.c | 21 +++++++++++---------- src/survive_vive.c | 18 ++++++++++++++---- 3 files changed, 27 insertions(+), 16 deletions(-) (limited to 'src') diff --git a/src/survive_cal.c b/src/survive_cal.c index f9ec595..81889ca 100755 --- a/src/survive_cal.c +++ b/src/survive_cal.c @@ -155,7 +155,7 @@ void survive_cal_install( struct SurviveContext * ctx ) { if (requiredTrackersFound != ((strlen(RequiredTrackersForCal) + 1) / 4)) { - SV_ERROR( "Error: cannot find all devices needed for calibration." ); + SV_ERROR( "Error: Did not find all devices required for calibration." ); free( cd ); return; } @@ -201,7 +201,7 @@ void survive_cal_light( struct SurviveObject * so, int sensor_id, int acode, int if( sensor_id < 0 ) { int lhid = -sensor_id-1; - // Take the OOTX data from the first device. + // Take the OOTX data from the first device. (if using HMD, WM0, WM1 only, this will be HMD) if( lhid < NUM_LIGHTHOUSES && so == cd->poseobjects[0] ) { uint8_t dbit = (acode & 2)>>1; diff --git a/src/survive_data.c b/src/survive_data.c index da4e0a2..ce263ed 100644 --- a/src/survive_data.c +++ b/src/survive_data.c @@ -208,13 +208,14 @@ void handle_lightcap2_sweep(SurviveObject * so, LightcapElement * le ) // assume that the longest (i.e. strongest signal) is most likely // the non-reflected signal. - if (le->length < 80) - { - // this is a low-quality read. Better to throw it out than to use it. - //fprintf(stderr, "%2d %d\n", le->sensor_id, le->length); - return; - } - fprintf(stderr, "%2d %d\n", le->sensor_id, le->length); + //if (le->length < 80) + //{ + // // this is a low-quality read. Better to throw it out than to use it. + // //fprintf(stderr, "%2d %d\n", le->sensor_id, le->length); + // return; + //} + //fprintf(stderr, "%2d %d\n", le->sensor_id, le->length); + //fprintf(stderr, "."); if (lcd->sweep.sweep_len[le->sensor_id] < le->length) { @@ -269,8 +270,8 @@ int32_t decode_acode(uint32_t length, int32_t main_divisor) { void handle_lightcap( SurviveObject * so, LightcapElement * le ) { SurviveContext * ctx = so->ctx; -// handle_lightcap2(so,le); -// return; + handle_lightcap2(so,le); + return; //int32_t deltat = (uint32_t)le->timestamp - (uint32_t)so->last_master_time; @@ -373,7 +374,7 @@ void handle_lightcap( SurviveObject * so, LightcapElement * le ) int base_station = is_new_pulse; //printf( "%s %d %d %d\n", so->codename, le->sensor_id, so->sync_set_number, le->length ); ctx->lightproc( so, le->sensor_id, -3 - so->sync_set_number, 0, le->timestamp, le->length, base_station); - } + } } diff --git a/src/survive_vive.c b/src/survive_vive.c index f6465b2..bfe4926 100755 --- a/src/survive_vive.c +++ b/src/survive_vive.c @@ -340,7 +340,7 @@ int survive_usb_init( SurviveViveData * sv, SurviveObject * hmd, SurviveObject * if (cur_dev->vendor_id == vendor_id && cur_dev->product_id == product_id) { - if( menum == enumid ) + if( cur_dev->interface_number == enumid ) { path_to_open = cur_dev->path; break; @@ -521,12 +521,21 @@ int survive_vive_send_magic(SurviveContext * ctx, void * drv, int magic_code, vo if (sv->udev[USB_DEV_W_WATCHMAN1]) { + //static uint8_t vive_magic_power_on[64] = { 0x04, 0x78, 0x29, 0x38 }; + static uint8_t vive_magic_power_on[5] = { 0x04 }; + //static uint8_t vive_magic_power_on[5] = { 0x04, 0x00, 0xc3, 0xe2, 0x04 }; + r = update_feature_report( sv->udev[USB_DEV_W_WATCHMAN1], 0, vive_magic_power_on, sizeof( vive_magic_power_on ) ); + if( r != sizeof( vive_magic_power_on ) ) return 5; + } + if (sv->udev[USB_DEV_W_WATCHMAN1_LIGHTCAP]) + { + //static uint8_t vive_magic_enable_lighthouse[64] = { 0x04, 0x78, 0x29, 0x38 }; static uint8_t vive_magic_enable_lighthouse[5] = { 0x04 }; - r = update_feature_report( sv->udev[USB_DEV_W_WATCHMAN1], 0, vive_magic_enable_lighthouse, sizeof( vive_magic_enable_lighthouse ) ); + r = update_feature_report( sv->udev[USB_DEV_W_WATCHMAN1_LIGHTCAP], 0, vive_magic_enable_lighthouse, sizeof( vive_magic_enable_lighthouse ) ); if( r != sizeof( vive_magic_enable_lighthouse ) ) return 5; static uint8_t vive_magic_enable_lighthouse2[5] = { 0x07, 0x02 }; //Switch to 0x25 mode (able to get more light updates) - r = update_feature_report( sv->udev[USB_DEV_W_WATCHMAN1], 0, vive_magic_enable_lighthouse2, sizeof( vive_magic_enable_lighthouse2 ) ); + r = update_feature_report( sv->udev[USB_DEV_W_WATCHMAN1_LIGHTCAP], 0, vive_magic_enable_lighthouse2, sizeof( vive_magic_enable_lighthouse2 ) ); if( r != sizeof( vive_magic_enable_lighthouse2 ) ) return 5; } @@ -534,7 +543,8 @@ int survive_vive_send_magic(SurviveContext * ctx, void * drv, int magic_code, vo for( int i = 0; i < 256; i++ ) { static uint8_t vive_controller_haptic_pulse[64] = { 0xff, 0x8f, 0xff, 0, 0, 0, 0, 0, 0, 0 }; - r = update_feature_report( sv->udev[USB_DEV_WATCHMAN1], 0, vive_controller_haptic_pulse, sizeof( vive_controller_haptic_pulse ) ); + //r = update_feature_report( sv->udev[USB_DEV_WATCHMAN1], 0, vive_controller_haptic_pulse, sizeof( vive_controller_haptic_pulse ) ); + r = update_feature_report( sv->udev[USB_DEV_W_WATCHMAN1_LIGHTCAP], 0, vive_controller_haptic_pulse, sizeof( vive_controller_haptic_pulse ) ); SV_INFO( "UCR: %d", r ); if( r != sizeof( vive_controller_haptic_pulse ) ) return 5; OGUSleep( 1000 ); -- cgit v1.2.3 From c55c5928485da0432140be4befe5165c68ae4ab0 Mon Sep 17 00:00:00 2001 From: mwturvey Date: Thu, 23 Mar 2017 16:46:04 -0700 Subject: Change disambiguator filter --- src/survive_data.c | 2 +- src/survive_vive.c | 45 +++++++++++++++++++++++++++++++++------------ 2 files changed, 34 insertions(+), 13 deletions(-) (limited to 'src') diff --git a/src/survive_data.c b/src/survive_data.c index ce263ed..849b722 100644 --- a/src/survive_data.c +++ b/src/survive_data.c @@ -71,7 +71,7 @@ void handle_lightcap2_process_sweep_data(SurviveObject *so) if (offset_from < 380000 && offset_from > 70000) { - if (longest_pulse *10 / 8 < lcd->sweep.sweep_len[i]) + //if (longest_pulse *10 / 8 < lcd->sweep.sweep_len[i]) { so->ctx->lightproc(so, i, lcd->global.activeAcode, offset_from, lcd->sweep.sweep_time[i], lcd->sweep.sweep_len[i], lcd->global.activeLighthouse); } diff --git a/src/survive_vive.c b/src/survive_vive.c index bfe4926..f58fb79 100755 --- a/src/survive_vive.c +++ b/src/survive_vive.c @@ -47,6 +47,7 @@ const short vidpids[] = { 0x28de, 0x2012, 0, //Valve Watchman, USB connected #ifdef HIDAPI 0x28de, 0x2000, 1, //Valve HMD lighthouse(B) (only used on HIDAPI, for lightcap) + 0x28de, 0x2022, 1, //HTC Tracker (only used on HIDAPI, for lightcap) 0x28de, 0x2012, 1, //Valve Watchman, USB connected (only used on HIDAPI, for lightcap) #endif }; //length MAX_USB_INTERFACES*2 @@ -60,6 +61,7 @@ const char * devnames[] = { "Wired Watchman 1", #ifdef HIDAPI "HMD Lightcap", + "Tracker 0 Lightcap", "Wired Watchman 1 Lightcap", #endif }; //length MAX_USB_INTERFACES @@ -74,8 +76,9 @@ const char * devnames[] = { #ifdef HIDAPI #define USB_DEV_LIGHTHOUSEB 6 -#define USB_DEV_W_WATCHMAN1_LIGHTCAP 7 -#define MAX_USB_DEVS 8 +#define USB_DEV_TRACKER0_LIGHTCAP 7 +#define USB_DEV_W_WATCHMAN1_LIGHTCAP 8 +#define MAX_USB_DEVS 9 #else #define MAX_USB_DEVS 6 #endif @@ -87,8 +90,9 @@ const char * devnames[] = { #define USB_IF_TRACKER0 4 #define USB_IF_W_WATCHMAN1 5 #define USB_IF_LIGHTCAP 6 -#define USB_IF_W_WATCHMAN1_LIGHTCAP 7 -#define MAX_INTERFACES 8 +#define USB_IF_TRACKER0_LIGHTCAP 7 +#define USB_IF_W_WATCHMAN1_LIGHTCAP 8 +#define MAX_INTERFACES 9 typedef struct SurviveUSBInterface SurviveUSBInterface; typedef struct SurviveViveData SurviveViveData; @@ -475,9 +479,11 @@ int survive_usb_init( SurviveViveData * sv, SurviveObject * hmd, SurviveObject * #ifdef HIDAPI //Tricky: use other interface for actual lightcap. XXX THIS IS NOT YET RIGHT!!! if( sv->udev[USB_DEV_LIGHTHOUSEB] && AttachInterface( sv, hmd, USB_IF_LIGHTCAP, sv->udev[USB_DEV_LIGHTHOUSEB], 0x82, survive_data_cb, "Lightcap")) { return -12; } + if( sv->udev[USB_DEV_TRACKER0_LIGHTCAP] && AttachInterface( sv, tr0, USB_IF_TRACKER0_LIGHTCAP, sv->udev[USB_DEV_TRACKER0_LIGHTCAP], 0x82, survive_data_cb, "Tracker 1 Lightcap")) { return -13; } if( sv->udev[USB_DEV_W_WATCHMAN1_LIGHTCAP] && AttachInterface( sv, ww0, USB_IF_W_WATCHMAN1_LIGHTCAP, sv->udev[USB_DEV_W_WATCHMAN1_LIGHTCAP], 0x82, survive_data_cb, "Wired Watchman 1 Lightcap")) { return -13; } #else if( sv->udev[USB_DEV_LIGHTHOUSE] && AttachInterface( sv, hmd, USB_IF_LIGHTCAP, sv->udev[USB_DEV_LIGHTHOUSE], 0x82, survive_data_cb, "Lightcap")) { return -12; } + if( sv->udev[USB_DEV_TRACKER0] && AttachInterface( sv, ww0, USB_IF_TRACKER0_LIGHTCAP, sv->udev[USB_DEV_TRACKER0], 0x82, survive_data_cb, "Tracker 0 Lightcap")) { return -13; } if( sv->udev[USB_DEV_W_WATCHMAN1] && AttachInterface( sv, ww0, USB_IF_W_WATCHMAN1_LIGHTCAP, sv->udev[USB_DEV_W_WATCHMAN1], 0x82, survive_data_cb, "Wired Watchman 1 Lightcap")) { return -13; } #endif SV_INFO( "All enumerated devices attached." ); @@ -521,9 +527,7 @@ int survive_vive_send_magic(SurviveContext * ctx, void * drv, int magic_code, vo if (sv->udev[USB_DEV_W_WATCHMAN1]) { - //static uint8_t vive_magic_power_on[64] = { 0x04, 0x78, 0x29, 0x38 }; static uint8_t vive_magic_power_on[5] = { 0x04 }; - //static uint8_t vive_magic_power_on[5] = { 0x04, 0x00, 0xc3, 0xe2, 0x04 }; r = update_feature_report( sv->udev[USB_DEV_W_WATCHMAN1], 0, vive_magic_power_on, sizeof( vive_magic_power_on ) ); if( r != sizeof( vive_magic_power_on ) ) return 5; } @@ -539,6 +543,23 @@ int survive_vive_send_magic(SurviveContext * ctx, void * drv, int magic_code, vo if( r != sizeof( vive_magic_enable_lighthouse2 ) ) return 5; } + if (sv->udev[USB_DEV_TRACKER0]) + { + static uint8_t vive_magic_power_on[5] = { 0x04 }; + r = update_feature_report( sv->udev[USB_DEV_TRACKER0], 0, vive_magic_power_on, sizeof( vive_magic_power_on ) ); + if( r != sizeof( vive_magic_power_on ) ) return 5; + } + if (sv->udev[USB_DEV_TRACKER0_LIGHTCAP]) + { + static uint8_t vive_magic_enable_lighthouse[5] = { 0x04 }; + r = update_feature_report( sv->udev[USB_DEV_TRACKER0_LIGHTCAP], 0, vive_magic_enable_lighthouse, sizeof( vive_magic_enable_lighthouse ) ); + if( r != sizeof( vive_magic_enable_lighthouse ) ) return 5; + + static uint8_t vive_magic_enable_lighthouse2[5] = { 0x07, 0x02 }; //Switch to 0x25 mode (able to get more light updates) + r = update_feature_report( sv->udev[USB_DEV_TRACKER0_LIGHTCAP], 0, vive_magic_enable_lighthouse2, sizeof( vive_magic_enable_lighthouse2 ) ); + if( r != sizeof( vive_magic_enable_lighthouse2 ) ) return 5; + } + #if 0 for( int i = 0; i < 256; i++ ) { @@ -551,12 +572,12 @@ int survive_vive_send_magic(SurviveContext * ctx, void * drv, int magic_code, vo } #endif - if (sv->udev[USB_DEV_TRACKER0]) - { - static uint8_t vive_magic_power_on[64] = { 0x04, 0x78, 0x29, 0x38 }; - r = update_feature_report( sv->udev[USB_DEV_TRACKER0], 0, vive_magic_power_on, sizeof( vive_magic_power_on ) ); - if( r != sizeof( vive_magic_power_on ) ) return 5; - } + //if (sv->udev[USB_DEV_TRACKER0]) + //{ + // static uint8_t vive_magic_power_on[64] = { 0x04, 0x78, 0x29, 0x38 }; + // r = update_feature_report( sv->udev[USB_DEV_TRACKER0], 0, vive_magic_power_on, sizeof( vive_magic_power_on ) ); + // if( r != sizeof( vive_magic_power_on ) ) return 5; + //} SV_INFO( "Powered unit on." ); } -- cgit v1.2.3 From 5404526ae8da8c5fdff81b8ee8120ffe73647747 Mon Sep 17 00:00:00 2001 From: cnlohr Date: Fri, 24 Mar 2017 01:05:07 -0400 Subject: Dave's affine solve is getting close. --- src/poser_daveortho.c | 68 +++++++++++++++++++++++++++++++++++++-------------- 1 file changed, 50 insertions(+), 18 deletions(-) (limited to 'src') diff --git a/src/poser_daveortho.c b/src/poser_daveortho.c index 80f65a9..906b21e 100644 --- a/src/poser_daveortho.c +++ b/src/poser_daveortho.c @@ -456,41 +456,73 @@ PRINT(ab,2,1); //------------------- // Orthogonalize the matrix //------------------- - - PRINT_MAT(T,4,4); -matrix44transpose(T2, T); -//matrix44copy(T2,T); - cross3d( &T2[0][0], &T2[1][0], &T2[2][0] ); - cross3d( &T2[2][0], &T2[0][0], &T2[1][0] ); - normalize3d( &T2[0][0], &T2[0][0] ); - normalize3d( &T2[1][0], &T2[1][0] ); - normalize3d( &T2[2][0], &T2[2][0] ); -//matrix44copy(T2,T); -matrix44transpose(T, T2); PRINT_MAT(T,4,4); +#if 1 +// matrix44transpose(T2, T); //Transpose so we are + matrix44copy(T2,T); + cross3d( &T2[1][0], &T2[0][0], &T2[2][0] ); + cross3d( &T2[2][0], &T2[1][0], &T2[0][0] ); //Replace axes in-place. + matrix44copy(T,T2); +// matrix44transpose(T, T2); + +#endif + + normalize3d( &T[0][0], &T[0][0] ); + normalize3d( &T[1][0], &T[1][0] ); + normalize3d( &T[2][0], &T[2][0] ); + //Change handedness + + T[1][0]*=-1; + T[1][1]*=-1; + T[1][2]*=-1; + +/* + //Check Orthogonality. Yep. It's orthogonal. + FLT tmp[3]; + cross3d( tmp, &T[0][0], &T[1][0] ); + printf( "M3: %f\n", magnitude3d( tmp ) ); + cross3d( tmp, &T[2][0], &T[1][0] ); + printf( "M3: %f\n", magnitude3d( tmp ) ); + cross3d( tmp, &T[2][0], &T[0][0] ); + printf( "M3: %f\n", magnitude3d( tmp ) ); +*/ + +// PRINT_MAT(T,4,4); +#if 1 - //memcpy(T2,T,16*sizeof(float)); -// matrix44copy(T2,T); + //matrix44copy(T2,T); matrix44transpose(T2,T); + quatfrommatrix( quat, &T2[0][0] ); + printf( "QM: %f\n", quatmagnitude( quat ) ); quatnormalize(quatNorm,quat); quattoeuler(euler,quatNorm); - quattomatrix( T2, quatNorm ); + quattomatrix( &T2[0][0], quatNorm ); + + PRINT_MAT(T2,4,4); printf("rot %f %f %f len %f\n", euler[0], euler[1], euler[2], quatmagnitude(quat)); - PRINT(T,4,4); +// PRINT(T,4,4); // matrix44copy(temp,T2); matrix44transpose(temp,T2); - memcpy(T2,temp,16*sizeof(float)); + +// matrix44transpose(T2, temp); +// memcpy(T2,temp,16*sizeof(float)); for (i=0; i<3; i++) { for (j=0; j<3; j++) { - T[i][j] = T2[j][i]; + T[i][j] = temp[i][j]; } } - PRINT(T2,4,4); + +/* PRINT(T2,4,4); */ +#endif + + T[1][0]*=-1; + T[1][1]*=-1; + T[1][2]*=-1; /* -- cgit v1.2.3 From 9f6c7e87266c5a7d721d4d8e3ace82dc46207387 Mon Sep 17 00:00:00 2001 From: Michael Turvey Date: Fri, 24 Mar 2017 10:10:14 -0700 Subject: Fix linux build Successfully tested with a wired watchman on linux. Wired tracker now succeeds in providing calibration info Still not getting light info from tracker. --- src/survive_vive.c | 31 ++++++++++++++++++++++++++++++- 1 file changed, 30 insertions(+), 1 deletion(-) (limited to 'src') diff --git a/src/survive_vive.c b/src/survive_vive.c index f58fb79..c1bcb26 100755 --- a/src/survive_vive.c +++ b/src/survive_vive.c @@ -531,9 +531,10 @@ int survive_vive_send_magic(SurviveContext * ctx, void * drv, int magic_code, vo r = update_feature_report( sv->udev[USB_DEV_W_WATCHMAN1], 0, vive_magic_power_on, sizeof( vive_magic_power_on ) ); if( r != sizeof( vive_magic_power_on ) ) return 5; } + +#ifdef HIDAPI if (sv->udev[USB_DEV_W_WATCHMAN1_LIGHTCAP]) { - //static uint8_t vive_magic_enable_lighthouse[64] = { 0x04, 0x78, 0x29, 0x38 }; static uint8_t vive_magic_enable_lighthouse[5] = { 0x04 }; r = update_feature_report( sv->udev[USB_DEV_W_WATCHMAN1_LIGHTCAP], 0, vive_magic_enable_lighthouse, sizeof( vive_magic_enable_lighthouse ) ); if( r != sizeof( vive_magic_enable_lighthouse ) ) return 5; @@ -543,12 +544,27 @@ int survive_vive_send_magic(SurviveContext * ctx, void * drv, int magic_code, vo if( r != sizeof( vive_magic_enable_lighthouse2 ) ) return 5; } +#else + if (sv->udev[USB_DEV_W_WATCHMAN1]) + { + static uint8_t vive_magic_enable_lighthouse[5] = { 0x04 }; + r = update_feature_report( sv->udev[USB_DEV_W_WATCHMAN1], 0, vive_magic_enable_lighthouse, sizeof( vive_magic_enable_lighthouse ) ); + if( r != sizeof( vive_magic_enable_lighthouse ) ) return 5; + + static uint8_t vive_magic_enable_lighthouse2[5] = { 0x07, 0x02 }; //Switch to 0x25 mode (able to get more light updates) + r = update_feature_report( sv->udev[USB_DEV_W_WATCHMAN1], 0, vive_magic_enable_lighthouse2, sizeof( vive_magic_enable_lighthouse2 ) ); + if( r != sizeof( vive_magic_enable_lighthouse2 ) ) return 5; + } + +#endif + if (sv->udev[USB_DEV_TRACKER0]) { static uint8_t vive_magic_power_on[5] = { 0x04 }; r = update_feature_report( sv->udev[USB_DEV_TRACKER0], 0, vive_magic_power_on, sizeof( vive_magic_power_on ) ); if( r != sizeof( vive_magic_power_on ) ) return 5; } +#ifdef HIDAPI if (sv->udev[USB_DEV_TRACKER0_LIGHTCAP]) { static uint8_t vive_magic_enable_lighthouse[5] = { 0x04 }; @@ -559,6 +575,19 @@ int survive_vive_send_magic(SurviveContext * ctx, void * drv, int magic_code, vo r = update_feature_report( sv->udev[USB_DEV_TRACKER0_LIGHTCAP], 0, vive_magic_enable_lighthouse2, sizeof( vive_magic_enable_lighthouse2 ) ); if( r != sizeof( vive_magic_enable_lighthouse2 ) ) return 5; } +#else + if (sv->udev[USB_DEV_TRACKER0]) + { + static uint8_t vive_magic_enable_lighthouse[5] = { 0x04 }; + r = update_feature_report( sv->udev[USB_DEV_TRACKER0], 0, vive_magic_enable_lighthouse, sizeof( vive_magic_enable_lighthouse ) ); + if( r != sizeof( vive_magic_enable_lighthouse ) ) return 5; + + static uint8_t vive_magic_enable_lighthouse2[5] = { 0x07, 0x02 }; //Switch to 0x25 mode (able to get more light updates) + r = update_feature_report( sv->udev[USB_DEV_TRACKER0], 0, vive_magic_enable_lighthouse2, sizeof( vive_magic_enable_lighthouse2 ) ); + if( r != sizeof( vive_magic_enable_lighthouse2 ) ) return 5; + } + +#endif #if 0 for( int i = 0; i < 256; i++ ) -- cgit v1.2.3 From a576703242adea11c012af5afdff38af84d22e2e Mon Sep 17 00:00:00 2001 From: mwturvey Date: Fri, 24 Mar 2017 13:44:38 -0700 Subject: Adaptive Acode Offset Compensation --- src/survive_cal.c | 5 +-- src/survive_data.c | 114 +++++++++++++++++++++++++++++++---------------------- src/survive_vive.c | 30 ++++++++------ 3 files changed, 85 insertions(+), 64 deletions(-) (limited to 'src') diff --git a/src/survive_cal.c b/src/survive_cal.c index 81889ca..22a8eff 100755 --- a/src/survive_cal.c +++ b/src/survive_cal.c @@ -200,6 +200,7 @@ void survive_cal_light( struct SurviveObject * so, int sensor_id, int acode, int //Collecting OOTX data. if( sensor_id < 0 ) { + //fprintf(stderr, "%s\n", so->codename); int lhid = -sensor_id-1; // Take the OOTX data from the first device. (if using HMD, WM0, WM1 only, this will be HMD) if( lhid < NUM_LIGHTHOUSES && so == cd->poseobjects[0] ) @@ -210,9 +211,7 @@ void survive_cal_light( struct SurviveObject * so, int sensor_id, int acode, int 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. <------- Revert This!!!!! - //if( i == 1 ) - //cd->stage = 2; //If all lighthouses have their OOTX set, move on. + if( i == NUM_LIGHTHOUSES ) cd->stage = 2; //TODO: Make this configuratble to allow single lighthouse. } break; case 3: //Look for light sync lengths. diff --git a/src/survive_data.c b/src/survive_data.c index 849b722..4e2479a 100644 --- a/src/survive_data.c +++ b/src/survive_data.c @@ -21,26 +21,42 @@ typedef struct int lh_start_time[NUM_LIGHTHOUSES]; int current_lh; // used knowing which sync pulse we're looking at. +} lightcap2_per_sweep_data; + +typedef struct +{ + float acode_offset; } lightcap2_global_data; typedef struct { lightcaps_sweep_data sweep; + lightcap2_per_sweep_data per_sweep; lightcap2_global_data global; } lightcap2_data; //static lightcap2_global_data lcgd = { 0 }; -int handle_lightcap2_getAcodeFromSyncPulse(int pulseLen) +int handle_lightcap2_getAcodeFromSyncPulse(SurviveObject * so, int pulseLen) { - if (pulseLen < 3125) return 0; - if (pulseLen < 3625) return 1; - if (pulseLen < 4125) return 2; - if (pulseLen < 4625) return 3; - if (pulseLen < 5125) return 4; - if (pulseLen < 5625) return 5; - if (pulseLen < 6125) return 6; + float oldOffset = ((lightcap2_data*)so->disambiguator_data)->global.acode_offset; + + int modifiedPulseLen = pulseLen - (int)oldOffset; + + float newOffset = (((pulseLen) + 250) % 500) - 250; + + ((lightcap2_data*)so->disambiguator_data)->global.acode_offset = oldOffset * 0.9 + newOffset * 0.1; + +//fprintf(stderr, " %f\n", oldOffset); +#define ACODE_OFFSET 0 + if (pulseLen < 3250 - ACODE_OFFSET) return 0; + if (pulseLen < 3750 - ACODE_OFFSET) return 1; + if (pulseLen < 4250 - ACODE_OFFSET) return 2; + if (pulseLen < 4750 - ACODE_OFFSET) return 3; + if (pulseLen < 5250 - ACODE_OFFSET) return 4; + if (pulseLen < 5750 - ACODE_OFFSET) return 5; + if (pulseLen < 6250 - ACODE_OFFSET) return 6; return 7; } void handle_lightcap2_process_sweep_data(SurviveObject *so) @@ -67,13 +83,13 @@ void handle_lightcap2_process_sweep_data(SurviveObject *so) { if (lcd->sweep.sweep_len[i] != 0) // if the sensor was hit, process it { - int offset_from = lcd->sweep.sweep_time[i] - lcd->global.activeSweepStartTime + lcd->sweep.sweep_len[i] / 2; + int offset_from = lcd->sweep.sweep_time[i] - lcd->per_sweep.activeSweepStartTime + lcd->sweep.sweep_len[i] / 2; if (offset_from < 380000 && offset_from > 70000) { //if (longest_pulse *10 / 8 < lcd->sweep.sweep_len[i]) { - so->ctx->lightproc(so, i, lcd->global.activeAcode, offset_from, lcd->sweep.sweep_time[i], lcd->sweep.sweep_len[i], lcd->global.activeLighthouse); + so->ctx->lightproc(so, i, lcd->per_sweep.activeAcode, offset_from, lcd->sweep.sweep_time[i], lcd->sweep.sweep_len[i], lcd->per_sweep.activeLighthouse); } } } @@ -96,54 +112,54 @@ void handle_lightcap2_sync(SurviveObject * so, LightcapElement * le ) // Process any sweep data we have handle_lightcap2_process_sweep_data(so); - int time_since_last_sync = (le->timestamp - lcd->global.recent_sync_time); + int time_since_last_sync = (le->timestamp - lcd->per_sweep.recent_sync_time); //fprintf(stderr, " %2d %8d %d\n", le->sensor_id, time_since_last_sync, le->length); // need to store up sync pulses, so we can take the earliest starting time for all sensors. if (time_since_last_sync < 2400) { - lcd->global.recent_sync_time = le->timestamp; + lcd->per_sweep.recent_sync_time = le->timestamp; // it's the same sync pulse; so->sync_set_number = 1; so->recent_sync_time = le->timestamp; - lcd->global.lh_pulse_len[lcd->global.current_lh] = le->length; - lcd->global.lh_start_time[lcd->global.current_lh] = le->timestamp; + lcd->per_sweep.lh_pulse_len[lcd->per_sweep.current_lh] = le->length; + lcd->per_sweep.lh_start_time[lcd->per_sweep.current_lh] = le->timestamp; - int acode = handle_lightcap2_getAcodeFromSyncPulse(le->length); + int acode = handle_lightcap2_getAcodeFromSyncPulse(so, le->length); if (!(acode >> 2 & 1)) // if the skip bit is not set { - lcd->global.activeLighthouse = lcd->global.current_lh; - lcd->global.activeSweepStartTime = le->timestamp; - lcd->global.activeAcode = acode; + lcd->per_sweep.activeLighthouse = lcd->per_sweep.current_lh; + lcd->per_sweep.activeSweepStartTime = le->timestamp; + lcd->per_sweep.activeAcode = acode; } else { - lcd->global.activeLighthouse = -1; - lcd->global.activeSweepStartTime = 0; - lcd->global.activeAcode = 0; + lcd->per_sweep.activeLighthouse = -1; + lcd->per_sweep.activeSweepStartTime = 0; + lcd->per_sweep.activeAcode = 0; } } else if (time_since_last_sync < 24000) { - lcd->global.recent_sync_time = le->timestamp; + lcd->per_sweep.recent_sync_time = le->timestamp; // I do believe we are lighthouse B - lcd->global.current_lh = 1; - lcd->global.lh_pulse_len[lcd->global.current_lh] = le->length; - lcd->global.lh_start_time[lcd->global.current_lh] = le->timestamp; + lcd->per_sweep.current_lh = 1; + lcd->per_sweep.lh_pulse_len[lcd->per_sweep.current_lh] = le->length; + lcd->per_sweep.lh_start_time[lcd->per_sweep.current_lh] = le->timestamp; - int acode = handle_lightcap2_getAcodeFromSyncPulse(le->length); + int acode = handle_lightcap2_getAcodeFromSyncPulse(so, le->length); if (!(acode >> 2 & 1)) // if the skip bit is not set { - if (lcd->global.activeLighthouse != -1) + if (lcd->per_sweep.activeLighthouse != -1) { // hmm, it appears we got two non-skip pulses at the same time. That should never happen fprintf(stderr, "WARNING: Two non-skip pulses received on the same cycle!\n"); } - lcd->global.activeLighthouse = 1; - lcd->global.activeSweepStartTime = le->timestamp; - lcd->global.activeAcode = acode; + lcd->per_sweep.activeLighthouse = 1; + lcd->per_sweep.activeSweepStartTime = le->timestamp; + lcd->per_sweep.activeAcode = acode; } } @@ -153,49 +169,50 @@ void handle_lightcap2_sync(SurviveObject * so, LightcapElement * le ) // first, send out the sync pulse data for the last round (for OOTX decoding { - if (lcd->global.lh_pulse_len[0] != 0) + if (lcd->per_sweep.lh_pulse_len[0] != 0) { so->ctx->lightproc( so, -1, - handle_lightcap2_getAcodeFromSyncPulse(lcd->global.lh_pulse_len[0]), - lcd->global.lh_pulse_len[0], - lcd->global.lh_start_time[0], + handle_lightcap2_getAcodeFromSyncPulse(so, lcd->per_sweep.lh_pulse_len[0]), + lcd->per_sweep.lh_pulse_len[0], + lcd->per_sweep.lh_start_time[0], 0, 0); } - if (lcd->global.lh_pulse_len[1] != 0) + if (lcd->per_sweep.lh_pulse_len[1] != 0) { so->ctx->lightproc( so, -2, - handle_lightcap2_getAcodeFromSyncPulse(lcd->global.lh_pulse_len[1]), - lcd->global.lh_pulse_len[1], - lcd->global.lh_start_time[1], + handle_lightcap2_getAcodeFromSyncPulse(so, lcd->per_sweep.lh_pulse_len[1]), + lcd->per_sweep.lh_pulse_len[1], + lcd->per_sweep.lh_start_time[1], 0, 1); } } + //fprintf(stderr, "************************************ Reinitializing Disambiguator!!!\n"); // initialize here. - memset(&lcd->global, 0, sizeof(lcd->global)); - lcd->global.activeLighthouse = -1; + memset(&lcd->per_sweep, 0, sizeof(lcd->per_sweep)); + lcd->per_sweep.activeLighthouse = -1; - lcd->global.recent_sync_time = le->timestamp; + lcd->per_sweep.recent_sync_time = le->timestamp; // I do believe we are lighthouse A - lcd->global.current_lh = 0; - lcd->global.lh_pulse_len[lcd->global.current_lh] = le->length; - lcd->global.lh_start_time[lcd->global.current_lh] = le->timestamp; + lcd->per_sweep.current_lh = 0; + lcd->per_sweep.lh_pulse_len[lcd->per_sweep.current_lh] = le->length; + lcd->per_sweep.lh_start_time[lcd->per_sweep.current_lh] = le->timestamp; - int acode = handle_lightcap2_getAcodeFromSyncPulse(le->length); + int acode = handle_lightcap2_getAcodeFromSyncPulse(so, le->length); if (!(acode >> 2 & 1)) // if the skip bit is not set { - lcd->global.activeLighthouse = 0; - lcd->global.activeSweepStartTime = le->timestamp; - lcd->global.activeAcode = acode; + lcd->per_sweep.activeLighthouse = 0; + lcd->per_sweep.activeSweepStartTime = le->timestamp; + lcd->per_sweep.activeAcode = acode; } } } @@ -230,6 +247,7 @@ void handle_lightcap2( SurviveObject * so, LightcapElement * le ) if (so->disambiguator_data == NULL) { + fprintf(stderr, "Initializing Disambiguator Data\n"); so->disambiguator_data = malloc(sizeof(lightcap2_data)); memset(so->disambiguator_data, 0, sizeof(lightcap2_data)); } diff --git a/src/survive_vive.c b/src/survive_vive.c index c1bcb26..55e949a 100755 --- a/src/survive_vive.c +++ b/src/survive_vive.c @@ -479,6 +479,9 @@ int survive_usb_init( SurviveViveData * sv, SurviveObject * hmd, SurviveObject * #ifdef HIDAPI //Tricky: use other interface for actual lightcap. XXX THIS IS NOT YET RIGHT!!! if( sv->udev[USB_DEV_LIGHTHOUSEB] && AttachInterface( sv, hmd, USB_IF_LIGHTCAP, sv->udev[USB_DEV_LIGHTHOUSEB], 0x82, survive_data_cb, "Lightcap")) { return -12; } + + // This is a HACK! But it works. Need to investigate further + sv->uiface[USB_DEV_TRACKER0_LIGHTCAP].actual_len = 64; if( sv->udev[USB_DEV_TRACKER0_LIGHTCAP] && AttachInterface( sv, tr0, USB_IF_TRACKER0_LIGHTCAP, sv->udev[USB_DEV_TRACKER0_LIGHTCAP], 0x82, survive_data_cb, "Tracker 1 Lightcap")) { return -13; } if( sv->udev[USB_DEV_W_WATCHMAN1_LIGHTCAP] && AttachInterface( sv, ww0, USB_IF_W_WATCHMAN1_LIGHTCAP, sv->udev[USB_DEV_W_WATCHMAN1_LIGHTCAP], 0x82, survive_data_cb, "Wired Watchman 1 Lightcap")) { return -13; } #else @@ -564,18 +567,18 @@ int survive_vive_send_magic(SurviveContext * ctx, void * drv, int magic_code, vo r = update_feature_report( sv->udev[USB_DEV_TRACKER0], 0, vive_magic_power_on, sizeof( vive_magic_power_on ) ); if( r != sizeof( vive_magic_power_on ) ) return 5; } -#ifdef HIDAPI - if (sv->udev[USB_DEV_TRACKER0_LIGHTCAP]) - { - static uint8_t vive_magic_enable_lighthouse[5] = { 0x04 }; - r = update_feature_report( sv->udev[USB_DEV_TRACKER0_LIGHTCAP], 0, vive_magic_enable_lighthouse, sizeof( vive_magic_enable_lighthouse ) ); - if( r != sizeof( vive_magic_enable_lighthouse ) ) return 5; - - static uint8_t vive_magic_enable_lighthouse2[5] = { 0x07, 0x02 }; //Switch to 0x25 mode (able to get more light updates) - r = update_feature_report( sv->udev[USB_DEV_TRACKER0_LIGHTCAP], 0, vive_magic_enable_lighthouse2, sizeof( vive_magic_enable_lighthouse2 ) ); - if( r != sizeof( vive_magic_enable_lighthouse2 ) ) return 5; - } -#else +//#ifdef HIDAPI +// if (sv->udev[USB_DEV_TRACKER0_LIGHTCAP]) +// { +// static uint8_t vive_magic_enable_lighthouse[5] = { 0x04 }; +// r = update_feature_report( sv->udev[USB_DEV_TRACKER0_LIGHTCAP], 0, vive_magic_enable_lighthouse, sizeof( vive_magic_enable_lighthouse ) ); +// if( r != sizeof( vive_magic_enable_lighthouse ) ) return 5; +// +// static uint8_t vive_magic_enable_lighthouse2[5] = { 0x07, 0x02 }; //Switch to 0x25 mode (able to get more light updates) +// r = update_feature_report( sv->udev[USB_DEV_TRACKER0_LIGHTCAP], 0, vive_magic_enable_lighthouse2, sizeof( vive_magic_enable_lighthouse2 ) ); +// if( r != sizeof( vive_magic_enable_lighthouse2 ) ) return 5; +// } +//#else if (sv->udev[USB_DEV_TRACKER0]) { static uint8_t vive_magic_enable_lighthouse[5] = { 0x04 }; @@ -587,7 +590,7 @@ int survive_vive_send_magic(SurviveContext * ctx, void * drv, int magic_code, vo if( r != sizeof( vive_magic_enable_lighthouse2 ) ) return 5; } -#endif +//#endif #if 0 for( int i = 0; i < 256; i++ ) @@ -1186,6 +1189,7 @@ void survive_data_cb( SurviveUSBInterface * si ) break; } case USB_IF_W_WATCHMAN1_LIGHTCAP: + case USB_IF_TRACKER0_LIGHTCAP: { int i=0; for( i = 0; i < 7; i++ ) -- cgit v1.2.3 From 4dc1d72785c660c206f8def9d8c8aa32289c2709 Mon Sep 17 00:00:00 2001 From: mwturvey Date: Fri, 24 Mar 2017 15:19:59 -0700 Subject: More cleanup & finishing genericization of calibrator --- src/survive_cal.c | 30 +++++++++++++++++++----------- src/survive_cal.h | 6 ++++-- src/survive_data.c | 6 +++--- src/survive_process.c | 6 +++--- src/survive_vive.c | 6 ++---- 5 files changed, 31 insertions(+), 23 deletions(-) (limited to 'src') diff --git a/src/survive_cal.c b/src/survive_cal.c index 22a8eff..6c153b4 100755 --- a/src/survive_cal.c +++ b/src/survive_cal.c @@ -220,10 +220,13 @@ void survive_cal_light( struct SurviveObject * so, int sensor_id, int acode, int else if( acode < -4 ) break; int lh = (-acode) - 3; - if( strcmp( so->codename, "WM0" ) == 0 ) - sensor_id += 32; - if( strcmp( so->codename, "WM1" ) == 0 ) - sensor_id += 64; + for (int i=0; i < min(MAX_DEVICES_TO_CAL, cd->numPoseObjects); i++) + { + if( strcmp( so->codename, cd->poseobjects[i]->codename ) == 0 ) + { + sensor_id += i*32; + } + } cd->all_sync_times[sensor_id][lh][cd->all_sync_counts[sensor_id][lh]++] = length; break; @@ -233,7 +236,7 @@ void survive_cal_light( struct SurviveObject * so, int sensor_id, int acode, int } -void survive_cal_angle( struct SurviveObject * so, int sensor_id, int acode, uint32_t timecode, FLT length, FLT angle ) +void survive_cal_angle( struct SurviveObject * so, int sensor_id, int acode, uint32_t timecode, FLT length, FLT angle, uint32_t lh ) { struct SurviveContext * ctx = so->ctx; struct SurviveCalData * cd = ctx->calptr; @@ -241,14 +244,18 @@ void survive_cal_angle( struct SurviveObject * so, int sensor_id, int acode, uin if( !cd ) return; int sensid = sensor_id; - if( strcmp( so->codename, "WM0" ) == 0 ) - sensid += 32; - if( strcmp( so->codename, "WM1" ) == 0 ) - sensid += 64; + + for (int i=0; i < min(MAX_DEVICES_TO_CAL, cd->numPoseObjects); i++) + { + if( strcmp( so->codename, cd->poseobjects[i]->codename ) == 0 ) + { + sensid += i*32; + } + } if( sensid >= MAX_SENSORS_TO_CAL || sensid < 0 ) return; - int lighthouse = acode>>2; + int lighthouse = lh; int axis = acode & 1; switch( cd->stage ) @@ -292,7 +299,8 @@ void survive_cal_angle( struct SurviveObject * so, int sensor_id, int acode, uin int min_peaks = PTS_BEFORE_COMMON; int i, j, k; cd->found_common = 1; - for( i = 0; i < MAX_SENSORS_TO_CAL/SENSORS_PER_OBJECT; i++ ) + for( i = 0; i < cd->numPoseObjects; i++ ) + //for( i = 0; i < MAX_SENSORS_TO_CAL/SENSORS_PER_OBJECT; i++ ) for( j = 0; j < NUM_LIGHTHOUSES; j++ ) { int sensors_visible = 0; diff --git a/src/survive_cal.h b/src/survive_cal.h index 8f4e4de..ae644d1 100644 --- a/src/survive_cal.h +++ b/src/survive_cal.h @@ -30,9 +30,11 @@ int survive_cal_get_status( SurviveContext * ctx, char * description, int descri //Called from survive_default_light_process void survive_cal_light( SurviveObject * so, int sensor_id, int acode, int timeinsweep, uint32_t timecode, uint32_t length, uint32_t lighthouse); -void survive_cal_angle( SurviveObject * so, int sensor_id, int acode, uint32_t timecode, FLT length, FLT angle ); +void survive_cal_angle( SurviveObject * so, int sensor_id, int acode, uint32_t timecode, FLT length, FLT angle, uint32_t lh ); -#define MAX_SENSORS_TO_CAL 96 +#define MAX_SENSORS_PER_DEVICE 32 +#define MAX_DEVICES_TO_CAL 3 +#define MAX_SENSORS_TO_CAL (MAX_SENSORS_PER_DEVICE * MAX_DEVICES_TO_CAL) #define MIN_PTS_BEFORE_CAL 24 diff --git a/src/survive_data.c b/src/survive_data.c index 4e2479a..9447104 100644 --- a/src/survive_data.c +++ b/src/survive_data.c @@ -25,7 +25,7 @@ typedef struct typedef struct { - float acode_offset; + double acode_offset; } lightcap2_global_data; typedef struct @@ -40,11 +40,11 @@ typedef struct int handle_lightcap2_getAcodeFromSyncPulse(SurviveObject * so, int pulseLen) { - float oldOffset = ((lightcap2_data*)so->disambiguator_data)->global.acode_offset; + double oldOffset = ((lightcap2_data*)so->disambiguator_data)->global.acode_offset; int modifiedPulseLen = pulseLen - (int)oldOffset; - float newOffset = (((pulseLen) + 250) % 500) - 250; + double newOffset = (((pulseLen) + 250) % 500) - 250; ((lightcap2_data*)so->disambiguator_data)->global.acode_offset = oldOffset * 0.9 + newOffset * 0.1; diff --git a/src/survive_process.c b/src/survive_process.c index d4604d8..b58b344 100644 --- a/src/survive_process.c +++ b/src/survive_process.c @@ -37,16 +37,16 @@ void survive_default_light_process( SurviveObject * so, int sensor_id, int acode #endif FLT length_sec = length / (FLT)so->timebase_hz; - ctx->angleproc( so, sensor_id, acode, timecode, length_sec, angle ); + ctx->angleproc( so, sensor_id, acode, timecode, length_sec, angle, lh); } -void survive_default_angle_process( 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, uint32_t lh) { SurviveContext * ctx = so->ctx; if( ctx->calptr ) { - survive_cal_angle( so, sensor_id, acode, timecode, length, angle ); + survive_cal_angle( so, sensor_id, acode, timecode, length, angle, lh ); } if( so->PoserFn ) { diff --git a/src/survive_vive.c b/src/survive_vive.c index 55e949a..a5c731d 100755 --- a/src/survive_vive.c +++ b/src/survive_vive.c @@ -834,7 +834,6 @@ static void handle_watchman( SurviveObject * w, uint8_t * readdata ) qty-=2; int propset = 0; int doimu = 0; - int i; if( (type & 0xf0) == 0xf0 ) { @@ -911,10 +910,9 @@ static void handle_watchman( SurviveObject * w, uint8_t * readdata ) *readdata = type; //Put 'type' back on stack. uint8_t * mptr = readdata + qty-3-1; //-3 for timecode, -1 to -//#define DEBUG_WATCHMAN #ifdef DEBUG_WATCHMAN printf( "_%s ", w->codename); - for( i = 0; i < qty; i++ ) + for(int i = 0; i < qty; i++ ) { printf( "%02x ", readdata[i] ); } @@ -1198,7 +1196,7 @@ void survive_data_cb( SurviveUSBInterface * si ) unsigned short *length = (unsigned short *)(&(readdata[2])); unsigned long *time = (unsigned long *)(&(readdata[4])); LightcapElement le; - le.sensor_id = POP2; + le.sensor_id = (uint8_t)POP2; le.length = POP2; le.timestamp = POP4; if( le.sensor_id == 0xff ) break; -- cgit v1.2.3 From a279a202768e1cae10f4fb651aad2b3b21db43ab Mon Sep 17 00:00:00 2001 From: mwturvey Date: Fri, 24 Mar 2017 15:36:43 -0700 Subject: remove min() --- src/survive_cal.c | 18 +++++++++++++----- 1 file changed, 13 insertions(+), 5 deletions(-) (limited to 'src') diff --git a/src/survive_cal.c b/src/survive_cal.c index 6c153b4..5414048 100755 --- a/src/survive_cal.c +++ b/src/survive_cal.c @@ -141,10 +141,17 @@ void survive_cal_install( struct SurviveContext * ctx ) if (AllowAllTrackersForCal || isRequiredTracker) { - cd->poseobjects[j] = ctx->objs[j]; - cd->numPoseObjects++; + if (MAX_DEVICES_TO_CAL < cd->numPoseObjects) + { + cd->poseobjects[j] = ctx->objs[j]; + cd->numPoseObjects++; - SV_INFO("Calibration is using %s", cd->poseobjects[j]->codename); + SV_INFO("Calibration is using %s", cd->poseobjects[j]->codename); + } + else + { + SV_ERROR("Calibration is NOT using %s; device count exceeds MAX_DEVICES_TO_CAL", cd->poseobjects[j]->codename); + } } } @@ -220,7 +227,7 @@ void survive_cal_light( struct SurviveObject * so, int sensor_id, int acode, int else if( acode < -4 ) break; int lh = (-acode) - 3; - for (int i=0; i < min(MAX_DEVICES_TO_CAL, cd->numPoseObjects); i++) + for (int i=0; i < cd->numPoseObjects; i++) { if( strcmp( so->codename, cd->poseobjects[i]->codename ) == 0 ) { @@ -234,6 +241,7 @@ void survive_cal_light( struct SurviveObject * so, int sensor_id, int acode, int } + } void survive_cal_angle( struct SurviveObject * so, int sensor_id, int acode, uint32_t timecode, FLT length, FLT angle, uint32_t lh ) @@ -245,7 +253,7 @@ void survive_cal_angle( struct SurviveObject * so, int sensor_id, int acode, uin int sensid = sensor_id; - for (int i=0; i < min(MAX_DEVICES_TO_CAL, cd->numPoseObjects); i++) + for (int i=0; i < cd->numPoseObjects; i++) { if( strcmp( so->codename, cd->poseobjects[i]->codename ) == 0 ) { -- cgit v1.2.3 From efe1b3e98866d45dd3bb5f8060712db152762edd Mon Sep 17 00:00:00 2001 From: Michael Turvey Date: Fri, 24 Mar 2017 16:00:20 -0700 Subject: Fixing linux --- src/survive_cal.c | 4 ++-- 1 file changed, 2 insertions(+), 2 deletions(-) (limited to 'src') diff --git a/src/survive_cal.c b/src/survive_cal.c index 5414048..ae92bad 100755 --- a/src/survive_cal.c +++ b/src/survive_cal.c @@ -141,7 +141,7 @@ void survive_cal_install( struct SurviveContext * ctx ) if (AllowAllTrackersForCal || isRequiredTracker) { - if (MAX_DEVICES_TO_CAL < cd->numPoseObjects) + if (MAX_DEVICES_TO_CAL > cd->numPoseObjects) { cd->poseobjects[j] = ctx->objs[j]; cd->numPoseObjects++; @@ -150,7 +150,7 @@ void survive_cal_install( struct SurviveContext * ctx ) } else { - SV_ERROR("Calibration is NOT using %s; device count exceeds MAX_DEVICES_TO_CAL", cd->poseobjects[j]->codename); + SV_INFO("Calibration is NOT using %s; device count exceeds MAX_DEVICES_TO_CAL", ctx->objs[j]->codename); } } -- cgit v1.2.3 From ce7fa5b134b1394f7bb9f4bddb1da9e83d792827 Mon Sep 17 00:00:00 2001 From: Mike Turvey Date: Sat, 25 Mar 2017 21:58:51 -0700 Subject: Adding Tori Poser --- src/poser_turveytori.c | 871 +++++++++++++++++++++++++++++++++++++++++++++++++ 1 file changed, 871 insertions(+) create mode 100644 src/poser_turveytori.c (limited to 'src') diff --git a/src/poser_turveytori.c b/src/poser_turveytori.c new file mode 100644 index 0000000..e9e5b7a --- /dev/null +++ b/src/poser_turveytori.c @@ -0,0 +1,871 @@ +#include +#include +#include +#include +#include +#include +#include "linmath.h" +#include +#include +#include + + +#define PointToFlts(x) ((FLT*)(x)) + +typedef struct +{ + FLT x; + FLT y; + FLT z; +} Point; + +void writePoint(FILE *file, double x, double y, double z, unsigned int rgb) {} +void updateHeader(FILE * file) {} +void writeAxes(FILE * file) {} +void drawLineBetweenPoints(FILE *file, Point a, Point b, unsigned int color) {} +void writePcdHeader(FILE * file) {} +void writePointCloud(FILE *f, Point *pointCloud, unsigned int Color) {} +void markPointWithStar(FILE *file, Point point, unsigned int color) {} + +typedef struct +{ + Point point; // location of the sensor on the tracked object; + Point normal; // unit vector indicating the normal for the sensor + double theta; // "horizontal" angular measurement from lighthouse radians + double phi; // "vertical" angular measurement from lighthouse in radians. +} TrackedSensor; + +typedef struct +{ + size_t numSensors; + TrackedSensor sensor[0]; +} TrackedObject; + + +#ifndef M_PI +#define M_PI 3.14159265358979323846264338327 +#endif + +#define SQUARED(x) ((x)*(x)) + +typedef union +{ + struct + { + unsigned char Blue; + unsigned char Green; + unsigned char Red; + unsigned char Alpha; + }; + uint32_t long_value; +} RGBValue; + +static RGBValue RED = { .Red = 255,.Green = 0,.Blue = 0,.Alpha = 125 }; +static RGBValue GREEN = { .Red = 0,.Green = 255,.Blue = 0,.Alpha = 125 }; +static RGBValue BLUE = { .Red = 0,.Green = 0,.Blue = 255,.Alpha = 125 }; + +static const double WORLD_BOUNDS = 100; +#define MAX_TRACKED_POINTS 40 + +static const float DefaultPointsPerOuterDiameter = 60; + +typedef struct +{ + int something; + //Stuff +} ToriData; + + + + + + + +static FLT distance(Point a, Point b) +{ + FLT x = a.x - b.x; + FLT y = a.y - b.y; + FLT z = a.z - b.z; + return FLT_SQRT(x*x + y*y + z*z); +} + +Matrix3x3 GetRotationMatrixForTorus(Point a, Point b) +{ + Matrix3x3 result; + FLT v1[3] = { 0, 0, 1 }; + FLT v2[3] = { a.x - b.x, a.y - b.y, a.z - b.z }; + + normalize3d(v2, v2); + + rotation_between_vecs_to_m3(&result, v1, v2); + + // Useful for debugging... + //FLT v2b[3]; + //rotate_vec(v2b, v1, result); + + return result; +} + +typedef struct +{ + Point a; + Point b; + FLT angle; + FLT tanAngle; // tangent of angle + Matrix3x3 rotation; + Matrix3x3 invRotation; // inverse of rotation + +} PointsAndAngle; + + +Point RotateAndTranslatePoint(Point p, Matrix3x3 rot, Point newOrigin) +{ + Point q; + + double pf[3] = { p.x, p.y, p.z }; + q.x = rot.val[0][0] * p.x + rot.val[1][0] * p.y + rot.val[2][0] * p.z + newOrigin.x; + q.y = rot.val[0][1] * p.x + rot.val[1][1] * p.y + rot.val[2][1] * p.z + newOrigin.y; + q.z = rot.val[0][2] * p.x + rot.val[1][2] * p.y + rot.val[2][2] * p.z + newOrigin.z; + + return q; +} + +double angleFromPoints(Point p1, Point p2, Point center) +{ + Point v1, v2, v1norm, v2norm; + v1.x = p1.x - center.x; + v1.y = p1.y - center.y; + v1.z = p1.z - center.z; + + v2.x = p2.x - center.x; + v2.y = p2.y - center.y; + v2.z = p2.z - center.z; + + double v1mag = sqrt(v1.x * v1.x + v1.y * v1.y + v1.z * v1.z); + v1norm.x = v1.x / v1mag; + v1norm.y = v1.y / v1mag; + v1norm.z = v1.z / v1mag; + + double v2mag = sqrt(v2.x * v2.x + v2.y * v2.y + v2.z * v2.z); + v2norm.x = v2.x / v2mag; + v2norm.y = v2.y / v2mag; + v2norm.z = v2.z / v2mag; + + double res = v1norm.x * v2norm.x + v1norm.y * v2norm.y + v1norm.z * v2norm.z; + + double angle = acos(res); + + return angle; +} + +Point midpoint(Point a, Point b) +{ + Point m; + m.x = (a.x + b.x) / 2; + m.y = (a.y + b.y) / 2; + m.z = (a.z + b.z) / 2; + + return m; +} + +// What we're doing here is: +// * Given a point in space +// * And points and a lighthouse angle that implicitly define a torus +// * for that torus, what is the toroidal angle of the plane that will go through that point in space +// * and given that toroidal angle, what is the poloidal angle that will be directed toward that point in space? +void estimateToroidalAndPoloidalAngleOfPoint( + PointsAndAngle *pna, + Point point, + double *toroidalSin, + double *toroidalCos, + double *poloidalAngle, + double *poloidalSin) +{ + // We take the inverse of the rotation matrix, and this now defines a rotation matrix that will take us from + // the tracked object coordinate system into the "easy" or "default" coordinate system of the torus. + // Using this will allow us to derive angles much more simply by being in a "friendly" coordinate system. + Matrix3x3 rot = pna->invRotation; + Point origin; + origin.x = 0; + origin.y = 0; + origin.z = 0; + + Point m = midpoint(pna->a, pna->b); + + // in this new coordinate system, we'll rename all of the points we care about to have an "F" after them + // This will be their representation in the "friendly" coordinate system + Point pointF; + + // Okay, I lied a little above. In addition to the rotation matrix that we care about, there was also + // a translation that we did to move the origin. If we're going to get to the "friendly" coordinate system + // of the torus, we need to first undo the translation, then undo the rotation. Below, we're undoing the translation. + pointF.x = point.x - m.x; + pointF.y = point.y - m.y; + pointF.z = point.z - m.z; + + // now we'll undo the rotation part. + pointF = RotateAndTranslatePoint(pointF, rot, origin); + + // hooray, now pointF is in our more-friendly coordinate system. + + // Now, it's time to figure out the toroidal angle to that point. This should be pretty easy. + // We will "flatten" the z dimension to only look at the x and y values. Then, we just need to measure the + // angle between a vector to pointF and a vector along the x axis. + + FLT toroidalHyp = FLT_SQRT(SQUARED(pointF.y) + SQUARED(pointF.x)); + + *toroidalSin = pointF.y / toroidalHyp; + + *toroidalCos = pointF.x / toroidalHyp; + + //*toroidalAngle = atan(pointF.y / pointF.x); + //if (pointF.x < 0) + //{ + // *toroidalAngle += M_PI; + //} + + //assert(*toroidalSin / FLT_SIN(*toroidalAngle) - 1 < 0.000001); + //assert(*toroidalSin / FLT_SIN(*toroidalAngle) - 1 > -0.000001); + + //assert(*toroidalCos / FLT_COS(*toroidalAngle) - 1 < 0.000001); + //assert(*toroidalCos / FLT_COS(*toroidalAngle) - 1 > -0.000001); + + // SCORE!! We've got the toroidal angle. We're half done! + + // Okay, what next...? Now, we will need to rotate the torus *again* to make it easy to + // figure out the poloidal angle. We should rotate the entire torus by the toroidal angle + // so that the point we're focusin on will lie on the x/z plane. We then should translate the + // torus so that the center of the poloidal circle is at the origin. At that point, it will + // be trivial to determine the poloidal angle-- it will be the angle on the xz plane of a + // vector from the origin to the point. + + // okay, instead of rotating the torus & point by the toroidal angle to get the point on + // the xz plane, we're going to take advantage of the radial symmetry of the torus + // (i.e. it's symmetric about the point we'd want to rotate it, so the rotation wouldn't + // change the torus at all). Therefore, we'll leave the torus as is, but we'll rotate the point + // This will only impact the x and y coordinates, and we'll use "G" as the postfix to represent + // this new coordinate system + + Point pointG; + pointG.z = pointF.z; + pointG.y = 0; + pointG.x = sqrt(SQUARED(pointF.x) + SQUARED(pointF.y)); + + // okay, that ended up being easier than I expected. Now that we have the point on the xZ plane, + // our next step will be to shift it down so that the center of the poloidal circle is at the origin. + // As you may have noticed, y has now gone to zero, and from here on out, we can basically treat + // this as a 2D problem. I think we're getting close... + + // I stole these lines from the torus generator. Gonna need the poloidal radius. + double distanceBetweenPoints = distance(pna->a, pna->b); // we don't care about the coordinate system of these points because we're just getting distance. + double toroidalRadius = distanceBetweenPoints / (2 * pna->tanAngle); + double poloidalRadius = sqrt(SQUARED(toroidalRadius) + SQUARED(distanceBetweenPoints / 2)); + + // The center of the polidal circle already lies on the z axis at this point, so we won't shift z at all. + // The shift along the X axis will be the toroidal radius. + + Point pointH; + pointH.z = pointG.z; + pointH.y = pointG.y; + pointH.x = pointG.x - toroidalRadius; + + // Okay, almost there. If we treat pointH as a vector on the XZ plane, if we get its angle, + // that will be the poloidal angle we're looking for. (crosses fingers) + + FLT poloidalHyp = FLT_SQRT(SQUARED(pointH.z) + SQUARED(pointH.x)); + + *poloidalSin = pointH.z / poloidalHyp; + + + *poloidalAngle = atan(pointH.z / pointH.x); + if (pointH.x < 0) + { + *poloidalAngle += M_PI; + } + + //assert(*toroidalSin / FLT_SIN(*toroidalAngle) - 1 < 0.000001); + //assert(*toroidalSin / FLT_SIN(*toroidalAngle) - 1 > -0.000001); + + + + // Wow, that ended up being not so much code, but a lot of interesting trig. + // can't remember the last time I spent so much time working through each line of code. + + return; +} + +#define MAX_POINT_PAIRS 100 + +FLT angleBetweenSensors(TrackedSensor *a, TrackedSensor *b) +{ + FLT angle = FLT_ACOS(FLT_COS(a->phi - b->phi)*FLT_COS(a->theta - b->theta)); + FLT angle2 = FLT_ACOS(FLT_COS(b->phi - a->phi)*FLT_COS(b->theta - a->theta)); + + return angle; +} + +// This provides a pretty good estimate of the angle above, probably better +// the further away the lighthouse is. But, it's not crazy-precise. +// It's main advantage is speed. +FLT pythAngleBetweenSensors2(TrackedSensor *a, TrackedSensor *b) +{ + FLT p = (a->phi - b->phi); + FLT d = (a->theta - b->theta); + + FLT adjd = FLT_SIN((a->phi + b->phi) / 2); + FLT adjP = FLT_SIN((a->theta + b->theta) / 2); + FLT pythAngle = sqrt(SQUARED(p*adjP) + SQUARED(d*adjd)); + return pythAngle; +} + +Point calculateTorusPointFromAngles(PointsAndAngle *pna, FLT toroidalSin, FLT toroidalCos, FLT poloidalAngle, FLT poloidalSin) +{ + Point result; + + FLT distanceBetweenPoints = distance(pna->a, pna->b); + Point m = midpoint(pna->a, pna->b); + Matrix3x3 rot = pna->rotation; + + FLT toroidalRadius = distanceBetweenPoints / (2 * pna->tanAngle); + FLT poloidalRadius = FLT_SQRT(SQUARED(toroidalRadius) + SQUARED(distanceBetweenPoints / 2)); + + result.x = (toroidalRadius + poloidalRadius*cos(poloidalAngle))*toroidalCos; + result.y = (toroidalRadius + poloidalRadius*cos(poloidalAngle))*toroidalSin; + result.z = poloidalRadius*poloidalSin; + result = RotateAndTranslatePoint(result, rot, m); + + return result; +} + +FLT getPointFitnessForPna(Point pointIn, PointsAndAngle *pna) +{ + + double toroidalSin = 0; + double toroidalCos = 0; + double poloidalAngle = 0; + double poloidalSin = 0; + + estimateToroidalAndPoloidalAngleOfPoint( + pna, + pointIn, + &toroidalSin, + &toroidalCos, + &poloidalAngle, + &poloidalSin); + + Point torusPoint = calculateTorusPointFromAngles(pna, toroidalSin, toroidalCos, poloidalAngle, poloidalSin); + + FLT dist = distance(pointIn, torusPoint); + + // This is some voodoo black magic. This is here to solve the problem that the origin + // (which is near the center of all the tori) erroniously will rank as a good match. + // through a lot of empiracle testing on how to compensate for this, the "fudge factor" + // below ended up being the best fit. As simple as it is, I have a strong suspicion + // that there's some crazy complex thesis-level math that could be used to derive this + // but it works so we'll run with it. + // Note that this may be resulting in a skewing of the found location by several millimeters. + // it is not clear if this is actually removing existing skew (to get a more accurate value) + // or if it is introducing an undesirable skew. + double fudge = FLT_SIN((poloidalAngle - M_PI) / 2); + dist = dist / fudge; + + return dist; +} + +FLT getPointFitness(Point pointIn, PointsAndAngle *pna, size_t pnaCount) +{ + FLT fitness; + + FLT resultSum = 0; + + for (size_t i = 0; i < pnaCount; i++) + { + fitness = getPointFitnessForPna(pointIn, &(pna[i])); + resultSum += SQUARED(fitness); + } + + return 1 / FLT_SQRT(resultSum); +} + +Point getGradient(Point pointIn, PointsAndAngle *pna, size_t pnaCount, FLT precision) +{ + Point result; + + Point tmpXplus = pointIn; + Point tmpXminus = pointIn; + tmpXplus.x = pointIn.x + precision; + tmpXminus.x = pointIn.x - precision; + result.x = getPointFitness(tmpXplus, pna, pnaCount) - getPointFitness(tmpXminus, pna, pnaCount); + + Point tmpYplus = pointIn; + Point tmpYminus = pointIn; + tmpYplus.y = pointIn.y + precision; + tmpYminus.y = pointIn.y - precision; + result.y = getPointFitness(tmpYplus, pna, pnaCount) - getPointFitness(tmpYminus, pna, pnaCount); + + Point tmpZplus = pointIn; + Point tmpZminus = pointIn; + tmpZplus.z = pointIn.z + precision; + tmpZminus.z = pointIn.z - precision; + result.z = getPointFitness(tmpZplus, pna, pnaCount) - getPointFitness(tmpZminus, pna, pnaCount); + + return result; +} + +Point getNormalizedVector(Point vectorIn, FLT desiredMagnitude) +{ + FLT distanceIn = sqrt(SQUARED(vectorIn.x) + SQUARED(vectorIn.y) + SQUARED(vectorIn.z)); + + FLT scale = desiredMagnitude / distanceIn; + + Point result = vectorIn; + + result.x *= scale; + result.y *= scale; + result.z *= scale; + + return result; +} + +Point getAvgPoints(Point a, Point b) +{ + Point result; + result.x = (a.x + b.x) / 2; + result.y = (a.y + b.y) / 2; + result.z = (a.z + b.z) / 2; + return result; +} + + +// This is modifies the basic gradient descent algorithm to better handle the shallow valley case, +// which appears to be typical of this convergence. +static Point RefineEstimateUsingModifiedGradientDescent1(Point initialEstimate, PointsAndAngle *pna, size_t pnaCount, FILE *logFile) +{ + int i = 0; + FLT lastMatchFitness = getPointFitness(initialEstimate, pna, pnaCount); + Point lastPoint = initialEstimate; + + // The values below are somewhat magic, and definitely tunable + // The initial vlue of g will represent the biggest step that the gradient descent can take at first. + // bigger values may be faster, especially when the initial guess is wildly off. + // The downside to a bigger starting guess is that if we've picked a good guess at the local minima + // if there are other local minima, we may accidentally jump to such a local minima and get stuck there. + // That's fairly unlikely with the lighthouse problem, from expereince. + // The other downside is that if it's too big, we may have to spend a few iterations before it gets down + // to a size that doesn't jump us out of our minima. + // The terminal value of g represents how close we want to get to the local minima before we're "done" + // The change in value of g for each iteration is intentionally very close to 1. + // in fact, it probably could probably be 1 without any issue. The main place where g is decremented + // is in the block below when we've made a jump that results in a worse fitness than we're starting at. + // In those cases, we don't take the jump, and instead lower the value of g and try again. + for (FLT g = 0.2; g > 0.00001; g *= 0.99) + { + i++; + Point point1 = lastPoint; + // let's get 3 iterations of gradient descent here. + Point gradient1 = getGradient(point1, pna, pnaCount, g / 1000 /*somewhat arbitrary*/); + Point gradientN1 = getNormalizedVector(gradient1, g); + + Point point2; + point2.x = point1.x + gradientN1.x; + point2.y = point1.y + gradientN1.y; + point2.z = point1.z + gradientN1.z; + + Point gradient2 = getGradient(point2, pna, pnaCount, g / 1000 /*somewhat arbitrary*/); + Point gradientN2 = getNormalizedVector(gradient2, g); + + Point point3; + point3.x = point2.x + gradientN2.x; + point3.y = point2.y + gradientN2.y; + point3.z = point2.z + gradientN2.z; + + // remember that gradient descent has a tendency to zig-zag when it encounters a narrow valley? + // Well, solving the lighthouse problem presents a very narrow valley, and the zig-zag of a basic + // gradient descent is kinda horrible here. Instead, think about the shape that a zig-zagging + // converging gradient descent makes. Instead of using the gradient as the best indicator of + // the direction we should follow, we're looking at one side of the zig-zag pattern, and specifically + // following *that* vector. As it turns out, this works *amazingly* well. + + Point specialGradient = { .x = point3.x - point1.x,.y = point3.y - point1.y,.z = point3.y - point1.y }; + + // The second parameter to this function is very much a tunable parameter. Different values will result + // in a different number of iterations before we get to the minimum. Numbers between 3-10 seem to work well + // It's not clear what would be optimum here. + specialGradient = getNormalizedVector(specialGradient, g / 4); + + Point point4; + + point4.x = point3.x + specialGradient.x; + point4.y = point3.y + specialGradient.y; + point4.z = point3.z + specialGradient.z; + + FLT newMatchFitness = getPointFitness(point4, pna, pnaCount); + + if (newMatchFitness > lastMatchFitness) + { + if (logFile) + { + writePoint(logFile, lastPoint.x, lastPoint.y, lastPoint.z, 0xFFFFFF); + } + + lastMatchFitness = newMatchFitness; + lastPoint = point4; +#ifdef TORI_DEBUG + printf("+"); +#endif + } + else + { +#ifdef TORI_DEBUG + printf("-"); +#endif + g *= 0.7; + + } + + + } + printf("\ni=%d\n", i); + + return lastPoint; +} + + +// interesting-- this is one place where we could use any sensors that are only hit by +// just an x or y axis to make our estimate better. TODO: bring that data to this fn. +FLT RotationEstimateFitness(Point lhPoint, FLT *quaternion, TrackedObject *obj) +{ + for (size_t i = 0; i < obj->numSensors; i++) + { + // first, get the normal of the plane for the horizonal sweep + FLT theta = obj->sensor[i].theta; + // make two vectors that lie on the plane + FLT t1[3] = { 1, tan(theta), 0 }; + FLT t2[3] = { 1, tan(theta), 1 }; + + FLT tNorm[3]; + + // the normal is the cross of two vectors on the plane. + cross3d(tNorm, t1, t2); + + // distance for this plane is d= fabs(A*x + B*y)/sqrt(A^2+B^2) (z term goes away since this plane is "vertical") + // where A is + //FLT d = + } +} + +static Point RefineRotationEstimate(Point initialEstimate, PointsAndAngle *pna, size_t pnaCount, FILE *logFile) +{ + int i = 0; + FLT lastMatchFitness = getPointFitness(initialEstimate, pna, pnaCount); + Point lastPoint = initialEstimate; + + // The values below are somewhat magic, and definitely tunable + // The initial vlue of g will represent the biggest step that the gradient descent can take at first. + // bigger values may be faster, especially when the initial guess is wildly off. + // The downside to a bigger starting guess is that if we've picked a good guess at the local minima + // if there are other local minima, we may accidentally jump to such a local minima and get stuck there. + // That's fairly unlikely with the lighthouse problem, from expereince. + // The other downside is that if it's too big, we may have to spend a few iterations before it gets down + // to a size that doesn't jump us out of our minima. + // The terminal value of g represents how close we want to get to the local minima before we're "done" + // The change in value of g for each iteration is intentionally very close to 1. + // in fact, it probably could probably be 1 without any issue. The main place where g is decremented + // is in the block below when we've made a jump that results in a worse fitness than we're starting at. + // In those cases, we don't take the jump, and instead lower the value of g and try again. + for (FLT g = 0.2; g > 0.00001; g *= 0.99) + { + i++; + Point point1 = lastPoint; + // let's get 3 iterations of gradient descent here. + Point gradient1 = getGradient(point1, pna, pnaCount, g / 1000 /*somewhat arbitrary*/); + Point gradientN1 = getNormalizedVector(gradient1, g); + + Point point2; + point2.x = point1.x + gradientN1.x; + point2.y = point1.y + gradientN1.y; + point2.z = point1.z + gradientN1.z; + + Point gradient2 = getGradient(point2, pna, pnaCount, g / 1000 /*somewhat arbitrary*/); + Point gradientN2 = getNormalizedVector(gradient2, g); + + Point point3; + point3.x = point2.x + gradientN2.x; + point3.y = point2.y + gradientN2.y; + point3.z = point2.z + gradientN2.z; + + // remember that gradient descent has a tendency to zig-zag when it encounters a narrow valley? + // Well, solving the lighthouse problem presents a very narrow valley, and the zig-zag of a basic + // gradient descent is kinda horrible here. Instead, think about the shape that a zig-zagging + // converging gradient descent makes. Instead of using the gradient as the best indicator of + // the direction we should follow, we're looking at one side of the zig-zag pattern, and specifically + // following *that* vector. As it turns out, this works *amazingly* well. + + Point specialGradient = { .x = point3.x - point1.x,.y = point3.y - point1.y,.z = point3.y - point1.y }; + + // The second parameter to this function is very much a tunable parameter. Different values will result + // in a different number of iterations before we get to the minimum. Numbers between 3-10 seem to work well + // It's not clear what would be optimum here. + specialGradient = getNormalizedVector(specialGradient, g / 4); + + Point point4; + + point4.x = point3.x + specialGradient.x; + point4.y = point3.y + specialGradient.y; + point4.z = point3.z + specialGradient.z; + + FLT newMatchFitness = getPointFitness(point4, pna, pnaCount); + + if (newMatchFitness > lastMatchFitness) + { + if (logFile) + { + writePoint(logFile, lastPoint.x, lastPoint.y, lastPoint.z, 0xFFFFFF); + } + + lastMatchFitness = newMatchFitness; + lastPoint = point4; +#ifdef TORI_DEBUG + printf("+"); +#endif + } + else + { +#ifdef TORI_DEBUG + printf("-"); +#endif + g *= 0.7; + + } + + + } + printf("\ni=%d\n", i); + + return lastPoint; +} + +void SolveForRotation(FLT rotOut[4], TrackedObject *obj, Point lh) +{ + + // Step 1, create initial quaternion for guess. + // This should have the lighthouse directly facing the tracked object. + Point trackedObjRelativeToLh = { .x = -lh.x,.y = -lh.y,.z = -lh.z }; + FLT theta = atan2(-lh.x, -lh.y); + FLT zAxis[3] = { 0, 0, 1 }; + FLT quat1[4]; + quatfromaxisangle(quat1, zAxis, theta); + // not correcting for phi, but that's less important. + + // Step 2, optimize the quaternion to match the data. + +} + + +Point SolveForLighthouse(TrackedObject *obj, char doLogOutput) +{ + PointsAndAngle pna[MAX_POINT_PAIRS]; + + volatile size_t sizeNeeded = sizeof(pna); + + Point avgNorm = { 0 }; + + size_t pnaCount = 0; + for (unsigned int i = 0; i < obj->numSensors; i++) + { + for (unsigned int j = 0; j < i; j++) + { + if (pnaCount < MAX_POINT_PAIRS) + { + pna[pnaCount].a = obj->sensor[i].point; + pna[pnaCount].b = obj->sensor[j].point; + + pna[pnaCount].angle = angleBetweenSensors(&obj->sensor[i], &obj->sensor[j]); + //pna[pnaCount].angle = pythAngleBetweenSensors2(&obj->sensor[i], &obj->sensor[j]); + pna[pnaCount].tanAngle = FLT_TAN(pna[pnaCount].angle); + + double pythAngle = sqrt(SQUARED(obj->sensor[i].phi - obj->sensor[j].phi) + SQUARED(obj->sensor[i].theta - obj->sensor[j].theta)); + + pna[pnaCount].rotation = GetRotationMatrixForTorus(pna[pnaCount].a, pna[pnaCount].b); + pna[pnaCount].invRotation = inverseM33(pna[pnaCount].rotation); + + + pnaCount++; + } + } + + avgNorm.x += obj->sensor[i].normal.x; + avgNorm.y += obj->sensor[i].normal.y; + avgNorm.z += obj->sensor[i].normal.z; + } + avgNorm.x = avgNorm.x / obj->numSensors; + avgNorm.y = avgNorm.y / obj->numSensors; + avgNorm.z = avgNorm.z / obj->numSensors; + + FLT avgNormF[3] = { avgNorm.x, avgNorm.y, avgNorm.z }; + + + FILE *logFile = NULL; + if (doLogOutput) + { + logFile = fopen("pointcloud2.pcd", "wb"); + writePcdHeader(logFile); + writeAxes(logFile); + } + + + // Point refinedEstimageGd = RefineEstimateUsingModifiedGradientDescent1(initialEstimate, pna, pnaCount, logFile); + + + // arbitrarily picking a value of 8 meters out to start from. + // intentionally picking the direction of the average normal vector of the sensors that see the lighthouse + // since this is least likely to pick the incorrect "mirror" point that would send us + // back into the search for the correct point (see "if (a1 > M_PI / 2)" below) + Point p1 = getNormalizedVector(avgNorm, 8); + + Point refinedEstimateGd = RefineEstimateUsingModifiedGradientDescent1(p1, pna, pnaCount, logFile); + + FLT pf1[3] = { refinedEstimateGd.x, refinedEstimateGd.y, refinedEstimateGd.z }; + + FLT a1 = anglebetween3d(pf1, avgNormF); + + if (a1 > M_PI / 2) + { + Point p2 = { .x = -refinedEstimateGd.x,.y = -refinedEstimateGd.y,.z = -refinedEstimateGd.z }; + refinedEstimateGd = RefineEstimateUsingModifiedGradientDescent1(p2, pna, pnaCount, logFile); + + //FLT pf2[3] = { refinedEstimageGd2.x, refinedEstimageGd2.y, refinedEstimageGd2.z }; + + //FLT a2 = anglebetween3d(pf2, avgNormF); + + } + + FLT fitGd = getPointFitness(refinedEstimateGd, pna, pnaCount); + + FLT distance = FLT_SQRT(SQUARED(refinedEstimateGd.x) + SQUARED(refinedEstimateGd.y) + SQUARED(refinedEstimateGd.z)); + printf("(%4.4f, %4.4f, %4.4f)\n", refinedEstimateGd.x, refinedEstimateGd.y, refinedEstimateGd.z); + printf("Distance is %f, Fitness is %f\n", distance, fitGd); + + if (logFile) + { + updateHeader(logFile); + fclose(logFile); + } + //fgetc(stdin); + return refinedEstimateGd; +} + + + + + + + + + + + + +int PoserTurveyTori( SurviveObject * so, PoserData * pd ) +{ + PoserType pt = pd->pt; + SurviveContext * ctx = so->ctx; + ToriData * dd = so->PoserData; + + if (!dd) + { + so->PoserData = dd = malloc(sizeof(ToriData)); + memset(dd, 0, sizeof(ToriData)); + } + + switch( pt ) + { + case POSERDATA_IMU: + { + PoserDataIMU * imu = (PoserDataIMU*)pd; + //printf( "IMU:%s (%f %f %f) (%f %f %f)\n", so->codename, imu->accel[0], imu->accel[1], imu->accel[2], imu->gyro[0], imu->gyro[1], imu->gyro[2] ); + break; + } + case POSERDATA_LIGHT: + { + PoserDataLight * l = (PoserDataLight*)pd; + //printf( "LIG:%s %d @ %f rad, %f s (AC %d) (TC %d)\n", so->codename, l->sensor_id, l->angle, l->length, l->acode, l->timecode ); + break; + } + case POSERDATA_FULL_SCENE: + { + TrackedObject *to; + + PoserDataFullScene * fs = (PoserDataFullScene*)pd; + + to = malloc(sizeof(TrackedObject) + (SENSORS_PER_OBJECT * sizeof(TrackedSensor))); + + //FLT lengths[SENSORS_PER_OBJECT][NUM_LIGHTHOUSES][2]; + //FLT angles[SENSORS_PER_OBJECT][NUM_LIGHTHOUSES][2]; //2 Axes (Angles in LH space) + //FLT synctimes[SENSORS_PER_OBJECT][NUM_LIGHTHOUSES]; + + //to->numSensors = so->nr_locations; + { + int sensorCount = 0; + + for (int i = 0; i < so->nr_locations; i++) + { + if (fs->lengths[i][0][0] != -1 && fs->lengths[i][0][1] != -1) //lh 0 + { + to->sensor[sensorCount].normal.x = so->sensor_normals[i * 3 + 0]; + to->sensor[sensorCount].normal.y = so->sensor_normals[i * 3 + 1]; + to->sensor[sensorCount].normal.z = so->sensor_normals[i * 3 + 2]; + to->sensor[sensorCount].point.x = so->sensor_locations[i * 3 + 0]; + to->sensor[sensorCount].point.y = so->sensor_locations[i * 3 + 1]; + to->sensor[sensorCount].point.z = so->sensor_locations[i * 3 + 2]; + to->sensor[sensorCount].theta = fs->angles[i][0][0] + LINMATHPI / 2; // lighthouse 0, angle 0 (horizontal) + to->sensor[sensorCount].phi = fs->angles[i][0][1] + LINMATHPI / 2; // lighthosue 0, angle 1 (vertical) + sensorCount++; + } + } + + to->numSensors = sensorCount; + + SolveForLighthouse(to, 0); + } + { + int sensorCount = 0; + int lh = 1; + + for (int i = 0; i < so->nr_locations; i++) + { + if (fs->lengths[i][lh][0] != -1 && fs->lengths[i][lh][1] != -1) + { + to->sensor[sensorCount].normal.x = so->sensor_normals[i * 3 + 0]; + to->sensor[sensorCount].normal.y = so->sensor_normals[i * 3 + 1]; + to->sensor[sensorCount].normal.z = so->sensor_normals[i * 3 + 2]; + to->sensor[sensorCount].point.x = so->sensor_locations[i * 3 + 0]; + to->sensor[sensorCount].point.y = so->sensor_locations[i * 3 + 1]; + to->sensor[sensorCount].point.z = so->sensor_locations[i * 3 + 2]; + to->sensor[sensorCount].theta = fs->angles[i][lh][0] + LINMATHPI / 2; // lighthouse 0, angle 0 (horizontal) + to->sensor[sensorCount].phi = fs->angles[i][lh][1] + LINMATHPI / 2; // lighthosue 0, angle 1 (vertical) + sensorCount++; + } + } + + to->numSensors = sensorCount; + + SolveForLighthouse(to, 0); + } + //printf( "Full scene data.\n" ); + break; + } + case POSERDATA_DISASSOCIATE: + { + free( dd ); + so->PoserData = 0; + //printf( "Need to disassociate.\n" ); + break; + } + } + return 0; +} + + +REGISTER_LINKTIME( PoserTurveyTori ); + -- cgit v1.2.3 From 17db4d9c369ee8633b05e1d19b6fbb3d61007598 Mon Sep 17 00:00:00 2001 From: Mike Turvey Date: Sun, 26 Mar 2017 09:34:08 -0700 Subject: Adding octavioradii OctavioRadii converges to the right radius!!!! --- src/poser_octavioradii.c | 542 +++++++++++++++++++++++++++++++++++++++++++++++ src/poser_turveytori.c | 2 +- 2 files changed, 543 insertions(+), 1 deletion(-) create mode 100644 src/poser_octavioradii.c (limited to 'src') diff --git a/src/poser_octavioradii.c b/src/poser_octavioradii.c new file mode 100644 index 0000000..84d69fb --- /dev/null +++ b/src/poser_octavioradii.c @@ -0,0 +1,542 @@ +#include +#include +#include + +typedef struct +{ + int something; + //Stuff +} OctavioRadiiData; + +#include +#include +#include "linmath.h" +#include +#include +#include + +#define PTS 32 +#define MAX_CHECKS 40000 +#define MIN_HITS_FOR_VALID 10 + +FLT hmd_points[PTS * 3]; +FLT hmd_norms[PTS * 3]; +FLT hmd_point_angles[PTS * 2]; +int hmd_point_counts[PTS * 2]; +int best_hmd_target = 0; +int LoadData(char Camera, const char * FileData); + +//Values used for RunTest() +FLT LighthousePos[3] = { 0, 0, 0 }; +FLT LighthouseQuat[4] = { 1, 0, 0, 0 }; + +FLT RunTest(int print); +void PrintOpti(); + +#define MAX_POINT_PAIRS 100 + +typedef struct +{ + FLT x; + FLT y; + FLT z; +} Point; + +typedef struct +{ + Point point; // location of the sensor on the tracked object; + Point normal; // unit vector indicating the normal for the sensor + double theta; // "horizontal" angular measurement from lighthouse radians + double phi; // "vertical" angular measurement from lighthouse in radians. +} TrackedSensor; + +typedef struct +{ + size_t numSensors; + TrackedSensor sensor[0]; +} TrackedObject; + +typedef struct +{ + unsigned char index1; + unsigned char index2; + FLT KnownDistance; +} PointPair; + +static FLT distance(Point a, Point b) +{ + FLT x = a.x - b.x; + FLT y = a.y - b.y; + FLT z = a.z - b.z; + return FLT_SQRT(x*x + y*y + z*z); +} + +typedef struct +{ + FLT HorizAngle; + FLT VertAngle; +} SensorAngles; + +#define SQUARED(x) ((x)*(x)) + +static FLT calculateFitnessOld(SensorAngles *angles, FLT *radii, PointPair *pairs, size_t numPairs) +{ + FLT fitness = 0; + for (size_t i = 0; i < numPairs; i++) + { + FLT estimatedDistanceBetweenPoints = + SQUARED(radii[pairs[i].index1]) + + SQUARED(radii[pairs[i].index2]) + - 2 * radii[pairs[i].index1] * radii[pairs[i].index2] + * FLT_SIN(angles[pairs[i].index1].HorizAngle) * FLT_SIN(angles[pairs[i].index2].HorizAngle) + * FLT_COS(angles[pairs[i].index1].VertAngle - angles[pairs[i].index2].VertAngle) + + FLT_COS(angles[pairs[i].index1].VertAngle) * FLT_COS(angles[pairs[i].index2].VertAngle); + + fitness += SQUARED(estimatedDistanceBetweenPoints - pairs[i].KnownDistance); + } + + return FLT_SQRT(fitness); +} + +// angles is an array of angles between a sensor pair +// pairs is an array of point pairs +// radii is the guess at the radii of those angles +static FLT calculateFitnessOld2(SensorAngles *angles, FLT *radii, PointPair *pairs, size_t numPairs) +{ + FLT fitness = 0; + for (size_t i = 0; i < numPairs; i++) + { + // These are the vectors that represent the direction for the two points. + // TODO: optimize by precomputing the tangent. + FLT v1[3], v2[3], diff[3]; + + v1[0] = 1; + v2[0] = 1; + v1[1] = tan(angles[pairs[i].index1].HorizAngle); // can be precomputed + v2[1] = tan(angles[pairs[i].index2].HorizAngle); // can be precomputed + v1[2] = tan(angles[pairs[i].index1].VertAngle); // can be precomputed + v2[2] = tan(angles[pairs[i].index2].VertAngle); // can be precomputed + + // Now, normalize the vectors + normalize3d(v1, v1); // can be precomputed + normalize3d(v2, v2); // can be precomputed + + // Now, given the specified radii, find where the new points are + scale3d(v1, v1, radii[pairs[i].index1]); + scale3d(v2, v2, radii[pairs[i].index2]); + + // Cool, now find the vector between these two points + // TODO: optimize the following two funcs into one. + sub3d(diff, v1, v2); + + FLT distance = magnitude3d(diff); + + FLT t1 = magnitude3d(v1); + FLT t2 = magnitude3d(v2); + + + + FLT estimatedDistanceBetweenPoints = + + SQUARED(radii[pairs[i].index1]) + + SQUARED(radii[pairs[i].index2]) + - 2 * radii[pairs[i].index1] * radii[pairs[i].index2] + * FLT_SIN(angles[pairs[i].index1].HorizAngle) * FLT_SIN(angles[pairs[i].index2].HorizAngle) + * FLT_COS(angles[pairs[i].index1].VertAngle - angles[pairs[i].index2].VertAngle) + + FLT_COS(angles[pairs[i].index1].VertAngle) * FLT_COS(angles[pairs[i].index2].VertAngle); + + + //fitness += SQUARED(estimatedDistanceBetweenPoints - pairs[i].KnownDistance); + fitness += SQUARED(distance - pairs[i].KnownDistance); + } + + return FLT_SQRT(fitness); +} + +static FLT angleBetweenSensors(SensorAngles *a, SensorAngles *b) +{ + FLT angle = FLT_ACOS(FLT_COS(a->VertAngle - b->VertAngle)*FLT_COS(a->HorizAngle - b->HorizAngle)); + //FLT angle2 = FLT_ACOS(FLT_COS(b->phi - a->phi)*FLT_COS(b->theta - a->theta)); + + return angle; +} + +// angles is an array of angles between a sensor pair +// pairs is an array of point pairs +// radii is the guess at the radii of those angles +static FLT calculateFitness(SensorAngles *angles, FLT *radii, PointPair *pairs, size_t numPairs) +{ + FLT fitness = 0; + for (size_t i = 0; i < numPairs; i++) + { + + FLT angle = angleBetweenSensors(&angles[pairs[i].index1], &angles[pairs[i].index2]); + + // now we have a side-angle-side triangle, and we need to find the third side. + + // The Law of Cosines says: a^2 = b^2 + c^2 ? 2bc * cosA, + // where A is the angle opposite side a. + + // Transform this to: + // a = sqrt(b^2 + c^2 - 2bc * cosA) and we know the length of the missing side! + + FLT b2 = (SQUARED(radii[pairs[i].index1])); + FLT c2 = (SQUARED(radii[pairs[i].index2])); + FLT bc2 = (2 * radii[pairs[i].index1] * radii[pairs[i].index2]); + FLT cosA = (FLT_COS(angle)); + + FLT angleInDegrees = angle * 180 / LINMATHPI; + + FLT dist = sqrt( (SQUARED(radii[pairs[i].index1])) + + (SQUARED(radii[pairs[i].index2])) - + ( (2 * radii[pairs[i].index1] * radii[pairs[i].index2]) * + (FLT_COS(angle)))); + + + FLT fitnessAdder = SQUARED(dist - pairs[i].KnownDistance); + + if (isnan(fitnessAdder)) + { + int a = 0; + } + + //printf(" %2d %f\n", i, fitnessAdder); + + //fitness += SQUARED(estimatedDistanceBetweenPoints - pairs[i].KnownDistance); + fitness += SQUARED(dist - pairs[i].KnownDistance); + } + + //fitness = 1 / fitness; + return FLT_SQRT(fitness); +} + +#define MAX_RADII 32 + +// note gradientOut will be of the same degree as numRadii +static void getGradient(FLT *gradientOut, SensorAngles *angles, FLT *radii, size_t numRadii, PointPair *pairs, size_t numPairs, const FLT precision) +{ + FLT baseline = calculateFitness(angles, radii, pairs, numPairs); + + for (size_t i = 0; i < numRadii; i++) + { + FLT tmpPlus[MAX_RADII]; + memcpy(tmpPlus, radii, sizeof(*radii) * numRadii); + tmpPlus[i] += precision; + gradientOut[i] = -(calculateFitness(angles, tmpPlus, pairs, numPairs) - baseline); + } + + return; +} + +static void normalizeAndMultiplyVector(FLT *vectorToNormalize, size_t count, FLT desiredMagnitude) +{ + FLT distanceIn = 0; + + for (size_t i = 0; i < count; i++) + { + distanceIn += SQUARED(vectorToNormalize[i]); + } + distanceIn = FLT_SQRT(distanceIn); + + + FLT scale = desiredMagnitude / distanceIn; + + for (size_t i = 0; i < count; i++) + { + vectorToNormalize[i] *= scale; + } + + return; +} + + +static RefineEstimateUsingGradientDescentRadii(FLT *estimateOut, SensorAngles *angles, FLT *initialEstimate, size_t numRadii, PointPair *pairs, size_t numPairs, FILE *logFile) +{ + int i = 0; + FLT lastMatchFitness = calculateFitness(angles, initialEstimate, pairs, numPairs); + if (estimateOut != initialEstimate) + { + memcpy(estimateOut, initialEstimate, sizeof(*estimateOut) * numRadii); + } + + + // The values below are somewhat magic, and definitely tunable + // The initial vlue of g will represent the biggest step that the gradient descent can take at first. + // bigger values may be faster, especially when the initial guess is wildly off. + // The downside to a bigger starting guess is that if we've picked a good guess at the local minima + // if there are other local minima, we may accidentally jump to such a local minima and get stuck there. + // That's fairly unlikely with the lighthouse problem, from expereince. + // The other downside is that if it's too big, we may have to spend a few iterations before it gets down + // to a size that doesn't jump us out of our minima. + // The terminal value of g represents how close we want to get to the local minima before we're "done" + // The change in value of g for each iteration is intentionally very close to 1. + // in fact, it probably could probably be 1 without any issue. The main place where g is decremented + // is in the block below when we've made a jump that results in a worse fitness than we're starting at. + // In those cases, we don't take the jump, and instead lower the value of g and try again. + for (FLT g = 0.4; g > 0.00001; g *= 0.9999) + { + i++; + + + + FLT point1[MAX_RADII]; + memcpy(point1, estimateOut, sizeof(*point1) * numRadii); + + // let's get 3 iterations of gradient descent here. + FLT gradient1[MAX_RADII]; + getGradient(gradient1, angles, point1, numRadii, pairs, numPairs, g / 1000 /*somewhat arbitrary*/); + normalizeAndMultiplyVector(gradient1, numRadii, g); + + FLT point2[MAX_RADII]; + for (size_t i = 0; i < numRadii; i++) + { + point2[i] = point1[i] + gradient1[i]; + } + FLT gradient2[MAX_RADII]; + getGradient(gradient2, angles, point2, numRadii, pairs, numPairs, g / 1000 /*somewhat arbitrary*/); + normalizeAndMultiplyVector(gradient2, numRadii, g); + + FLT point3[MAX_RADII]; + for (size_t i = 0; i < numRadii; i++) + { + point3[i] = point2[i] + gradient2[i]; + } + + // remember that gradient descent has a tendency to zig-zag when it encounters a narrow valley? + // Well, solving the lighthouse problem presents a very narrow valley, and the zig-zag of a basic + // gradient descent is kinda horrible here. Instead, think about the shape that a zig-zagging + // converging gradient descent makes. Instead of using the gradient as the best indicator of + // the direction we should follow, we're looking at one side of the zig-zag pattern, and specifically + // following *that* vector. As it turns out, this works *amazingly* well. + + FLT specialGradient[MAX_RADII]; + for (size_t i = 0; i < numRadii; i++) + { + specialGradient[i] = point3[i] - point1[i]; + } + + // The second parameter to this function is very much a tunable parameter. Different values will result + // in a different number of iterations before we get to the minimum. Numbers between 3-10 seem to work well + // It's not clear what would be optimum here. + normalizeAndMultiplyVector(specialGradient, numRadii, g / 4); + + + FLT point4[MAX_RADII]; + for (size_t i = 0; i < numRadii; i++) + { + point4[i] = point3[i] + specialGradient[i]; + } + + + FLT newMatchFitness = calculateFitness(angles, point4, pairs, numPairs); + + + if (newMatchFitness < lastMatchFitness) + { + //if (logFile) + //{ + // writePoint(logFile, lastPoint.x, lastPoint.y, lastPoint.z, 0xFFFFFF); + //} + + lastMatchFitness = newMatchFitness; + memcpy(estimateOut, point4, sizeof(*estimateOut) * numRadii); + +#ifdef RADII_DEBUG + printf("+ %d %0.9f (%0.9f) \n", i, newMatchFitness, g); +#endif + g = g * 1.05; + } + else + { +//#ifdef RADII_DEBUG + // printf("-"); + printf("- %d %0.9f (%0.9f) [%0.9f] \n", i, newMatchFitness, g, estimateOut[0]); +//#endif + // if it wasn't a match, back off on the distance we jump + g *= 0.7; + + } + +#ifdef RADII_DEBUG + FLT avg = 0; + FLT diffFromAvg[MAX_RADII]; + + for (size_t m = 0; m < numRadii; m++) + { + avg += estimateOut[m]; + } + avg = avg / numRadii; + + for (size_t m = 0; m < numRadii; m++) + { + diffFromAvg[m] = estimateOut[m] - avg;; + } + printf("[avg:%f] ", avg); + + for (size_t x = 0; x < numRadii; x++) + { + printf("%f, ", diffFromAvg[x]); + //printf("%f, ", estimateOut[x]); + } + printf("\n"); + + +#endif + + + } + printf("\ni=%d\n", i); +} + +void SolveForLighthouseRadii(Point *objPosition, FLT *objOrientation, TrackedObject *obj) +{ + FLT estimate[MAX_RADII]; + + for (size_t i = 0; i < MAX_RADII; i++) + { + estimate[i] = 2.2; + } + + SensorAngles angles[MAX_RADII]; + PointPair pairs[MAX_POINT_PAIRS]; + + size_t pairCount = 0; + + //obj->numSensors = 5; // TODO: HACK!!!! + + for (size_t i = 0; i < obj->numSensors; i++) + { + angles[i].HorizAngle = obj->sensor[i].theta; + angles[i].VertAngle = obj->sensor[i].phi; + } + + for (size_t i = 0; i < obj->numSensors - 1; i++) + { + for (size_t j = i + 1; j < obj->numSensors; j++) + { + pairs[pairCount].index1 = i; + pairs[pairCount].index2 = j; + pairs[pairCount].KnownDistance = distance(obj->sensor[i].point, obj->sensor[j].point); + pairCount++; + } + } + + + RefineEstimateUsingGradientDescentRadii(estimate, angles, estimate, obj->numSensors, pairs, pairCount, NULL); + + // we should now have an estimate of the radii. + + for (size_t i = 0; i < obj->numSensors; i++) + { + printf("radius[%d]: %f\n", i, estimate[i]); + } + // (FLT *estimateOut, SensorAngles *angles, FLT *initialEstimate, size_t numRadii, PointPair *pairs, size_t numPairs, FILE *logFile) + + return; +} + +int PoserOctavioRadii( SurviveObject * so, PoserData * pd ) +{ + PoserType pt = pd->pt; + SurviveContext * ctx = so->ctx; + OctavioRadiiData * dd = so->PoserData; + + if( !dd ) so->PoserData = dd = malloc( sizeof(OctavioRadiiData) ); + + switch( pt ) + { + case POSERDATA_IMU: + { + PoserDataIMU * imu = (PoserDataIMU*)pd; + //printf( "IMU:%s (%f %f %f) (%f %f %f)\n", so->codename, imu->accel[0], imu->accel[1], imu->accel[2], imu->gyro[0], imu->gyro[1], imu->gyro[2] ); + break; + } + case POSERDATA_LIGHT: + { + PoserDataLight * l = (PoserDataLight*)pd; + //printf( "LIG:%s %d @ %f rad, %f s (AC %d) (TC %d)\n", so->codename, l->sensor_id, l->angle, l->length, l->acode, l->timecode ); + break; + } + case POSERDATA_FULL_SCENE: + { + TrackedObject *to; + + PoserDataFullScene * fs = (PoserDataFullScene*)pd; + + to = malloc(sizeof(TrackedObject) + (SENSORS_PER_OBJECT * sizeof(TrackedSensor))); + + //FLT lengths[SENSORS_PER_OBJECT][NUM_LIGHTHOUSES][2]; + //FLT angles[SENSORS_PER_OBJECT][NUM_LIGHTHOUSES][2]; //2 Axes (Angles in LH space) + //FLT synctimes[SENSORS_PER_OBJECT][NUM_LIGHTHOUSES]; + + //to->numSensors = so->nr_locations; + { + int sensorCount = 0; + + for (int i = 0; i < so->nr_locations; i++) + { + if (fs->lengths[i][0][0] != -1 && fs->lengths[i][0][1] != -1) //lh 0 + { + to->sensor[sensorCount].normal.x = so->sensor_normals[i * 3 + 0]; + to->sensor[sensorCount].normal.y = so->sensor_normals[i * 3 + 1]; + to->sensor[sensorCount].normal.z = so->sensor_normals[i * 3 + 2]; + to->sensor[sensorCount].point.x = so->sensor_locations[i * 3 + 0]; + to->sensor[sensorCount].point.y = so->sensor_locations[i * 3 + 1]; + to->sensor[sensorCount].point.z = so->sensor_locations[i * 3 + 2]; + to->sensor[sensorCount].theta = fs->angles[i][0][0] + LINMATHPI / 2; // lighthouse 0, angle 0 (horizontal) + to->sensor[sensorCount].phi = fs->angles[i][0][1] + LINMATHPI / 2; // lighthosue 0, angle 1 (vertical) + sensorCount++; + } + } + + to->numSensors = sensorCount; + + Point position; + FLT orientation[4]; + + SolveForLighthouseRadii(&position, &orientation, to); + } + { + int sensorCount = 0; + int lh = 1; + + for (int i = 0; i < so->nr_locations; i++) + { + if (fs->lengths[i][lh][0] != -1 && fs->lengths[i][lh][1] != -1) + { + to->sensor[sensorCount].normal.x = so->sensor_normals[i * 3 + 0]; + to->sensor[sensorCount].normal.y = so->sensor_normals[i * 3 + 1]; + to->sensor[sensorCount].normal.z = so->sensor_normals[i * 3 + 2]; + to->sensor[sensorCount].point.x = so->sensor_locations[i * 3 + 0]; + to->sensor[sensorCount].point.y = so->sensor_locations[i * 3 + 1]; + to->sensor[sensorCount].point.z = so->sensor_locations[i * 3 + 2]; + to->sensor[sensorCount].theta = fs->angles[i][lh][0] + LINMATHPI / 2; // lighthouse 0, angle 0 (horizontal) + to->sensor[sensorCount].phi = fs->angles[i][lh][1] + LINMATHPI / 2; // lighthosue 0, angle 1 (vertical) + sensorCount++; + } + } + + to->numSensors = sensorCount; + + Point position; + FLT orientation[4]; + + SolveForLighthouseRadii(&position, &orientation, to); + } + //printf( "Full scene data.\n" ); + break; + } + case POSERDATA_DISASSOCIATE: + { + free( dd ); + so->PoserData = 0; + //printf( "Need to disassociate.\n" ); + break; + } + } + return 0; +} + + +REGISTER_LINKTIME( PoserOctavioRadii ); + diff --git a/src/poser_turveytori.c b/src/poser_turveytori.c index e9e5b7a..4e602f3 100644 --- a/src/poser_turveytori.c +++ b/src/poser_turveytori.c @@ -299,7 +299,7 @@ void estimateToroidalAndPoloidalAngleOfPoint( FLT angleBetweenSensors(TrackedSensor *a, TrackedSensor *b) { FLT angle = FLT_ACOS(FLT_COS(a->phi - b->phi)*FLT_COS(a->theta - b->theta)); - FLT angle2 = FLT_ACOS(FLT_COS(b->phi - a->phi)*FLT_COS(b->theta - a->theta)); + //FLT angle2 = FLT_ACOS(FLT_COS(b->phi - a->phi)*FLT_COS(b->theta - a->theta)); return angle; } -- cgit v1.2.3 From 51b8cf7083d712eac730fe90f383abfabc7f2676 Mon Sep 17 00:00:00 2001 From: Mike Turvey Date: Sun, 26 Mar 2017 11:49:51 -0700 Subject: Tweaking --- src/poser_octavioradii.c | 4 ++-- 1 file changed, 2 insertions(+), 2 deletions(-) (limited to 'src') diff --git a/src/poser_octavioradii.c b/src/poser_octavioradii.c index 84d69fb..18d4026 100644 --- a/src/poser_octavioradii.c +++ b/src/poser_octavioradii.c @@ -350,7 +350,7 @@ static RefineEstimateUsingGradientDescentRadii(FLT *estimateOut, SensorAngles *a { //#ifdef RADII_DEBUG // printf("-"); - printf("- %d %0.9f (%0.9f) [%0.9f] \n", i, newMatchFitness, g, estimateOut[0]); + //printf("- %d %0.9f (%0.9f) [%0.9f] \n", i, newMatchFitness, g, estimateOut[0]); //#endif // if it wasn't a match, back off on the distance we jump g *= 0.7; @@ -394,7 +394,7 @@ void SolveForLighthouseRadii(Point *objPosition, FLT *objOrientation, TrackedObj for (size_t i = 0; i < MAX_RADII; i++) { - estimate[i] = 2.2; + estimate[i] = 2.38; } SensorAngles angles[MAX_RADII]; -- cgit v1.2.3 From ef039bb3ee8ce95f25e86baddbc4b3d5f21e6743 Mon Sep 17 00:00:00 2001 From: mwturvey Date: Mon, 27 Mar 2017 10:20:05 -0700 Subject: Fixing compiler warnings --- src/poser_octavioradii.c | 13 +++++-------- 1 file changed, 5 insertions(+), 8 deletions(-) (limited to 'src') diff --git a/src/poser_octavioradii.c b/src/poser_octavioradii.c index 18d4026..3893085 100644 --- a/src/poser_octavioradii.c +++ b/src/poser_octavioradii.c @@ -24,14 +24,11 @@ FLT hmd_norms[PTS * 3]; FLT hmd_point_angles[PTS * 2]; int hmd_point_counts[PTS * 2]; int best_hmd_target = 0; -int LoadData(char Camera, const char * FileData); //Values used for RunTest() FLT LighthousePos[3] = { 0, 0, 0 }; FLT LighthouseQuat[4] = { 1, 0, 0, 0 }; -FLT RunTest(int print); -void PrintOpti(); #define MAX_POINT_PAIRS 100 @@ -410,9 +407,9 @@ void SolveForLighthouseRadii(Point *objPosition, FLT *objOrientation, TrackedObj angles[i].VertAngle = obj->sensor[i].phi; } - for (size_t i = 0; i < obj->numSensors - 1; i++) + for (unsigned char i = 0; i < obj->numSensors - 1; i++) { - for (size_t j = i + 1; j < obj->numSensors; j++) + for (unsigned char j = i + 1; j < obj->numSensors; j++) { pairs[pairCount].index1 = i; pairs[pairCount].index2 = j; @@ -426,7 +423,7 @@ void SolveForLighthouseRadii(Point *objPosition, FLT *objOrientation, TrackedObj // we should now have an estimate of the radii. - for (size_t i = 0; i < obj->numSensors; i++) + for (int i = 0; i < obj->numSensors; i++) { printf("radius[%d]: %f\n", i, estimate[i]); } @@ -494,7 +491,7 @@ int PoserOctavioRadii( SurviveObject * so, PoserData * pd ) Point position; FLT orientation[4]; - SolveForLighthouseRadii(&position, &orientation, to); + SolveForLighthouseRadii(&position, orientation, to); } { int sensorCount = 0; @@ -521,7 +518,7 @@ int PoserOctavioRadii( SurviveObject * so, PoserData * pd ) Point position; FLT orientation[4]; - SolveForLighthouseRadii(&position, &orientation, to); + SolveForLighthouseRadii(&position, orientation, to); } //printf( "Full scene data.\n" ); break; -- cgit v1.2.3 From f923e3c5ad03e7942eb46b67666807b738a1428a Mon Sep 17 00:00:00 2001 From: mwturvey Date: Mon, 27 Mar 2017 10:59:26 -0700 Subject: completed fitness func for tori poser rotation --- src/poser_turveytori.c | 60 ++++++++++++++++++++++++++++++++++++++++++++------ 1 file changed, 53 insertions(+), 7 deletions(-) (limited to 'src') diff --git a/src/poser_turveytori.c b/src/poser_turveytori.c index 4e602f3..37f79bb 100644 --- a/src/poser_turveytori.c +++ b/src/poser_turveytori.c @@ -535,23 +535,69 @@ static Point RefineEstimateUsingModifiedGradientDescent1(Point initialEstimate, // just an x or y axis to make our estimate better. TODO: bring that data to this fn. FLT RotationEstimateFitness(Point lhPoint, FLT *quaternion, TrackedObject *obj) { + FLT fitness = 0; for (size_t i = 0; i < obj->numSensors; i++) { // first, get the normal of the plane for the horizonal sweep FLT theta = obj->sensor[i].theta; // make two vectors that lie on the plane - FLT t1[3] = { 1, tan(theta), 0 }; - FLT t2[3] = { 1, tan(theta), 1 }; + FLT t1H[3] = { 1, tan(theta), 0 }; + FLT t2H[3] = { 1, tan(theta), 1 }; - FLT tNorm[3]; + FLT tNormH[3]; // the normal is the cross of two vectors on the plane. - cross3d(tNorm, t1, t2); + cross3d(tNormH, t1H, t2H); - // distance for this plane is d= fabs(A*x + B*y)/sqrt(A^2+B^2) (z term goes away since this plane is "vertical") - // where A is - //FLT d = + normalize3d(tNormH, tNormH); + + // Now do the same for the vertical sweep + + // first, get the normal of the plane for the horizonal sweep + FLT phi = obj->sensor[i].phi; + // make two vectors that lie on the plane + FLT t1V[3] = { 0, 1, tan(phi)}; + FLT t2V[3] = { 1, 1, tan(phi)}; + + FLT tNormV[3]; + + // the normal is the cross of two vectors on the plane. + cross3d(tNormV, t1V, t2V); + + normalize3d(tNormV, tNormV); + + + // First, where is the sensor in the object's reference frame? + FLT sensor_in_obj_reference_frame[3] = {obj->sensor->point.x, obj->sensor->point.y, obj->sensor->point.z}; + // Where is the point, in the reference frame of the lighthouse? + // This has two steps, first we translate from the object's location being the + // origin to the lighthouse being the origin. + // And second, we apply the quaternion to rotate into the proper reference frame for the lighthouse. + + FLT sensor_in_lh_reference_frame[3]; + sub3d(sensor_in_lh_reference_frame, sensor_in_obj_reference_frame, (FLT[3]){lhPoint.x, lhPoint.y, lhPoint.z}); + + quatrotatevector(sensor_in_lh_reference_frame, quaternion, sensor_in_lh_reference_frame); + + // now the we've got the location of the sensor in the lighthouses's reference frame, given lhPoint and quaternion inputs. + + // We need an arbitrary vector from the plane to the point. + // Since the plane goes through the origin, this is trivial. + // The sensor point itself is such a vector! + + // And go calculate the distances! + // TODO: don't need to ABS these because we square them below. + FLT dH = FLT_FABS(dot3d(sensor_in_lh_reference_frame, tNormH)); + FLT dV = FLT_FABS(dot3d(sensor_in_lh_reference_frame, tNormV)); + + + fitness += SQUARED(dH); + fitness += SQUARED(dV); } + + fitness = FLT_SQRT(fitness); + + return fitness; } static Point RefineRotationEstimate(Point initialEstimate, PointsAndAngle *pna, size_t pnaCount, FILE *logFile) -- cgit v1.2.3 From b9e9c89a46a91cbc69d7832d6f67e571723d11e6 Mon Sep 17 00:00:00 2001 From: mwturvey Date: Mon, 27 Mar 2017 12:48:40 -0700 Subject: Tori puzzle pieces in place, rotation not converging --- src/poser_turveytori.c | 197 +++++++++++++++++++++++++++++++++++++------------ 1 file changed, 149 insertions(+), 48 deletions(-) (limited to 'src') diff --git a/src/poser_turveytori.c b/src/poser_turveytori.c index 37f79bb..15961c8 100644 --- a/src/poser_turveytori.c +++ b/src/poser_turveytori.c @@ -412,7 +412,7 @@ Point getGradient(Point pointIn, PointsAndAngle *pna, size_t pnaCount, FLT preci return result; } -Point getNormalizedVector(Point vectorIn, FLT desiredMagnitude) +Point getNormalizedAndScaledVector(Point vectorIn, FLT desiredMagnitude) { FLT distanceIn = sqrt(SQUARED(vectorIn.x) + SQUARED(vectorIn.y) + SQUARED(vectorIn.z)); @@ -464,7 +464,7 @@ static Point RefineEstimateUsingModifiedGradientDescent1(Point initialEstimate, Point point1 = lastPoint; // let's get 3 iterations of gradient descent here. Point gradient1 = getGradient(point1, pna, pnaCount, g / 1000 /*somewhat arbitrary*/); - Point gradientN1 = getNormalizedVector(gradient1, g); + Point gradientN1 = getNormalizedAndScaledVector(gradient1, g); Point point2; point2.x = point1.x + gradientN1.x; @@ -472,7 +472,7 @@ static Point RefineEstimateUsingModifiedGradientDescent1(Point initialEstimate, point2.z = point1.z + gradientN1.z; Point gradient2 = getGradient(point2, pna, pnaCount, g / 1000 /*somewhat arbitrary*/); - Point gradientN2 = getNormalizedVector(gradient2, g); + Point gradientN2 = getNormalizedAndScaledVector(gradient2, g); Point point3; point3.x = point2.x + gradientN2.x; @@ -491,7 +491,7 @@ static Point RefineEstimateUsingModifiedGradientDescent1(Point initialEstimate, // The second parameter to this function is very much a tunable parameter. Different values will result // in a different number of iterations before we get to the minimum. Numbers between 3-10 seem to work well // It's not clear what would be optimum here. - specialGradient = getNormalizedVector(specialGradient, g / 4); + specialGradient = getNormalizedAndScaledVector(specialGradient, g / 4); Point point4; @@ -531,6 +531,75 @@ static Point RefineEstimateUsingModifiedGradientDescent1(Point initialEstimate, } +// interesting-- this is one place where we could use any sensors that are only hit by +// just an x or y axis to make our estimate better. TODO: bring that data to this fn. +FLT RotationEstimateFitnessOld(Point lhPoint, FLT *quaternion, TrackedObject *obj) +{ + FLT fitness = 0; + for (size_t i = 0; i < obj->numSensors; i++) + { + // first, get the normal of the plane for the horizonal sweep + FLT theta = obj->sensor[i].theta; + // make two vectors that lie on the plane + FLT t1H[3] = { 1, tan(theta-LINMATHPI/2), 0 }; + FLT t2H[3] = { 1, tan(theta-LINMATHPI/2), 1 }; + + FLT tNormH[3]; + + // the normal is the cross of two vectors on the plane. + cross3d(tNormH, t1H, t2H); + + normalize3d(tNormH, tNormH); + + // Now do the same for the vertical sweep + + // first, get the normal of the plane for the horizonal sweep + FLT phi = obj->sensor[i].phi; + // make two vectors that lie on the plane + FLT t1V[3] = { 0, 1, tan(phi-LINMATHPI/2)}; + FLT t2V[3] = { 1, 1, tan(phi-LINMATHPI/2)}; + + FLT tNormV[3]; + + // the normal is the cross of two vectors on the plane. + cross3d(tNormV, t1V, t2V); + + normalize3d(tNormV, tNormV); + + + // First, where is the sensor in the object's reference frame? + FLT sensor_in_obj_reference_frame[3] = {obj->sensor->point.x, obj->sensor->point.y, obj->sensor->point.z}; + // Where is the point, in the reference frame of the lighthouse? + // This has two steps, first we translate from the object's location being the + // origin to the lighthouse being the origin. + // And second, we apply the quaternion to rotate into the proper reference frame for the lighthouse. + + FLT sensor_in_lh_reference_frame[3]; + sub3d(sensor_in_lh_reference_frame, sensor_in_obj_reference_frame, (FLT[3]){lhPoint.x, lhPoint.y, lhPoint.z}); + + quatrotatevector(sensor_in_lh_reference_frame, quaternion, sensor_in_lh_reference_frame); + + // now the we've got the location of the sensor in the lighthouses's reference frame, given lhPoint and quaternion inputs. + + // We need an arbitrary vector from the plane to the point. + // Since the plane goes through the origin, this is trivial. + // The sensor point itself is such a vector! + + // And go calculate the distances! + // TODO: don't need to ABS these because we square them below. + FLT dH = FLT_FABS(dot3d(sensor_in_lh_reference_frame, tNormH)); + FLT dV = FLT_FABS(dot3d(sensor_in_lh_reference_frame, tNormV)); + + + fitness += SQUARED(dH); + fitness += SQUARED(dV); + } + + fitness = FLT_SQRT(fitness); + + return fitness; +} + // interesting-- this is one place where we could use any sensors that are only hit by // just an x or y axis to make our estimate better. TODO: bring that data to this fn. FLT RotationEstimateFitness(Point lhPoint, FLT *quaternion, TrackedObject *obj) @@ -541,8 +610,8 @@ FLT RotationEstimateFitness(Point lhPoint, FLT *quaternion, TrackedObject *obj) // first, get the normal of the plane for the horizonal sweep FLT theta = obj->sensor[i].theta; // make two vectors that lie on the plane - FLT t1H[3] = { 1, tan(theta), 0 }; - FLT t2H[3] = { 1, tan(theta), 1 }; + FLT t1H[3] = { 1, tan(theta-LINMATHPI/2), 0 }; + FLT t2H[3] = { 1, tan(theta-LINMATHPI/2), 1 }; FLT tNormH[3]; @@ -556,8 +625,8 @@ FLT RotationEstimateFitness(Point lhPoint, FLT *quaternion, TrackedObject *obj) // first, get the normal of the plane for the horizonal sweep FLT phi = obj->sensor[i].phi; // make two vectors that lie on the plane - FLT t1V[3] = { 0, 1, tan(phi)}; - FLT t2V[3] = { 1, 1, tan(phi)}; + FLT t1V[3] = { 0, 1, tan(phi-LINMATHPI/2)}; + FLT t2V[3] = { 1, 1, tan(phi-LINMATHPI/2)}; FLT tNormV[3]; @@ -600,11 +669,43 @@ FLT RotationEstimateFitness(Point lhPoint, FLT *quaternion, TrackedObject *obj) return fitness; } -static Point RefineRotationEstimate(Point initialEstimate, PointsAndAngle *pna, size_t pnaCount, FILE *logFile) +void getRotationGradient(FLT *gradientOut, Point lhPoint, FLT *quaternion, TrackedObject *obj, FLT precision) +{ + + FLT baseFitness = RotationEstimateFitness(lhPoint, quaternion, obj); + + FLT tmp0plus[4]; + quatadd(tmp0plus, quaternion, (FLT[4]){precision, 0, 0, 0}); + gradientOut[0] = RotationEstimateFitness(lhPoint, tmp0plus, obj) - baseFitness; + + FLT tmp1plus[4]; + quatadd(tmp1plus, quaternion, (FLT[4]){0, precision, 0, 0}); + gradientOut[1] = RotationEstimateFitness(lhPoint, tmp1plus, obj) - baseFitness; + + FLT tmp2plus[4]; + quatadd(tmp2plus, quaternion, (FLT[4]){0, 0, precision, 0}); + gradientOut[2] = RotationEstimateFitness(lhPoint, tmp2plus, obj) - baseFitness; + + FLT tmp3plus[4]; + quatadd(tmp3plus, quaternion, (FLT[4]){0, 0, 0, precision}); + gradientOut[3] = RotationEstimateFitness(lhPoint, tmp3plus, obj) - baseFitness; + + return; +} + +void getNormalizedAndScaledRotationGradient(FLT *vectorToScale, FLT desiredMagnitude) +{ + quatnormalize(vectorToScale, vectorToScale); + quatscale(vectorToScale, vectorToScale, desiredMagnitude); + return; +} + +static void RefineRotationEstimate(FLT *rotOut, Point lhPoint, FLT *initialEstimate, TrackedObject *obj) { int i = 0; - FLT lastMatchFitness = getPointFitness(initialEstimate, pna, pnaCount); - Point lastPoint = initialEstimate; + FLT lastMatchFitness = RotationEstimateFitness(lhPoint, initialEstimate, obj); + + quatcopy(rotOut, initialEstimate); // The values below are somewhat magic, and definitely tunable // The initial vlue of g will represent the biggest step that the gradient descent can take at first. @@ -622,23 +723,25 @@ static Point RefineRotationEstimate(Point initialEstimate, PointsAndAngle *pna, for (FLT g = 0.2; g > 0.00001; g *= 0.99) { i++; - Point point1 = lastPoint; + FLT point1[3]; + copy3d(point1, rotOut); // let's get 3 iterations of gradient descent here. - Point gradient1 = getGradient(point1, pna, pnaCount, g / 1000 /*somewhat arbitrary*/); - Point gradientN1 = getNormalizedVector(gradient1, g); + FLT gradient1[4]; + + getRotationGradient(gradient1, lhPoint, point1, obj, g/1000); + getNormalizedAndScaledRotationGradient(gradient1,g); - Point point2; - point2.x = point1.x + gradientN1.x; - point2.y = point1.y + gradientN1.y; - point2.z = point1.z + gradientN1.z; + FLT point2[4]; + quatadd(point2, gradient1, point1); + quatnormalize(point2,point2); - Point gradient2 = getGradient(point2, pna, pnaCount, g / 1000 /*somewhat arbitrary*/); - Point gradientN2 = getNormalizedVector(gradient2, g); + FLT gradient2[4]; + getRotationGradient(gradient2, lhPoint, point2, obj, g/1000); + getNormalizedAndScaledRotationGradient(gradient2,g); - Point point3; - point3.x = point2.x + gradientN2.x; - point3.y = point2.y + gradientN2.y; - point3.z = point2.z + gradientN2.z; + FLT point3[4]; + quatadd(point3, gradient2, point2); + quatnormalize(point3,point3); // remember that gradient descent has a tendency to zig-zag when it encounters a narrow valley? // Well, solving the lighthouse problem presents a very narrow valley, and the zig-zag of a basic @@ -647,48 +750,41 @@ static Point RefineRotationEstimate(Point initialEstimate, PointsAndAngle *pna, // the direction we should follow, we're looking at one side of the zig-zag pattern, and specifically // following *that* vector. As it turns out, this works *amazingly* well. - Point specialGradient = { .x = point3.x - point1.x,.y = point3.y - point1.y,.z = point3.y - point1.y }; + FLT specialGradient[4]; + quatsub(specialGradient,point3,point1); // The second parameter to this function is very much a tunable parameter. Different values will result // in a different number of iterations before we get to the minimum. Numbers between 3-10 seem to work well // It's not clear what would be optimum here. - specialGradient = getNormalizedVector(specialGradient, g / 4); + getNormalizedAndScaledRotationGradient(specialGradient,g/4); - Point point4; + FLT point4[4]; + quatadd(point4, specialGradient, point3); + quatnormalize(point4,point4); - point4.x = point3.x + specialGradient.x; - point4.y = point3.y + specialGradient.y; - point4.z = point3.z + specialGradient.z; - - FLT newMatchFitness = getPointFitness(point4, pna, pnaCount); + FLT newMatchFitness = RotationEstimateFitness(lhPoint, point4, obj); if (newMatchFitness > lastMatchFitness) { - if (logFile) - { - writePoint(logFile, lastPoint.x, lastPoint.y, lastPoint.z, 0xFFFFFF); - } lastMatchFitness = newMatchFitness; - lastPoint = point4; -#ifdef TORI_DEBUG + quatcopy(rotOut, point4); +//#ifdef TORI_DEBUG printf("+"); -#endif +//#endif } else { -#ifdef TORI_DEBUG +//#ifdef TORI_DEBUG printf("-"); -#endif +//#endif g *= 0.7; } } - printf("\ni=%d\n", i); - - return lastPoint; + printf("\nRi=%d\n", i); } void SolveForRotation(FLT rotOut[4], TrackedObject *obj, Point lh) @@ -698,12 +794,14 @@ void SolveForRotation(FLT rotOut[4], TrackedObject *obj, Point lh) // This should have the lighthouse directly facing the tracked object. Point trackedObjRelativeToLh = { .x = -lh.x,.y = -lh.y,.z = -lh.z }; FLT theta = atan2(-lh.x, -lh.y); - FLT zAxis[3] = { 0, 0, 1 }; - FLT quat1[4]; - quatfromaxisangle(quat1, zAxis, theta); + FLT zAxis[4] = { 0, 0, 1 ,0}; + //FLT quat1[4]; + //quatfromaxisangle(quat1, zAxis, theta); // not correcting for phi, but that's less important. + // Step 2, optimize the quaternion to match the data. + RefineRotationEstimate(rotOut, lh, zAxis, obj); } @@ -767,7 +865,7 @@ Point SolveForLighthouse(TrackedObject *obj, char doLogOutput) // intentionally picking the direction of the average normal vector of the sensors that see the lighthouse // since this is least likely to pick the incorrect "mirror" point that would send us // back into the search for the correct point (see "if (a1 > M_PI / 2)" below) - Point p1 = getNormalizedVector(avgNorm, 8); + Point p1 = getNormalizedAndScaledVector(avgNorm, 8); Point refinedEstimateGd = RefineEstimateUsingModifiedGradientDescent1(p1, pna, pnaCount, logFile); @@ -792,6 +890,9 @@ Point SolveForLighthouse(TrackedObject *obj, char doLogOutput) printf("(%4.4f, %4.4f, %4.4f)\n", refinedEstimateGd.x, refinedEstimateGd.y, refinedEstimateGd.z); printf("Distance is %f, Fitness is %f\n", distance, fitGd); + FLT rot[4]; + SolveForRotation(rot, obj, refinedEstimateGd); + if (logFile) { updateHeader(logFile); -- cgit v1.2.3 From 9ead7de95621f1d7d59fed26fc7431344fdd9db4 Mon Sep 17 00:00:00 2001 From: mwturvey Date: Mon, 27 Mar 2017 13:58:04 -0700 Subject: Something is converging. --- src/poser_turveytori.c | 29 +++++++++++++++++++---------- 1 file changed, 19 insertions(+), 10 deletions(-) (limited to 'src') diff --git a/src/poser_turveytori.c b/src/poser_turveytori.c index 15961c8..824fabb 100644 --- a/src/poser_turveytori.c +++ b/src/poser_turveytori.c @@ -646,7 +646,8 @@ FLT RotationEstimateFitness(Point lhPoint, FLT *quaternion, TrackedObject *obj) FLT sensor_in_lh_reference_frame[3]; sub3d(sensor_in_lh_reference_frame, sensor_in_obj_reference_frame, (FLT[3]){lhPoint.x, lhPoint.y, lhPoint.z}); - quatrotatevector(sensor_in_lh_reference_frame, quaternion, sensor_in_lh_reference_frame); + //quatrotatevector(sensor_in_lh_reference_frame, quaternion, sensor_in_lh_reference_frame); + rotatearoundaxis(sensor_in_lh_reference_frame, sensor_in_lh_reference_frame, quaternion, quaternion[3]); // now the we've got the location of the sensor in the lighthouses's reference frame, given lhPoint and quaternion inputs. @@ -666,7 +667,7 @@ FLT RotationEstimateFitness(Point lhPoint, FLT *quaternion, TrackedObject *obj) fitness = FLT_SQRT(fitness); - return fitness; + return 1/fitness; } void getRotationGradient(FLT *gradientOut, Point lhPoint, FLT *quaternion, TrackedObject *obj, FLT precision) @@ -693,10 +694,18 @@ void getRotationGradient(FLT *gradientOut, Point lhPoint, FLT *quaternion, Track return; } +//void getNormalizedAndScaledRotationGradient(FLT *vectorToScale, FLT desiredMagnitude) +//{ +// quatnormalize(vectorToScale, vectorToScale); +// quatscale(vectorToScale, vectorToScale, desiredMagnitude); +// return; +//} void getNormalizedAndScaledRotationGradient(FLT *vectorToScale, FLT desiredMagnitude) { quatnormalize(vectorToScale, vectorToScale); quatscale(vectorToScale, vectorToScale, desiredMagnitude); + //vectorToScale[3] = desiredMagnitude; + return; } @@ -723,8 +732,8 @@ static void RefineRotationEstimate(FLT *rotOut, Point lhPoint, FLT *initialEstim for (FLT g = 0.2; g > 0.00001; g *= 0.99) { i++; - FLT point1[3]; - copy3d(point1, rotOut); + FLT point1[4]; + quatcopy(point1, rotOut); // let's get 3 iterations of gradient descent here. FLT gradient1[4]; @@ -733,7 +742,7 @@ static void RefineRotationEstimate(FLT *rotOut, Point lhPoint, FLT *initialEstim FLT point2[4]; quatadd(point2, gradient1, point1); - quatnormalize(point2,point2); + //quatnormalize(point2,point2); FLT gradient2[4]; getRotationGradient(gradient2, lhPoint, point2, obj, g/1000); @@ -741,7 +750,7 @@ static void RefineRotationEstimate(FLT *rotOut, Point lhPoint, FLT *initialEstim FLT point3[4]; quatadd(point3, gradient2, point2); - quatnormalize(point3,point3); + //quatnormalize(point3,point3); // remember that gradient descent has a tendency to zig-zag when it encounters a narrow valley? // Well, solving the lighthouse problem presents a very narrow valley, and the zig-zag of a basic @@ -760,7 +769,7 @@ static void RefineRotationEstimate(FLT *rotOut, Point lhPoint, FLT *initialEstim FLT point4[4]; quatadd(point4, specialGradient, point3); - quatnormalize(point4,point4); + //quatnormalize(point4,point4); FLT newMatchFitness = RotationEstimateFitness(lhPoint, point4, obj); @@ -770,13 +779,13 @@ static void RefineRotationEstimate(FLT *rotOut, Point lhPoint, FLT *initialEstim lastMatchFitness = newMatchFitness; quatcopy(rotOut, point4); //#ifdef TORI_DEBUG - printf("+"); + printf("+ %8.8f, %f\n", newMatchFitness, point4[3]); //#endif } else { //#ifdef TORI_DEBUG - printf("-"); + printf("- , %f\n", point4[3]); //#endif g *= 0.7; @@ -794,7 +803,7 @@ void SolveForRotation(FLT rotOut[4], TrackedObject *obj, Point lh) // This should have the lighthouse directly facing the tracked object. Point trackedObjRelativeToLh = { .x = -lh.x,.y = -lh.y,.z = -lh.z }; FLT theta = atan2(-lh.x, -lh.y); - FLT zAxis[4] = { 0, 0, 1 ,0}; + FLT zAxis[4] = { 0, 0, 1 , theta}; //FLT quat1[4]; //quatfromaxisangle(quat1, zAxis, theta); // not correcting for phi, but that's less important. -- cgit v1.2.3 From e5c15af3af93356bb056624726e0b6068354690f Mon Sep 17 00:00:00 2001 From: mwturvey Date: Mon, 27 Mar 2017 14:20:29 -0700 Subject: Getting very close --- src/poser_turveytori.c | 17 ++++++++++++++++- 1 file changed, 16 insertions(+), 1 deletion(-) (limited to 'src') diff --git a/src/poser_turveytori.c b/src/poser_turveytori.c index 824fabb..d737723 100644 --- a/src/poser_turveytori.c +++ b/src/poser_turveytori.c @@ -779,8 +779,10 @@ static void RefineRotationEstimate(FLT *rotOut, Point lhPoint, FLT *initialEstim lastMatchFitness = newMatchFitness; quatcopy(rotOut, point4); //#ifdef TORI_DEBUG - printf("+ %8.8f, %f\n", newMatchFitness, point4[3]); + printf("+ %8.8f, (%8.8f, %8.8f, %8.8f) %f\n", newMatchFitness, point4[0], point4[1], point4[2], point4[3]); //#endif + g *= 1.03; + } else { @@ -796,6 +798,17 @@ static void RefineRotationEstimate(FLT *rotOut, Point lhPoint, FLT *initialEstim printf("\nRi=%d\n", i); } +static void WhereIsTheTrackedObject(FLT *rotation, Point lhPoint) +{ + FLT reverseRotation[4] = {rotation[0], rotation[1], rotation[2], -rotation[3]}; + FLT objPoint[3] = {lhPoint.x, lhPoint.y, lhPoint.z}; + + rotatearoundaxis(objPoint, objPoint, reverseRotation, reverseRotation[3]); + + printf("The tracked object is at location (%f, %f, %f)\n", objPoint[0], objPoint[1], objPoint[2]); +} + + void SolveForRotation(FLT rotOut[4], TrackedObject *obj, Point lh) { @@ -812,6 +825,8 @@ void SolveForRotation(FLT rotOut[4], TrackedObject *obj, Point lh) // Step 2, optimize the quaternion to match the data. RefineRotationEstimate(rotOut, lh, zAxis, obj); + WhereIsTheTrackedObject(rotOut, lh); + } -- cgit v1.2.3 From e557b5128a32c99dcda0e6a9784a507258342301 Mon Sep 17 00:00:00 2001 From: mwturvey Date: Mon, 27 Mar 2017 14:36:54 -0700 Subject: Small tweak. Using axis/angle --- src/poser_turveytori.c | 4 ++-- 1 file changed, 2 insertions(+), 2 deletions(-) (limited to 'src') diff --git a/src/poser_turveytori.c b/src/poser_turveytori.c index d737723..62deccf 100644 --- a/src/poser_turveytori.c +++ b/src/poser_turveytori.c @@ -729,7 +729,7 @@ static void RefineRotationEstimate(FLT *rotOut, Point lhPoint, FLT *initialEstim // in fact, it probably could probably be 1 without any issue. The main place where g is decremented // is in the block below when we've made a jump that results in a worse fitness than we're starting at. // In those cases, we don't take the jump, and instead lower the value of g and try again. - for (FLT g = 0.2; g > 0.00001; g *= 0.99) + for (FLT g = 0.2; g > 0.000000001; g *= 0.99) { i++; FLT point1[4]; @@ -781,7 +781,7 @@ static void RefineRotationEstimate(FLT *rotOut, Point lhPoint, FLT *initialEstim //#ifdef TORI_DEBUG printf("+ %8.8f, (%8.8f, %8.8f, %8.8f) %f\n", newMatchFitness, point4[0], point4[1], point4[2], point4[3]); //#endif - g *= 1.03; + g *= 1.02; } else -- cgit v1.2.3 From f86a7a9927c484f46768b14a5944a03863beee34 Mon Sep 17 00:00:00 2001 From: mwturvey Date: Mon, 27 Mar 2017 15:30:54 -0700 Subject: sucky angle estimators (quat & angle axis) --- src/poser_turveytori.c | 265 ++++++++++++++++++++++++++++++++++++++++++++----- 1 file changed, 242 insertions(+), 23 deletions(-) (limited to 'src') diff --git a/src/poser_turveytori.c b/src/poser_turveytori.c index 62deccf..da64850 100644 --- a/src/poser_turveytori.c +++ b/src/poser_turveytori.c @@ -602,7 +602,7 @@ FLT RotationEstimateFitnessOld(Point lhPoint, FLT *quaternion, TrackedObject *ob // interesting-- this is one place where we could use any sensors that are only hit by // just an x or y axis to make our estimate better. TODO: bring that data to this fn. -FLT RotationEstimateFitness(Point lhPoint, FLT *quaternion, TrackedObject *obj) +FLT RotationEstimateFitnessAxisAngle(Point lhPoint, FLT *quaternion, TrackedObject *obj) { FLT fitness = 0; for (size_t i = 0; i < obj->numSensors; i++) @@ -670,26 +670,121 @@ FLT RotationEstimateFitness(Point lhPoint, FLT *quaternion, TrackedObject *obj) return 1/fitness; } -void getRotationGradient(FLT *gradientOut, Point lhPoint, FLT *quaternion, TrackedObject *obj, FLT precision) +// interesting-- this is one place where we could use any sensors that are only hit by +// just an x or y axis to make our estimate better. TODO: bring that data to this fn. +FLT RotationEstimateFitnessQuaternion(Point lhPoint, FLT *quaternion, TrackedObject *obj) { + FLT fitness = 0; + for (size_t i = 0; i < obj->numSensors; i++) + { + // first, get the normal of the plane for the horizonal sweep + FLT theta = obj->sensor[i].theta; + // make two vectors that lie on the plane + FLT t1H[3] = { 1, tan(theta-LINMATHPI/2), 0 }; + FLT t2H[3] = { 1, tan(theta-LINMATHPI/2), 1 }; + + FLT tNormH[3]; + + // the normal is the cross of two vectors on the plane. + cross3d(tNormH, t1H, t2H); + + normalize3d(tNormH, tNormH); + + // Now do the same for the vertical sweep + + // first, get the normal of the plane for the horizonal sweep + FLT phi = obj->sensor[i].phi; + // make two vectors that lie on the plane + FLT t1V[3] = { 0, 1, tan(phi-LINMATHPI/2)}; + FLT t2V[3] = { 1, 1, tan(phi-LINMATHPI/2)}; + + FLT tNormV[3]; + + // the normal is the cross of two vectors on the plane. + cross3d(tNormV, t1V, t2V); + + normalize3d(tNormV, tNormV); + + + // First, where is the sensor in the object's reference frame? + FLT sensor_in_obj_reference_frame[3] = {obj->sensor->point.x, obj->sensor->point.y, obj->sensor->point.z}; + // Where is the point, in the reference frame of the lighthouse? + // This has two steps, first we translate from the object's location being the + // origin to the lighthouse being the origin. + // And second, we apply the quaternion to rotate into the proper reference frame for the lighthouse. + + FLT sensor_in_lh_reference_frame[3]; + sub3d(sensor_in_lh_reference_frame, sensor_in_obj_reference_frame, (FLT[3]){lhPoint.x, lhPoint.y, lhPoint.z}); + + quatrotatevector(sensor_in_lh_reference_frame, quaternion, sensor_in_lh_reference_frame); + //rotatearoundaxis(sensor_in_lh_reference_frame, sensor_in_lh_reference_frame, quaternion, quaternion[3]); - FLT baseFitness = RotationEstimateFitness(lhPoint, quaternion, obj); + // now the we've got the location of the sensor in the lighthouses's reference frame, given lhPoint and quaternion inputs. + + // We need an arbitrary vector from the plane to the point. + // Since the plane goes through the origin, this is trivial. + // The sensor point itself is such a vector! + + // And go calculate the distances! + // TODO: don't need to ABS these because we square them below. + FLT dH = FLT_FABS(dot3d(sensor_in_lh_reference_frame, tNormH)); + FLT dV = FLT_FABS(dot3d(sensor_in_lh_reference_frame, tNormV)); + + + fitness += SQUARED(dH); + fitness += SQUARED(dV); + } + + fitness = FLT_SQRT(fitness); + + return 1/fitness; +} + + +void getRotationGradientQuaternion(FLT *gradientOut, Point lhPoint, FLT *quaternion, TrackedObject *obj, FLT precision) +{ + + FLT baseFitness = RotationEstimateFitnessQuaternion(lhPoint, quaternion, obj); FLT tmp0plus[4]; quatadd(tmp0plus, quaternion, (FLT[4]){precision, 0, 0, 0}); - gradientOut[0] = RotationEstimateFitness(lhPoint, tmp0plus, obj) - baseFitness; + gradientOut[0] = RotationEstimateFitnessQuaternion(lhPoint, tmp0plus, obj) - baseFitness; FLT tmp1plus[4]; quatadd(tmp1plus, quaternion, (FLT[4]){0, precision, 0, 0}); - gradientOut[1] = RotationEstimateFitness(lhPoint, tmp1plus, obj) - baseFitness; + gradientOut[1] = RotationEstimateFitnessQuaternion(lhPoint, tmp1plus, obj) - baseFitness; FLT tmp2plus[4]; quatadd(tmp2plus, quaternion, (FLT[4]){0, 0, precision, 0}); - gradientOut[2] = RotationEstimateFitness(lhPoint, tmp2plus, obj) - baseFitness; + gradientOut[2] = RotationEstimateFitnessQuaternion(lhPoint, tmp2plus, obj) - baseFitness; FLT tmp3plus[4]; quatadd(tmp3plus, quaternion, (FLT[4]){0, 0, 0, precision}); - gradientOut[3] = RotationEstimateFitness(lhPoint, tmp3plus, obj) - baseFitness; + gradientOut[3] = RotationEstimateFitnessQuaternion(lhPoint, tmp3plus, obj) - baseFitness; + + return; +} + +void getRotationGradientAxisAngle(FLT *gradientOut, Point lhPoint, FLT *quaternion, TrackedObject *obj, FLT precision) +{ + + FLT baseFitness = RotationEstimateFitnessAxisAngle(lhPoint, quaternion, obj); + + FLT tmp0plus[4]; + quatadd(tmp0plus, quaternion, (FLT[4]){precision, 0, 0, 0}); + gradientOut[0] = RotationEstimateFitnessAxisAngle(lhPoint, tmp0plus, obj) - baseFitness; + + FLT tmp1plus[4]; + quatadd(tmp1plus, quaternion, (FLT[4]){0, precision, 0, 0}); + gradientOut[1] = RotationEstimateFitnessAxisAngle(lhPoint, tmp1plus, obj) - baseFitness; + + FLT tmp2plus[4]; + quatadd(tmp2plus, quaternion, (FLT[4]){0, 0, precision, 0}); + gradientOut[2] = RotationEstimateFitnessAxisAngle(lhPoint, tmp2plus, obj) - baseFitness; + + FLT tmp3plus[4]; + quatadd(tmp3plus, quaternion, (FLT[4]){0, 0, 0, precision}); + gradientOut[3] = RotationEstimateFitnessAxisAngle(lhPoint, tmp3plus, obj) - baseFitness; return; } @@ -709,10 +804,20 @@ void getNormalizedAndScaledRotationGradient(FLT *vectorToScale, FLT desiredMagni return; } -static void RefineRotationEstimate(FLT *rotOut, Point lhPoint, FLT *initialEstimate, TrackedObject *obj) +static void WhereIsTheTrackedObjectAxisAngle(FLT *rotation, Point lhPoint) +{ + FLT reverseRotation[4] = {rotation[0], rotation[1], rotation[2], -rotation[3]}; + FLT objPoint[3] = {lhPoint.x, lhPoint.y, lhPoint.z}; + + rotatearoundaxis(objPoint, objPoint, reverseRotation, reverseRotation[3]); + + printf("The tracked object is at location (%f, %f, %f)\n", objPoint[0], objPoint[1], objPoint[2]); +} + +static void RefineRotationEstimateAxisAngle(FLT *rotOut, Point lhPoint, FLT *initialEstimate, TrackedObject *obj) { int i = 0; - FLT lastMatchFitness = RotationEstimateFitness(lhPoint, initialEstimate, obj); + FLT lastMatchFitness = RotationEstimateFitnessAxisAngle(lhPoint, initialEstimate, obj); quatcopy(rotOut, initialEstimate); @@ -729,7 +834,7 @@ static void RefineRotationEstimate(FLT *rotOut, Point lhPoint, FLT *initialEstim // in fact, it probably could probably be 1 without any issue. The main place where g is decremented // is in the block below when we've made a jump that results in a worse fitness than we're starting at. // In those cases, we don't take the jump, and instead lower the value of g and try again. - for (FLT g = 0.2; g > 0.000000001; g *= 0.99) + for (FLT g = 0.1; g > 0.000000001; g *= 0.99) { i++; FLT point1[4]; @@ -737,19 +842,26 @@ static void RefineRotationEstimate(FLT *rotOut, Point lhPoint, FLT *initialEstim // let's get 3 iterations of gradient descent here. FLT gradient1[4]; - getRotationGradient(gradient1, lhPoint, point1, obj, g/1000); + normalize3d(point1, point1); + + getRotationGradientAxisAngle(gradient1, lhPoint, point1, obj, g/10000); getNormalizedAndScaledRotationGradient(gradient1,g); FLT point2[4]; quatadd(point2, gradient1, point1); //quatnormalize(point2,point2); + normalize3d(point1, point1); + FLT gradient2[4]; - getRotationGradient(gradient2, lhPoint, point2, obj, g/1000); + getRotationGradientAxisAngle(gradient2, lhPoint, point2, obj, g/10000); getNormalizedAndScaledRotationGradient(gradient2,g); FLT point3[4]; quatadd(point3, gradient2, point2); + + normalize3d(point1, point1); + //quatnormalize(point3,point3); // remember that gradient descent has a tendency to zig-zag when it encounters a narrow valley? @@ -770,8 +882,9 @@ static void RefineRotationEstimate(FLT *rotOut, Point lhPoint, FLT *initialEstim FLT point4[4]; quatadd(point4, specialGradient, point3); //quatnormalize(point4,point4); + normalize3d(point1, point1); - FLT newMatchFitness = RotationEstimateFitness(lhPoint, point4, obj); + FLT newMatchFitness = RotationEstimateFitnessAxisAngle(lhPoint, point4, obj); if (newMatchFitness > lastMatchFitness) { @@ -797,18 +910,117 @@ static void RefineRotationEstimate(FLT *rotOut, Point lhPoint, FLT *initialEstim } printf("\nRi=%d\n", i); } - -static void WhereIsTheTrackedObject(FLT *rotation, Point lhPoint) +static void WhereIsTheTrackedObjectQuaternion(FLT *rotation, Point lhPoint) { FLT reverseRotation[4] = {rotation[0], rotation[1], rotation[2], -rotation[3]}; FLT objPoint[3] = {lhPoint.x, lhPoint.y, lhPoint.z}; - rotatearoundaxis(objPoint, objPoint, reverseRotation, reverseRotation[3]); - + //rotatearoundaxis(objPoint, objPoint, reverseRotation, reverseRotation[3]); + quatrotatevector(objPoint, rotation, objPoint); printf("The tracked object is at location (%f, %f, %f)\n", objPoint[0], objPoint[1], objPoint[2]); } + +static void RefineRotationEstimateQuaternion(FLT *rotOut, Point lhPoint, FLT *initialEstimate, TrackedObject *obj) +{ + int i = 0; + FLT lastMatchFitness = RotationEstimateFitnessQuaternion(lhPoint, initialEstimate, obj); + + quatcopy(rotOut, initialEstimate); + + // The values below are somewhat magic, and definitely tunable + // The initial vlue of g will represent the biggest step that the gradient descent can take at first. + // bigger values may be faster, especially when the initial guess is wildly off. + // The downside to a bigger starting guess is that if we've picked a good guess at the local minima + // if there are other local minima, we may accidentally jump to such a local minima and get stuck there. + // That's fairly unlikely with the lighthouse problem, from expereince. + // The other downside is that if it's too big, we may have to spend a few iterations before it gets down + // to a size that doesn't jump us out of our minima. + // The terminal value of g represents how close we want to get to the local minima before we're "done" + // The change in value of g for each iteration is intentionally very close to 1. + // in fact, it probably could probably be 1 without any issue. The main place where g is decremented + // is in the block below when we've made a jump that results in a worse fitness than we're starting at. + // In those cases, we don't take the jump, and instead lower the value of g and try again. + for (FLT g = 0.1; g > 0.000000001; g *= 0.99) + { + i++; + FLT point1[4]; + quatcopy(point1, rotOut); + // let's get 3 iterations of gradient descent here. + FLT gradient1[4]; + + //normalize3d(point1, point1); + + getRotationGradientQuaternion(gradient1, lhPoint, point1, obj, g/10000); + getNormalizedAndScaledRotationGradient(gradient1,g); + + FLT point2[4]; + quatadd(point2, gradient1, point1); + quatnormalize(point2,point2); + + //normalize3d(point1, point1); + + FLT gradient2[4]; + getRotationGradientQuaternion(gradient2, lhPoint, point2, obj, g/10000); + getNormalizedAndScaledRotationGradient(gradient2,g); + + FLT point3[4]; + quatadd(point3, gradient2, point2); + + //normalize3d(point1, point1); + + quatnormalize(point3,point3); + + // remember that gradient descent has a tendency to zig-zag when it encounters a narrow valley? + // Well, solving the lighthouse problem presents a very narrow valley, and the zig-zag of a basic + // gradient descent is kinda horrible here. Instead, think about the shape that a zig-zagging + // converging gradient descent makes. Instead of using the gradient as the best indicator of + // the direction we should follow, we're looking at one side of the zig-zag pattern, and specifically + // following *that* vector. As it turns out, this works *amazingly* well. + + FLT specialGradient[4]; + quatsub(specialGradient,point3,point1); + + // The second parameter to this function is very much a tunable parameter. Different values will result + // in a different number of iterations before we get to the minimum. Numbers between 3-10 seem to work well + // It's not clear what would be optimum here. + getNormalizedAndScaledRotationGradient(specialGradient,g/4); + + FLT point4[4]; + quatadd(point4, specialGradient, point3); + quatnormalize(point4,point4); + //normalize3d(point1, point1); + + FLT newMatchFitness = RotationEstimateFitnessQuaternion(lhPoint, point4, obj); + + if (newMatchFitness > lastMatchFitness) + { + + lastMatchFitness = newMatchFitness; + quatcopy(rotOut, point4); +//#ifdef TORI_DEBUG + //printf("+ %8.8f, (%8.8f, %8.8f, %8.8f) %f\n", newMatchFitness, point4[0], point4[1], point4[2], point4[3]); +//#endif + g *= 1.02; + printf("+"); + WhereIsTheTrackedObjectQuaternion(rotOut, lhPoint); + } + else + { +//#ifdef TORI_DEBUG + //printf("- , %f\n", point4[3]); +//#endif + g *= 0.7; + printf("-"); + } + + + } + printf("\nRi=%d Fitness=%3f\n", i, lastMatchFitness); +} + + void SolveForRotation(FLT rotOut[4], TrackedObject *obj, Point lh) { @@ -816,16 +1028,23 @@ void SolveForRotation(FLT rotOut[4], TrackedObject *obj, Point lh) // This should have the lighthouse directly facing the tracked object. Point trackedObjRelativeToLh = { .x = -lh.x,.y = -lh.y,.z = -lh.z }; FLT theta = atan2(-lh.x, -lh.y); - FLT zAxis[4] = { 0, 0, 1 , theta}; - //FLT quat1[4]; - //quatfromaxisangle(quat1, zAxis, theta); + FLT zAxis[4] = { 0, 0, 1 , theta-LINMATHPI/2}; + FLT quat1[4]; + quatfromaxisangle(quat1, zAxis, theta); + + //quatfrom2vectors(0,0) // not correcting for phi, but that's less important. - // Step 2, optimize the quaternion to match the data. - RefineRotationEstimate(rotOut, lh, zAxis, obj); + // Step 2, optimize the axis/ angle to match the data. + RefineRotationEstimateAxisAngle(rotOut, lh, zAxis, obj); + + WhereIsTheTrackedObjectAxisAngle(rotOut, lh); + + //// Step 2, optimize the quaternion to match the data. + //RefineRotationEstimateQuaternion(rotOut, lh, quat1, obj); - WhereIsTheTrackedObject(rotOut, lh); + //WhereIsTheTrackedObjectQuaternion(rotOut, lh); } -- cgit v1.2.3 From d8c4b23789fd3aedb150144bed6beb286d1504a9 Mon Sep 17 00:00:00 2001 From: mwturvey Date: Mon, 27 Mar 2017 16:21:52 -0700 Subject: Reliable detection of orientation of lighthouse Lots of cruft. Lots of room for improving performance. --- src/poser_turveytori.c | 44 +++++++++++++++++++++++++++++++++++++++++--- 1 file changed, 41 insertions(+), 3 deletions(-) (limited to 'src') diff --git a/src/poser_turveytori.c b/src/poser_turveytori.c index da64850..5c3350b 100644 --- a/src/poser_turveytori.c +++ b/src/poser_turveytori.c @@ -600,9 +600,47 @@ FLT RotationEstimateFitnessOld(Point lhPoint, FLT *quaternion, TrackedObject *ob return fitness; } +FLT RotationEstimateFitnessAxisAngle(Point lh, FLT *AxisAngle, TrackedObject *obj) +{ + // For this fitness calculator, we're going to use the rotation information to figure out where + // we expect to see the tracked object sensors, and we'll do a sum of squares to grade + // the quality of the guess formed by the AxisAngle; + + FLT fitness = 0; + + // for each point in the tracked object + for (int i=0; i< obj->numSensors; i++) + { + + + + // let's see... we need to figure out where this sensor should be in the LH reference frame. + FLT sensorLocation[3] = {obj->sensor[i].point.x-lh.x, obj->sensor[i].point.y-lh.y, obj->sensor[i].point.z-lh.z}; + + // And this puppy needs to be rotated... + + rotatearoundaxis(sensorLocation, sensorLocation, AxisAngle, AxisAngle[3]); + + // Now, the vector indicating the position of the sensor, as seen by the lighthouse is: + FLT realVectFromLh[3] = {1, tan(obj->sensor[i].theta - LINMATHPI/2), tan(obj->sensor[i].phi - LINMATHPI/2)}; + + // and the vector we're calculating given the rotation passed in is the same as the sensor location: + FLT calcVectFromLh[3] = {sensorLocation[0], sensorLocation[1], sensorLocation[2]}; + + FLT angleBetween = anglebetween3d( realVectFromLh, calcVectFromLh ); + + fitness += SQUARED(angleBetween); + } + + return 1/FLT_SQRT(fitness); +} + +// This figures out how far away from the scanned planes each point is, then does a sum of squares +// for the fitness. +// // interesting-- this is one place where we could use any sensors that are only hit by // just an x or y axis to make our estimate better. TODO: bring that data to this fn. -FLT RotationEstimateFitnessAxisAngle(Point lhPoint, FLT *quaternion, TrackedObject *obj) +FLT RotationEstimateFitnessAxisAngleOriginal(Point lhPoint, FLT *quaternion, TrackedObject *obj) { FLT fitness = 0; for (size_t i = 0; i < obj->numSensors; i++) @@ -892,7 +930,7 @@ static void RefineRotationEstimateAxisAngle(FLT *rotOut, Point lhPoint, FLT *ini lastMatchFitness = newMatchFitness; quatcopy(rotOut, point4); //#ifdef TORI_DEBUG - printf("+ %8.8f, (%8.8f, %8.8f, %8.8f) %f\n", newMatchFitness, point4[0], point4[1], point4[2], point4[3]); + //printf("+ %8.8f, (%8.8f, %8.8f, %8.8f) %f\n", newMatchFitness, point4[0], point4[1], point4[2], point4[3]); //#endif g *= 1.02; @@ -900,7 +938,7 @@ static void RefineRotationEstimateAxisAngle(FLT *rotOut, Point lhPoint, FLT *ini else { //#ifdef TORI_DEBUG - printf("- , %f\n", point4[3]); + //printf("- , %f\n", point4[3]); //#endif g *= 0.7; -- cgit v1.2.3 From 6469d979046bf40c4ee76b89d3fc8311dcb16d33 Mon Sep 17 00:00:00 2001 From: Mike Turvey Date: Mon, 27 Mar 2017 23:17:05 -0700 Subject: Use IMU to orient world coordinate system --- src/poser_turveytori.c | 80 ++++++++++++++++++++++++++++++++++---------------- 1 file changed, 54 insertions(+), 26 deletions(-) (limited to 'src') diff --git a/src/poser_turveytori.c b/src/poser_turveytori.c index 5c3350b..5db2bc4 100644 --- a/src/poser_turveytori.c +++ b/src/poser_turveytori.c @@ -71,6 +71,7 @@ static const float DefaultPointsPerOuterDiameter = 60; typedef struct { + FLT down[3]; // populated by the IMU for posing int something; //Stuff } ToriData; @@ -1194,23 +1195,32 @@ Point SolveForLighthouse(TrackedObject *obj, char doLogOutput) -int PoserTurveyTori( SurviveObject * so, PoserData * pd ) +int PoserTurveyTori( SurviveObject * so, PoserData * poserData ) { - PoserType pt = pd->pt; + PoserType pt = poserData->pt; SurviveContext * ctx = so->ctx; - ToriData * dd = so->PoserData; + ToriData * pd = so->PoserData; - if (!dd) + + if (!pd) { - so->PoserData = dd = malloc(sizeof(ToriData)); - memset(dd, 0, sizeof(ToriData)); + so->PoserData = pd = malloc(sizeof(ToriData)); + memset(pd, 0, sizeof(ToriData)); } switch( pt ) { case POSERDATA_IMU: { - PoserDataIMU * imu = (PoserDataIMU*)pd; + PoserDataIMU * tmpImu = (PoserDataIMU*)pd; + + // store off data we can use for figuring out what direction is down when doing calibration. + if (tmpImu->datamask & 1) // accelerometer data is present + { + pd->down[0] = pd->down[0] * 0.98 + 0.2 * tmpImu->accel[0]; + pd->down[1] = pd->down[1] * 0.98 + 0.2 * tmpImu->accel[1]; + pd->down[2] = pd->down[2] * 0.98 + 0.2 * tmpImu->accel[2]; + } //printf( "IMU:%s (%f %f %f) (%f %f %f)\n", so->codename, imu->accel[0], imu->accel[1], imu->accel[2], imu->gyro[0], imu->gyro[1], imu->gyro[2] ); break; } @@ -1228,30 +1238,40 @@ int PoserTurveyTori( SurviveObject * so, PoserData * pd ) to = malloc(sizeof(TrackedObject) + (SENSORS_PER_OBJECT * sizeof(TrackedSensor))); - //FLT lengths[SENSORS_PER_OBJECT][NUM_LIGHTHOUSES][2]; - //FLT angles[SENSORS_PER_OBJECT][NUM_LIGHTHOUSES][2]; //2 Axes (Angles in LH space) - //FLT synctimes[SENSORS_PER_OBJECT][NUM_LIGHTHOUSES]; + // if we rotate the internal reference frame of of the tracked object from having -z being arbitrary + // to being the down direction as defined by the accelerometer, then when we have come up + // with world coordinate system, it will have Z oriented correctly. + + // let's get the quaternion that represents this rotation. + FLT downQuat[4]; + FLT negZ[3] = { 0,0,-1 }; + quatfrom2vectors(downQuat, negZ, pd->down); - //to->numSensors = so->nr_locations; { int sensorCount = 0; + for (int i = 0; i < so->nr_locations; i++) { if (fs->lengths[i][0][0] != -1 && fs->lengths[i][0][1] != -1) //lh 0 { - to->sensor[sensorCount].normal.x = so->sensor_normals[i * 3 + 0]; - to->sensor[sensorCount].normal.y = so->sensor_normals[i * 3 + 1]; - to->sensor[sensorCount].normal.z = so->sensor_normals[i * 3 + 2]; - to->sensor[sensorCount].point.x = so->sensor_locations[i * 3 + 0]; - to->sensor[sensorCount].point.y = so->sensor_locations[i * 3 + 1]; - to->sensor[sensorCount].point.z = so->sensor_locations[i * 3 + 2]; + FLT norm[3] = { so->sensor_normals[i * 3 + 0] , so->sensor_normals[i * 3 + 1] , so->sensor_normals[i * 3 + 2] }; + FLT point[3] = { so->sensor_locations[i * 3 + 0] , so->sensor_locations[i * 3 + 1] , so->sensor_locations[i * 3 + 2] }; + + quatrotatevector(norm, downQuat, norm); + quatrotatevector(point, downQuat, point); + + to->sensor[sensorCount].normal.x = norm[0]; + to->sensor[sensorCount].normal.y = norm[1]; + to->sensor[sensorCount].normal.z = norm[2]; + to->sensor[sensorCount].point.x = point[0]; + to->sensor[sensorCount].point.y = point[1]; + to->sensor[sensorCount].point.z = point[2]; to->sensor[sensorCount].theta = fs->angles[i][0][0] + LINMATHPI / 2; // lighthouse 0, angle 0 (horizontal) to->sensor[sensorCount].phi = fs->angles[i][0][1] + LINMATHPI / 2; // lighthosue 0, angle 1 (vertical) sensorCount++; } } - to->numSensors = sensorCount; SolveForLighthouse(to, 0); @@ -1264,12 +1284,18 @@ int PoserTurveyTori( SurviveObject * so, PoserData * pd ) { if (fs->lengths[i][lh][0] != -1 && fs->lengths[i][lh][1] != -1) { - to->sensor[sensorCount].normal.x = so->sensor_normals[i * 3 + 0]; - to->sensor[sensorCount].normal.y = so->sensor_normals[i * 3 + 1]; - to->sensor[sensorCount].normal.z = so->sensor_normals[i * 3 + 2]; - to->sensor[sensorCount].point.x = so->sensor_locations[i * 3 + 0]; - to->sensor[sensorCount].point.y = so->sensor_locations[i * 3 + 1]; - to->sensor[sensorCount].point.z = so->sensor_locations[i * 3 + 2]; + FLT norm[3] = { so->sensor_normals[i * 3 + 0] , so->sensor_normals[i * 3 + 1] , so->sensor_normals[i * 3 + 2] }; + FLT point[3] = { so->sensor_locations[i * 3 + 0] , so->sensor_locations[i * 3 + 1] , so->sensor_locations[i * 3 + 2] }; + + quatrotatevector(norm, downQuat, norm); + quatrotatevector(point, downQuat, point); + + to->sensor[sensorCount].normal.x = norm[0]; + to->sensor[sensorCount].normal.y = norm[1]; + to->sensor[sensorCount].normal.z = norm[2]; + to->sensor[sensorCount].point.x = point[0]; + to->sensor[sensorCount].point.y = point[1]; + to->sensor[sensorCount].point.z = point[2]; to->sensor[sensorCount].theta = fs->angles[i][lh][0] + LINMATHPI / 2; // lighthouse 0, angle 0 (horizontal) to->sensor[sensorCount].phi = fs->angles[i][lh][1] + LINMATHPI / 2; // lighthosue 0, angle 1 (vertical) sensorCount++; @@ -1280,13 +1306,15 @@ int PoserTurveyTori( SurviveObject * so, PoserData * pd ) SolveForLighthouse(to, 0); } + + free(to); //printf( "Full scene data.\n" ); break; } case POSERDATA_DISASSOCIATE: { - free( dd ); - so->PoserData = 0; + free( pd ); + so->PoserData = NULL; //printf( "Need to disassociate.\n" ); break; } -- cgit v1.2.3 From b3bdcdb838ed57986f359b2181a624bfd1acbbf1 Mon Sep 17 00:00:00 2001 From: mwturvey Date: Tue, 28 Mar 2017 10:15:36 -0700 Subject: Fix Tracker IMU --- src/poser_turveytori.c | 2 +- src/survive_data.c | 10 ++++++++-- src/survive_vive.c | 17 +---------------- 3 files changed, 10 insertions(+), 19 deletions(-) (limited to 'src') diff --git a/src/poser_turveytori.c b/src/poser_turveytori.c index 5c3350b..df691e3 100644 --- a/src/poser_turveytori.c +++ b/src/poser_turveytori.c @@ -1211,7 +1211,7 @@ int PoserTurveyTori( SurviveObject * so, PoserData * pd ) case POSERDATA_IMU: { PoserDataIMU * imu = (PoserDataIMU*)pd; - //printf( "IMU:%s (%f %f %f) (%f %f %f)\n", so->codename, imu->accel[0], imu->accel[1], imu->accel[2], imu->gyro[0], imu->gyro[1], imu->gyro[2] ); + printf( "IMU:%s (%f %f %f) (%f %f %f)\n", so->codename, imu->accel[0], imu->accel[1], imu->accel[2], imu->gyro[0], imu->gyro[1], imu->gyro[2] ); break; } case POSERDATA_LIGHT: diff --git a/src/survive_data.c b/src/survive_data.c index 9447104..3eb5890 100644 --- a/src/survive_data.c +++ b/src/survive_data.c @@ -154,8 +154,14 @@ void handle_lightcap2_sync(SurviveObject * so, LightcapElement * le ) { if (lcd->per_sweep.activeLighthouse != -1) { - // hmm, it appears we got two non-skip pulses at the same time. That should never happen - fprintf(stderr, "WARNING: Two non-skip pulses received on the same cycle!\n"); + static int pulseWarningCount=0; + + if (pulseWarningCount < 5) + { + pulseWarningCount++; + // hmm, it appears we got two non-skip pulses at the same time. That should never happen + fprintf(stderr, "WARNING: Two non-skip pulses received on the same cycle!\n"); + } } lcd->per_sweep.activeLighthouse = 1; lcd->per_sweep.activeSweepStartTime = le->timestamp; diff --git a/src/survive_vive.c b/src/survive_vive.c index a5c731d..757fa11 100755 --- a/src/survive_vive.c +++ b/src/survive_vive.c @@ -1105,6 +1105,7 @@ void survive_data_cb( SurviveUSBInterface * si ) } case USB_IF_LIGHTHOUSE: case USB_IF_W_WATCHMAN1: + case USB_IF_TRACKER0: { int i; //printf( "%d -> ", size ); @@ -1156,22 +1157,6 @@ void survive_data_cb( SurviveUSBInterface * si ) } break; } - case USB_IF_TRACKER0: - { - SurviveObject * w = obj; - if( id == 32 ) - { - // TODO: Looks like this will need to be handle_tracker, since - // it appears the interface is sufficiently different. - // More work needd to reverse engineer it. - //handle_wired_watchman( w, readdata, size); - } - else - { - SV_INFO( "Unknown tracker code %d\n", id ); - } - break; - } case USB_IF_LIGHTCAP: { int i; -- cgit v1.2.3 From fba18d9de738fd07a0b6db944369127a6a66f0d8 Mon Sep 17 00:00:00 2001 From: mwturvey Date: Tue, 28 Mar 2017 11:19:39 -0700 Subject: Add lh to poser light data --- src/poser_turveytori.c | 38 ++++++++++++++++++++++++-------------- src/survive_process.c | 1 + 2 files changed, 25 insertions(+), 14 deletions(-) (limited to 'src') diff --git a/src/poser_turveytori.c b/src/poser_turveytori.c index 177a16a..8b92860 100644 --- a/src/poser_turveytori.c +++ b/src/poser_turveytori.c @@ -1199,42 +1199,51 @@ int PoserTurveyTori( SurviveObject * so, PoserData * poserData ) { PoserType pt = poserData->pt; SurviveContext * ctx = so->ctx; - ToriData * pd = so->PoserData; + ToriData * td = so->PoserData; - if (!pd) + if (!td) { - so->PoserData = pd = malloc(sizeof(ToriData)); - memset(pd, 0, sizeof(ToriData)); + so->PoserData = td = malloc(sizeof(ToriData)); + memset(td, 0, sizeof(ToriData)); } switch( pt ) { case POSERDATA_IMU: { - PoserDataIMU * tmpImu = (PoserDataIMU*)pd; + PoserDataIMU * tmpImu = (PoserDataIMU*)poserData; // store off data we can use for figuring out what direction is down when doing calibration. - if (tmpImu->datamask & 1) // accelerometer data is present + //TODO: looks like the data mask isn't getting set correctly. + //if (tmpImu->datamask & 1) // accelerometer data is present { - pd->down[0] = pd->down[0] * 0.98 + 0.2 * tmpImu->accel[0]; - pd->down[1] = pd->down[1] * 0.98 + 0.2 * tmpImu->accel[1]; - pd->down[2] = pd->down[2] * 0.98 + 0.2 * tmpImu->accel[2]; + td->down[0] = td->down[0] * 0.98 + 0.02 * tmpImu->accel[0]; + td->down[1] = td->down[1] * 0.98 + 0.02 * tmpImu->accel[1]; + td->down[2] = td->down[2] * 0.98 + 0.02 * tmpImu->accel[2]; } - printf( "IMU:%s (%f %f %f) (%f %f %f)\n", so->codename, imu->accel[0], imu->accel[1], imu->accel[2], imu->gyro[0], imu->gyro[1], imu->gyro[2] ); + //printf( "IMU:%s (%f %f %f) (%f %f %f)\n", so->codename, tmpImu->accel[0], tmpImu->accel[1], tmpImu->accel[2], tmpImu->gyro[0], tmpImu->gyro[1], tmpImu->gyro[2] ); + //printf( "Down: (%f %f %f)\n", td->down[0], td->down[1], td->down[2] ); break; } case POSERDATA_LIGHT: { - PoserDataLight * l = (PoserDataLight*)pd; + PoserDataLight * l = (PoserDataLight*)poserData; //printf( "LIG:%s %d @ %f rad, %f s (AC %d) (TC %d)\n", so->codename, l->sensor_id, l->angle, l->length, l->acode, l->timecode ); + if (0 == l->lh) + { + if (l->acode & 0x1) + { + printf("%2d: %8f\n", l->sensor_id, l->angle); + } + } break; } case POSERDATA_FULL_SCENE: { TrackedObject *to; - PoserDataFullScene * fs = (PoserDataFullScene*)pd; + PoserDataFullScene * fs = (PoserDataFullScene*)poserData; to = malloc(sizeof(TrackedObject) + (SENSORS_PER_OBJECT * sizeof(TrackedSensor))); @@ -1245,7 +1254,8 @@ int PoserTurveyTori( SurviveObject * so, PoserData * poserData ) // let's get the quaternion that represents this rotation. FLT downQuat[4]; FLT negZ[3] = { 0,0,-1 }; - quatfrom2vectors(downQuat, negZ, pd->down); + //quatfrom2vectors(downQuat, negZ, td->down); + quatfrom2vectors(downQuat, td->down, negZ); { int sensorCount = 0; @@ -1313,7 +1323,7 @@ int PoserTurveyTori( SurviveObject * so, PoserData * poserData ) } case POSERDATA_DISASSOCIATE: { - free( pd ); + free( so->PoserData ); so->PoserData = NULL; //printf( "Need to disassociate.\n" ); break; diff --git a/src/survive_process.c b/src/survive_process.c index b58b344..3af2da9 100644 --- a/src/survive_process.c +++ b/src/survive_process.c @@ -57,6 +57,7 @@ void survive_default_angle_process( SurviveObject * so, int sensor_id, int acode .timecode = timecode, .length = length, .angle = angle, + .lh = lh, }; so->PoserFn( so, (PoserData *)&l ); } -- cgit v1.2.3 From 3ca8ba3d69de226ae8835bb29e45c7bcd35793fe Mon Sep 17 00:00:00 2001 From: mwturvey Date: Wed, 29 Mar 2017 16:39:47 -0700 Subject: Tori Poser Works! There's a ton of code cruft, and the algorithm is currently too slow. BUT I can track an object using only 1 lighthouse for tracking, at (I believe) an update rate of at least 7.5 HZ. By tracking, I know the position and orientation of the lighthouses relative to the tracked object, and I know the tracked object's location relative to the lighthouse. I don't have the orientation of the tracked object relative to the lighthouse yet, but that should be easy given the rest of the "knowns." --- src/poser_octavioradii.c | 198 +++++++++++++++++++++++++++++++++++++++++++++-- src/poser_turveytori.c | 174 +++++++++++++++++++++++++++++++++++++---- src/survive_data.c | 37 ++++++++- 3 files changed, 381 insertions(+), 28 deletions(-) (limited to 'src') diff --git a/src/poser_octavioradii.c b/src/poser_octavioradii.c index 3893085..0d8674c 100644 --- a/src/poser_octavioradii.c +++ b/src/poser_octavioradii.c @@ -4,8 +4,12 @@ typedef struct { - int something; - //Stuff +#define OLD_ANGLES_BUFF_LEN 3 + FLT oldAngles[SENSORS_PER_OBJECT][2][NUM_LIGHTHOUSES][OLD_ANGLES_BUFF_LEN]; // sensor, sweep axis, lighthouse, instance + int angleIndex[NUM_LIGHTHOUSES][2]; // index into circular buffer ahead. separate index for each axis. + int lastAxis[NUM_LIGHTHOUSES]; + + int hitCount[SENSORS_PER_OBJECT][NUM_LIGHTHOUSES][2]; } OctavioRadiiData; #include @@ -45,6 +49,7 @@ typedef struct Point normal; // unit vector indicating the normal for the sensor double theta; // "horizontal" angular measurement from lighthouse radians double phi; // "vertical" angular measurement from lighthouse in radians. + int id; } TrackedSensor; typedef struct @@ -382,10 +387,10 @@ static RefineEstimateUsingGradientDescentRadii(FLT *estimateOut, SensorAngles *a } - printf("\ni=%d\n", i); + printf(" i=%d ", i); } -void SolveForLighthouseRadii(Point *objPosition, FLT *objOrientation, TrackedObject *obj) +static void SolveForLighthouseRadii(Point *objPosition, FLT *objOrientation, TrackedObject *obj) { FLT estimate[MAX_RADII]; @@ -394,6 +399,12 @@ void SolveForLighthouseRadii(Point *objPosition, FLT *objOrientation, TrackedObj estimate[i] = 2.38; } + + //for (int i=0; i < obj->numSensors; i++) + //{ + // printf("%d, ", obj->sensor[i].id); + //} + SensorAngles angles[MAX_RADII]; PointPair pairs[MAX_POINT_PAIRS]; @@ -423,22 +434,119 @@ void SolveForLighthouseRadii(Point *objPosition, FLT *objOrientation, TrackedObj // we should now have an estimate of the radii. - for (int i = 0; i < obj->numSensors; i++) + //for (int i = 0; i < obj->numSensors; i++) + for (int i = 0; i < 1; i++) { printf("radius[%d]: %f\n", i, estimate[i]); } + // (FLT *estimateOut, SensorAngles *angles, FLT *initialEstimate, size_t numRadii, PointPair *pairs, size_t numPairs, FILE *logFile) return; } +static void QuickPose(SurviveObject *so) +{ + OctavioRadiiData * td = so->PoserData; + + + //for (int i=0; i < so->nr_locations; i++) + //{ + // FLT x0=td->oldAngles[i][0][0][td->angleIndex[0][0]]; + // FLT y0=td->oldAngles[i][1][0][td->angleIndex[0][1]]; + // //FLT x1=td->oldAngles[i][0][1][td->angleIndex[1][0]]; + // //FLT y1=td->oldAngles[i][1][1][td->angleIndex[1][1]]; + // //printf("%2d: %8.8f, %8.8f %8.8f, %8.8f \n", + // // i, + // // x0, + // // y0, + // // x1, + // // y1 + // // ); + // printf("%2d: %8.8f, %8.8f \n", + // i, + // x0, + // y0 + // ); + //} + //printf("\n"); + + TrackedObject *to; + + to = malloc(sizeof(TrackedObject) + (SENSORS_PER_OBJECT * sizeof(TrackedSensor))); + + { + int sensorCount = 0; + + for (int i = 0; i < so->nr_locations; i++) + { + int lh = 0; + //printf("%d[%d], ",i,td->hitCount[i][lh][0]); + + int angleIndex0 = (td->angleIndex[lh][0] + 1 + OLD_ANGLES_BUFF_LEN) % OLD_ANGLES_BUFF_LEN; + int angleIndex1 = (td->angleIndex[lh][1] + 1 + OLD_ANGLES_BUFF_LEN) % OLD_ANGLES_BUFF_LEN; + if ((td->oldAngles[i][0][lh][angleIndex0] != 0 && td->oldAngles[i][1][lh][angleIndex1] != 0)) + + + { + if (td->hitCount[i][lh][0] > 10 && td->hitCount[i][lh][1] > 10) + { + FLT norm[3] = { so->sensor_normals[i * 3 + 0] , so->sensor_normals[i * 3 + 1] , so->sensor_normals[i * 3 + 2] }; + FLT point[3] = { so->sensor_locations[i * 3 + 0] , so->sensor_locations[i * 3 + 1] , so->sensor_locations[i * 3 + 2] }; + + to->sensor[sensorCount].normal.x = norm[0]; + to->sensor[sensorCount].normal.y = norm[1]; + to->sensor[sensorCount].normal.z = norm[2]; + to->sensor[sensorCount].point.x = point[0]; + to->sensor[sensorCount].point.y = point[1]; + to->sensor[sensorCount].point.z = point[2]; + to->sensor[sensorCount].theta = td->oldAngles[i][0][lh][angleIndex0] + LINMATHPI / 2; // lighthouse 0, angle 0 (horizontal) + to->sensor[sensorCount].phi = td->oldAngles[i][1][lh][angleIndex1] + LINMATHPI / 2; // lighthouse 0, angle 1 (vertical) + to->sensor[sensorCount].id=i; + + + + //printf("%2d: %8.8f, %8.8f \n", + // i, + // to->sensor[sensorCount].theta, + // to->sensor[sensorCount].phi + // ); + + sensorCount++; + } + } + } + //printf("\n"); + to->numSensors = sensorCount; + + if (sensorCount > 4) + { + FLT pos[3]; + FLT orient[4]; + SolveForLighthouseRadii(pos, orient, to); + } + + + } + + + free(to); + +} + + int PoserOctavioRadii( SurviveObject * so, PoserData * pd ) { PoserType pt = pd->pt; SurviveContext * ctx = so->ctx; OctavioRadiiData * dd = so->PoserData; - if( !dd ) so->PoserData = dd = malloc( sizeof(OctavioRadiiData) ); + if( !dd ) + { + so->PoserData = dd = malloc( sizeof(OctavioRadiiData) ); + memset(dd, 0, sizeof(OctavioRadiiData)); + } + switch( pt ) { @@ -451,9 +559,81 @@ int PoserOctavioRadii( SurviveObject * so, PoserData * pd ) case POSERDATA_LIGHT: { PoserDataLight * l = (PoserDataLight*)pd; + + if (l->lh >= NUM_LIGHTHOUSES || l->lh < 0) + { + // should never happen. Famous last words... + break; + } + int axis = l->acode & 0x1; + + //printf("%d ", l->sensor_id); + + //printf( "LIG:%s %d @ %f rad, %f s (AC %d) (TC %d)\n", so->codename, l->sensor_id, l->angle, l->length, l->acode, l->timecode ); - break; - } + if ((dd->lastAxis[l->lh] != (l->acode & 0x1)) ) + { + int lastAxis = dd->lastAxis[l->lh]; + //printf("\n"); + //if (0 == l->lh) + // printf("or[%d,%d] ", l->lh,lastAxis); + + for (int i=0; i < SENSORS_PER_OBJECT; i++) + { + //FLT oldAngles[SENSORS_PER_OBJECT][2][NUM_LIGHTHOUSES][OLD_ANGLES_BUFF_LEN]; // sensor, sweep axis, lighthouse, instance + int index = dd->angleIndex[l->lh][axis]; + if (dd->oldAngles[i][axis][l->lh][dd->angleIndex[l->lh][axis]] != 0) + { + //if (0 == l->lh) + // printf("%d ", i); + + dd->hitCount[i][l->lh][axis]++; + } + else + { + dd->hitCount[i][l->lh][axis] *= 0.5; + } + } + //if (0 == l->lh) + // printf("\n"); + //int foo = l->acode & 0x1; + //printf("%d", foo); + + + //if (axis) + { + if (0 == l->lh && axis) // only once per full cycle... + { + static unsigned int counter = 1; + + counter++; + + // let's just do this occasionally for now... + if (counter % 4 == 0) + QuickPose(so); + } + // axis changed, time to increment the circular buffer index. + + + dd->angleIndex[l->lh][axis]++; + dd->angleIndex[l->lh][axis] = dd->angleIndex[l->lh][axis] % OLD_ANGLES_BUFF_LEN; + + // and clear out the data. + for (int i=0; i < SENSORS_PER_OBJECT; i++) + { + dd->oldAngles[i][axis][l->lh][dd->angleIndex[l->lh][axis]] = 0; + } + + } + dd->lastAxis[l->lh] = axis; + } + + //if (0 == l->lh) + // printf("(%d) ", l->sensor_id); + + //FLT oldAngles[SENSORS_PER_OBJECT][2][NUM_LIGHTHOUSES][OLD_ANGLES_BUFF_LEN]; // sensor, sweep axis, lighthouse, instance + dd->oldAngles[l->sensor_id][axis][l->lh][dd->angleIndex[l->lh][axis]] = l->angle; + break; } case POSERDATA_FULL_SCENE: { TrackedObject *to; @@ -482,6 +662,7 @@ int PoserOctavioRadii( SurviveObject * so, PoserData * pd ) to->sensor[sensorCount].point.z = so->sensor_locations[i * 3 + 2]; to->sensor[sensorCount].theta = fs->angles[i][0][0] + LINMATHPI / 2; // lighthouse 0, angle 0 (horizontal) to->sensor[sensorCount].phi = fs->angles[i][0][1] + LINMATHPI / 2; // lighthosue 0, angle 1 (vertical) + to->sensor[sensorCount].id=i; sensorCount++; } } @@ -509,6 +690,7 @@ int PoserOctavioRadii( SurviveObject * so, PoserData * pd ) to->sensor[sensorCount].point.z = so->sensor_locations[i * 3 + 2]; to->sensor[sensorCount].theta = fs->angles[i][lh][0] + LINMATHPI / 2; // lighthouse 0, angle 0 (horizontal) to->sensor[sensorCount].phi = fs->angles[i][lh][1] + LINMATHPI / 2; // lighthosue 0, angle 1 (vertical) + to->sensor[sensorCount].id=i; sensorCount++; } } diff --git a/src/poser_turveytori.c b/src/poser_turveytori.c index 8b92860..22098d0 100644 --- a/src/poser_turveytori.c +++ b/src/poser_turveytori.c @@ -74,6 +74,11 @@ typedef struct FLT down[3]; // populated by the IMU for posing int something; //Stuff + +#define OLD_ANGLES_BUFF_LEN 3 + FLT oldAngles[SENSORS_PER_OBJECT][2][NUM_LIGHTHOUSES][OLD_ANGLES_BUFF_LEN]; // sensor, sweep axis, lighthouse, instance + int angleIndex[NUM_LIGHTHOUSES][2]; // index into circular buffer ahead. separate index for each axis. + int lastAxis[NUM_LIGHTHOUSES]; } ToriData; @@ -526,7 +531,7 @@ static Point RefineEstimateUsingModifiedGradientDescent1(Point initialEstimate, } - printf("\ni=%d\n", i); + printf(" i=%d ", i); return lastPoint; } @@ -845,12 +850,12 @@ void getNormalizedAndScaledRotationGradient(FLT *vectorToScale, FLT desiredMagni static void WhereIsTheTrackedObjectAxisAngle(FLT *rotation, Point lhPoint) { - FLT reverseRotation[4] = {rotation[0], rotation[1], rotation[2], -rotation[3]}; + FLT reverseRotation[4] = {rotation[0], rotation[1], rotation[2], rotation[3]}; FLT objPoint[3] = {lhPoint.x, lhPoint.y, lhPoint.z}; rotatearoundaxis(objPoint, objPoint, reverseRotation, reverseRotation[3]); - printf("The tracked object is at location (%f, %f, %f)\n", objPoint[0], objPoint[1], objPoint[2]); + printf("{%8.8f, %8.8f, %8.8f} ", objPoint[0], objPoint[1], objPoint[2]); } static void RefineRotationEstimateAxisAngle(FLT *rotOut, Point lhPoint, FLT *initialEstimate, TrackedObject *obj) @@ -873,7 +878,7 @@ static void RefineRotationEstimateAxisAngle(FLT *rotOut, Point lhPoint, FLT *ini // in fact, it probably could probably be 1 without any issue. The main place where g is decremented // is in the block below when we've made a jump that results in a worse fitness than we're starting at. // In those cases, we don't take the jump, and instead lower the value of g and try again. - for (FLT g = 0.1; g > 0.000000001; g *= 0.99) + for (FLT g = 0.1; g > 0.000000001 || i > 10000; g *= 0.99) { i++; FLT point1[4]; @@ -947,7 +952,7 @@ static void RefineRotationEstimateAxisAngle(FLT *rotOut, Point lhPoint, FLT *ini } - printf("\nRi=%d\n", i); + printf(" Ri=%d ", i); } static void WhereIsTheTrackedObjectQuaternion(FLT *rotation, Point lhPoint) { @@ -956,7 +961,7 @@ static void WhereIsTheTrackedObjectQuaternion(FLT *rotation, Point lhPoint) //rotatearoundaxis(objPoint, objPoint, reverseRotation, reverseRotation[3]); quatrotatevector(objPoint, rotation, objPoint); - printf("The tracked object is at location (%f, %f, %f)\n", objPoint[0], objPoint[1], objPoint[2]); + printf("(%f, %f, %f)\n", objPoint[0], objPoint[1], objPoint[2]); } @@ -1056,7 +1061,7 @@ static void RefineRotationEstimateQuaternion(FLT *rotOut, Point lhPoint, FLT *in } - printf("\nRi=%d Fitness=%3f\n", i, lastMatchFitness); + printf("Ri=%3d Fitness=%3f ", i, lastMatchFitness); } @@ -1088,8 +1093,23 @@ void SolveForRotation(FLT rotOut[4], TrackedObject *obj, Point lh) } -Point SolveForLighthouse(TrackedObject *obj, char doLogOutput) +static Point SolveForLighthouse(TrackedObject *obj, char doLogOutput) { + //printf("Solving for Lighthouse\n"); + + //printf("obj->numSensors = %d;\n", obj->numSensors); + + //for (int i=0; i < obj->numSensors; i++) + //{ + // printf("obj->sensor[%d].normal.x = %f;\n", i, obj->sensor[i].normal.x); + // printf("obj->sensor[%d].normal.y = %f;\n", i, obj->sensor[i].normal.y); + // printf("obj->sensor[%d].normal.z = %f;\n", i, obj->sensor[i].normal.z); + // printf("obj->sensor[%d].point.x = %f;\n", i, obj->sensor[i].point.x); + // printf("obj->sensor[%d].point.y = %f;\n", i, obj->sensor[i].point.y); + // printf("obj->sensor[%d].point.z = %f;\n", i, obj->sensor[i].point.z); + // printf("obj->sensor[%d].phi = %f;\n", i, obj->sensor[i].phi); + // printf("obj->sensor[%d].theta = %f;\n\n", i, obj->sensor[i].theta); + //} PointsAndAngle pna[MAX_POINT_PAIRS]; volatile size_t sizeNeeded = sizeof(pna); @@ -1168,13 +1188,17 @@ Point SolveForLighthouse(TrackedObject *obj, char doLogOutput) FLT fitGd = getPointFitness(refinedEstimateGd, pna, pnaCount); - FLT distance = FLT_SQRT(SQUARED(refinedEstimateGd.x) + SQUARED(refinedEstimateGd.y) + SQUARED(refinedEstimateGd.z)); - printf("(%4.4f, %4.4f, %4.4f)\n", refinedEstimateGd.x, refinedEstimateGd.y, refinedEstimateGd.z); - printf("Distance is %f, Fitness is %f\n", distance, fitGd); + printf("(%4.4f, %4.4f, %4.4f) Dist: %8.8f Fit:%4f ", refinedEstimateGd.x, refinedEstimateGd.y, refinedEstimateGd.z, distance, fitGd); - FLT rot[4]; - SolveForRotation(rot, obj, refinedEstimateGd); + if (fitGd > 5) + { + FLT distance = FLT_SQRT(SQUARED(refinedEstimateGd.x) + SQUARED(refinedEstimateGd.y) + SQUARED(refinedEstimateGd.z)); + printf("(%4.4f, %4.4f, %4.4f) Dist: %8.8f Fit:%4f ", refinedEstimateGd.x, refinedEstimateGd.y, refinedEstimateGd.z, distance, fitGd); + //printf("Distance is %f, Fitness is %f\n", distance, fitGd); + FLT rot[4]; + SolveForRotation(rot, obj, refinedEstimateGd); + } if (logFile) { updateHeader(logFile); @@ -1191,7 +1215,82 @@ Point SolveForLighthouse(TrackedObject *obj, char doLogOutput) +static void QuickPose(SurviveObject *so) +{ + ToriData * td = so->PoserData; + + + //for (int i=0; i < so->nr_locations; i++) + //{ + // FLT x0=td->oldAngles[i][0][0][td->angleIndex[0][0]]; + // FLT y0=td->oldAngles[i][1][0][td->angleIndex[0][1]]; + // FLT x1=td->oldAngles[i][0][1][td->angleIndex[1][0]]; + // FLT y1=td->oldAngles[i][1][1][td->angleIndex[1][1]]; + // //printf("%2d: %8.8f, %8.8f %8.8f, %8.8f \n", + // // i, + // // x0, + // // y0, + // // x1, + // // y1 + // // ); + // printf("%2d: %8.8f, %8.8f \n", + // i, + // x0, + // y0 + // ); + //} + //printf("\n"); + + TrackedObject *to; + + to = malloc(sizeof(TrackedObject) + (SENSORS_PER_OBJECT * sizeof(TrackedSensor))); + { + int sensorCount = 0; + + for (int i = 0; i < so->nr_locations; i++) + { + int lh = 0; + int angleIndex0 = (td->angleIndex[lh][0] + 1 + OLD_ANGLES_BUFF_LEN) % OLD_ANGLES_BUFF_LEN; + int angleIndex1 = (td->angleIndex[lh][1] + 1 + OLD_ANGLES_BUFF_LEN) % OLD_ANGLES_BUFF_LEN; + if (td->oldAngles[i][0][lh][angleIndex0] != 0 && td->oldAngles[i][1][lh][angleIndex1] != 0) + { + FLT norm[3] = { so->sensor_normals[i * 3 + 0] , so->sensor_normals[i * 3 + 1] , so->sensor_normals[i * 3 + 2] }; + FLT point[3] = { so->sensor_locations[i * 3 + 0] , so->sensor_locations[i * 3 + 1] , so->sensor_locations[i * 3 + 2] }; + + to->sensor[sensorCount].normal.x = norm[0]; + to->sensor[sensorCount].normal.y = norm[1]; + to->sensor[sensorCount].normal.z = norm[2]; + to->sensor[sensorCount].point.x = point[0]; + to->sensor[sensorCount].point.y = point[1]; + to->sensor[sensorCount].point.z = point[2]; + to->sensor[sensorCount].theta = td->oldAngles[i][0][lh][angleIndex0] + LINMATHPI / 2; // lighthouse 0, angle 0 (horizontal) + to->sensor[sensorCount].phi = td->oldAngles[i][1][lh][angleIndex1] + LINMATHPI / 2; // lighthouse 0, angle 1 (vertical) + + //printf("%2d: %8.8f, %8.8f \n", + // i, + // to->sensor[sensorCount].theta, + // to->sensor[sensorCount].phi + // ); + + sensorCount++; + } + } + to->numSensors = sensorCount; + + if (sensorCount > 4) + { + SolveForLighthouse(to, 0); + printf("!\n"); + } + + + } + + + free(to); + +} @@ -1229,14 +1328,47 @@ int PoserTurveyTori( SurviveObject * so, PoserData * poserData ) case POSERDATA_LIGHT: { PoserDataLight * l = (PoserDataLight*)poserData; + + if (l->lh >= NUM_LIGHTHOUSES || l->lh < 0) + { + // should never happen. Famous last words... + break; + } + int axis = l->acode & 0x1; //printf( "LIG:%s %d @ %f rad, %f s (AC %d) (TC %d)\n", so->codename, l->sensor_id, l->angle, l->length, l->acode, l->timecode ); - if (0 == l->lh) + if ((td->lastAxis[l->lh] != (l->acode & 0x1)) ) { - if (l->acode & 0x1) + int foo = l->acode & 0x1; + //printf("%d", foo); + + + //if (axis) { - printf("%2d: %8f\n", l->sensor_id, l->angle); + if (0 == l->lh && axis) // only once per full cycle... + { + static unsigned int counter = 1; + + counter++; + + // let's just do this occasionally for now... + if (counter % 4 == 0) + QuickPose(so); + } + // axis changed, time to increment the circular buffer index. + td->angleIndex[l->lh][axis]++; + td->angleIndex[l->lh][axis] = td->angleIndex[l->lh][axis] % OLD_ANGLES_BUFF_LEN; + + // and clear out the data. + for (int i=0; i < SENSORS_PER_OBJECT; i++) + { + td->oldAngles[i][axis][l->lh][td->angleIndex[l->lh][axis]] = 0; + } + } + td->lastAxis[l->lh] = axis; } + + td->oldAngles[l->sensor_id][axis][l->lh][td->angleIndex[l->lh][axis]] = l->angle; break; } case POSERDATA_FULL_SCENE: @@ -1278,12 +1410,20 @@ int PoserTurveyTori( SurviveObject * so, PoserData * poserData ) to->sensor[sensorCount].point.y = point[1]; to->sensor[sensorCount].point.z = point[2]; to->sensor[sensorCount].theta = fs->angles[i][0][0] + LINMATHPI / 2; // lighthouse 0, angle 0 (horizontal) - to->sensor[sensorCount].phi = fs->angles[i][0][1] + LINMATHPI / 2; // lighthosue 0, angle 1 (vertical) + to->sensor[sensorCount].phi = fs->angles[i][0][1] + LINMATHPI / 2; // lighthouse 0, angle 1 (vertical) + + //printf("%2d: %8.8f, %8.8f \n", + // i, + // to->sensor[sensorCount].theta, + // to->sensor[sensorCount].phi + // ); + sensorCount++; } } to->numSensors = sensorCount; + SolveForLighthouse(to, 0); } { diff --git a/src/survive_data.c b/src/survive_data.c index 3eb5890..3f16c7c 100644 --- a/src/survive_data.c +++ b/src/survive_data.c @@ -79,10 +79,40 @@ void handle_lightcap2_process_sweep_data(SurviveObject *so) } } + int allZero = 1; + for (int q=0; q< 32; q++) + if (lcd->sweep.sweep_len[q] != 0) + allZero=0; + //if (!allZero) + // printf("a[%d]l[%d] ", lcd->per_sweep.activeAcode & 5, lcd->per_sweep.activeLighthouse); for (int i = 0; i < SENSORS_PER_OBJECT; i++) { + { + static int counts[SENSORS_PER_OBJECT][2] = {0}; + + if (lcd->per_sweep.activeLighthouse == 0 && !allZero) + { + if (lcd->sweep.sweep_len[i] != 0) + { + //printf("%d ", i); + //counts[i][lcd->per_sweep.activeAcode & 1] ++; + } + else + { + counts[i][lcd->per_sweep.activeAcode & 1] =0; + } + + //if (counts[i][0] > 10 && counts[i][1] > 10) + //{ + //printf("%d(%d,%d), ", i, counts[i][0], counts[i][1]); + //} + } + } + + if (lcd->sweep.sweep_len[i] != 0) // if the sensor was hit, process it { + int offset_from = lcd->sweep.sweep_time[i] - lcd->per_sweep.activeSweepStartTime + lcd->sweep.sweep_len[i] / 2; if (offset_from < 380000 && offset_from > 70000) @@ -94,6 +124,9 @@ void handle_lightcap2_process_sweep_data(SurviveObject *so) } } } + //if (!allZero) + // printf(" ..:..\n"); + } // clear out sweep data (could probably limit this to only after a "first" sync. // this is slightly more robust, so doing it here for now. @@ -108,6 +141,7 @@ void handle_lightcap2_sync(SurviveObject * so, LightcapElement * le ) //static unsigned int recent_sync_count = -1; //static unsigned int activeSweepStartTime; + int acode = handle_lightcap2_getAcodeFromSyncPulse(so, le->length); // Process any sweep data we have handle_lightcap2_process_sweep_data(so); @@ -126,7 +160,6 @@ void handle_lightcap2_sync(SurviveObject * so, LightcapElement * le ) lcd->per_sweep.lh_pulse_len[lcd->per_sweep.current_lh] = le->length; lcd->per_sweep.lh_start_time[lcd->per_sweep.current_lh] = le->timestamp; - int acode = handle_lightcap2_getAcodeFromSyncPulse(so, le->length); if (!(acode >> 2 & 1)) // if the skip bit is not set { lcd->per_sweep.activeLighthouse = lcd->per_sweep.current_lh; @@ -148,8 +181,6 @@ void handle_lightcap2_sync(SurviveObject * so, LightcapElement * le ) lcd->per_sweep.lh_pulse_len[lcd->per_sweep.current_lh] = le->length; lcd->per_sweep.lh_start_time[lcd->per_sweep.current_lh] = le->timestamp; - int acode = handle_lightcap2_getAcodeFromSyncPulse(so, le->length); - if (!(acode >> 2 & 1)) // if the skip bit is not set { if (lcd->per_sweep.activeLighthouse != -1) -- cgit v1.2.3 From 9706cf9ee66134e5c37e8c860035d133b56dff66 Mon Sep 17 00:00:00 2001 From: mwturvey Date: Wed, 29 Mar 2017 16:51:27 -0700 Subject: little bit o' spit 'n polish --- src/poser_turveytori.c | 4 ++-- 1 file changed, 2 insertions(+), 2 deletions(-) (limited to 'src') diff --git a/src/poser_turveytori.c b/src/poser_turveytori.c index 22098d0..76dd7fe 100644 --- a/src/poser_turveytori.c +++ b/src/poser_turveytori.c @@ -1190,7 +1190,7 @@ static Point SolveForLighthouse(TrackedObject *obj, char doLogOutput) printf("(%4.4f, %4.4f, %4.4f) Dist: %8.8f Fit:%4f ", refinedEstimateGd.x, refinedEstimateGd.y, refinedEstimateGd.z, distance, fitGd); - if (fitGd > 5) + //if (fitGd > 5) { FLT distance = FLT_SQRT(SQUARED(refinedEstimateGd.x) + SQUARED(refinedEstimateGd.y) + SQUARED(refinedEstimateGd.z)); printf("(%4.4f, %4.4f, %4.4f) Dist: %8.8f Fit:%4f ", refinedEstimateGd.x, refinedEstimateGd.y, refinedEstimateGd.z, distance, fitGd); @@ -1351,7 +1351,7 @@ int PoserTurveyTori( SurviveObject * so, PoserData * poserData ) counter++; // let's just do this occasionally for now... - if (counter % 4 == 0) + if (counter % 1 == 0) QuickPose(so); } // axis changed, time to increment the circular buffer index. -- cgit v1.2.3 From a3837d79442e1845baf441b8f84b41da3e8d81bc Mon Sep 17 00:00:00 2001 From: mwturvey Date: Thu, 30 Mar 2017 11:07:39 -0700 Subject: Limit amount of time searching for best solution Sometimes the search algorithm would spend way too much time making a bunch of small improvements, when it would have been better to just stop and call it "good enough" --- src/poser_turveytori.c | 35 ++++++++++++++++++++++------------- 1 file changed, 22 insertions(+), 13 deletions(-) (limited to 'src') diff --git a/src/poser_turveytori.c b/src/poser_turveytori.c index 76dd7fe..47147b0 100644 --- a/src/poser_turveytori.c +++ b/src/poser_turveytori.c @@ -529,7 +529,16 @@ static Point RefineEstimateUsingModifiedGradientDescent1(Point initialEstimate, } - + // from empiracle evidence, we're probably "good enough" at this point. + // So, even though we could still improve, we're likely to be improving + // very slowly, and we should just take what we've got and move on. + // This also seems to happen almost only when data is a little more "dirty" + // because the tracker is being rotated. + if (i > 120) + { + //printf("i got big"); + break; + } } printf(" i=%d ", i); @@ -950,7 +959,11 @@ static void RefineRotationEstimateAxisAngle(FLT *rotOut, Point lhPoint, FLT *ini } - + if (i > 1000) + { + //printf("Ri got big"); + break; + } } printf(" Ri=%d ", i); } @@ -1188,18 +1201,14 @@ static Point SolveForLighthouse(TrackedObject *obj, char doLogOutput) FLT fitGd = getPointFitness(refinedEstimateGd, pna, pnaCount); - printf("(%4.4f, %4.4f, %4.4f) Dist: %8.8f Fit:%4f ", refinedEstimateGd.x, refinedEstimateGd.y, refinedEstimateGd.z, distance, fitGd); + FLT distance = FLT_SQRT(SQUARED(refinedEstimateGd.x) + SQUARED(refinedEstimateGd.y) + SQUARED(refinedEstimateGd.z)); + printf(" SensorCount(%d) LhPos:(%4.4f, %4.4f, %4.4f) Dist: %8.8f ", obj->numSensors, refinedEstimateGd.x, refinedEstimateGd.y, refinedEstimateGd.z, distance); + //printf("Distance is %f, Fitness is %f\n", distance, fitGd); - //if (fitGd > 5) - { - FLT distance = FLT_SQRT(SQUARED(refinedEstimateGd.x) + SQUARED(refinedEstimateGd.y) + SQUARED(refinedEstimateGd.z)); - printf("(%4.4f, %4.4f, %4.4f) Dist: %8.8f Fit:%4f ", refinedEstimateGd.x, refinedEstimateGd.y, refinedEstimateGd.z, distance, fitGd); - //printf("Distance is %f, Fitness is %f\n", distance, fitGd); + FLT rot[4]; + SolveForRotation(rot, obj, refinedEstimateGd); - FLT rot[4]; - SolveForRotation(rot, obj, refinedEstimateGd); - } - if (logFile) + if (logFile) { updateHeader(logFile); fclose(logFile); @@ -1351,7 +1360,7 @@ int PoserTurveyTori( SurviveObject * so, PoserData * poserData ) counter++; // let's just do this occasionally for now... - if (counter % 1 == 0) + if (counter % 2 == 0) QuickPose(so); } // axis changed, time to increment the circular buffer index. -- cgit v1.2.3 From 7ea900d3c0ce02bf5867e73dc2bbe3988207c477 Mon Sep 17 00:00:00 2001 From: mwturvey Date: Thu, 30 Mar 2017 11:50:25 -0700 Subject: poser tweaks --- src/poser_turveytori.c | 4 ++-- 1 file changed, 2 insertions(+), 2 deletions(-) (limited to 'src') diff --git a/src/poser_turveytori.c b/src/poser_turveytori.c index 47147b0..5d25294 100644 --- a/src/poser_turveytori.c +++ b/src/poser_turveytori.c @@ -534,13 +534,13 @@ static Point RefineEstimateUsingModifiedGradientDescent1(Point initialEstimate, // very slowly, and we should just take what we've got and move on. // This also seems to happen almost only when data is a little more "dirty" // because the tracker is being rotated. - if (i > 120) + if (i > 900) { //printf("i got big"); break; } } - printf(" i=%d ", i); + printf(" i=%3d ", i); return lastPoint; } -- cgit v1.2.3 From c783f758751b9a64ca09b41e6417222a5e6cbff2 Mon Sep 17 00:00:00 2001 From: Mike Turvey Date: Thu, 30 Mar 2017 22:56:19 -0700 Subject: Checking Smallest Angle --- src/poser_turveytori.c | 9 ++++++++- 1 file changed, 8 insertions(+), 1 deletion(-) (limited to 'src') diff --git a/src/poser_turveytori.c b/src/poser_turveytori.c index 5d25294..c2d7410 100644 --- a/src/poser_turveytori.c +++ b/src/poser_turveytori.c @@ -1129,6 +1129,8 @@ static Point SolveForLighthouse(TrackedObject *obj, char doLogOutput) Point avgNorm = { 0 }; + FLT smallestAngle = 20.0; + size_t pnaCount = 0; for (unsigned int i = 0; i < obj->numSensors; i++) { @@ -1143,6 +1145,11 @@ static Point SolveForLighthouse(TrackedObject *obj, char doLogOutput) //pna[pnaCount].angle = pythAngleBetweenSensors2(&obj->sensor[i], &obj->sensor[j]); pna[pnaCount].tanAngle = FLT_TAN(pna[pnaCount].angle); + if (pna[pnaCount].angle < smallestAngle) + { + smallestAngle = pna[pnaCount].angle; + } + double pythAngle = sqrt(SQUARED(obj->sensor[i].phi - obj->sensor[j].phi) + SQUARED(obj->sensor[i].theta - obj->sensor[j].theta)); pna[pnaCount].rotation = GetRotationMatrixForTorus(pna[pnaCount].a, pna[pnaCount].b); @@ -1202,7 +1209,7 @@ static Point SolveForLighthouse(TrackedObject *obj, char doLogOutput) FLT fitGd = getPointFitness(refinedEstimateGd, pna, pnaCount); FLT distance = FLT_SQRT(SQUARED(refinedEstimateGd.x) + SQUARED(refinedEstimateGd.y) + SQUARED(refinedEstimateGd.z)); - printf(" SensorCount(%d) LhPos:(%4.4f, %4.4f, %4.4f) Dist: %8.8f ", obj->numSensors, refinedEstimateGd.x, refinedEstimateGd.y, refinedEstimateGd.z, distance); + printf(" sma(%d) SnsrCnt(%d) LhPos:(%4.4f, %4.4f, %4.4f) Dist: %8.8f ", smallestAngle, obj->numSensors, refinedEstimateGd.x, refinedEstimateGd.y, refinedEstimateGd.z, distance); //printf("Distance is %f, Fitness is %f\n", distance, fitGd); FLT rot[4]; -- cgit v1.2.3 From b562a8214508c74ed8094514816be012224abcd9 Mon Sep 17 00:00:00 2001 From: mwturvey Date: Fri, 31 Mar 2017 10:23:08 -0700 Subject: hacks that make tori more usable --- src/poser_turveytori.c | 68 ++++++++++++++++++++++++++++++++++++++++++-------- 1 file changed, 57 insertions(+), 11 deletions(-) (limited to 'src') diff --git a/src/poser_turveytori.c b/src/poser_turveytori.c index c2d7410..62cd12c 100644 --- a/src/poser_turveytori.c +++ b/src/poser_turveytori.c @@ -8,6 +8,11 @@ #include #include #include +#if defined(__FreeBSD__) || defined(__APPLE__) +#include +#else +#include //for alloca +#endif #define PointToFlts(x) ((FLT*)(x)) @@ -120,7 +125,8 @@ typedef struct FLT tanAngle; // tangent of angle Matrix3x3 rotation; Matrix3x3 invRotation; // inverse of rotation - + char ai; + char bi; } PointsAndAngle; @@ -378,21 +384,52 @@ FLT getPointFitnessForPna(Point pointIn, PointsAndAngle *pna) return dist; } -FLT getPointFitness(Point pointIn, PointsAndAngle *pna, size_t pnaCount) +FLT getPointFitness(Point pointIn, PointsAndAngle *pna, size_t pnaCount, int deubgPrint) { FLT fitness; FLT resultSum = 0; + FLT *fitnesses = alloca(sizeof(FLT) * pnaCount); + int i=0, j=0; + + FLT worstFitness = 0; for (size_t i = 0; i < pnaCount; i++) { fitness = getPointFitnessForPna(pointIn, &(pna[i])); - resultSum += SQUARED(fitness); + + if (worstFitness < fitness) + { + i = pna[i].ai; + j = pna[i].bi; + worstFitness = fitness; + } + + fitnesses[i] = fitness; + if (deubgPrint) + { + printf(" [%d, %d](%f)\n", pna[i].ai, pna[i].bi, fitness); + } } + for (size_t i = 0; i < pnaCount; i++) + { + // TODO: This is an UGLY HACK!!! It is NOT ROBUST and MUST BE BETTER + // Right now, we're just throwing away the single worst fitness value + // this works frequently, but we REALLY need to do a better job of determing + // exactly when we should throw away a bad value. I'm thinking that decision + // alone could be a master's thesis, so lots of work to be done. + // This is just a stupid general approach that helps in a lot of cases, + // but is NOT suitable for long term use. + //if (pna[i].bi != i && pna[i].bi != j && pna[i].ai != i && pna[i].ai != j) + if (fitnesses[i] != worstFitness) + resultSum += SQUARED(fitnesses[i]); + } return 1 / FLT_SQRT(resultSum); } +// TODO: Use a central point instead of separate "minus" points for each axis. This will reduce +// the number of fitness calls by 1/3. Point getGradient(Point pointIn, PointsAndAngle *pna, size_t pnaCount, FLT precision) { Point result; @@ -401,19 +438,19 @@ Point getGradient(Point pointIn, PointsAndAngle *pna, size_t pnaCount, FLT preci Point tmpXminus = pointIn; tmpXplus.x = pointIn.x + precision; tmpXminus.x = pointIn.x - precision; - result.x = getPointFitness(tmpXplus, pna, pnaCount) - getPointFitness(tmpXminus, pna, pnaCount); + result.x = getPointFitness(tmpXplus, pna, pnaCount, 0) - getPointFitness(tmpXminus, pna, pnaCount, 0); Point tmpYplus = pointIn; Point tmpYminus = pointIn; tmpYplus.y = pointIn.y + precision; tmpYminus.y = pointIn.y - precision; - result.y = getPointFitness(tmpYplus, pna, pnaCount) - getPointFitness(tmpYminus, pna, pnaCount); + result.y = getPointFitness(tmpYplus, pna, pnaCount, 0) - getPointFitness(tmpYminus, pna, pnaCount, 0); Point tmpZplus = pointIn; Point tmpZminus = pointIn; tmpZplus.z = pointIn.z + precision; tmpZminus.z = pointIn.z - precision; - result.z = getPointFitness(tmpZplus, pna, pnaCount) - getPointFitness(tmpZminus, pna, pnaCount); + result.z = getPointFitness(tmpZplus, pna, pnaCount, 0) - getPointFitness(tmpZminus, pna, pnaCount, 0); return result; } @@ -448,7 +485,7 @@ Point getAvgPoints(Point a, Point b) static Point RefineEstimateUsingModifiedGradientDescent1(Point initialEstimate, PointsAndAngle *pna, size_t pnaCount, FILE *logFile) { int i = 0; - FLT lastMatchFitness = getPointFitness(initialEstimate, pna, pnaCount); + FLT lastMatchFitness = getPointFitness(initialEstimate, pna, pnaCount, 0); Point lastPoint = initialEstimate; // The values below are somewhat magic, and definitely tunable @@ -505,7 +542,7 @@ static Point RefineEstimateUsingModifiedGradientDescent1(Point initialEstimate, point4.y = point3.y + specialGradient.y; point4.z = point3.z + specialGradient.z; - FLT newMatchFitness = getPointFitness(point4, pna, pnaCount); + FLT newMatchFitness = getPointFitness(point4, pna, pnaCount, 0); if (newMatchFitness > lastMatchFitness) { @@ -534,7 +571,7 @@ static Point RefineEstimateUsingModifiedGradientDescent1(Point initialEstimate, // very slowly, and we should just take what we've got and move on. // This also seems to happen almost only when data is a little more "dirty" // because the tracker is being rotated. - if (i > 900) + if (i > 120) { //printf("i got big"); break; @@ -1130,6 +1167,7 @@ static Point SolveForLighthouse(TrackedObject *obj, char doLogOutput) Point avgNorm = { 0 }; FLT smallestAngle = 20.0; + FLT largestAngle = 0; size_t pnaCount = 0; for (unsigned int i = 0; i < obj->numSensors; i++) @@ -1150,10 +1188,18 @@ static Point SolveForLighthouse(TrackedObject *obj, char doLogOutput) smallestAngle = pna[pnaCount].angle; } + if (pna[pnaCount].angle > largestAngle) + { + largestAngle = pna[pnaCount].angle; + } + double pythAngle = sqrt(SQUARED(obj->sensor[i].phi - obj->sensor[j].phi) + SQUARED(obj->sensor[i].theta - obj->sensor[j].theta)); pna[pnaCount].rotation = GetRotationMatrixForTorus(pna[pnaCount].a, pna[pnaCount].b); pna[pnaCount].invRotation = inverseM33(pna[pnaCount].rotation); + pna[pnaCount].ai = i; + pna[pnaCount].bi = j; + pnaCount++; @@ -1206,10 +1252,10 @@ static Point SolveForLighthouse(TrackedObject *obj, char doLogOutput) } - FLT fitGd = getPointFitness(refinedEstimateGd, pna, pnaCount); + FLT fitGd = getPointFitness(refinedEstimateGd, pna, pnaCount, 0); FLT distance = FLT_SQRT(SQUARED(refinedEstimateGd.x) + SQUARED(refinedEstimateGd.y) + SQUARED(refinedEstimateGd.z)); - printf(" sma(%d) SnsrCnt(%d) LhPos:(%4.4f, %4.4f, %4.4f) Dist: %8.8f ", smallestAngle, obj->numSensors, refinedEstimateGd.x, refinedEstimateGd.y, refinedEstimateGd.z, distance); + printf(" la(%f) SnsrCnt(%d) LhPos:(%4.4f, %4.4f, %4.4f) Dist: %8.8f ", largestAngle, obj->numSensors, refinedEstimateGd.x, refinedEstimateGd.y, refinedEstimateGd.z, distance); //printf("Distance is %f, Fitness is %f\n", distance, fitGd); FLT rot[4]; -- cgit v1.2.3 From 957945c223169073962b4987ee3bf73ab711d0a2 Mon Sep 17 00:00:00 2001 From: mwturvey Date: Fri, 31 Mar 2017 10:41:18 -0700 Subject: formatting changes --- src/poser_turveytori.c | 6 +++--- 1 file changed, 3 insertions(+), 3 deletions(-) (limited to 'src') diff --git a/src/poser_turveytori.c b/src/poser_turveytori.c index 62cd12c..3bb8d59 100644 --- a/src/poser_turveytori.c +++ b/src/poser_turveytori.c @@ -901,7 +901,7 @@ static void WhereIsTheTrackedObjectAxisAngle(FLT *rotation, Point lhPoint) rotatearoundaxis(objPoint, objPoint, reverseRotation, reverseRotation[3]); - printf("{%8.8f, %8.8f, %8.8f} ", objPoint[0], objPoint[1], objPoint[2]); + printf("{% 08.8f, % 08.8f, % 08.8f} ", objPoint[0], objPoint[1], objPoint[2]); } static void RefineRotationEstimateAxisAngle(FLT *rotOut, Point lhPoint, FLT *initialEstimate, TrackedObject *obj) @@ -996,7 +996,7 @@ static void RefineRotationEstimateAxisAngle(FLT *rotOut, Point lhPoint, FLT *ini } - if (i > 1000) + if (i > 998) { //printf("Ri got big"); break; @@ -1255,7 +1255,7 @@ static Point SolveForLighthouse(TrackedObject *obj, char doLogOutput) FLT fitGd = getPointFitness(refinedEstimateGd, pna, pnaCount, 0); FLT distance = FLT_SQRT(SQUARED(refinedEstimateGd.x) + SQUARED(refinedEstimateGd.y) + SQUARED(refinedEstimateGd.z)); - printf(" la(%f) SnsrCnt(%d) LhPos:(%4.4f, %4.4f, %4.4f) Dist: %8.8f ", largestAngle, obj->numSensors, refinedEstimateGd.x, refinedEstimateGd.y, refinedEstimateGd.z, distance); + printf(" la(% 04.4f) SnsrCnt(%2d) LhPos:(% 04.4f, % 04.4f, % 04.4f) Dist: % 08.8f ", largestAngle, (int)obj->numSensors, refinedEstimateGd.x, refinedEstimateGd.y, refinedEstimateGd.z, distance); //printf("Distance is %f, Fitness is %f\n", distance, fitGd); FLT rot[4]; -- cgit v1.2.3 From 4bcb16aae63e18085b73646a0c1dde94b10bb867 Mon Sep 17 00:00:00 2001 From: mwturvey Date: Fri, 31 Mar 2017 14:29:52 -0700 Subject: Wonky World View --- src/poser_turveytori.c | 103 ++++++++++++++++++++++++++++++++++++++++--------- 1 file changed, 85 insertions(+), 18 deletions(-) (limited to 'src') diff --git a/src/poser_turveytori.c b/src/poser_turveytori.c index 3bb8d59..8b131b5 100644 --- a/src/poser_turveytori.c +++ b/src/poser_turveytori.c @@ -77,7 +77,6 @@ static const float DefaultPointsPerOuterDiameter = 60; typedef struct { FLT down[3]; // populated by the IMU for posing - int something; //Stuff #define OLD_ANGLES_BUFF_LEN 3 @@ -894,14 +893,15 @@ void getNormalizedAndScaledRotationGradient(FLT *vectorToScale, FLT desiredMagni return; } -static void WhereIsTheTrackedObjectAxisAngle(FLT *rotation, Point lhPoint) +static void WhereIsTheTrackedObjectAxisAngle(FLT *posOut, FLT *rotation, Point lhPoint) { - FLT reverseRotation[4] = {rotation[0], rotation[1], rotation[2], rotation[3]}; - FLT objPoint[3] = {lhPoint.x, lhPoint.y, lhPoint.z}; + posOut[0] = lhPoint.x; + posOut[1] = lhPoint.y; + posOut[2] = lhPoint.z; - rotatearoundaxis(objPoint, objPoint, reverseRotation, reverseRotation[3]); + rotatearoundaxis(posOut, posOut, rotation, rotation[3]); - printf("{% 08.8f, % 08.8f, % 08.8f} ", objPoint[0], objPoint[1], objPoint[2]); + printf("{% 04.4f, % 04.4f, % 04.4f} ", posOut[0], posOut[1], posOut[2]); } static void RefineRotationEstimateAxisAngle(FLT *rotOut, Point lhPoint, FLT *initialEstimate, TrackedObject *obj) @@ -1133,7 +1133,6 @@ void SolveForRotation(FLT rotOut[4], TrackedObject *obj, Point lh) // Step 2, optimize the axis/ angle to match the data. RefineRotationEstimateAxisAngle(rotOut, lh, zAxis, obj); - WhereIsTheTrackedObjectAxisAngle(rotOut, lh); //// Step 2, optimize the quaternion to match the data. //RefineRotationEstimateQuaternion(rotOut, lh, quat1, obj); @@ -1143,7 +1142,7 @@ void SolveForRotation(FLT rotOut[4], TrackedObject *obj, Point lh) } -static Point SolveForLighthouse(TrackedObject *obj, char doLogOutput) +static Point SolveForLighthouse(FLT posOut[3], FLT quatOut[4], TrackedObject *obj, SurviveObject *so, char doLogOutput, int lh, int setLhCalibration) { //printf("Solving for Lighthouse\n"); @@ -1258,10 +1257,55 @@ static Point SolveForLighthouse(TrackedObject *obj, char doLogOutput) printf(" la(% 04.4f) SnsrCnt(%2d) LhPos:(% 04.4f, % 04.4f, % 04.4f) Dist: % 08.8f ", largestAngle, (int)obj->numSensors, refinedEstimateGd.x, refinedEstimateGd.y, refinedEstimateGd.z, distance); //printf("Distance is %f, Fitness is %f\n", distance, fitGd); - FLT rot[4]; + FLT rot[4]; // this is axis/ angle rotation, not a quaternion! SolveForRotation(rot, obj, refinedEstimateGd); + FLT objPos[3]; + + WhereIsTheTrackedObjectAxisAngle(objPos, rot, refinedEstimateGd); + + FLT rotQuat[4]; + + quatfromaxisangle(rotQuat, rot, rot[3]); + + //{ + FLT tmpPos[3] = {refinedEstimateGd.x, refinedEstimateGd.y, refinedEstimateGd.z}; + + quatrotatevector(tmpPos, rotQuat, tmpPos); + //} + + if (setLhCalibration) + { + if (so->ctx->bsd[lh].PositionSet) + { + printf("Warning: resetting base station calibration data"); + } + + FLT invRot[4]; + quatgetreciprocal(invRot, rotQuat); + + so->ctx->bsd[lh].Pose.Pos[0] = refinedEstimateGd.x; + so->ctx->bsd[lh].Pose.Pos[1] = refinedEstimateGd.y; + so->ctx->bsd[lh].Pose.Pos[2] = refinedEstimateGd.z; + so->ctx->bsd[lh].Pose.Rot[0] = invRot[0]; + so->ctx->bsd[lh].Pose.Rot[1] = invRot[1]; + so->ctx->bsd[lh].Pose.Rot[2] = invRot[2]; + so->ctx->bsd[lh].Pose.Rot[3] = invRot[3]; + so->ctx->bsd[lh].PositionSet = 1; + } + + FLT wcPos[3]; // position in wold coordinates + + quatrotatevector(wcPos, so->ctx->bsd[lh].Pose.Rot, objPos); - if (logFile) + wcPos[0] -= so->ctx->bsd[lh].Pose.Pos[0]; + wcPos[1] -= so->ctx->bsd[lh].Pose.Pos[1]; + wcPos[2] -= so->ctx->bsd[lh].Pose.Pos[2]; + + printf(" <% 04.4f, % 04.4f, % 04.4f > ", wcPos[0], wcPos[1], wcPos[2]); + + //posOut = + + if (logFile) { updateHeader(logFile); fclose(logFile); @@ -1277,10 +1321,17 @@ static Point SolveForLighthouse(TrackedObject *obj, char doLogOutput) -static void QuickPose(SurviveObject *so) +static void QuickPose(SurviveObject *so, int lh) { + + ToriData * td = so->PoserData; + if (! so->ctx->bsd[lh].PositionSet) + { + // we don't know where we are! Augh!!! + return; + } //for (int i=0; i < so->nr_locations; i++) //{ @@ -1310,9 +1361,16 @@ static void QuickPose(SurviveObject *so) { int sensorCount = 0; + // TODO: remove, for debug purposes only! + FLT downQuat[4]; + FLT negZ[3] = { 0,0,-1 }; + quatfrom2vectors(downQuat, negZ, td->down); + //quatfrom2vectors(downQuat, td->down, negZ); + // end TODO + + for (int i = 0; i < so->nr_locations; i++) { - int lh = 0; int angleIndex0 = (td->angleIndex[lh][0] + 1 + OLD_ANGLES_BUFF_LEN) % OLD_ANGLES_BUFF_LEN; int angleIndex1 = (td->angleIndex[lh][1] + 1 + OLD_ANGLES_BUFF_LEN) % OLD_ANGLES_BUFF_LEN; if (td->oldAngles[i][0][lh][angleIndex0] != 0 && td->oldAngles[i][1][lh][angleIndex1] != 0) @@ -1320,6 +1378,10 @@ static void QuickPose(SurviveObject *so) FLT norm[3] = { so->sensor_normals[i * 3 + 0] , so->sensor_normals[i * 3 + 1] , so->sensor_normals[i * 3 + 2] }; FLT point[3] = { so->sensor_locations[i * 3 + 0] , so->sensor_locations[i * 3 + 1] , so->sensor_locations[i * 3 + 2] }; + // TODO: remove these two lines!!! + //quatrotatevector(norm, downQuat, norm); + //quatrotatevector(point, downQuat, point); + to->sensor[sensorCount].normal.x = norm[0]; to->sensor[sensorCount].normal.y = norm[1]; to->sensor[sensorCount].normal.z = norm[2]; @@ -1342,7 +1404,9 @@ static void QuickPose(SurviveObject *so) if (sensorCount > 4) { - SolveForLighthouse(to, 0); + FLT pos[3], quat[4]; + + SolveForLighthouse(pos, quat, to, so, 0, lh, 0); printf("!\n"); } @@ -1414,7 +1478,7 @@ int PoserTurveyTori( SurviveObject * so, PoserData * poserData ) // let's just do this occasionally for now... if (counter % 2 == 0) - QuickPose(so); + QuickPose(so, 0); } // axis changed, time to increment the circular buffer index. td->angleIndex[l->lh][axis]++; @@ -1485,8 +1549,9 @@ int PoserTurveyTori( SurviveObject * so, PoserData * poserData ) } to->numSensors = sensorCount; + FLT pos[3], quat[4]; - SolveForLighthouse(to, 0); + SolveForLighthouse(pos, quat, to, so, 0, 0, 1); } { int sensorCount = 0; @@ -1499,8 +1564,8 @@ int PoserTurveyTori( SurviveObject * so, PoserData * poserData ) FLT norm[3] = { so->sensor_normals[i * 3 + 0] , so->sensor_normals[i * 3 + 1] , so->sensor_normals[i * 3 + 2] }; FLT point[3] = { so->sensor_locations[i * 3 + 0] , so->sensor_locations[i * 3 + 1] , so->sensor_locations[i * 3 + 2] }; - quatrotatevector(norm, downQuat, norm); - quatrotatevector(point, downQuat, point); + //quatrotatevector(norm, downQuat, norm); + //quatrotatevector(point, downQuat, point); to->sensor[sensorCount].normal.x = norm[0]; to->sensor[sensorCount].normal.y = norm[1]; @@ -1516,7 +1581,9 @@ int PoserTurveyTori( SurviveObject * so, PoserData * poserData ) to->numSensors = sensorCount; - SolveForLighthouse(to, 0); + FLT pos[3], quat[4]; + + SolveForLighthouse(pos, quat, to, so, 0, 1, 1); } free(to); -- cgit v1.2.3 From 198082258a62ab6313569e7950c0420baad6ddef Mon Sep 17 00:00:00 2001 From: mwturvey Date: Fri, 31 Mar 2017 15:19:00 -0700 Subject: Sane World View The z=0 plane is now initially aligned to the device's z=0 plane during calibration. (works really well for the tracker, which is oriented with Z in the proper direction when sitting on a horizontal surface) --- src/poser_turveytori.c | 30 ++++++++++++++++++------------ 1 file changed, 18 insertions(+), 12 deletions(-) (limited to 'src') diff --git a/src/poser_turveytori.c b/src/poser_turveytori.c index 8b131b5..d3dd2ff 100644 --- a/src/poser_turveytori.c +++ b/src/poser_turveytori.c @@ -895,9 +895,9 @@ void getNormalizedAndScaledRotationGradient(FLT *vectorToScale, FLT desiredMagni static void WhereIsTheTrackedObjectAxisAngle(FLT *posOut, FLT *rotation, Point lhPoint) { - posOut[0] = lhPoint.x; - posOut[1] = lhPoint.y; - posOut[2] = lhPoint.z; + posOut[0] = -lhPoint.x; + posOut[1] = -lhPoint.y; + posOut[2] = -lhPoint.z; rotatearoundaxis(posOut, posOut, rotation, rotation[3]); @@ -1273,8 +1273,12 @@ static Point SolveForLighthouse(FLT posOut[3], FLT quatOut[4], TrackedObject *ob quatrotatevector(tmpPos, rotQuat, tmpPos); //} + //static int foo = 0; + + //if (0 == foo) if (setLhCalibration) - { + { + //foo = 1; if (so->ctx->bsd[lh].PositionSet) { printf("Warning: resetting base station calibration data"); @@ -1297,20 +1301,22 @@ static Point SolveForLighthouse(FLT posOut[3], FLT quatOut[4], TrackedObject *ob quatrotatevector(wcPos, so->ctx->bsd[lh].Pose.Rot, objPos); - wcPos[0] -= so->ctx->bsd[lh].Pose.Pos[0]; - wcPos[1] -= so->ctx->bsd[lh].Pose.Pos[1]; - wcPos[2] -= so->ctx->bsd[lh].Pose.Pos[2]; + wcPos[0] += so->ctx->bsd[lh].Pose.Pos[0]; + wcPos[1] += so->ctx->bsd[lh].Pose.Pos[1]; + wcPos[2] += so->ctx->bsd[lh].Pose.Pos[2]; - printf(" <% 04.4f, % 04.4f, % 04.4f > ", wcPos[0], wcPos[1], wcPos[2]); + so->OutPose.Pos[0] = wcPos[0]; + so->OutPose.Pos[1] = wcPos[1]; + so->OutPose.Pos[2] = wcPos[2]; - //posOut = + printf(" <% 04.4f, % 04.4f, % 04.4f > ", wcPos[0], wcPos[1], wcPos[2]); if (logFile) { updateHeader(logFile); fclose(logFile); } - //fgetc(stdin); + return refinedEstimateGd; } @@ -1526,8 +1532,8 @@ int PoserTurveyTori( SurviveObject * so, PoserData * poserData ) FLT norm[3] = { so->sensor_normals[i * 3 + 0] , so->sensor_normals[i * 3 + 1] , so->sensor_normals[i * 3 + 2] }; FLT point[3] = { so->sensor_locations[i * 3 + 0] , so->sensor_locations[i * 3 + 1] , so->sensor_locations[i * 3 + 2] }; - quatrotatevector(norm, downQuat, norm); - quatrotatevector(point, downQuat, point); + //quatrotatevector(norm, downQuat, norm); + //quatrotatevector(point, downQuat, point); to->sensor[sensorCount].normal.x = norm[0]; to->sensor[sensorCount].normal.y = norm[1]; -- cgit v1.2.3 From eb5fae1c2aa6007ec84fdf0c5405308c7ea274ca Mon Sep 17 00:00:00 2001 From: mwturvey Date: Fri, 31 Mar 2017 15:30:35 -0700 Subject: Cleanup --- src/poser_turveytori.c | 58 ++++++++++++++++++-------------------------------- 1 file changed, 21 insertions(+), 37 deletions(-) (limited to 'src') diff --git a/src/poser_turveytori.c b/src/poser_turveytori.c index d3dd2ff..6997aa7 100644 --- a/src/poser_turveytori.c +++ b/src/poser_turveytori.c @@ -1367,12 +1367,12 @@ static void QuickPose(SurviveObject *so, int lh) { int sensorCount = 0; - // TODO: remove, for debug purposes only! - FLT downQuat[4]; - FLT negZ[3] = { 0,0,-1 }; - quatfrom2vectors(downQuat, negZ, td->down); + //// TODO: remove, for debug purposes only! + //FLT downQuat[4]; + //FLT negZ[3] = { 0,0,-1 }; + ////quatfrom2vectors(downQuat, negZ, td->down); //quatfrom2vectors(downQuat, td->down, negZ); - // end TODO + //// end TODO for (int i = 0; i < so->nr_locations; i++) @@ -1397,11 +1397,6 @@ static void QuickPose(SurviveObject *so, int lh) to->sensor[sensorCount].theta = td->oldAngles[i][0][lh][angleIndex0] + LINMATHPI / 2; // lighthouse 0, angle 0 (horizontal) to->sensor[sensorCount].phi = td->oldAngles[i][1][lh][angleIndex1] + LINMATHPI / 2; // lighthouse 0, angle 1 (vertical) - //printf("%2d: %8.8f, %8.8f \n", - // i, - // to->sensor[sensorCount].theta, - // to->sensor[sensorCount].phi - // ); sensorCount++; } @@ -1470,33 +1465,28 @@ int PoserTurveyTori( SurviveObject * so, PoserData * poserData ) //printf( "LIG:%s %d @ %f rad, %f s (AC %d) (TC %d)\n", so->codename, l->sensor_id, l->angle, l->length, l->acode, l->timecode ); if ((td->lastAxis[l->lh] != (l->acode & 0x1)) ) { - int foo = l->acode & 0x1; - //printf("%d", foo); - //if (axis) + if (0 == l->lh && axis) // only once per full cycle... { - if (0 == l->lh && axis) // only once per full cycle... - { - static unsigned int counter = 1; + static unsigned int counter = 1; - counter++; + counter++; - // let's just do this occasionally for now... - if (counter % 2 == 0) - QuickPose(so, 0); - } - // axis changed, time to increment the circular buffer index. - td->angleIndex[l->lh][axis]++; - td->angleIndex[l->lh][axis] = td->angleIndex[l->lh][axis] % OLD_ANGLES_BUFF_LEN; - - // and clear out the data. - for (int i=0; i < SENSORS_PER_OBJECT; i++) - { - td->oldAngles[i][axis][l->lh][td->angleIndex[l->lh][axis]] = 0; - } + // let's just do this occasionally for now... + if (counter % 2 == 0) + QuickPose(so, 0); + } + // axis changed, time to increment the circular buffer index. + td->angleIndex[l->lh][axis]++; + td->angleIndex[l->lh][axis] = td->angleIndex[l->lh][axis] % OLD_ANGLES_BUFF_LEN; + // and clear out the data. + for (int i=0; i < SENSORS_PER_OBJECT; i++) + { + td->oldAngles[i][axis][l->lh][td->angleIndex[l->lh][axis]] = 0; } + td->lastAxis[l->lh] = axis; } @@ -1517,7 +1507,7 @@ int PoserTurveyTori( SurviveObject * so, PoserData * poserData ) // let's get the quaternion that represents this rotation. FLT downQuat[4]; - FLT negZ[3] = { 0,0,-1 }; + FLT negZ[3] = { 0,0,1 }; //quatfrom2vectors(downQuat, negZ, td->down); quatfrom2vectors(downQuat, td->down, negZ); @@ -1544,12 +1534,6 @@ int PoserTurveyTori( SurviveObject * so, PoserData * poserData ) to->sensor[sensorCount].theta = fs->angles[i][0][0] + LINMATHPI / 2; // lighthouse 0, angle 0 (horizontal) to->sensor[sensorCount].phi = fs->angles[i][0][1] + LINMATHPI / 2; // lighthouse 0, angle 1 (vertical) - //printf("%2d: %8.8f, %8.8f \n", - // i, - // to->sensor[sensorCount].theta, - // to->sensor[sensorCount].phi - // ); - sensorCount++; } } -- cgit v1.2.3 From 66eb9b0508c6a5b1af5d1bc8fb5cac74573f8b20 Mon Sep 17 00:00:00 2001 From: mwturvey Date: Fri, 31 Mar 2017 15:36:36 -0700 Subject: Added rotation to object pose Haven't tested it yet, low confidence it's right, but the code is on the right track. --- src/poser_turveytori.c | 8 ++++++++ 1 file changed, 8 insertions(+) (limited to 'src') diff --git a/src/poser_turveytori.c b/src/poser_turveytori.c index 6997aa7..c251040 100644 --- a/src/poser_turveytori.c +++ b/src/poser_turveytori.c @@ -1301,6 +1301,9 @@ static Point SolveForLighthouse(FLT posOut[3], FLT quatOut[4], TrackedObject *ob quatrotatevector(wcPos, so->ctx->bsd[lh].Pose.Rot, objPos); + FLT newOrientation[4]; + quatrotateabout(newOrientation, rotQuat, so->ctx->bsd[lh].Pose.Rot ); + wcPos[0] += so->ctx->bsd[lh].Pose.Pos[0]; wcPos[1] += so->ctx->bsd[lh].Pose.Pos[1]; wcPos[2] += so->ctx->bsd[lh].Pose.Pos[2]; @@ -1309,6 +1312,11 @@ static Point SolveForLighthouse(FLT posOut[3], FLT quatOut[4], TrackedObject *ob so->OutPose.Pos[1] = wcPos[1]; so->OutPose.Pos[2] = wcPos[2]; + so->OutPose.Rot[0] = newOrientation[0]; + so->OutPose.Rot[1] = newOrientation[1]; + so->OutPose.Rot[2] = newOrientation[2]; + so->OutPose.Rot[3] = newOrientation[3]; + printf(" <% 04.4f, % 04.4f, % 04.4f > ", wcPos[0], wcPos[1], wcPos[2]); if (logFile) -- cgit v1.2.3 From 0e69fac4d8565926ac3cff6084c95f204d8e173f Mon Sep 17 00:00:00 2001 From: Mike Turvey Date: Sat, 1 Apr 2017 23:37:03 -0700 Subject: Fix bug that prevented HMD from being added --- src/survive_vive.c | 22 +++++++++++----------- 1 file changed, 11 insertions(+), 11 deletions(-) (limited to 'src') diff --git a/src/survive_vive.c b/src/survive_vive.c index 757fa11..5224728 100755 --- a/src/survive_vive.c +++ b/src/survive_vive.c @@ -68,14 +68,14 @@ const char * devnames[] = { #define USB_DEV_HMD 0 -#define USB_DEV_LIGHTHOUSE 1 +#define USB_DEV_HMD_IMU_LH 1 #define USB_DEV_WATCHMAN1 2 #define USB_DEV_WATCHMAN2 3 #define USB_DEV_TRACKER0 4 #define USB_DEV_W_WATCHMAN1 5 // Wired Watchman attached via USB #ifdef HIDAPI -#define USB_DEV_LIGHTHOUSEB 6 +#define USB_DEV_HMD_IMU_LHB 6 #define USB_DEV_TRACKER0_LIGHTCAP 7 #define USB_DEV_W_WATCHMAN1_LIGHTCAP 8 #define MAX_USB_DEVS 9 @@ -84,7 +84,7 @@ const char * devnames[] = { #endif #define USB_IF_HMD 0 -#define USB_IF_LIGHTHOUSE 1 +#define USB_IF_HMD_IMU_LH 1 #define USB_IF_WATCHMAN1 2 #define USB_IF_WATCHMAN2 3 #define USB_IF_TRACKER0 4 @@ -471,14 +471,14 @@ int survive_usb_init( SurviveViveData * sv, SurviveObject * hmd, SurviveObject * #endif if( sv->udev[USB_DEV_HMD] && AttachInterface( sv, hmd, USB_IF_HMD, sv->udev[USB_DEV_HMD], 0x81, survive_data_cb, "Mainboard" ) ) { return -6; } - if( sv->udev[USB_DEV_LIGHTHOUSE] && AttachInterface( sv, hmd, USB_IF_LIGHTHOUSE, sv->udev[USB_DEV_LIGHTHOUSE], 0x81, survive_data_cb, "Lighthouse" ) ) { return -7; } + if( sv->udev[USB_DEV_HMD_IMU_LH] && AttachInterface( sv, hmd, USB_IF_HMD_IMU_LH, sv->udev[USB_DEV_HMD_IMU_LH], 0x81, survive_data_cb, "Lighthouse" ) ) { return -7; } if( sv->udev[USB_DEV_WATCHMAN1] && AttachInterface( sv, wm0, USB_IF_WATCHMAN1, sv->udev[USB_DEV_WATCHMAN1], 0x81, survive_data_cb, "Watchman 1" ) ) { return -8; } if( sv->udev[USB_DEV_WATCHMAN2] && AttachInterface( sv, wm1, USB_IF_WATCHMAN2, sv->udev[USB_DEV_WATCHMAN2], 0x81, survive_data_cb, "Watchman 2")) { return -9; } if( sv->udev[USB_DEV_TRACKER0] && AttachInterface( sv, tr0, USB_IF_TRACKER0, sv->udev[USB_DEV_TRACKER0], 0x81, survive_data_cb, "Tracker 1")) { return -10; } if( sv->udev[USB_DEV_W_WATCHMAN1] && AttachInterface( sv, ww0, USB_IF_W_WATCHMAN1, sv->udev[USB_DEV_W_WATCHMAN1], 0x81, survive_data_cb, "Wired Watchman 1")) { return -11; } #ifdef HIDAPI //Tricky: use other interface for actual lightcap. XXX THIS IS NOT YET RIGHT!!! - if( sv->udev[USB_DEV_LIGHTHOUSEB] && AttachInterface( sv, hmd, USB_IF_LIGHTCAP, sv->udev[USB_DEV_LIGHTHOUSEB], 0x82, survive_data_cb, "Lightcap")) { return -12; } + if( sv->udev[USB_DEV_HMD_IMU_LHB] && AttachInterface( sv, hmd, USB_IF_LIGHTCAP, sv->udev[USB_DEV_HMD_IMU_LHB], 0x82, survive_data_cb, "Lightcap")) { return -12; } // This is a HACK! But it works. Need to investigate further sv->uiface[USB_DEV_TRACKER0_LIGHTCAP].actual_len = 64; @@ -517,14 +517,14 @@ int survive_vive_send_magic(SurviveContext * ctx, void * drv, int magic_code, vo if( r != sizeof( vive_magic_power_on ) ) return 5; } - if (sv->udev[USB_DEV_LIGHTHOUSE]) + if (sv->udev[USB_DEV_HMD_IMU_LH]) { static uint8_t vive_magic_enable_lighthouse[5] = { 0x04 }; - r = update_feature_report( sv->udev[USB_DEV_LIGHTHOUSE], 0, vive_magic_enable_lighthouse, sizeof( vive_magic_enable_lighthouse ) ); + r = update_feature_report( sv->udev[USB_DEV_HMD_IMU_LH], 0, vive_magic_enable_lighthouse, sizeof( vive_magic_enable_lighthouse ) ); if( r != sizeof( vive_magic_enable_lighthouse ) ) return 5; static uint8_t vive_magic_enable_lighthouse2[5] = { 0x07, 0x02 }; //Switch to 0x25 mode (able to get more light updates) - r = update_feature_report( sv->udev[USB_DEV_LIGHTHOUSE], 0, vive_magic_enable_lighthouse2, sizeof( vive_magic_enable_lighthouse2 ) ); + r = update_feature_report( sv->udev[USB_DEV_HMD_IMU_LH], 0, vive_magic_enable_lighthouse2, sizeof( vive_magic_enable_lighthouse2 ) ); if( r != sizeof( vive_magic_enable_lighthouse2 ) ) return 5; } @@ -1103,7 +1103,7 @@ void survive_data_cb( SurviveUSBInterface * si ) headset->ison = 1; break; } - case USB_IF_LIGHTHOUSE: + case USB_IF_HMD_IMU_LH: case USB_IF_W_WATCHMAN1: case USB_IF_TRACKER0: { @@ -1395,7 +1395,7 @@ int DriverRegHTCVive( SurviveContext * ctx ) } //Next, pull out the config stuff. - if( sv->udev[USB_DEV_HMD] && LoadConfig( sv, hmd, 1, 0, 0 )) { SV_INFO( "HMD config issue." ); } + if( sv->udev[USB_DEV_HMD_IMU_LH] && LoadConfig( sv, hmd, 1, 0, 0 )) { SV_INFO( "HMD config issue." ); } if( sv->udev[USB_DEV_WATCHMAN1] && LoadConfig( sv, wm0, 2, 0, 1 )) { SV_INFO( "Watchman 0 config issue." ); } if( sv->udev[USB_DEV_WATCHMAN2] && LoadConfig( sv, wm1, 3, 0, 1 )) { SV_INFO( "Watchman 1 config issue." ); } if( sv->udev[USB_DEV_TRACKER0] && LoadConfig( sv, tr0, 4, 0, 0 )) { SV_INFO( "Tracker 0 config issue." ); } @@ -1450,7 +1450,7 @@ int DriverRegHTCVive( SurviveContext * ctx ) */ //Add the drivers. - if( sv->udev[USB_DEV_HMD] ) { survive_add_object( ctx, hmd ); } + if( sv->udev[USB_DEV_HMD_IMU_LH] ) { survive_add_object( ctx, hmd ); } if( sv->udev[USB_DEV_WATCHMAN1] ) { survive_add_object( ctx, wm0 ); } if( sv->udev[USB_DEV_WATCHMAN2] ) { survive_add_object( ctx, wm1 ); } if( sv->udev[USB_DEV_TRACKER0] ) { survive_add_object( ctx, tr0 ); } -- cgit v1.2.3 From 443811f15c00df3df9cc14bc859e4cce146cd55f Mon Sep 17 00:00:00 2001 From: mwturvey Date: Wed, 5 Apr 2017 11:26:09 -0700 Subject: Fix USB_DEV_HMD_IMU_LH --- src/survive_vive.c | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) (limited to 'src') diff --git a/src/survive_vive.c b/src/survive_vive.c index 5224728..1f0b5b9 100755 --- a/src/survive_vive.c +++ b/src/survive_vive.c @@ -485,7 +485,7 @@ int survive_usb_init( SurviveViveData * sv, SurviveObject * hmd, SurviveObject * if( sv->udev[USB_DEV_TRACKER0_LIGHTCAP] && AttachInterface( sv, tr0, USB_IF_TRACKER0_LIGHTCAP, sv->udev[USB_DEV_TRACKER0_LIGHTCAP], 0x82, survive_data_cb, "Tracker 1 Lightcap")) { return -13; } if( sv->udev[USB_DEV_W_WATCHMAN1_LIGHTCAP] && AttachInterface( sv, ww0, USB_IF_W_WATCHMAN1_LIGHTCAP, sv->udev[USB_DEV_W_WATCHMAN1_LIGHTCAP], 0x82, survive_data_cb, "Wired Watchman 1 Lightcap")) { return -13; } #else - if( sv->udev[USB_DEV_LIGHTHOUSE] && AttachInterface( sv, hmd, USB_IF_LIGHTCAP, sv->udev[USB_DEV_LIGHTHOUSE], 0x82, survive_data_cb, "Lightcap")) { return -12; } + if( sv->udev[USB_DEV_HMD_IMU_LH] && AttachInterface( sv, hmd, USB_IF_LIGHTCAP, sv->udev[USB_DEV_HMD_IMU_LH], 0x82, survive_data_cb, "Lightcap")) { return -12; } if( sv->udev[USB_DEV_TRACKER0] && AttachInterface( sv, ww0, USB_IF_TRACKER0_LIGHTCAP, sv->udev[USB_DEV_TRACKER0], 0x82, survive_data_cb, "Tracker 0 Lightcap")) { return -13; } if( sv->udev[USB_DEV_W_WATCHMAN1] && AttachInterface( sv, ww0, USB_IF_W_WATCHMAN1_LIGHTCAP, sv->udev[USB_DEV_W_WATCHMAN1], 0x82, survive_data_cb, "Wired Watchman 1 Lightcap")) { return -13; } #endif -- cgit v1.2.3 From e030943790f202cc8af858135911df347a96cf8c Mon Sep 17 00:00:00 2001 From: cnlohr Date: Fri, 7 Apr 2017 00:53:11 -0400 Subject: Refactor a bit of survive_vive to re-use existing functions and simplify execution. --- src/survive_vive.c | 4 +--- 1 file changed, 1 insertion(+), 3 deletions(-) (limited to 'src') diff --git a/src/survive_vive.c b/src/survive_vive.c index 1f0b5b9..8e22ea1 100755 --- a/src/survive_vive.c +++ b/src/survive_vive.c @@ -723,11 +723,9 @@ int survive_get_config( char ** config, SurviveViveData * sv, int devno, int ifa #else //Switch mode to pull config? - SV_INFO( "XXX TODO See if this can be update_feature_report" ); for( k = 0; k < 10; k++ ) { - int rk = libusb_control_transfer(dev, LIBUSB_REQUEST_TYPE_CLASS | LIBUSB_RECIPIENT_INTERFACE | LIBUSB_ENDPOINT_OUT, - 0x09, 0x300 | cfgbuff_send[0], iface, cfgbuff_send, 64, 1000 ); + update_feature_report( dev, iface, cfgbuff_send, 64 ); OGUSleep(1000); } #endif -- cgit v1.2.3 From ffe28a0d0079f56191e82eb6590c59442357983a Mon Sep 17 00:00:00 2001 From: cnlohr Date: Fri, 7 Apr 2017 00:53:50 -0400 Subject: Remove feature printing out all config items at start. --- src/survive_config.c | 5 ++++- 1 file changed, 4 insertions(+), 1 deletion(-) (limited to 'src') diff --git a/src/survive_config.c b/src/survive_config.c index 005cfaf..3a83902 100644 --- a/src/survive_config.c +++ b/src/survive_config.c @@ -360,7 +360,10 @@ int parse_uint32(char* tag, char** values, uint16_t count) { } void handle_tag_value(char* tag, char** values, uint8_t count) { - print_json_value(tag,values,count); + + //Uncomment for more debugging of input configuration. + //print_json_value(tag,values,count); + config_group* cg = cg_stack[cg_stack_head]; if (NULL != *values){ -- cgit v1.2.3 From b93bedcc6c3fb08d028d5e787446721691b86616 Mon Sep 17 00:00:00 2001 From: cnlohr Date: Fri, 7 Apr 2017 00:54:51 -0400 Subject: Fix warnings in dave's ortho. --- src/poser_daveortho.c | 8 ++++---- 1 file changed, 4 insertions(+), 4 deletions(-) (limited to 'src') diff --git a/src/poser_daveortho.c b/src/poser_daveortho.c index 906b21e..9d02bb3 100644 --- a/src/poser_daveortho.c +++ b/src/poser_daveortho.c @@ -460,10 +460,10 @@ PRINT(ab,2,1); #if 1 // matrix44transpose(T2, T); //Transpose so we are - matrix44copy(T2,T); + matrix44copy((FLT*)T2,(FLT*)T); cross3d( &T2[1][0], &T2[0][0], &T2[2][0] ); cross3d( &T2[2][0], &T2[1][0], &T2[0][0] ); //Replace axes in-place. - matrix44copy(T,T2); + matrix44copy((FLT*)T,(FLT*)T2); // matrix44transpose(T, T2); #endif @@ -493,7 +493,7 @@ PRINT(ab,2,1); #if 1 //matrix44copy(T2,T); - matrix44transpose(T2,T); + matrix44transpose((FLT*)T2,(FLT*)T); quatfrommatrix( quat, &T2[0][0] ); printf( "QM: %f\n", quatmagnitude( quat ) ); @@ -506,7 +506,7 @@ PRINT(ab,2,1); // PRINT(T,4,4); // matrix44copy(temp,T2); - matrix44transpose(temp,T2); + matrix44transpose((FLT*)temp,(FLT*)T2); // matrix44transpose(T2, temp); -- cgit v1.2.3 From b6787c60171724360f097a16cc7ec967995ff335 Mon Sep 17 00:00:00 2001 From: cnlohr Date: Fri, 7 Apr 2017 00:55:21 -0400 Subject: Update, fixing charles' disambiguator, adding flag for TURVEYBIGUATOR! --- src/survive_data.c | 21 +++++++++++++++------ 1 file changed, 15 insertions(+), 6 deletions(-) (limited to 'src') diff --git a/src/survive_data.c b/src/survive_data.c index 3f16c7c..4bb1526 100644 --- a/src/survive_data.c +++ b/src/survive_data.c @@ -5,6 +5,10 @@ #include #include +#define USE_TURVEYBIGUATOR + +#ifdef USE_TURVEYBIGUATOR + typedef struct { unsigned int sweep_time[SENSORS_PER_OBJECT]; @@ -311,6 +315,9 @@ void handle_lightcap2( SurviveObject * so, LightcapElement * le ) } + +#endif + int32_t decode_acode(uint32_t length, int32_t main_divisor) { //+50 adds a small offset and seems to help always get it right. //Check the +50 in the future to see how well this works on a variety of hardware. @@ -325,9 +332,13 @@ int32_t decode_acode(uint32_t length, int32_t main_divisor) { void handle_lightcap( SurviveObject * so, LightcapElement * le ) { SurviveContext * ctx = so->ctx; + +#ifdef USE_TURVEYBIGUATOR handle_lightcap2(so,le); return; +#else + //int32_t deltat = (uint32_t)le->timestamp - (uint32_t)so->last_master_time; if( le->sensor_id > SENSORS_PER_OBJECT ) @@ -377,8 +388,6 @@ void handle_lightcap( SurviveObject * so, LightcapElement * le ) { int is_new_pulse = delta > so->pulselength_min_sync /*1500*/ + last_sync_length; - - so->did_handle_ootx = 0; if( is_new_pulse ) @@ -398,6 +407,7 @@ void handle_lightcap( SurviveObject * so, LightcapElement * le ) else { ssn = ++so->sync_set_number; + if( so->sync_set_number >= NUM_LIGHTHOUSES ) { SV_INFO( "Warning. Received an extra, unassociated sync pulse." ); @@ -432,8 +442,6 @@ void handle_lightcap( SurviveObject * so, LightcapElement * le ) } } - - //See if this is a valid actual pulse. else if( le->length < so->pulse_max_for_sweep && delta > so->pulse_in_clear_time && ssn >= 0 ) { @@ -496,7 +504,6 @@ void handle_lightcap( SurviveObject * so, LightcapElement * le ) so->did_handle_ootx = 1; } - if (acode > 3) { if( ssn == 0 ) { @@ -512,7 +519,7 @@ void handle_lightcap( SurviveObject * so, LightcapElement * le ) //Make sure pulse is in valid window if( offset_from < 380000 && offset_from > 70000 ) { - ctx->lightproc( so, le->sensor_id, acode, offset_from, le->timestamp, le->length, so->sync_set_number ); + ctx->lightproc( so, le->sensor_id, acode, offset_from, le->timestamp, le->length, !(acode>>2) ); } } else @@ -520,6 +527,8 @@ void handle_lightcap( SurviveObject * so, LightcapElement * le ) //printf( "FAIL %d %d - %d = %d\n", le->length, so->last_photo_time, le->timestamp, so->last_photo_time - le->timestamp ); //Runt pulse, or no sync pulses available. } +#endif + } -- cgit v1.2.3 From 454bb09f5ac0a72149c82a2231a9460e06657dea Mon Sep 17 00:00:00 2001 From: Joshua Allen Date: Sat, 8 Apr 2017 10:32:59 -0400 Subject: Delay picking active lighthouse until the sweep pass so we can be sure we have the longest synce pulse time and acode. HMD reads both lighthouses. --- src/survive_data.c | 80 ++++++++++++++++++++++++++++++++++++++++++------------ 1 file changed, 62 insertions(+), 18 deletions(-) (limited to 'src') diff --git a/src/survive_data.c b/src/survive_data.c index 4bb1526..f61ae8b 100644 --- a/src/survive_data.c +++ b/src/survive_data.c @@ -21,8 +21,10 @@ typedef struct int activeSweepStartTime; int activeAcode; - int lh_pulse_len[NUM_LIGHTHOUSES]; +// int lh_pulse_len[NUM_LIGHTHOUSES]; int lh_start_time[NUM_LIGHTHOUSES]; + int lh_max_pulse_length[NUM_LIGHTHOUSES]; + uint16_t lh_acode[NUM_LIGHTHOUSES]; int current_lh; // used knowing which sync pulse we're looking at. } lightcap2_per_sweep_data; @@ -145,25 +147,34 @@ void handle_lightcap2_sync(SurviveObject * so, LightcapElement * le ) //static unsigned int recent_sync_count = -1; //static unsigned int activeSweepStartTime; - int acode = handle_lightcap2_getAcodeFromSyncPulse(so, le->length); + int acode = handle_lightcap2_getAcodeFromSyncPulse(so, le->length); //acode for this sensor reading // Process any sweep data we have handle_lightcap2_process_sweep_data(so); int time_since_last_sync = (le->timestamp - lcd->per_sweep.recent_sync_time); - //fprintf(stderr, " %2d %8d %d\n", le->sensor_id, time_since_last_sync, le->length); + + fprintf(stderr, " %2d %8d %d\n", le->sensor_id, time_since_last_sync, le->length); // need to store up sync pulses, so we can take the earliest starting time for all sensors. if (time_since_last_sync < 2400) { lcd->per_sweep.recent_sync_time = le->timestamp; // it's the same sync pulse; - so->sync_set_number = 1; +// so->sync_set_number = 1; so->recent_sync_time = le->timestamp; - lcd->per_sweep.lh_pulse_len[lcd->per_sweep.current_lh] = le->length; - lcd->per_sweep.lh_start_time[lcd->per_sweep.current_lh] = le->timestamp; +// lcd->per_sweep.lh_pulse_len[lcd->per_sweep.current_lh] = le->length; +// lcd->per_sweep.lh_start_time[lcd->per_sweep.current_lh] = le->timestamp; + if (le->length > lcd->per_sweep.lh_max_pulse_length[lcd->per_sweep.current_lh]) { + lcd->per_sweep.lh_max_pulse_length[lcd->per_sweep.current_lh] = le->length; + lcd->per_sweep.lh_start_time[lcd->per_sweep.current_lh] = le->timestamp; + lcd->per_sweep.lh_acode[lcd->per_sweep.current_lh] = acode; + } + +/* + //this stuff should probably be happening on the sweep so that we can filter out erroneous a codes if (!(acode >> 2 & 1)) // if the skip bit is not set { lcd->per_sweep.activeLighthouse = lcd->per_sweep.current_lh; @@ -172,19 +183,26 @@ void handle_lightcap2_sync(SurviveObject * so, LightcapElement * le ) } else { + //this causes the master lighthouse to be ignored from the HMD lcd->per_sweep.activeLighthouse = -1; lcd->per_sweep.activeSweepStartTime = 0; lcd->per_sweep.activeAcode = 0; } + */ } else if (time_since_last_sync < 24000) { + lcd->per_sweep.activeLighthouse != -1; + lcd->per_sweep.recent_sync_time = le->timestamp; // I do believe we are lighthouse B lcd->per_sweep.current_lh = 1; - lcd->per_sweep.lh_pulse_len[lcd->per_sweep.current_lh] = le->length; +// lcd->per_sweep.lh_pulse_len[lcd->per_sweep.current_lh] = le->length; lcd->per_sweep.lh_start_time[lcd->per_sweep.current_lh] = le->timestamp; + lcd->per_sweep.lh_max_pulse_length[lcd->per_sweep.current_lh] = le->length; + lcd->per_sweep.lh_acode[lcd->per_sweep.current_lh] = acode; +/* if (!(acode >> 2 & 1)) // if the skip bit is not set { if (lcd->per_sweep.activeLighthouse != -1) @@ -198,10 +216,12 @@ void handle_lightcap2_sync(SurviveObject * so, LightcapElement * le ) fprintf(stderr, "WARNING: Two non-skip pulses received on the same cycle!\n"); } } + lcd->per_sweep.activeLighthouse = 1; lcd->per_sweep.activeSweepStartTime = le->timestamp; lcd->per_sweep.activeAcode = acode; } + */ } else if (time_since_last_sync > 370000) @@ -210,24 +230,24 @@ void handle_lightcap2_sync(SurviveObject * so, LightcapElement * le ) // first, send out the sync pulse data for the last round (for OOTX decoding { - if (lcd->per_sweep.lh_pulse_len[0] != 0) + if (lcd->per_sweep.lh_max_pulse_length[0] != 0) { so->ctx->lightproc( so, -1, - handle_lightcap2_getAcodeFromSyncPulse(so, lcd->per_sweep.lh_pulse_len[0]), - lcd->per_sweep.lh_pulse_len[0], + handle_lightcap2_getAcodeFromSyncPulse(so, lcd->per_sweep.lh_max_pulse_length[0]), + lcd->per_sweep.lh_max_pulse_length[0], lcd->per_sweep.lh_start_time[0], 0, 0); } - if (lcd->per_sweep.lh_pulse_len[1] != 0) + if (lcd->per_sweep.lh_max_pulse_length[1] != 0) { so->ctx->lightproc( so, -2, - handle_lightcap2_getAcodeFromSyncPulse(so, lcd->per_sweep.lh_pulse_len[1]), - lcd->per_sweep.lh_pulse_len[1], + handle_lightcap2_getAcodeFromSyncPulse(so, lcd->per_sweep.lh_max_pulse_length[1]), + lcd->per_sweep.lh_max_pulse_length[1], lcd->per_sweep.lh_start_time[1], 0, 1); @@ -239,23 +259,27 @@ void handle_lightcap2_sync(SurviveObject * so, LightcapElement * le ) memset(&lcd->per_sweep, 0, sizeof(lcd->per_sweep)); lcd->per_sweep.activeLighthouse = -1; - - lcd->per_sweep.recent_sync_time = le->timestamp; // I do believe we are lighthouse A lcd->per_sweep.current_lh = 0; - lcd->per_sweep.lh_pulse_len[lcd->per_sweep.current_lh] = le->length; +// lcd->per_sweep.lh_pulse_len[lcd->per_sweep.current_lh] = le->length; lcd->per_sweep.lh_start_time[lcd->per_sweep.current_lh] = le->timestamp; + lcd->per_sweep.lh_max_pulse_length[lcd->per_sweep.current_lh] = le->length; + lcd->per_sweep.lh_acode[lcd->per_sweep.current_lh] = acode; - int acode = handle_lightcap2_getAcodeFromSyncPulse(so, le->length); +// int acode = handle_lightcap2_getAcodeFromSyncPulse(so, le->length); +/* if (!(acode >> 2 & 1)) // if the skip bit is not set { lcd->per_sweep.activeLighthouse = 0; lcd->per_sweep.activeSweepStartTime = le->timestamp; lcd->per_sweep.activeAcode = acode; } + */ } + +// printf("%d %d\n", acode, lcd->per_sweep.activeLighthouse ); } void handle_lightcap2_sweep(SurviveObject * so, LightcapElement * le ) @@ -275,6 +299,25 @@ void handle_lightcap2_sweep(SurviveObject * so, LightcapElement * le ) //fprintf(stderr, "%2d %d\n", le->sensor_id, le->length); //fprintf(stderr, "."); + lcd->per_sweep.activeLighthouse = -1; + lcd->per_sweep.activeSweepStartTime = 0; + lcd->per_sweep.activeAcode = 0; + + for (uint8_t i=0; i< NUM_LIGHTHOUSES;++i) { + int acode = lcd->per_sweep.lh_acode[i]; + if ((acode>0) && (!(acode >> 2 & 1))) { + lcd->per_sweep.activeLighthouse = i; + lcd->per_sweep.activeSweepStartTime = lcd->per_sweep.lh_start_time[i]; + lcd->per_sweep.activeAcode = acode; + } + } + + if (lcd->per_sweep.activeLighthouse < 0) { + fprintf(stderr, "WARNING: No active lighthouse!\n"); + fprintf(stderr, " %2d %8d\n", le->sensor_id, le->length); + return; + } + if (lcd->sweep.sweep_len[le->sensor_id] < le->length) { lcd->sweep.sweep_len[le->sensor_id] = le->length; @@ -303,7 +346,8 @@ void handle_lightcap2( SurviveObject * so, LightcapElement * le ) // Should never get a reading so high. Odd. return; } - if (le->length >= 2750) +// if (le->length >= 2750) + if (le->length >= 2500) { // Looks like a sync pulse, process it! handle_lightcap2_sync(so, le); -- cgit v1.2.3 From 96cec93d3c7461587673b5dc35c6fba1e9db899f Mon Sep 17 00:00:00 2001 From: Joshua Allen Date: Sat, 8 Apr 2017 13:36:42 -0400 Subject: clear acode at beginning of pulses. only process valid acodes sweeps. makes HMD work again with single or double lighthouses. --- src/survive_data.c | 18 +++++++++++++----- 1 file changed, 13 insertions(+), 5 deletions(-) (limited to 'src') diff --git a/src/survive_data.c b/src/survive_data.c index f61ae8b..989eb59 100644 --- a/src/survive_data.c +++ b/src/survive_data.c @@ -24,7 +24,7 @@ typedef struct // int lh_pulse_len[NUM_LIGHTHOUSES]; int lh_start_time[NUM_LIGHTHOUSES]; int lh_max_pulse_length[NUM_LIGHTHOUSES]; - uint16_t lh_acode[NUM_LIGHTHOUSES]; + int8_t lh_acode[NUM_LIGHTHOUSES]; int current_lh; // used knowing which sync pulse we're looking at. } lightcap2_per_sweep_data; @@ -96,7 +96,8 @@ void handle_lightcap2_process_sweep_data(SurviveObject *so) { static int counts[SENSORS_PER_OBJECT][2] = {0}; - if (lcd->per_sweep.activeLighthouse == 0 && !allZero) +// if (lcd->per_sweep.activeLighthouse == 0 && !allZero) + if (lcd->per_sweep.activeLighthouse > -1 && !allZero) { if (lcd->sweep.sweep_len[i] != 0) { @@ -226,6 +227,9 @@ void handle_lightcap2_sync(SurviveObject * so, LightcapElement * le ) } else if (time_since_last_sync > 370000) { + // XXX CAUTION: if we lose sight of a lighthouse then, the remaining lighthouse will default to master + //this should probably be fixed. Maybe some kind of timing based guess at which lighthouse. + // looks like this is the first sync pulse. Cool! // first, send out the sync pulse data for the last round (for OOTX decoding @@ -259,6 +263,10 @@ void handle_lightcap2_sync(SurviveObject * so, LightcapElement * le ) memset(&lcd->per_sweep, 0, sizeof(lcd->per_sweep)); lcd->per_sweep.activeLighthouse = -1; + for (uint8_t i=0; i < NUM_LIGHTHOUSES;++i) { + lcd->per_sweep.lh_acode[i] = -1; + } + lcd->per_sweep.recent_sync_time = le->timestamp; // I do believe we are lighthouse A lcd->per_sweep.current_lh = 0; @@ -303,9 +311,9 @@ void handle_lightcap2_sweep(SurviveObject * so, LightcapElement * le ) lcd->per_sweep.activeSweepStartTime = 0; lcd->per_sweep.activeAcode = 0; - for (uint8_t i=0; i< NUM_LIGHTHOUSES;++i) { + for (uint8_t i=0; i < NUM_LIGHTHOUSES;++i) { int acode = lcd->per_sweep.lh_acode[i]; - if ((acode>0) && (!(acode >> 2 & 1))) { + if ( (acode>=0) && !(acode >> 2 & 1)) { lcd->per_sweep.activeLighthouse = i; lcd->per_sweep.activeSweepStartTime = lcd->per_sweep.lh_start_time[i]; lcd->per_sweep.activeAcode = acode; @@ -314,7 +322,7 @@ void handle_lightcap2_sweep(SurviveObject * so, LightcapElement * le ) if (lcd->per_sweep.activeLighthouse < 0) { fprintf(stderr, "WARNING: No active lighthouse!\n"); - fprintf(stderr, " %2d %8d\n", le->sensor_id, le->length); + fprintf(stderr, " %2d %8d %d %d\n", le->sensor_id, le->length,lcd->per_sweep.lh_acode[0],lcd->per_sweep.lh_acode[1]); return; } -- cgit v1.2.3 From e2d553191e8693bb8937aac8b991d9c09e0df6cc Mon Sep 17 00:00:00 2001 From: Joshua Allen Date: Sat, 8 Apr 2017 17:42:28 -0400 Subject: try to remove outliers --- src/survive_data.c | 63 +++++++++++++++++++++++++++++++++++++++++++++++++++--- 1 file changed, 60 insertions(+), 3 deletions(-) (limited to 'src') diff --git a/src/survive_data.c b/src/survive_data.c index 989eb59..647a5d7 100644 --- a/src/survive_data.c +++ b/src/survive_data.c @@ -65,10 +65,64 @@ int handle_lightcap2_getAcodeFromSyncPulse(SurviveObject * so, int pulseLen) if (pulseLen < 6250 - ACODE_OFFSET) return 6; return 7; } + +uint8_t remove_outliers(SurviveObject *so) { + lightcap2_data *lcd = so->disambiguator_data; + + uint32_t sum = 0; + uint8_t non_zero_count = 0; + uint32_t mean = 0; + + for (int i = 0; i < SENSORS_PER_OBJECT; i++) + { + if (lcd->sweep.sweep_len[i] > 0) { + sum += lcd->sweep.sweep_len[i]; + ++non_zero_count; + } + } + + if (non_zero_count==0) return 0; + + mean = sum/non_zero_count; + + float standard_deviation = 0.0f; + sum = 0; + for (int i = 0; i < SENSORS_PER_OBJECT; i++) + { + uint16_t len = lcd->sweep.sweep_len[i]; + if (len > 0) { + sum += (len - mean)*(len - mean); + } + } + standard_deviation = sqrt( ((float)sum)/((float)non_zero_count) ); + +// printf("%f\n", standard_deviation); + + float fake_tao_test = standard_deviation*2; + uint8_t removed_outliers = 0; + + for (int i = 0; i < SENSORS_PER_OBJECT; i++) + { + uint16_t len = lcd->sweep.sweep_len[i]; + if (len == 0) continue; + + if ( abs(len-mean) > fake_tao_test ) + { +// fprintf(stderr, "removing %d\n", len); + lcd->sweep.sweep_len[i] = 0; + removed_outliers = 1; + } + } + + return removed_outliers; +} + void handle_lightcap2_process_sweep_data(SurviveObject *so) { lightcap2_data *lcd = so->disambiguator_data; + while(remove_outliers(so)); + // look at all of the sensors we found, and process the ones that were hit. // TODO: find the sensor(s) with the longest pulse length, and assume // those are the "highest quality". Then, reject any pulses that are sufficiently @@ -93,6 +147,7 @@ void handle_lightcap2_process_sweep_data(SurviveObject *so) // printf("a[%d]l[%d] ", lcd->per_sweep.activeAcode & 5, lcd->per_sweep.activeLighthouse); for (int i = 0; i < SENSORS_PER_OBJECT; i++) { + { static int counts[SENSORS_PER_OBJECT][2] = {0}; @@ -119,10 +174,10 @@ void handle_lightcap2_process_sweep_data(SurviveObject *so) if (lcd->sweep.sweep_len[i] != 0) // if the sensor was hit, process it { - +//printf("%4d\n", lcd->sweep.sweep_len[i]); int offset_from = lcd->sweep.sweep_time[i] - lcd->per_sweep.activeSweepStartTime + lcd->sweep.sweep_len[i] / 2; - if (offset_from < 380000 && offset_from > 70000) +// if (offset_from < 380000 && offset_from > 70000) { //if (longest_pulse *10 / 8 < lcd->sweep.sweep_len[i]) { @@ -134,7 +189,9 @@ void handle_lightcap2_process_sweep_data(SurviveObject *so) //if (!allZero) // printf(" ..:..\n"); +// if (!allZero) printf("\n"); } + // clear out sweep data (could probably limit this to only after a "first" sync. // this is slightly more robust, so doing it here for now. memset(&(((lightcap2_data*)so->disambiguator_data)->sweep), 0, sizeof(lightcaps_sweep_data)); @@ -156,7 +213,7 @@ void handle_lightcap2_sync(SurviveObject * so, LightcapElement * le ) int time_since_last_sync = (le->timestamp - lcd->per_sweep.recent_sync_time); - fprintf(stderr, " %2d %8d %d\n", le->sensor_id, time_since_last_sync, le->length); +// fprintf(stderr, " %2d %8d %d\n", le->sensor_id, time_since_last_sync, le->length); // need to store up sync pulses, so we can take the earliest starting time for all sensors. if (time_since_last_sync < 2400) { -- cgit v1.2.3 From 7686ad97d5f2542338f88e6f6e8055953609651c Mon Sep 17 00:00:00 2001 From: Josh Allen Date: Sun, 9 Apr 2017 08:38:40 -0400 Subject: untested tau test --- src/survive_data.c | 68 ++++++++++++++++++++++++++++++++++++++++++------------ 1 file changed, 53 insertions(+), 15 deletions(-) (limited to 'src') diff --git a/src/survive_data.c b/src/survive_data.c index 647a5d7..8d115fd 100644 --- a/src/survive_data.c +++ b/src/survive_data.c @@ -4,16 +4,25 @@ #include "survive_internal.h" #include #include +#include #define USE_TURVEYBIGUATOR #ifdef USE_TURVEYBIGUATOR +static const float tau_table[33] = { 0, 0, 0, 1.151140982, 1.425, 1.5712213707, 1.656266074, 1.7110275587, 1.7490784054, + 1.7770229476, 1.798410005, 1.8153056661, 1.8289916275, 1.8403044103, 1.8498129961, 1.8579178211, + 1.864908883, 1.8710013691, 1.8763583296, 1.881105575, 1.885341741, 1.8891452542, 1.8925792599, + 1.8956951735, 1.8985352854, 1.9011347009, 1.9035228046, 1.9057243816, 1.9077604832, 1.9096491058, + 1.9114057255, 1.9130437248, 1.914574735 +}; + typedef struct { unsigned int sweep_time[SENSORS_PER_OBJECT]; - unsigned int sweep_len[SENSORS_PER_OBJECT]; + uint16_t sweep_len[SENSORS_PER_OBJECT]; //might want to align this to cache lines, will be hot for frequent access } lightcaps_sweep_data; + typedef struct { int recent_sync_time; @@ -73,12 +82,16 @@ uint8_t remove_outliers(SurviveObject *so) { uint8_t non_zero_count = 0; uint32_t mean = 0; - for (int i = 0; i < SENSORS_PER_OBJECT; i++) + uint16_t* min = NULL; + uint16_t* max = NULL; + uint8_t found_first = 0; + + //future: https://gcc.gnu.org/projects/tree-ssa/vectorization.html#vectorizab + + for (uint8_t i = 0; i < SENSORS_PER_OBJECT; i++) { - if (lcd->sweep.sweep_len[i] > 0) { - sum += lcd->sweep.sweep_len[i]; - ++non_zero_count; - } + sum += lcd->sweep.sweep_len[i]; + if (lcd->sweep.sweep_len[i] > 0) ++non_zero_count; } if (non_zero_count==0) return 0; @@ -87,34 +100,59 @@ uint8_t remove_outliers(SurviveObject *so) { float standard_deviation = 0.0f; sum = 0; - for (int i = 0; i < SENSORS_PER_OBJECT; i++) + for (uint8_t i = 0; i < SENSORS_PER_OBJECT; i++) { uint16_t len = lcd->sweep.sweep_len[i]; if (len > 0) { sum += (len - mean)*(len - mean); + + if (found_first==0) { + max = min = lcd->sweep.sweep_len + i; + found_first=1; + } else { + if(lcd->sweep.sweep_len[i] < *min) min=lcd->sweep.sweep_len + i; + if(lcd->sweep.sweep_len[i] > *max) max=lcd->sweep.sweep_len + i; + } } } - standard_deviation = sqrt( ((float)sum)/((float)non_zero_count) ); + standard_deviation = sqrtf( ((float)sum)/((float)non_zero_count) ); // printf("%f\n", standard_deviation); - float fake_tao_test = standard_deviation*2; - uint8_t removed_outliers = 0; + float tau_test = standard_deviation; + + if (non_zero_count > 2) tau_test = standard_deviation*tau_table[non_zero_count]; - for (int i = 0; i < SENSORS_PER_OBJECT; i++) +// uint8_t removed_outliers = 0; + + uint32_t d1 = *min - mean; + uint32_t d2 = *max - mean; + + if (d1>d2 && d1>tau_test) { + *min = 0; + return 1; + } + else if (d2>tau_test) { + *max = 0; + return 1; + } + + return 0; +/* + for (uint8_t i = 0; i < SENSORS_PER_OBJECT; i++) { uint16_t len = lcd->sweep.sweep_len[i]; if (len == 0) continue; - if ( abs(len-mean) > fake_tao_test ) + if ( abs(len-mean) > tau_test ) { // fprintf(stderr, "removing %d\n", len); lcd->sweep.sweep_len[i] = 0; removed_outliers = 1; } } - - return removed_outliers; +*/ +// return removed_outliers; } void handle_lightcap2_process_sweep_data(SurviveObject *so) @@ -250,7 +288,7 @@ void handle_lightcap2_sync(SurviveObject * so, LightcapElement * le ) } else if (time_since_last_sync < 24000) { - lcd->per_sweep.activeLighthouse != -1; + lcd->per_sweep.activeLighthouse = -1; lcd->per_sweep.recent_sync_time = le->timestamp; // I do believe we are lighthouse B -- cgit v1.2.3 From 8dee94008fec1506a3a2f58292a45e1f93b6d333 Mon Sep 17 00:00:00 2001 From: Josh Allen Date: Sun, 9 Apr 2017 08:51:29 -0400 Subject: should be absolute value if statement should not be combined --- src/survive_data.c | 12 +++++++----- 1 file changed, 7 insertions(+), 5 deletions(-) (limited to 'src') diff --git a/src/survive_data.c b/src/survive_data.c index 8d115fd..22ce8c2 100644 --- a/src/survive_data.c +++ b/src/survive_data.c @@ -125,12 +125,14 @@ uint8_t remove_outliers(SurviveObject *so) { // uint8_t removed_outliers = 0; - uint32_t d1 = *min - mean; - uint32_t d2 = *max - mean; + uint32_t d1 = abs(*min - mean); + uint32_t d2 = abs(*max - mean); - if (d1>d2 && d1>tau_test) { - *min = 0; - return 1; + if (d1>d2) { + if (d1 > tau_test) { + *min = 0; + return 1; + } } else if (d2>tau_test) { *max = 0; -- cgit v1.2.3 From 6423c27236d7ee3bfaac293990ef78779507bb02 Mon Sep 17 00:00:00 2001 From: mwturvey Date: Tue, 11 Apr 2017 10:57:58 -0700 Subject: Perf: Take advantage of past poses for future estimates --- src/poser_turveytori.c | 36 ++++++++++++++++++++++++++++++++++++ 1 file changed, 36 insertions(+) (limited to 'src') diff --git a/src/poser_turveytori.c b/src/poser_turveytori.c index c251040..5ad5c1b 100644 --- a/src/poser_turveytori.c +++ b/src/poser_turveytori.c @@ -83,6 +83,9 @@ typedef struct FLT oldAngles[SENSORS_PER_OBJECT][2][NUM_LIGHTHOUSES][OLD_ANGLES_BUFF_LEN]; // sensor, sweep axis, lighthouse, instance int angleIndex[NUM_LIGHTHOUSES][2]; // index into circular buffer ahead. separate index for each axis. int lastAxis[NUM_LIGHTHOUSES]; + + Point lastLhPos[NUM_LIGHTHOUSES]; + FLT lastLhRotAxisAngle[NUM_LIGHTHOUSES][4]; } ToriData; @@ -1144,6 +1147,8 @@ void SolveForRotation(FLT rotOut[4], TrackedObject *obj, Point lh) static Point SolveForLighthouse(FLT posOut[3], FLT quatOut[4], TrackedObject *obj, SurviveObject *so, char doLogOutput, int lh, int setLhCalibration) { + ToriData *toriData = so->PoserData; + //printf("Solving for Lighthouse\n"); //printf("obj->numSensors = %d;\n", obj->numSensors); @@ -1234,6 +1239,14 @@ static Point SolveForLighthouse(FLT posOut[3], FLT quatOut[4], TrackedObject *ob // back into the search for the correct point (see "if (a1 > M_PI / 2)" below) Point p1 = getNormalizedAndScaledVector(avgNorm, 8); + // if the last lighthouse position has been populated (extremely rare it would be 0) + if (toriData->lastLhPos[lh].x != 0) + { + p1.x = toriData->lastLhPos[lh].x; + p1.y = toriData->lastLhPos[lh].y; + p1.z = toriData->lastLhPos[lh].z; + } + Point refinedEstimateGd = RefineEstimateUsingModifiedGradientDescent1(p1, pna, pnaCount, logFile); FLT pf1[3] = { refinedEstimateGd.x, refinedEstimateGd.y, refinedEstimateGd.z }; @@ -1258,11 +1271,29 @@ static Point SolveForLighthouse(FLT posOut[3], FLT quatOut[4], TrackedObject *ob //printf("Distance is %f, Fitness is %f\n", distance, fitGd); FLT rot[4]; // this is axis/ angle rotation, not a quaternion! + + if (toriData->lastLhRotAxisAngle[lh][0] != 0) + { + rot[0] = toriData->lastLhRotAxisAngle[lh][0]; + rot[1] = toriData->lastLhRotAxisAngle[lh][1]; + rot[2] = toriData->lastLhRotAxisAngle[lh][2]; + rot[3] = toriData->lastLhRotAxisAngle[lh][3]; + } + + SolveForRotation(rot, obj, refinedEstimateGd); FLT objPos[3]; + { + toriData->lastLhRotAxisAngle[lh][0] = rot[0]; + toriData->lastLhRotAxisAngle[lh][1] = rot[1]; + toriData->lastLhRotAxisAngle[lh][2] = rot[2]; + toriData->lastLhRotAxisAngle[lh][3] = rot[3]; + } + WhereIsTheTrackedObjectAxisAngle(objPos, rot, refinedEstimateGd); + FLT rotQuat[4]; quatfromaxisangle(rotQuat, rot, rot[3]); @@ -1325,6 +1356,11 @@ static Point SolveForLighthouse(FLT posOut[3], FLT quatOut[4], TrackedObject *ob fclose(logFile); } + + toriData->lastLhPos[lh].x = refinedEstimateGd.x; + toriData->lastLhPos[lh].y = refinedEstimateGd.y; + toriData->lastLhPos[lh].z = refinedEstimateGd.z; + return refinedEstimateGd; } -- cgit v1.2.3 From 30531472d0afccdb3b06b2e8fe8894ef4f9440be Mon Sep 17 00:00:00 2001 From: mwturvey Date: Tue, 11 Apr 2017 11:59:06 -0700 Subject: Improve performance of gradient calculation --- src/poser_turveytori.c | 10 ++++++---- 1 file changed, 6 insertions(+), 4 deletions(-) (limited to 'src') diff --git a/src/poser_turveytori.c b/src/poser_turveytori.c index 5ad5c1b..c526161 100644 --- a/src/poser_turveytori.c +++ b/src/poser_turveytori.c @@ -436,23 +436,25 @@ Point getGradient(Point pointIn, PointsAndAngle *pna, size_t pnaCount, FLT preci { Point result; + FLT baseFitness = getPointFitness(pointIn, pna, pnaCount, 0); + Point tmpXplus = pointIn; Point tmpXminus = pointIn; tmpXplus.x = pointIn.x + precision; tmpXminus.x = pointIn.x - precision; - result.x = getPointFitness(tmpXplus, pna, pnaCount, 0) - getPointFitness(tmpXminus, pna, pnaCount, 0); + result.x = baseFitness - getPointFitness(tmpXminus, pna, pnaCount, 0); Point tmpYplus = pointIn; Point tmpYminus = pointIn; tmpYplus.y = pointIn.y + precision; tmpYminus.y = pointIn.y - precision; - result.y = getPointFitness(tmpYplus, pna, pnaCount, 0) - getPointFitness(tmpYminus, pna, pnaCount, 0); + result.y = baseFitness - getPointFitness(tmpYminus, pna, pnaCount, 0); Point tmpZplus = pointIn; Point tmpZminus = pointIn; tmpZplus.z = pointIn.z + precision; tmpZminus.z = pointIn.z - precision; - result.z = getPointFitness(tmpZplus, pna, pnaCount, 0) - getPointFitness(tmpZminus, pna, pnaCount, 0); + result.z = baseFitness - getPointFitness(tmpZminus, pna, pnaCount, 0); return result; } @@ -1518,7 +1520,7 @@ int PoserTurveyTori( SurviveObject * so, PoserData * poserData ) counter++; // let's just do this occasionally for now... - if (counter % 2 == 0) + //if (counter % 1 == 0) QuickPose(so, 0); } // axis changed, time to increment the circular buffer index. -- cgit v1.2.3 From 0b16cebf7ca20da43da99cf9d59ab9ed8a3c205f Mon Sep 17 00:00:00 2001 From: Mike Turvey Date: Tue, 11 Apr 2017 17:38:15 -0700 Subject: Better messaging when finding a device --- src/survive_vive.c | 3 ++- 1 file changed, 2 insertions(+), 1 deletion(-) (limited to 'src') diff --git a/src/survive_vive.c b/src/survive_vive.c index 8e22ea1..3ce985d 100755 --- a/src/survive_vive.c +++ b/src/survive_vive.c @@ -54,7 +54,7 @@ const short vidpids[] = { const char * devnames[] = { "HMD", - "Lighthouse", + "HMD IMU & LH", "Watchman 1", "Watchman 2", "Tracker 0", @@ -370,6 +370,7 @@ int survive_usb_init( SurviveViveData * sv, SurviveObject * hmd, SurviveObject * wchar_t wstr[255]; res = hid_get_serial_number_string(handle, wstr, 255); + printf("Found %s. ", devnames[i]); wprintf(L"Serial Number String: (%d) %s for %04x:%04x@%d (Dev: %p)\n", wstr[0], wstr,vendor_id, product_id, menum, handle); sv->udev[i] = handle; -- cgit v1.2.3 From 4b842a1a73a2713726c3d999ab4b6886e7e9fb69 Mon Sep 17 00:00:00 2001 From: Mike Turvey Date: Tue, 11 Apr 2017 17:42:04 -0700 Subject: Disabling remove_outliers because it's removing too many non-outliers --- src/survive_data.c | 1 + 1 file changed, 1 insertion(+) (limited to 'src') diff --git a/src/survive_data.c b/src/survive_data.c index 647a5d7..0273532 100644 --- a/src/survive_data.c +++ b/src/survive_data.c @@ -67,6 +67,7 @@ int handle_lightcap2_getAcodeFromSyncPulse(SurviveObject * so, int pulseLen) } uint8_t remove_outliers(SurviveObject *so) { + return 0; // disabling this for now because it seems remove almost all the points for wired watchman and wired tracker. lightcap2_data *lcd = so->disambiguator_data; uint32_t sum = 0; -- cgit v1.2.3 From a7d4ccbde10be9c14ae690d3c9a21627195cfaa5 Mon Sep 17 00:00:00 2001 From: "Dr. Orion Lawlor" Date: Mon, 17 Apr 2017 21:42:05 -0800 Subject: Fix warnings by including --- src/survive_data.c | 1 + 1 file changed, 1 insertion(+) (limited to 'src') diff --git a/src/survive_data.c b/src/survive_data.c index 0273532..7b9d051 100644 --- a/src/survive_data.c +++ b/src/survive_data.c @@ -4,6 +4,7 @@ #include "survive_internal.h" #include #include +#include /* for sqrt */ #define USE_TURVEYBIGUATOR -- cgit v1.2.3 From 1a6f5f8316b2dfa1baa923f5ef109b7a4890300f Mon Sep 17 00:00:00 2001 From: cnlohr Date: Sat, 29 Apr 2017 17:44:22 -0400 Subject: Fix survive_vive's accidental collection of code 0xfe data elements. --- src/survive_vive.c | 4 ++-- 1 file changed, 2 insertions(+), 2 deletions(-) (limited to 'src') diff --git a/src/survive_vive.c b/src/survive_vive.c index 3ce985d..4acc4e4 100755 --- a/src/survive_vive.c +++ b/src/survive_vive.c @@ -1165,7 +1165,7 @@ void survive_data_cb( SurviveUSBInterface * si ) le.sensor_id = POP1; le.length = POP2; le.timestamp = POP4; - if( le.sensor_id == 0xff ) break; + if( le.sensor_id > 0xfd ) break; handle_lightcap( obj, &le ); } break; @@ -1183,7 +1183,7 @@ void survive_data_cb( SurviveUSBInterface * si ) le.sensor_id = (uint8_t)POP2; le.length = POP2; le.timestamp = POP4; - if( le.sensor_id == 0xff ) break; + if( le.sensor_id > 0xfd ) break; handle_lightcap( obj, &le ); } break; -- cgit v1.2.3 From 155124e6ac5cba02bb6370779ed3cc35bccd9d01 Mon Sep 17 00:00:00 2001 From: cnlohr Date: Sun, 30 Apr 2017 00:10:55 -0400 Subject: Fix mistaken edit to survive_vive which dropped rest of packet when invalid sensor was found. Feels odd that this is actually an issue. --- src/survive_vive.c | 4 ++-- 1 file changed, 2 insertions(+), 2 deletions(-) (limited to 'src') diff --git a/src/survive_vive.c b/src/survive_vive.c index 4acc4e4..398ff99 100755 --- a/src/survive_vive.c +++ b/src/survive_vive.c @@ -1165,7 +1165,7 @@ void survive_data_cb( SurviveUSBInterface * si ) le.sensor_id = POP1; le.length = POP2; le.timestamp = POP4; - if( le.sensor_id > 0xfd ) break; + if( le.sensor_id > 0xfd ) continue; handle_lightcap( obj, &le ); } break; @@ -1183,7 +1183,7 @@ void survive_data_cb( SurviveUSBInterface * si ) le.sensor_id = (uint8_t)POP2; le.length = POP2; le.timestamp = POP4; - if( le.sensor_id > 0xfd ) break; + if( le.sensor_id > 0xfd ) continue; // handle_lightcap( obj, &le ); } break; -- cgit v1.2.3 From f177f87e98637de8f07488d820390f322c45413c Mon Sep 17 00:00:00 2001 From: cnlohr Date: Sun, 30 Apr 2017 00:13:42 -0400 Subject: Update the charles disambiguator --- src/survive_data.c | 53 ++++++++++++++++++++++++++++++++++++++--------------- 1 file changed, 38 insertions(+), 15 deletions(-) (limited to 'src') diff --git a/src/survive_data.c b/src/survive_data.c index 7b9d051..0e2e828 100644 --- a/src/survive_data.c +++ b/src/survive_data.c @@ -6,7 +6,7 @@ #include #include /* for sqrt */ -#define USE_TURVEYBIGUATOR +//#define USE_TURVEYBIGUATOR #ifdef USE_TURVEYBIGUATOR @@ -449,6 +449,7 @@ void handle_lightcap( SurviveObject * so, LightcapElement * le ) return; #else + //printf( "LE%3d%6d%12d\n", le->sensor_id, le->length, le->timestamp ); //int32_t deltat = (uint32_t)le->timestamp - (uint32_t)so->last_master_time; @@ -482,6 +483,9 @@ void handle_lightcap( SurviveObject * so, LightcapElement * le ) //unified driver. int ssn = so->sync_set_number; //lighthouse number if( ssn < 0 ) ssn = 0; +#ifdef DEBUG + if( ssn >= NUM_LIGHTHOUSES ) { SV_INFO( "ALGORITHMIC WARNING: ssn exceeds NUM_LIGHTHOUSES" ); } +#endif int last_sync_time = so->last_sync_time [ssn]; int last_sync_length = so->last_sync_length[ssn]; int32_t delta = le->timestamp - last_sync_time; //Handle time wrapping (be sure to be int32) @@ -498,16 +502,29 @@ void handle_lightcap( SurviveObject * so, LightcapElement * le ) if( le->length > so->pulselength_min_sync ) //Pulse longer indicates a sync pulse. { int is_new_pulse = delta > so->pulselength_min_sync /*1500*/ + last_sync_length; - + //printf( "INP: %d %d\n", is_new_pulse, so->sync_set_number ); so->did_handle_ootx = 0; if( is_new_pulse ) { int is_master_sync_pulse = delta > so->pulse_in_clear_time /*40000*/; + int is_pulse_from_same_lh_as_last_sweep; + int tp = delta % ( so->timecenter_ticks * 2); + is_pulse_from_same_lh_as_last_sweep = tp < so->pulse_synctime_slack && tp > -so->pulse_synctime_slack; - if( is_master_sync_pulse ) + if( is_master_sync_pulse ) //Could also be called by slave if no master was seen. { - ssn = so->sync_set_number = 0; + ssn = so->sync_set_number = is_pulse_from_same_lh_as_last_sweep?(so->sync_set_number):0; //If repeated lighthouse, just back off one. + if( ssn < 0 ) { SV_INFO( "SEVERE WARNING: Pulse codes for tracking not able to be backed out.\n" ); ssn = 0; } + if( ssn != 0 ) + { + //If it's the slave that is repeated, be sure to zero out its sync info. + so->last_sync_length[0] = 0; + } + else + { + so->last_sync_length[1] = 0; + } so->last_sync_time[ssn] = le->timestamp; so->last_sync_length[ssn] = le->length; } @@ -544,15 +561,19 @@ void handle_lightcap( SurviveObject * so, LightcapElement * le ) } } - //Extra tidbit for storing length-of-sync-pulses. +#if 0 + //Extra tidbit for storing length-of-sync-pulses, if you want to try to use this to determine AoI or distance to LH. { int32_t main_divisor = so->timebase_hz / 384000; //125 @ 48 MHz. int base_station = is_new_pulse; - //printf( "%s %d %d %d\n", so->codename, le->sensor_id, so->sync_set_number, le->length ); - ctx->lightproc( so, le->sensor_id, -3 - so->sync_set_number, 0, le->timestamp, le->length, base_station); + printf( "%s %d %d %d\n", so->codename, le->sensor_id, so->sync_set_number, le->length ); //XXX sync_set_number is wrong here. + ctx->lightproc( so, le->sensor_id, -3 - so->sync_set_number, 0, le->timestamp, le->length, base_station); //XXX sync_set_number is wrong here. } +#endif } + //Any else- statements below here are + //See if this is a valid actual pulse. else if( le->length < so->pulse_max_for_sweep && delta > so->pulse_in_clear_time && ssn >= 0 ) { @@ -576,9 +597,11 @@ void handle_lightcap( SurviveObject * so, LightcapElement * le ) decode_acode(so->last_sync_length[1],main_divisor) }; + //printf( "%d %d\n", acode_array[0], acode_array[1] ); + //XXX: TODO: Capture error count here. - if( acode_array[0] < 0 ) return; - if( acode_array[1] < 0 ) return; + //if( acode_array[0] < 0 ) return; + //if( acode_array[1] < 0 ) return; int acode = acode_array[0]; @@ -587,13 +610,14 @@ void handle_lightcap( SurviveObject * so, LightcapElement * le ) int32_t delta1 = so->last_sync_time[0] - so->recent_sync_time; int32_t delta2 = so->last_sync_time[1] - so->last_sync_time[0]; - ctx->lightproc( so, -1, acode_array[0], delta1, so->last_sync_time[0], so->last_sync_length[0], 0 ); - ctx->lightproc( so, -2, acode_array[1], delta2, so->last_sync_time[1], so->last_sync_length[1], 1 ); + //printf( "%p %p %d %d %d %p\n", ctx, so, so->last_sync_time[0], acode_array, so->last_sync_length[0], ctx->lightproc ); + if( acode_array[0] >= 0 ) ctx->lightproc( so, -1, acode_array[0], delta1, so->last_sync_time[0], so->last_sync_length[0], 0 ); + if( acode_array[1] >= 0 ) ctx->lightproc( so, -2, acode_array[1], delta2, so->last_sync_time[1], so->last_sync_length[1], 1 ); so->recent_sync_time = so->last_sync_time[1]; //Throw out everything if our sync pulses look like they're bad. - +/* 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; @@ -611,7 +635,7 @@ void handle_lightcap( SurviveObject * so, LightcapElement * le ) so->sync_set_number = -1; return; } - +*/ so->did_handle_ootx = 1; } @@ -628,7 +652,7 @@ void handle_lightcap( SurviveObject * so, LightcapElement * le ) int32_t offset_from = le->timestamp - dl + le->length/2; //Make sure pulse is in valid window - if( offset_from < 380000 && offset_from > 70000 ) + if( offset_from < so->timecenter_ticks*2-so->pulse_in_clear_time && offset_from > so->pulse_in_clear_time && acode >= 0 ) { ctx->lightproc( so, le->sensor_id, acode, offset_from, le->timestamp, le->length, !(acode>>2) ); } @@ -639,7 +663,6 @@ void handle_lightcap( SurviveObject * so, LightcapElement * le ) //Runt pulse, or no sync pulses available. } #endif - } -- cgit v1.2.3 From 947dfaff73c0b758a21a410776543340e732d705 Mon Sep 17 00:00:00 2001 From: cnlohr Date: Sun, 30 Apr 2017 00:46:57 -0400 Subject: Fix charles' disambiguator to work with one lighthouse and @axlecrusher's OOTX reader. --- src/survive_data.c | 125 ++++++++++++++++++++++++++++------------------------- 1 file changed, 65 insertions(+), 60 deletions(-) (limited to 'src') diff --git a/src/survive_data.c b/src/survive_data.c index 0e2e828..4f94106 100644 --- a/src/survive_data.c +++ b/src/survive_data.c @@ -432,13 +432,59 @@ void handle_lightcap2( SurviveObject * so, LightcapElement * le ) int32_t decode_acode(uint32_t length, int32_t main_divisor) { //+50 adds a small offset and seems to help always get it right. //Check the +50 in the future to see how well this works on a variety of hardware. - + if( !main_divisor ) return -1; int32_t acode = (length+main_divisor+50)/(main_divisor*2); if( acode & 1 ) return -1; return (acode>>1) - 6; } + +void HandleOOTX( SurviveContext * ctx, SurviveObject * so ) +{ + int32_t main_divisor = so->timebase_hz / 384000; //125 @ 48 MHz. + + int32_t acode_array[2] = + { + decode_acode(so->last_sync_length[0],main_divisor), + decode_acode(so->last_sync_length[1],main_divisor) + }; + + + int32_t delta1 = so->last_sync_time[0] - so->recent_sync_time; + int32_t delta2 = so->last_sync_time[1] - so->last_sync_time[0]; + + //printf( "%p %p %d %d %d %p\n", ctx, so, so->last_sync_time[0], acode_array, so->last_sync_length[0], ctx->lightproc ); + if( acode_array[0] >= 0 ) ctx->lightproc( so, -1, acode_array[0], delta1, so->last_sync_time[0], so->last_sync_length[0], 0 ); + if( acode_array[1] >= 0 ) ctx->lightproc( so, -2, acode_array[1], delta2, so->last_sync_time[1], so->last_sync_length[1], 1 ); + + so->recent_sync_time = so->last_sync_time[1]; + +/* + //Throw out everything if our sync pulses look like they're bad. + //This actually doesn't seem to hold anymore, now that we're looking for multiple LHs. + 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 < center_2 - slack || delta2 > center_2 + slack ) + { + //XXX: TODO: Count faults. + so->sync_set_number = -1; + return; + } +*/ + so->did_handle_ootx = 1; +} + + //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 ) { @@ -502,9 +548,18 @@ void handle_lightcap( SurviveObject * so, LightcapElement * le ) if( le->length > so->pulselength_min_sync ) //Pulse longer indicates a sync pulse. { int is_new_pulse = delta > so->pulselength_min_sync /*1500*/ + last_sync_length; - //printf( "INP: %d %d\n", is_new_pulse, so->sync_set_number ); + + //TRICKY: If we didn't see anything from the other lighthouse, we might just not see it... But, we still have to send our sync + //information to the rest of libsurvive. This could be turned into a function and combined with the code below. + if( !so->did_handle_ootx && is_new_pulse ) + { + HandleOOTX( ctx, so ); + } so->did_handle_ootx = 0; + + //printf( "INP: %d %d\n", is_new_pulse, so->sync_set_number ); + if( is_new_pulse ) { int is_master_sync_pulse = delta > so->pulse_in_clear_time /*40000*/; @@ -563,6 +618,7 @@ void handle_lightcap( SurviveObject * so, LightcapElement * le ) #if 0 //Extra tidbit for storing length-of-sync-pulses, if you want to try to use this to determine AoI or distance to LH. + //We don't actually use this anywhere, and I doubt we ever will? Though, it could be useful at a later time to improve tracking. { int32_t main_divisor = so->timebase_hz / 384000; //125 @ 48 MHz. int base_station = is_new_pulse; @@ -590,71 +646,20 @@ void handle_lightcap( SurviveObject * so, LightcapElement * le ) //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] = - { - decode_acode(so->last_sync_length[0],main_divisor), - decode_acode(so->last_sync_length[1],main_divisor) - }; - - //printf( "%d %d\n", acode_array[0], acode_array[1] ); - - //XXX: TODO: Capture error count here. - //if( acode_array[0] < 0 ) return; - //if( acode_array[1] < 0 ) return; - - int acode = acode_array[0]; + int acode = decode_acode(so->last_sync_length[0],main_divisor); if( !so->did_handle_ootx ) - { - int32_t delta1 = so->last_sync_time[0] - so->recent_sync_time; - int32_t delta2 = so->last_sync_time[1] - so->last_sync_time[0]; - - //printf( "%p %p %d %d %d %p\n", ctx, so, so->last_sync_time[0], acode_array, so->last_sync_length[0], ctx->lightproc ); - if( acode_array[0] >= 0 ) ctx->lightproc( so, -1, acode_array[0], delta1, so->last_sync_time[0], so->last_sync_length[0], 0 ); - if( acode_array[1] >= 0 ) ctx->lightproc( so, -2, acode_array[1], delta2, so->last_sync_time[1], so->last_sync_length[1], 1 ); - - so->recent_sync_time = so->last_sync_time[1]; - - //Throw out everything if our sync pulses look like they're bad. -/* - 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 < center_2 - slack || delta2 > center_2 + slack ) - { - //XXX: TODO: Count faults. - so->sync_set_number = -1; - return; - } -*/ - so->did_handle_ootx = 1; - } - - if (acode > 3) { - if( ssn == 0 ) - { - //SV_INFO( "Warning: got a slave marker but only got a master sync." ); - //This happens too frequently. Consider further examination. - } - dl = so->last_sync_time[1]; - tpco = so->last_sync_length[1]; - } + HandleOOTX( ctx, so ); int32_t offset_from = le->timestamp - dl + le->length/2; //Make sure pulse is in valid window - if( offset_from < so->timecenter_ticks*2-so->pulse_in_clear_time && offset_from > so->pulse_in_clear_time && acode >= 0 ) + if( offset_from < so->timecenter_ticks*2-so->pulse_in_clear_time && offset_from > so->pulse_in_clear_time ) { - ctx->lightproc( so, le->sensor_id, acode, offset_from, le->timestamp, le->length, !(acode>>2) ); + int whichlh; + if( acode < 0 ) whichlh = 1; + else whichlh = !(acode>>2); + ctx->lightproc( so, le->sensor_id, acode, offset_from, le->timestamp, le->length, whichlh ); } } else -- cgit v1.2.3 From ba11bb2755dd1dddf6f82893b897513d2e387239 Mon Sep 17 00:00:00 2001 From: Mike Turvey Date: Sun, 30 Apr 2017 10:37:45 -0700 Subject: Fix regression that broke support for wireless controllers on Windows --- src/survive_vive.c | 3 ++- 1 file changed, 2 insertions(+), 1 deletion(-) (limited to 'src') diff --git a/src/survive_vive.c b/src/survive_vive.c index 3ce985d..8f2a42e 100755 --- a/src/survive_vive.c +++ b/src/survive_vive.c @@ -344,7 +344,8 @@ int survive_usb_init( SurviveViveData * sv, SurviveObject * hmd, SurviveObject * if (cur_dev->vendor_id == vendor_id && cur_dev->product_id == product_id) { - if( cur_dev->interface_number == enumid ) + if( cur_dev->interface_number == enumid || + cur_dev->interface_number == -1 && menum == enumid) { path_to_open = cur_dev->path; break; -- cgit v1.2.3 From 3560f3c7e397c0dd2bcadaeb028b8a9fdcb17096 Mon Sep 17 00:00:00 2001 From: Michael Turvey Date: Sun, 30 Apr 2017 22:22:55 -0700 Subject: Reduce Poser Calls I've been seeing starvation that manifests itself as weird light data coming into the disambiguator. A major cause ended up being starvation. This change will cut the frequency of poser calls by by 75% --- src/poser_turveytori.c | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) (limited to 'src') diff --git a/src/poser_turveytori.c b/src/poser_turveytori.c index c526161..7abf5d0 100644 --- a/src/poser_turveytori.c +++ b/src/poser_turveytori.c @@ -1520,7 +1520,7 @@ int PoserTurveyTori( SurviveObject * so, PoserData * poserData ) counter++; // let's just do this occasionally for now... - //if (counter % 1 == 0) + if (counter % 4 == 0) QuickPose(so, 0); } // axis changed, time to increment the circular buffer index. -- cgit v1.2.3 From d53ef13d5f8154d225c9f763131129d179db2441 Mon Sep 17 00:00:00 2001 From: Joshua Allen Date: Fri, 5 May 2017 15:50:41 -0400 Subject: calibration values from MPU-6500 datasheet. I'm pretty sure the accelerometer values are correct. The gyroscope values may not be correct. --- src/survive_vive.c | 11 +++++++++++ 1 file changed, 11 insertions(+) (limited to 'src') diff --git a/src/survive_vive.c b/src/survive_vive.c index 030db8a..88b9fee 100755 --- a/src/survive_vive.c +++ b/src/survive_vive.c @@ -1127,6 +1127,17 @@ void survive_data_cb( SurviveUSBInterface * si ) acceldata[3], acceldata[4], acceldata[5], 0,0,0 }; + //1G for accelerometer, from MPU6500 datasheet + //this can change if the firmware changes the sensitivity. + agm[0]/=8192.0f; + agm[1]/=8192.0f; + agm[2]/=8192.0f; + + //1000 deg/s for gyroscope, from MPU6500 datasheet + agm[3]/=65.5f; + agm[4]/=65.5f; + agm[5]/=65.5f; + ctx->imuproc( obj, 3, agm, timecode, code ); } } -- cgit v1.2.3 From 730bbd51f9aa0f0c6b57e9607d2b682ea1bf5ad2 Mon Sep 17 00:00:00 2001 From: Joshua Allen Date: Fri, 5 May 2017 15:52:26 -0400 Subject: fix comment --- src/survive_vive.c | 1 + 1 file changed, 1 insertion(+) (limited to 'src') diff --git a/src/survive_vive.c b/src/survive_vive.c index 88b9fee..1f6a975 100755 --- a/src/survive_vive.c +++ b/src/survive_vive.c @@ -1133,6 +1133,7 @@ void survive_data_cb( SurviveUSBInterface * si ) agm[1]/=8192.0f; agm[2]/=8192.0f; + //65.5 deg/s for gyroscope, from MPU6500 datasheet //1000 deg/s for gyroscope, from MPU6500 datasheet agm[3]/=65.5f; agm[4]/=65.5f; -- cgit v1.2.3 From 6c748afc397f29cd461e54c09d8a3cb45272f4ad Mon Sep 17 00:00:00 2001 From: Joshua Allen Date: Sat, 6 May 2017 22:16:12 -0400 Subject: use calibration numbers from device json files --- src/survive_vive.c | 77 ++++++++++++++++++++++++++++++++++++++++++++++++++++++ 1 file changed, 77 insertions(+) (limited to 'src') diff --git a/src/survive_vive.c b/src/survive_vive.c index 1f6a975..720df77 100755 --- a/src/survive_vive.c +++ b/src/survive_vive.c @@ -22,6 +22,8 @@ #include // for alloca #endif +#include "json_helpers.h" + #ifdef HIDAPI #if defined(WINDOWS) || defined(WIN32) || defined (_WIN32) #include @@ -1057,6 +1059,34 @@ static void handle_watchman( SurviveObject * w, uint8_t * readdata ) } } +void calibrate_acc(SurviveObject* so, FLT* agm) { + if (so->acc_bias != NULL) { + agm[0] -= so->acc_bias[0]; + agm[1] -= so->acc_bias[1]; + agm[2] -= so->acc_bias[2]; + } + + if (so->acc_scale != NULL) { + agm[0] *= so->acc_scale[0]; + agm[1] *= so->acc_scale[1]; + agm[2] *= so->acc_scale[2]; + } +} + +void calibrate_gyro(SurviveObject* so, FLT* agm) { + if (so->gyro_bias != NULL) { + agm[0] -= so->gyro_bias[0]; + agm[1] -= so->gyro_bias[1]; + agm[2] -= so->gyro_bias[2]; + } + + if (so->gyro_bias != NULL) { + agm[0] *= so->gyro_scale[0]; + agm[1] *= so->gyro_scale[1]; + agm[2] *= so->gyro_scale[2]; + } +} + void survive_data_cb( SurviveUSBInterface * si ) { @@ -1127,12 +1157,16 @@ void survive_data_cb( SurviveUSBInterface * si ) acceldata[3], acceldata[4], acceldata[5], 0,0,0 }; + calibrate_acc(obj, agm); + //1G for accelerometer, from MPU6500 datasheet //this can change if the firmware changes the sensitivity. agm[0]/=8192.0f; agm[1]/=8192.0f; agm[2]/=8192.0f; + calibrate_gyro(obj, agm+3); + //65.5 deg/s for gyroscope, from MPU6500 datasheet //1000 deg/s for gyroscope, from MPU6500 datasheet agm[3]/=65.5f; @@ -1322,6 +1356,37 @@ printf( "Loading config: %d\n", len ); break; } } + + + if (jsoneq(ct0conf, tk, "acc_bias") == 0) { + int32_t count = (tk+1)->size; + FLT* values = NULL; + if ( parse_float_array(ct0conf, tk+2, &values, count) >0 ) { + so->acc_bias = values; + } + } + if (jsoneq(ct0conf, tk, "acc_scale") == 0) { + int32_t count = (tk+1)->size; + FLT* values = NULL; + if ( parse_float_array(ct0conf, tk+2, &values, count) >0 ) { + so->acc_scale = values; + } + } + + if (jsoneq(ct0conf, tk, "gyro_bias") == 0) { + int32_t count = (tk+1)->size; + FLT* values = NULL; + if ( parse_float_array(ct0conf, tk+2, &values, count) >0 ) { + so->gyro_bias = values; + } + } + if (jsoneq(ct0conf, tk, "gyro_scale") == 0) { + int32_t count = (tk+1)->size; + FLT* values = NULL; + if ( parse_float_array(ct0conf, tk+2, &values, count) >0 ) { + so->gyro_scale = values; + } + } } } else @@ -1361,6 +1426,12 @@ int survive_vive_close( SurviveContext * ctx, void * driver ) return 0; } +void init_SurviveObject(SurviveObject* so) { + so->acc_scale = NULL; + so->acc_bias = NULL; + so->gyro_scale = NULL; + so->gyro_bias = NULL; +} int DriverRegHTCVive( SurviveContext * ctx ) { @@ -1372,6 +1443,12 @@ int DriverRegHTCVive( SurviveContext * ctx ) SurviveObject * ww0 = calloc( 1, sizeof( SurviveObject ) ); SurviveViveData * sv = calloc( 1, sizeof( SurviveViveData ) ); + init_SurviveObject(hmd); + init_SurviveObject(wm0); + init_SurviveObject(wm1); + init_SurviveObject(tr0); + init_SurviveObject(ww0); + sv->ctx = ctx; #ifdef _WIN32 -- cgit v1.2.3 From a93ea0625daa2024ca20e04f1aa6da1d0ecf018c Mon Sep 17 00:00:00 2001 From: Joshua Allen Date: Sat, 6 May 2017 22:19:14 -0400 Subject: check proper pointer for null --- src/survive_vive.c | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) (limited to 'src') diff --git a/src/survive_vive.c b/src/survive_vive.c index 720df77..95e688e 100755 --- a/src/survive_vive.c +++ b/src/survive_vive.c @@ -1080,7 +1080,7 @@ void calibrate_gyro(SurviveObject* so, FLT* agm) { agm[2] -= so->gyro_bias[2]; } - if (so->gyro_bias != NULL) { + if (so->gyro_scale != NULL) { agm[0] *= so->gyro_scale[0]; agm[1] *= so->gyro_scale[1]; agm[2] *= so->gyro_scale[2]; -- cgit v1.2.3 From 85eaaff4d295771d28e11e80ae7f27d60eef0ed6 Mon Sep 17 00:00:00 2001 From: cnlohr Date: Sat, 6 May 2017 23:54:04 -0400 Subject: Tweak calibration values. --- src/survive_vive.c | 17 +++++++++++------ 1 file changed, 11 insertions(+), 6 deletions(-) (limited to 'src') diff --git a/src/survive_vive.c b/src/survive_vive.c index 95e688e..c29465e 100755 --- a/src/survive_vive.c +++ b/src/survive_vive.c @@ -1085,6 +1085,7 @@ void calibrate_gyro(SurviveObject* so, FLT* agm) { agm[1] *= so->gyro_scale[1]; agm[2] *= so->gyro_scale[2]; } + } @@ -1157,21 +1158,22 @@ void survive_data_cb( SurviveUSBInterface * si ) acceldata[3], acceldata[4], acceldata[5], 0,0,0 }; + agm[0]*=(float)(1./8192.0); + agm[1]*=(float)(1./8192.0); + agm[2]*=(float)(1./8192.0); calibrate_acc(obj, agm); //1G for accelerometer, from MPU6500 datasheet //this can change if the firmware changes the sensitivity. - agm[0]/=8192.0f; - agm[1]/=8192.0f; - agm[2]/=8192.0f; + + agm[3]*=(float)((1./32.768)*(3.14159/180.)); + agm[4]*=(float)((1./32.768)*(3.14159/180.)); + agm[5]*=(float)((1./32.768)*(3.14159/180.)); calibrate_gyro(obj, agm+3); //65.5 deg/s for gyroscope, from MPU6500 datasheet //1000 deg/s for gyroscope, from MPU6500 datasheet - agm[3]/=65.5f; - agm[4]/=65.5f; - agm[5]/=65.5f; ctx->imuproc( obj, 3, agm, timecode, code ); } @@ -1363,6 +1365,9 @@ printf( "Loading config: %d\n", len ); FLT* values = NULL; if ( parse_float_array(ct0conf, tk+2, &values, count) >0 ) { so->acc_bias = values; + so->acc_bias[0] *= .125; //XXX Wat? Observed by CNL. Biasing by more than this seems to hose things. + so->acc_bias[1] *= .125; + so->acc_bias[2] *= .125; } } if (jsoneq(ct0conf, tk, "acc_scale") == 0) { -- cgit v1.2.3 From d8fe40d838badac49a013b2d1dc8c330411052c2 Mon Sep 17 00:00:00 2001 From: Joshua Allen Date: Sun, 7 May 2017 09:04:43 -0400 Subject: handle applying calibration numbers to watchman devices --- src/survive_vive.c | 73 ++++++++++++++++++++++++++++++++---------------------- 1 file changed, 44 insertions(+), 29 deletions(-) (limited to 'src') diff --git a/src/survive_vive.c b/src/survive_vive.c index 95e688e..6a685eb 100755 --- a/src/survive_vive.c +++ b/src/survive_vive.c @@ -814,6 +814,35 @@ int survive_get_config( char ** config, SurviveViveData * sv, int devno, int ifa #define POP2 (*(((uint16_t*)((readdata+=2)-2)))) #define POP4 (*(((uint32_t*)((readdata+=4)-4)))) +void calibrate_acc(SurviveObject* so, FLT* agm) { + if (so->acc_bias != NULL) { + agm[0] -= so->acc_bias[0]; + agm[1] -= so->acc_bias[1]; + agm[2] -= so->acc_bias[2]; + } + + if (so->acc_scale != NULL) { + agm[0] *= so->acc_scale[0]; + agm[1] *= so->acc_scale[1]; + agm[2] *= so->acc_scale[2]; + } +} + +void calibrate_gyro(SurviveObject* so, FLT* agm) { + if (so->gyro_bias != NULL) { + agm[0] -= so->gyro_bias[0]; + agm[1] -= so->gyro_bias[1]; + agm[2] -= so->gyro_bias[2]; + } + + if (so->gyro_scale != NULL) { + agm[0] *= so->gyro_scale[0]; + agm[1] *= so->gyro_scale[1]; + agm[2] *= so->gyro_scale[2]; + } +} + + static void handle_watchman( SurviveObject * w, uint8_t * readdata ) { @@ -891,6 +920,21 @@ static void handle_watchman( SurviveObject * w, uint8_t * readdata ) readdata[4], readdata[5], readdata[6], 0,0,0 }; +// if (w->acc_scale) printf("%f %f %f\n",w->acc_scale[0],w->acc_scale[1],w->acc_scale[2]); + calibrate_acc(w, agm); + + //I don't understand where these numbers come from but the data from the WMD seems to max out at 255... + agm[0]/=255.0f; + agm[1]/=255.0f; + agm[2]/=255.0f; + + calibrate_gyro(w, agm+3); + + //I don't understand where these numbers come from but the data from the WMD seems to max out at 255... + agm[3]/=255.0f; + agm[4]/=255.0f; + agm[5]/=255.0f; + w->ctx->imuproc( w, 3, agm, (time1<<24)|(time2<<16)|readdata[0], 0 ); int16_t * k = (int16_t *)readdata+1; @@ -1059,35 +1103,6 @@ static void handle_watchman( SurviveObject * w, uint8_t * readdata ) } } -void calibrate_acc(SurviveObject* so, FLT* agm) { - if (so->acc_bias != NULL) { - agm[0] -= so->acc_bias[0]; - agm[1] -= so->acc_bias[1]; - agm[2] -= so->acc_bias[2]; - } - - if (so->acc_scale != NULL) { - agm[0] *= so->acc_scale[0]; - agm[1] *= so->acc_scale[1]; - agm[2] *= so->acc_scale[2]; - } -} - -void calibrate_gyro(SurviveObject* so, FLT* agm) { - if (so->gyro_bias != NULL) { - agm[0] -= so->gyro_bias[0]; - agm[1] -= so->gyro_bias[1]; - agm[2] -= so->gyro_bias[2]; - } - - if (so->gyro_scale != NULL) { - agm[0] *= so->gyro_scale[0]; - agm[1] *= so->gyro_scale[1]; - agm[2] *= so->gyro_scale[2]; - } -} - - void survive_data_cb( SurviveUSBInterface * si ) { int size = si->actual_len; -- cgit v1.2.3 From 746a1eb0bf8df3effa55e79ff3513d9dcd9ac87c Mon Sep 17 00:00:00 2001 From: Joshua Allen Date: Sun, 7 May 2017 09:14:07 -0400 Subject: use multiplication --- src/survive_vive.c | 12 ++++++------ 1 file changed, 6 insertions(+), 6 deletions(-) (limited to 'src') diff --git a/src/survive_vive.c b/src/survive_vive.c index b44e4fc..9a3cb03 100755 --- a/src/survive_vive.c +++ b/src/survive_vive.c @@ -924,16 +924,16 @@ static void handle_watchman( SurviveObject * w, uint8_t * readdata ) calibrate_acc(w, agm); //I don't understand where these numbers come from but the data from the WMD seems to max out at 255... - agm[0]/=255.0f; - agm[1]/=255.0f; - agm[2]/=255.0f; + agm[0]*=(1.0f/255.0f); + agm[1]*=(1.0f/255.0f); + agm[2]*=(1.0f/255.0f); calibrate_gyro(w, agm+3); //I don't understand where these numbers come from but the data from the WMD seems to max out at 255... - agm[3]/=255.0f; - agm[4]/=255.0f; - agm[5]/=255.0f; + agm[3]*=(1.0f/255.0f); + agm[4]*=(1.0f/255.0f); + agm[5]*=(1.0f/255.0f); w->ctx->imuproc( w, 3, agm, (time1<<24)|(time2<<16)|readdata[0], 0 ); -- cgit v1.2.3 From 78ac5da64c299bcf0f2e1c8d19c876e7723faccf Mon Sep 17 00:00:00 2001 From: cnlohr Date: Sat, 13 May 2017 03:47:35 -0400 Subject: Fix my disambiguator. Seems to work with either-or lighthouse. --- src/survive_data.c | 46 ++++++++++++++++------------------------------ 1 file changed, 16 insertions(+), 30 deletions(-) (limited to 'src') diff --git a/src/survive_data.c b/src/survive_data.c index df8df8e..157650d 100644 --- a/src/survive_data.c +++ b/src/survive_data.c @@ -468,6 +468,7 @@ void handle_lightcap2( SurviveObject * so, LightcapElement * le ) #endif + int32_t decode_acode(uint32_t length, int32_t main_divisor) { //+50 adds a small offset and seems to help always get it right. //Check the +50 in the future to see how well this works on a variety of hardware. @@ -478,6 +479,10 @@ int32_t decode_acode(uint32_t length, int32_t main_divisor) { return (acode>>1) - 6; } +//////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////// +///////The charles disambiguator. Don't use this, mostly here for debugging./////////////////////////////////////////////////////// +//////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////// + void HandleOOTX( SurviveContext * ctx, SurviveObject * so ) { @@ -499,30 +504,9 @@ void HandleOOTX( SurviveContext * ctx, SurviveObject * so ) so->recent_sync_time = so->last_sync_time[1]; -/* - //Throw out everything if our sync pulses look like they're bad. - //This actually doesn't seem to hold anymore, now that we're looking for multiple LHs. - 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 < center_2 - slack || delta2 > center_2 + slack ) - { - //XXX: TODO: Count faults. - so->sync_set_number = -1; - return; - } -*/ so->did_handle_ootx = 1; } - + //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 ) @@ -588,16 +572,8 @@ void handle_lightcap( SurviveObject * so, LightcapElement * le ) { int is_new_pulse = delta > so->pulselength_min_sync /*1500*/ + last_sync_length; - //TRICKY: If we didn't see anything from the other lighthouse, we might just not see it... But, we still have to send our sync - //information to the rest of libsurvive. This could be turned into a function and combined with the code below. - if( !so->did_handle_ootx && is_new_pulse ) - { - HandleOOTX( ctx, so ); - } - so->did_handle_ootx = 0; - //printf( "INP: %d %d\n", is_new_pulse, so->sync_set_number ); if( is_new_pulse ) { @@ -606,6 +582,16 @@ void handle_lightcap( SurviveObject * so, LightcapElement * le ) int tp = delta % ( so->timecenter_ticks * 2); is_pulse_from_same_lh_as_last_sweep = tp < so->pulse_synctime_slack && tp > -so->pulse_synctime_slack; + if( !so->did_handle_ootx ) + { + HandleOOTX( ctx, so ); + } + if( !is_master_sync_pulse ) + { + so->did_handle_ootx = 0; + } + + if( is_master_sync_pulse ) //Could also be called by slave if no master was seen. { ssn = so->sync_set_number = is_pulse_from_same_lh_as_last_sweep?(so->sync_set_number):0; //If repeated lighthouse, just back off one. -- cgit v1.2.3