From eb7a93cefc958421d3bebbedc1163c73ae2e2778 Mon Sep 17 00:00:00 2001 From: cnlohr Date: Fri, 2 Dec 2016 01:23:59 -0500 Subject: Update: Reading almost everything! --- src/survive.c | 19 ++- src/survive_data.c | 312 +++++++++++++++++++++++++++---------------------- src/survive_internal.h | 23 +++- src/survive_process.c | 15 +++ 4 files changed, 225 insertions(+), 144 deletions(-) create mode 100644 src/survive_process.c (limited to 'src') diff --git a/src/survive.c b/src/survive.c index 97493a0..751cc01 100644 --- a/src/survive.c +++ b/src/survive.c @@ -2,20 +2,35 @@ #include "survive_internal.h" #include #include +#include struct SurviveContext * survive_init( void(*ff)( struct SurviveContext * ctx, const char * fault ), void(*notefunction)( struct SurviveContext * ctx, const char * note ) ) { int r = 0; struct SurviveContext * ret = calloc( 1, sizeof( struct SurviveContext ) ); + + ret->headset.sensors = 32; + ret->headset.ctx = ret; + memcpy( ret->headset.codename, "HED", 4 ); + + ret->watchman[0].sensors = 16; + ret->watchman[0].ctx = ret; + memcpy( ret->watchman[0].codename, "CT0", 4 ); + + ret->watchman[1].sensors = 16; + ret->watchman[1].ctx = ret; + memcpy( ret->watchman[1].codename, "CT1", 4 ); + + ret->faultfunction = ff; ret->notefunction = notefunction; + + //USB must happen last. if( r = survive_usb_init( ret ) ) { return 0; } - ret->headset.sensors = 32; - ret->headset.ctx = ret; //ret->headset->photos = malloc( ret->headset->sensors * sizeof(struct SurvivePhoto) ); return ret; } diff --git a/src/survive_data.c b/src/survive_data.c index 7f709a5..256c6d6 100644 --- a/src/survive_data.c +++ b/src/survive_data.c @@ -6,29 +6,9 @@ #define POP1 (*(readdata++)) #define POP2 (*(((uint16_t*)((readdata+=2)-2)))) -#define POP4 (*(((uint32_t*)readdata)++)) +#define POP4 (*(((uint32_t*)((readdata+=4)-4)))) -struct LightpulseStructure -{ - uint8_t id; //Random divisible-by-2 numbers - uint8_t type; //Always 0 - int16_t unknown1; //About 7,500 - int16_t unknown2; //About -3,600 - int16_t unknown3; //About 89 to 107 - int16_t unknown4; //Normally around 0. - int16_t unknown5; //About -45, to -50 - uint32_t timestamp; - uint8_t unknown6; -} __attribute__((packed)); - - -static void handle_lightdata( struct LightpulseStructure * p ) -{ - //TODO: Wat? - //printf( "%4d %4d (%6d %6d %6d %6d %6d) %10d %6d\n", p->id, p->type, p->unknown1, p->unknown2, p->unknown3, p->unknown4, p->unknown5, p->timestamp, p->unknown6 ); -} - struct LightcapElement { @@ -39,150 +19,195 @@ struct LightcapElement } __attribute__((packed)); -//XXX: TODO: Is the 'timestamp' in the middle of the pulse or beginning??? + static void handle_lightcap( struct SurviveObject * so, struct LightcapElement * le ) { - if( le->type != 0xfe || le->length < 50 ) return; + struct SurviveContext * ct = so->ctx; + if( le->type != 0xfe || le->length < 50 ) return; //le->timestamp += (le->length/2); if( le->length > 900 ) //Pulse longer than 18us? { - int32_t deltat = le->timestamp - so->last_photo_time; - if( deltat > 2000 ) //New pulse. + int32_t deltat = (uint32_t)le->timestamp - (uint32_t)ct->last_photo_time; + if( deltat > 2000 || deltat < -2000 ) //New pulse. (may be inverted) { - so->last_photo_time = le->timestamp; - so->total_photo_time = 0; - so->total_photos = 0; + ct->last_photo_time = le->timestamp; + ct->total_photo_time = 0; + ct->total_photos = 0; + ct->total_pulsecode_time = 0; + survive_light_process( so, le->sensor_id, -1, 0, le->timestamp ); } else { - so->total_photo_time += deltat; - so->total_photos++; + ct->total_pulsecode_time += le->length; + ct->total_photo_time += deltat; + ct->total_photos++; } } - else if( le->length < 900 && le->length > 50 && so->total_photos ) + else if( le->length < 900 && le->length > 50 && ct->total_photos ) { - int32_t offset_from = le->timestamp - (so->total_photo_time/so->total_photos) - so->last_photo_time; - if( le->sensor_id == 9 ) - - if( offset_from > 200000 ) - printf( "%3d %3d %3d %3d\n", le->sensor_id, le->type, le->length, offset_from ); + int32_t dl = (ct->total_photo_time/ct->total_photos); + int32_t tpco = (ct->total_pulsecode_time/ct->total_photos); + //Adding length + int32_t offset_from = le->timestamp - dl - ct->last_photo_time + le->length/2; + + //Long pulse-code from IR flood. + //Make sure it fits nicely into a divisible-by-500 time. + int32_t acode = (tpco+125)/250; + if( acode & 1 ) return; + acode>>=1; + + survive_light_process( so, le->sensor_id, acode, offset_from, le->timestamp ); } else { //Runt pulse. } -/* - struct SurviveContext * ctx; - int last_photo_time; - int total_photos; - float total_photo_time; - - int sensors; - - printf( "%3d %3d %6d %8d\n", le->sensor_id, le->type, le->length, le->timestamp );*/ } +static void handle_watchman( struct SurviveObject * w, uint8_t * readdata ) +{ + uint8_t time1 = POP1; + uint8_t qty = POP1; + uint8_t time2 = POP1; + uint8_t type = POP1; + int i; + qty-=2; + int propset = 0; + if( (type & 0xf0) == 0xf0 ) + { + propset |= 4; + //printf( "%02x %02x %02x %02x\n", qty, type, time1, time2 ); + type &= ~0x10; -/* -struct vive_controller_analog_trigger_message { - __u8 squeeze; - __u8 unknown[4]; -} __attribute__((packed)); - -struct vive_controller_button_message { - __u8 buttons; -} __attribute__((packed)); + if( type & 0x01 ) + { + qty-=1; + w->buttonmask = POP1; + type &= ~0x01; + } + if( type & 0x04 ) + { + qty-=1; + w->axis1 = ( POP1 ) * 256; + type &= ~0x04; + } + if( type & 0x02 ) + { + qty-=4; + w->axis2 = POP2; + w->axis3 = POP2; + type &= ~0x02; + } -struct vive_controller_touch_move_message { - __le16 pos[2]; - __u8 unknown[4]; -} __attribute__((packed)); + //XXX TODO: Maybe more data is here? + if( type == 0xe0 ) + { + type = 0x00; + } + } -struct vive_controller_touch_press_message { - __u8 buttons; - __le16 pos[2]; - __u8 unknown[4]; -} __attribute__((packed)); + if( type == 0xe1 ) + { + propset |= 1; + w->charging = readdata[0]>>7; + w->charge = POP1&0x7f; qty--; + w->ison = 1; + if( qty ) + { + qty--; + type = POP1; //IMU usually follows. + } + } -struct vive_controller_imu_message { - __u8 time3; - __le16 accel[3]; - __le16 gyro[3]; - __u8 unknown[4]; -} __attribute__((packed)); + if( ( type & 0xe8 ) == 0xe8 ) + { + propset |= 2; + survive_imu_process( w, (int16_t *)&readdata[1], (time1<<24)|(time2<<16)|readdata[0], 0 ); + int16_t * k = (int16_t *)readdata+1; + //printf( "Match8 %d %d %d %d %d %3d %3d\n", qty, k[0], k[1], k[2], k[3], k[4], k[5] ); + readdata += 13; qty -= 13; + type &= ~0xe8; + if( qty ) + { + qty--; + type = POP1; + } + } -struct vive_controller_ping_message { - __u8 charge : 7; - __u8 charging : 1; - __u8 unknown1[2]; - __le16 accel[3]; - __le16 gyro[3]; - __u8 unknown2[5]; -} __attribute__((packed)); -struct vive_controller_message { - __u8 time1; - __u8 sensor_id; - __u8 time2; - __u8 type; - union { - struct vive_controller_analog_trigger_message analog_trigger; - struct vive_controller_button_message button; - struct vive_controller_touch_move_message touch_move; - struct vive_controller_touch_press_message touch_press; - struct vive_controller_imu_message imu; - struct vive_controller_ping_message ping; - __u8 unknown[25]; - }; -} __attribute__((packed)); -*/ + if( qty ) + { + qty++; +#if 0 //Doesn't work!!!! + int reads = qty/6; -static void handle_watchman( int whichwatch, uint8_t * readdata ) -{ - uint8_t time1 = POP1; - uint8_t sensor_id = POP1; - uint8_t time2 = POP1; - uint8_t type = POP1; - int i; + int leds[6]; + leds[0] = type; + for( i = 1; i < reads; i++ ) + { + leds[i] = POP1; + } + for( i = 0; i < reads; i++ ) + { + printf( "%02x: ", leds[i] ); + for( i = 0; i < 5; i++ ) + { + printf( "%02x ", POP1 ); + } + printf( "\n" ); + } +#endif - switch(type) - { - case 0xe1: //Ping - break; - case 0xe8: //IMU - break; - case 0xf2: //Touch - break; - case 0xf3: //Button? - break; //...many more F's. - default: - //It's a light! - //printf( "WM: %3d %3d %3d %3d %02x: ", whichwatch, time1, sensor_id, time2, type ); - //for( i = 0; i < 26; i++ ) - //printf( "%02x ", readdata[i] ); - //printf("\n" ); +#if 0 + printf( "POST %d: %4d %02x (%02x%02x) - ", propset, qty, type, time1, time2 ); + for( i = 0; i < qty; i++ ) + { + printf( "%02x ", readdata[i] ); + } + printf("\n"); +#endif + } - break; + return; + //NO, seriously, there is something wacky going on here. +// else if( type == 0xe8 && sensor_id == 15 ) +// { +// printf( "IMU\n" ); +// } - }; +/* + { + printf( "PSIDPIN:%3d ", qty ); + w->charging = readdata[0]>>7; + w->charge = readdata[0]&0x7f; + w->ison = 1; + printf( "%02x/%02x/%02x/%02x: ", type, qty, time1, time2 ); + for( i = 0; i < 25; i++ ) + { + printf( "%02x ", readdata[i] ); + } + printf("\n"); -/*enum class vl_controller_type : uint8_t { - PING = 0xe1, - IMU = 0xe8, - TOUCH = 0xf2, - ANALOG_TRIGGER = 0xf4,*/ +/* + //WHAT IS THIS??? + printf( "%02x/%02x/%02x/%02x: ", type, sensor_id, time1, time2 ); + for( i = 0; i < 25; i++ ) + { + printf( "%02x ", readdata[i] ); + } + printf("\n");*/ } void survive_data_cb( struct SurviveUSBInterface * si ) { int size = si->actual_len; - + struct SurviveContext * ctx = si->ctx; #if 0 int i; printf( "%16s: %d: ", si->hname, len ); @@ -205,18 +230,18 @@ void survive_data_cb( struct SurviveUSBInterface * si ) { case USB_IF_HMD: { + struct SurviveObject * headset = &ctx->headset; readdata+=2; - int lens = POP1; //Lens - int lens_sep = POP2; //Lens Separation + headset->buttonmask = POP1; //Lens + headset->axis2 = POP2; //Lens Separation readdata+=2; - int btn = POP1; //Button + headset->buttonmask |= POP1; //Button readdata+=3; - readdata++; //Proxchange, No change = 0, Decrease = 1, Increase = 2 + readdata++; //Proxchange, No change = 0, Decrease = 1, Increase = 2 readdata++; - int proximity = POP2; //Proximity << how close to face are you? Less than 80 = not on face. - int ipd = POP2; //IPD << what is this? - - //TODO: Store in thing. + headset->axis3 = POP2; //Proximity << how close to face are you? Less than 80 = not on face. + headset->axis1 = POP2; //IPD << what is this? + headset->ison = 1; break; } case USB_IF_LIGHTHOUSE: @@ -224,35 +249,44 @@ void survive_data_cb( struct SurviveUSBInterface * si ) int i; for( i = 0; i < 3; i++ ) { - handle_lightdata( (struct LightpulseStructure *)readdata ); - readdata+= 17; + //handle_lightdata( (struct LightpulseStructure *)readdata ); + int16_t * acceldata = (int16_t*)readdata; + readdata += 12; + uint32_t timecode = POP4; + survive_imu_process( &ctx->headset, acceldata, timecode, POP1 ); } break; } case USB_IF_WATCHMAN1: case USB_IF_WATCHMAN2: { + struct SurviveObject * w = &ctx->watchman[si->which_interface_am_i-USB_IF_WATCHMAN1]; if( id == 35 ) { - handle_watchman( si->which_interface_am_i-USB_IF_WATCHMAN1, readdata); + handle_watchman( w, readdata); } else if( id == 36 ) { - handle_watchman( si->which_interface_am_i-USB_IF_WATCHMAN1, readdata); - handle_watchman( si->which_interface_am_i-USB_IF_WATCHMAN1, readdata+29 ); + handle_watchman( w, readdata); + handle_watchman( w, readdata+29 ); + } + else if( id == 38 ) + { + w->ison = 0; } else { - printf( "Unknown watchman code\n" ); + SV_INFO( "Unknown watchman code %d\n", id ); } break; } case USB_IF_LIGHTCAP: { + //Done! int i; for( i = 0; i < 7; i++ ) { - handle_lightcap( &si->ctx->headset, (struct LightcapElement*)&readdata[i*8] ); + handle_lightcap( &ctx->headset, (struct LightcapElement*)&readdata[i*8] ); } break; } diff --git a/src/survive_internal.h b/src/survive_internal.h index a7d8ca9..c3f8df3 100644 --- a/src/survive_internal.h +++ b/src/survive_internal.h @@ -49,9 +49,14 @@ struct SurviveUSBInterface struct SurviveObject { struct SurviveContext * ctx; - int32_t last_photo_time; - short total_photos; - int32_t total_photo_time; + char codename[4]; + int16_t buttonmask; + int16_t axis1; + int16_t axis2; + int16_t axis3; + int8_t charge; + int8_t charging:1; + int8_t ison:1; int sensors; }; @@ -64,8 +69,15 @@ struct SurviveContext struct libusb_device_handle * udev[MAX_USB_DEVS]; struct SurviveUSBInterface uiface[MAX_INTERFACES]; + //Flood info, for calculating which laser is currently sweeping. + int32_t last_photo_time; + short total_photos; + int32_t total_photo_time; + int32_t total_pulsecode_time; + //Data Subsystem struct SurviveObject headset; + struct SurviveObject watchman[2]; // struct SurvivePhoto }; @@ -78,6 +90,11 @@ int survive_usb_poll( struct SurviveContext * ctx ); //Accept Data from backend. void survive_data_cb( struct SurviveUSBInterface * si ); +//Accept higher-level data. +void survive_light_process( struct SurviveObject * so, int sensor_id, int acode, int timeinsweep, uint32_t timecode ); +void survive_imu_process( struct SurviveObject * so, int16_t * accelgyro, uint32_t timecode, int id ); + + #endif diff --git a/src/survive_process.c b/src/survive_process.c new file mode 100644 index 0000000..c344306 --- /dev/null +++ b/src/survive_process.c @@ -0,0 +1,15 @@ +#include "survive_internal.h" + +void survive_light_process( struct SurviveObject * so, int sensor_id, int acode, int timeinsweep, uint32_t timecode ) +{ + //timeinsweep = 200,000 1/48,000,000ths of a second is "center-of-image" + printf( "L %s %d %d %d %d\n", so->codename, timecode, sensor_id, acode, timeinsweep ); +} + +void survive_imu_process( struct SurviveObject * so, int16_t * accelgyro, uint32_t timecode, int id ) +{ + if( so->codename[0] == 'H' ) + { + printf( "I %s %d %d %d %d %d %d %d %d\n", so->codename, timecode, accelgyro[0], accelgyro[1], accelgyro[2], accelgyro[3], accelgyro[4], accelgyro[5], id ); + } +} -- cgit v1.2.3