From f183aa480c549695ac5b481fade04e62f71d1e0a Mon Sep 17 00:00:00 2001 From: Mike Turvey Date: Wed, 3 Jan 2018 18:58:43 -0700 Subject: Controller Buttons Fully Implemented Fully plumbed support for controller buttons Also, commented haptic call because it messed with the vive_magic calls, given where I had it. --- src/survive_process.c | 11 +++++++++++ 1 file changed, 11 insertions(+) (limited to 'src/survive_process.c') diff --git a/src/survive_process.c b/src/survive_process.c index 3af2da9..eaed881 100644 --- a/src/survive_process.c +++ b/src/survive_process.c @@ -63,6 +63,17 @@ void survive_default_angle_process( SurviveObject * so, int sensor_id, int acode } } +void survive_default_button_process(SurviveObject * so, uint8_t eventType, uint8_t buttonId, uint8_t axis1Id, uint16_t axis1Val, uint8_t axis2Id, uint16_t axis2Val) +{ + // do nothing. + printf("ButtonEntry: eventType:%x, buttonId:%d, axis1:%d, axis1Val:%8.8x, axis2:%d, axis2Val:%8.8x\n", + eventType, + buttonId, + axis1Id, + axis1Val, + axis2Id, + axis2Val); +} void survive_default_imu_process( SurviveObject * so, int mask, FLT * accelgyromag, uint32_t timecode, int id ) { -- cgit v1.3.1 From 499b80ae7b538f8e66f5ec8bfa60c7136a3babf5 Mon Sep 17 00:00:00 2001 From: Mike Turvey Date: Thu, 4 Jan 2018 08:47:42 -0700 Subject: Haptic Call Plumbed The plumbing is now in place for the haptic call. Left in place a "demo" where haptic is called when a controller's trigger is pulled --- include/libsurvive/survive.h | 5 +++ include/libsurvive/survive_types.h | 2 + src/survive.c | 13 +++++- src/survive_process.c | 26 +++++++++--- src/survive_vive.c | 87 +++++++++++++++++++++++++++++++++----- 5 files changed, 115 insertions(+), 18 deletions(-) (limited to 'src/survive_process.c') diff --git a/include/libsurvive/survive.h b/include/libsurvive/survive.h index e4afadf..0cfab1f 100644 --- a/include/libsurvive/survive.h +++ b/include/libsurvive/survive.h @@ -19,6 +19,7 @@ struct SurviveObject char codename[4]; //3 letters, null-terminated. Currently HMD, WM0, WM1. char drivername[4]; //3 letters for driver. Currently "HTC" + void *driver; int32_t buttonmask; int16_t axis1; @@ -68,6 +69,7 @@ struct SurviveObject FLT* gyro_bias; // size is FLT*3. contains x,y,z FLT* gyro_scale; // size is FLT*3. contains x,y,z + haptic_func haptic; //Debug int tsl; @@ -179,6 +181,9 @@ void survive_cal_install( SurviveContext * ctx ); //XXX This will be removed if // Read back a human-readable string description of the calibration status int survive_cal_get_status( struct SurviveContext * ctx, char * description, int description_length ); +// Induce haptic feedback +int survive_haptic(SurviveObject * so, uint8_t reserved, uint16_t pulseHigh, uint16_t pulseLow, uint16_t repeatCount); + //Call these from your callback if overridden. //Accept higher-level data. void survive_default_light_process( SurviveObject * so, int sensor_id, int acode, int timeinsweep, uint32_t timecode, uint32_t length , uint32_t lh); diff --git a/include/libsurvive/survive_types.h b/include/libsurvive/survive_types.h index fa3eb2f..9a6e148 100644 --- a/include/libsurvive/survive_types.h +++ b/include/libsurvive/survive_types.h @@ -44,6 +44,8 @@ typedef void (*imu_process_func)( SurviveObject * so, int mask, FLT * accelgyro, typedef void (*angle_process_func)( SurviveObject * so, int sensor_id, int acode, uint32_t timecode, FLT length, FLT angle, uint32_t lh); typedef void (*button_process_func)(SurviveObject * so, uint8_t eventType, uint8_t buttonId, uint8_t axis1Id, uint16_t axis1Val, uint8_t axis2Id, uint16_t axis2Val); +typedef int(*haptic_func)(SurviveObject * so, uint8_t reserved, uint16_t pulseHigh , uint16_t pulseLow, uint16_t repeatCount); + //Device drivers (prefix your drivers with "DriverReg") i.e. // REGISTER_LINKTIME( DriverRegHTCVive ); typedef int (*DeviceDriver)( SurviveContext * ctx ); diff --git a/src/survive.c b/src/survive.c index d3c0918..75c07bf 100755 --- a/src/survive.c +++ b/src/survive.c @@ -60,7 +60,7 @@ static void button_servicer(void * context) // should never happen. indicates failure of code pushing stuff onto // the buttonQueue // if it does happen, it will kill all future button input - printf("ERROR: Unpopulated ButtonQueueEntry!"); + printf("ERROR: Unpopulated ButtonQueueEntry! NextReadIndex=%d\n", ctx->buttonQueue.nextReadIndex); return; } @@ -272,6 +272,17 @@ int survive_send_magic( SurviveContext * ctx, int magic_code, void * data, int d return 0; } +int survive_haptic(SurviveObject * so, uint8_t reserved, uint16_t pulseHigh, uint16_t pulseLow, uint16_t repeatCount) +{ + if (NULL == so || NULL == so->haptic) + { + return -404; + } + + return so->haptic(so, reserved, pulseHigh, pulseLow, repeatCount); +} + + void survive_close( SurviveContext * ctx ) { const char * DriverName; diff --git a/src/survive_process.c b/src/survive_process.c index eaed881..4b86144 100644 --- a/src/survive_process.c +++ b/src/survive_process.c @@ -66,13 +66,25 @@ void survive_default_angle_process( SurviveObject * so, int sensor_id, int acode void survive_default_button_process(SurviveObject * so, uint8_t eventType, uint8_t buttonId, uint8_t axis1Id, uint16_t axis1Val, uint8_t axis2Id, uint16_t axis2Val) { // do nothing. - printf("ButtonEntry: eventType:%x, buttonId:%d, axis1:%d, axis1Val:%8.8x, axis2:%d, axis2Val:%8.8x\n", - eventType, - buttonId, - axis1Id, - axis1Val, - axis2Id, - axis2Val); + //printf("ButtonEntry: eventType:%x, buttonId:%d, axis1:%d, axis1Val:%8.8x, axis2:%d, axis2Val:%8.8x\n", + // eventType, + // buttonId, + // axis1Id, + // axis1Val, + // axis2Id, + // axis2Val); + if (buttonId == 24 && eventType == 1) // trigger engage + { + for (int j = 0; j < 40; j++) + { + for (int i = 0; i < 0x1; i++) + { + survive_haptic(so, 0, 0xf401, 0xb5a2, 0x0100); + OGUSleep(5000); + } + OGUSleep(20000); + } + } } void survive_default_imu_process( SurviveObject * so, int mask, FLT * accelgyromag, uint32_t timecode, int id ) diff --git a/src/survive_vive.c b/src/survive_vive.c index 3df60ba..9e4a8b8 100755 --- a/src/survive_vive.c +++ b/src/survive_vive.c @@ -620,17 +620,24 @@ int survive_vive_send_magic(SurviveContext * ctx, void * drv, int magic_code, vo //#endif #if 0 - for (int i = 0; i < 0xf; i++) + for (int j=0; j < 40; j++) { - //uint8_t vive_controller_haptic_pulse[64] = { 0xff, 0x8f, 0x7, 0 /*right*/, 0xFF /*period on*/, 0xFF/*period on*/, 0xFF/*period off*/, 0xFF/*period off*/, 0xFF /* repeat Count */, 0xFF /* repeat count */ }; - //uint8_t vive_controller_haptic_pulse[64] = { 0xff, 0x8f, 0x07, 0x00, 0xf4, 0x01, 0xb5, 0xa2, 0x01, 0x00 }; // data taken from Nairol's captures - uint8_t vive_controller_haptic_pulse[64] = { 0xff, 0x8f, 0x07, 0x00, 0xf4, 0x01, 0xb5, 0xa2, 0x01, 0x00 }; - r = update_feature_report( sv->udev[USB_DEV_WATCHMAN1], 0, vive_controller_haptic_pulse, sizeof( vive_controller_haptic_pulse ) ); - r = getupdate_feature_report(sv->udev[USB_DEV_WATCHMAN1], 0, vive_controller_haptic_pulse, sizeof(vive_controller_haptic_pulse)); - SV_INFO("UCR: %d", r); - if (r != sizeof(vive_controller_haptic_pulse)) printf("HAPTIC FAILED **************************\n"); // return 5; - OGUSleep(1000); + for (int i = 0; i < 0x1; i++) + { + //uint8_t vive_controller_haptic_pulse[64] = { 0xff, 0x8f, 0x7, 0 /*right*/, 0xFF /*period on*/, 0xFF/*period on*/, 0xFF/*period off*/, 0xFF/*period off*/, 0xFF /* repeat Count */, 0xFF /* repeat count */ }; + //uint8_t vive_controller_haptic_pulse[64] = { 0xff, 0x8f, 0x07, 0x00, 0xf4, 0x01, 0xb5, 0xa2, 0x01, 0x00 }; // data taken from Nairol's captures + uint8_t vive_controller_haptic_pulse[64] = { 0xff, 0x8f, 0x07, 0x00, 0xf4, 0x01, 0xb5, 0xa2, 0x01* j, 0x00 }; + r = update_feature_report( sv->udev[USB_DEV_WATCHMAN1], 0, vive_controller_haptic_pulse, sizeof( vive_controller_haptic_pulse ) ); + r = getupdate_feature_report(sv->udev[USB_DEV_WATCHMAN1], 0, vive_controller_haptic_pulse, sizeof(vive_controller_haptic_pulse)); + SV_INFO("UCR: %d", r); + if (r != sizeof(vive_controller_haptic_pulse)) printf("HAPTIC FAILED **************************\n"); // return 5; + OGUSleep(5000); + } + + OGUSleep(20000); } + + #endif #if 0 @@ -686,6 +693,54 @@ int survive_vive_send_magic(SurviveContext * ctx, void * drv, int magic_code, vo return 0; } +int survive_vive_send_haptic(SurviveObject * so, uint8_t reserved, uint16_t pulseHigh, uint16_t pulseLow, uint16_t repeatCount) +{ + SurviveViveData *sv = (SurviveViveData*)so->driver; + + if (NULL == sv) + { + return -500; + } + + int r; + uint8_t vive_controller_haptic_pulse[64] = { + 0xff, 0x8f, 0x07, 0x00, + pulseHigh & 0xff00 >> 8, pulseHigh & 0xff, + pulseLow & 0xff00 >> 8, pulseLow & 0xff, + repeatCount & 0xff00 >> 8, repeatCount & 0xff, + }; + + r = update_feature_report(sv->udev[USB_DEV_WATCHMAN1], 0, vive_controller_haptic_pulse, sizeof(vive_controller_haptic_pulse)); + r = getupdate_feature_report(sv->udev[USB_DEV_WATCHMAN1], 0, vive_controller_haptic_pulse, sizeof(vive_controller_haptic_pulse)); + //SV_INFO("UCR: %d", r); + if (r != sizeof(vive_controller_haptic_pulse)) + { + printf("HAPTIC FAILED **************************\n"); + return -1; + } + + return 0; + + //for (int j = 0; j < 40; j++) + //{ + // for (int i = 0; i < 0x1; i++) + // { + // //uint8_t vive_controller_haptic_pulse[64] = { 0xff, 0x8f, 0x7, 0 /*right*/, 0xFF /*period on*/, 0xFF/*period on*/, 0xFF/*period off*/, 0xFF/*period off*/, 0xFF /* repeat Count */, 0xFF /* repeat count */ }; + // //uint8_t vive_controller_haptic_pulse[64] = { 0xff, 0x8f, 0x07, 0x00, 0xf4, 0x01, 0xb5, 0xa2, 0x01, 0x00 }; // data taken from Nairol's captures + // uint8_t vive_controller_haptic_pulse[64] = { 0xff, 0x8f, 0x07, 0x00, 0xf4, 0x01, 0xb5, 0xa2, 0x01 * j, 0x00 }; + // r = update_feature_report(sv->udev[USB_DEV_WATCHMAN1], 0, vive_controller_haptic_pulse, sizeof(vive_controller_haptic_pulse)); + // r = getupdate_feature_report(sv->udev[USB_DEV_WATCHMAN1], 0, vive_controller_haptic_pulse, sizeof(vive_controller_haptic_pulse)); + // if (r != sizeof(vive_controller_haptic_pulse)) printf("HAPTIC FAILED **************************\n"); // return 5; + // OGUSleep(5000); + // } + + // OGUSleep(20000); + //} + + ////OGUSleep(5000); + //return 0; +} + void survive_vive_usb_close( SurviveViveData * sv ) { int i; @@ -903,9 +958,10 @@ void incrementAndPostButtonQueue(SurviveContext *ctx) { ButtonQueueEntry *entry = &(ctx->buttonQueue.entry[ctx->buttonQueue.nextWriteIndex]); - if (OGGetSema(ctx->buttonQueue.buttonservicesem) >= BUTTON_QUEUE_MAX_LEN) + if (OGGetSema(ctx->buttonQueue.buttonservicesem) >= BUTTON_QUEUE_MAX_LEN-1) { // There's not enough space to write this entry. Clear it out and move along + //printf("Button Buffer Full\n"); memset(entry, 0, sizeof(ButtonQueueEntry)); return; } @@ -1759,6 +1815,9 @@ void init_SurviveObject(SurviveObject* so) { so->acc_bias = NULL; so->gyro_scale = NULL; so->gyro_bias = NULL; + so->haptic = NULL; + so->PoserData = NULL; + so->disambiguator_data = NULL; } int DriverRegHTCVive( SurviveContext * ctx ) @@ -1789,18 +1848,23 @@ int DriverRegHTCVive( SurviveContext * ctx ) hmd->ctx = ctx; + hmd->driver = sv; memcpy( hmd->codename, "HMD", 4 ); memcpy( hmd->drivername, "HTC", 4 ); wm0->ctx = ctx; + wm0->driver = sv; memcpy( wm0->codename, "WM0", 4 ); memcpy( wm0->drivername, "HTC", 4 ); wm1->ctx = ctx; + wm1->driver = sv; memcpy( wm1->codename, "WM1", 4 ); memcpy( wm1->drivername, "HTC", 4 ); tr0->ctx = ctx; + tr0->driver = sv; memcpy( tr0->codename, "TR0", 4 ); memcpy( tr0->drivername, "HTC", 4 ); ww0->ctx = ctx; + ww0->driver = sv; memcpy( ww0->codename, "WW0", 4 ); memcpy( ww0->drivername, "HTC", 4 ); @@ -1844,6 +1908,9 @@ int DriverRegHTCVive( SurviveContext * ctx ) wm1->timecenter_ticks = wm1->timebase_hz / 240; tr0->timecenter_ticks = tr0->timebase_hz / 240; ww0->timecenter_ticks = ww0->timebase_hz / 240; + + wm0->haptic = survive_vive_send_haptic; + wm1->haptic = survive_vive_send_haptic; /* int i; int locs = hmd->nr_locations; -- cgit v1.3.1 From 735a8bd11070b0c563e891ff8b70ce297a52a367 Mon Sep 17 00:00:00 2001 From: Mike Turvey Date: Fri, 5 Jan 2018 03:54:29 -0700 Subject: Add standard output mechanism for posers Added a raw pose output/ callback that the posers can call when they have calculated a pose. --- include/libsurvive/survive.h | 5 ++++- include/libsurvive/survive_types.h | 3 ++- src/poser_turveytori.c | 8 ++++++-- src/survive.c | 9 ++++++++- src/survive_data.c | 4 ++-- src/survive_process.c | 39 +++++++++++++++++++++++++++++--------- 6 files changed, 52 insertions(+), 16 deletions(-) (limited to 'src/survive_process.c') diff --git a/include/libsurvive/survive.h b/include/libsurvive/survive.h index 0cfab1f..4821e63 100644 --- a/include/libsurvive/survive.h +++ b/include/libsurvive/survive.h @@ -129,6 +129,7 @@ struct SurviveContext imu_process_func imuproc; angle_process_func angleproc; button_process_func buttonproc; + raw_pose_func rawposeproc; struct config_group* global_config_values; struct config_group* lh_config; //lighthouse configs @@ -163,7 +164,8 @@ void survive_install_error_fn( SurviveContext * ctx, text_feedback_func fbp ); void survive_install_light_fn( SurviveContext * ctx, light_process_func fbp ); void survive_install_imu_fn( SurviveContext * ctx, imu_process_func fbp ); void survive_install_angle_fn( SurviveContext * ctx, angle_process_func fbp ); -void survive_install_button_fn( SurviveContext * ctx, button_process_func fbp ); +void survive_install_button_fn(SurviveContext * ctx, button_process_func fbp); +void survive_install_raw_pose_fn(SurviveContext * ctx, raw_pose_func fbp); void survive_close( SurviveContext * ctx ); int survive_poll( SurviveContext * ctx ); @@ -190,6 +192,7 @@ void survive_default_light_process( SurviveObject * so, int sensor_id, int acode void survive_default_imu_process( SurviveObject * so, int mode, FLT * accelgyro, uint32_t timecode, int id ); void survive_default_angle_process( SurviveObject * so, int sensor_id, int acode, uint32_t timecode, FLT length, FLT angle, uint32_t lh ); void survive_default_button_process(SurviveObject * so, uint8_t eventType, uint8_t buttonId, uint8_t axis1Id, uint16_t axis1Val, uint8_t axis2Id, uint16_t axis2Val); +void survive_default_raw_pose_process(SurviveObject * so, uint8_t lighthouse, FLT *position, FLT *quaternion); ////////////////////// Survive Drivers //////////////////////////// diff --git a/include/libsurvive/survive_types.h b/include/libsurvive/survive_types.h index 9a6e148..5384345 100644 --- a/include/libsurvive/survive_types.h +++ b/include/libsurvive/survive_types.h @@ -42,7 +42,8 @@ typedef void (*text_feedback_func)( SurviveContext * ctx, const char * fault ); typedef void (*light_process_func)( SurviveObject * so, int sensor_id, int acode, int timeinsweep, uint32_t timecode, uint32_t length, uint32_t lighthouse); typedef void (*imu_process_func)( SurviveObject * so, int mask, FLT * accelgyro, uint32_t timecode, int id ); typedef void (*angle_process_func)( SurviveObject * so, int sensor_id, int acode, uint32_t timecode, FLT length, FLT angle, uint32_t lh); -typedef void (*button_process_func)(SurviveObject * so, uint8_t eventType, uint8_t buttonId, uint8_t axis1Id, uint16_t axis1Val, uint8_t axis2Id, uint16_t axis2Val); +typedef void(*button_process_func)(SurviveObject * so, uint8_t eventType, uint8_t buttonId, uint8_t axis1Id, uint16_t axis1Val, uint8_t axis2Id, uint16_t axis2Val); +typedef void(*raw_pose_func)(SurviveObject * so, uint8_t lighthouse, FLT *position, FLT *quaternion); typedef int(*haptic_func)(SurviveObject * so, uint8_t reserved, uint16_t pulseHigh , uint16_t pulseLow, uint16_t repeatCount); diff --git a/src/poser_turveytori.c b/src/poser_turveytori.c index ac979c7..28e533e 100644 --- a/src/poser_turveytori.c +++ b/src/poser_turveytori.c @@ -1177,7 +1177,7 @@ static void RefineRotationEstimateQuaternion(FLT *rotOut, Point lhPoint, FLT *in } - printf("Ri=%3d Fitness=%3f ", i, lastMatchFitness); + if (ttDebug) printf("Ri=%3d Fitness=%3f ", i, lastMatchFitness); } @@ -1638,7 +1638,11 @@ static void QuickPose(SurviveObject *so, int lh) SolveForLighthouse(pos, quat, to, so, 0, lh, 0); - printf("P&O: [% 08.8f,% 08.8f,% 08.8f] [% 08.8f,% 08.8f,% 08.8f,% 08.8f]\n", pos[0], pos[1], pos[2], quat[0], quat[1], quat[2], quat[3]); + //printf("P&O: [% 08.8f,% 08.8f,% 08.8f] [% 08.8f,% 08.8f,% 08.8f,% 08.8f]\n", pos[0], pos[1], pos[2], quat[0], quat[1], quat[2], quat[3]); + if (so->ctx->rawposeproc) + { + so->ctx->rawposeproc(so, lh, pos, quat); + } if (ttDebug) printf("!\n"); } diff --git a/src/survive.c b/src/survive.c index 75c07bf..1c406e8 100755 --- a/src/survive.c +++ b/src/survive.c @@ -184,6 +184,7 @@ SurviveContext * survive_init( int headless ) // start the thread to process button data ctx->buttonservicethread = OGCreateThread(button_servicer, ctx); survive_install_button_fn(ctx, NULL); + survive_install_raw_pose_fn(ctx, NULL); return ctx; } @@ -235,9 +236,15 @@ void survive_install_button_fn(SurviveContext * ctx, button_process_func fbp) ctx->buttonproc = fbp; else ctx->buttonproc = survive_default_button_process; - } +void survive_install_raw_pose_fn(SurviveContext * ctx, raw_pose_func fbp) +{ + if (fbp) + ctx->rawposeproc = fbp; + else + ctx->rawposeproc = survive_default_raw_pose_process; +} int survive_add_object( SurviveContext * ctx, SurviveObject * obj ) { int oldct = ctx->objs_ct; diff --git a/src/survive_data.c b/src/survive_data.c index 1b80269..0427659 100644 --- a/src/survive_data.c +++ b/src/survive_data.c @@ -419,8 +419,8 @@ 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 %d %d\n", le->sensor_id, le->length,lcd->per_sweep.lh_acode[0],lcd->per_sweep.lh_acode[1]); + //fprintf(stderr, "WARNING: No active lighthouse!\n"); + //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; } diff --git a/src/survive_process.c b/src/survive_process.c index 4b86144..42d2897 100644 --- a/src/survive_process.c +++ b/src/survive_process.c @@ -66,20 +66,34 @@ void survive_default_angle_process( SurviveObject * so, int sensor_id, int acode void survive_default_button_process(SurviveObject * so, uint8_t eventType, uint8_t buttonId, uint8_t axis1Id, uint16_t axis1Val, uint8_t axis2Id, uint16_t axis2Val) { // do nothing. - //printf("ButtonEntry: eventType:%x, buttonId:%d, axis1:%d, axis1Val:%8.8x, axis2:%d, axis2Val:%8.8x\n", - // eventType, - // buttonId, - // axis1Id, - // axis1Val, - // axis2Id, - // axis2Val); + printf("ButtonEntry: eventType:%x, buttonId:%d, axis1:%d, axis1Val:%8.8x, axis2:%d, axis2Val:%8.8x\n", + eventType, + buttonId, + axis1Id, + axis1Val, + axis2Id, + axis2Val); if (buttonId == 24 && eventType == 1) // trigger engage { - for (int j = 0; j < 40; j++) + for (int j = 0; j < 6; j++) { - for (int i = 0; i < 0x1; i++) + for (int i = 0; i < 0x5; i++) { survive_haptic(so, 0, 0xf401, 0xb5a2, 0x0100); + //survive_haptic(so, 0, 0xf401, 0xb5a2, 0x0100); + OGUSleep(1000); + } + OGUSleep(20000); + } + } + if (buttonId == 2 && eventType == 1) // trigger engage + { + for (int j = 0; j < 6; j++) + { + for (int i = 0; i < 0x1; i++) + { + survive_haptic(so, 0, 0xf401, 0x05a2, 0xf100); + //survive_haptic(so, 0, 0xf401, 0xb5a2, 0x0100); OGUSleep(5000); } OGUSleep(20000); @@ -87,6 +101,13 @@ void survive_default_button_process(SurviveObject * so, uint8_t eventType, uint8 } } +void survive_default_raw_pose_process(SurviveObject * so, uint8_t lighthouse, FLT *pos, FLT *quat) +{ + // print the pose; + printf("Pose: [%2.2x][% 08.8f,% 08.8f,% 08.8f] [% 08.8f,% 08.8f,% 08.8f,% 08.8f]\n", lighthouse, pos[0], pos[1], pos[2], quat[0], quat[1], quat[2], quat[3]); + +} + void survive_default_imu_process( SurviveObject * so, int mask, FLT * accelgyromag, uint32_t timecode, int id ) { if( so->PoserFn ) -- cgit v1.3.1 From ed85661121c3ed8769a69d059fd3162aec33c6fe Mon Sep 17 00:00:00 2001 From: Mike Turvey Date: Fri, 5 Jan 2018 04:09:54 -0700 Subject: Update test.c to do calibration --- src/poser_turveytori.c | 12 ++++++++++++ src/survive_process.c | 2 +- test.c | 2 ++ 3 files changed, 15 insertions(+), 1 deletion(-) (limited to 'src/survive_process.c') diff --git a/src/poser_turveytori.c b/src/poser_turveytori.c index 28e533e..ad30f6c 100644 --- a/src/poser_turveytori.c +++ b/src/poser_turveytori.c @@ -1856,6 +1856,18 @@ int PoserTurveyTori( SurviveObject * so, PoserData * poserData ) // } //} + for (int i=0; i < ctx->activeLighthouses; i++) + { + printf("Lighthouse Pose: [%1.1x][% 08.8f,% 08.8f,% 08.8f] [% 08.8f,% 08.8f,% 08.8f,% 08.8f]\n", + i, + ctx->bsd[i].Pose.Pos[0], + ctx->bsd[i].Pose.Pos[1], + ctx->bsd[i].Pose.Pos[2], + ctx->bsd[i].Pose.Rot[0], + ctx->bsd[i].Pose.Rot[1], + ctx->bsd[i].Pose.Rot[2], + ctx->bsd[i].Pose.Rot[3]); + } config_set_lighthouse(ctx->lh_config, ctx->bsd[0], 0); config_set_lighthouse(ctx->lh_config, ctx->bsd[1], 1); diff --git a/src/survive_process.c b/src/survive_process.c index 42d2897..1df24e9 100644 --- a/src/survive_process.c +++ b/src/survive_process.c @@ -104,7 +104,7 @@ void survive_default_button_process(SurviveObject * so, uint8_t eventType, uint8 void survive_default_raw_pose_process(SurviveObject * so, uint8_t lighthouse, FLT *pos, FLT *quat) { // print the pose; - printf("Pose: [%2.2x][% 08.8f,% 08.8f,% 08.8f] [% 08.8f,% 08.8f,% 08.8f,% 08.8f]\n", lighthouse, pos[0], pos[1], pos[2], quat[0], quat[1], quat[2], quat[3]); + printf("Pose: [%1.1x][%s][% 08.8f,% 08.8f,% 08.8f] [% 08.8f,% 08.8f,% 08.8f,% 08.8f]\n", lighthouse, so->codename, pos[0], pos[1], pos[2], quat[0], quat[1], quat[2], quat[3]); } diff --git a/test.c b/test.c index 4909d50..c9f03d7 100644 --- a/test.c +++ b/test.c @@ -85,6 +85,8 @@ int main() dump_iface( survive_get_so_by_name( ctx, "TR0" ), "TR0" ); dump_iface( survive_get_so_by_name( ctx, "WW0" ), "WW0" ); + survive_cal_install(ctx); + while(survive_poll(ctx) == 0) { double Now = OGGetAbsoluteTime(); -- cgit v1.3.1 From e642395e61691a6dab749c2085410117d88956f5 Mon Sep 17 00:00:00 2001 From: Mike Turvey Date: Sat, 6 Jan 2018 11:23:07 -0700 Subject: A few changes to test.c Hopefully, this will make it easier for anyone wanting to use libsurvive to have a simple starting point for doing so. --- src/survive_process.c | 68 ++++++++++++++++++++++---------------------- test.c | 79 +++++++++++++++++++++++++++++++++++++++++++++++---- 2 files changed, 107 insertions(+), 40 deletions(-) (limited to 'src/survive_process.c') diff --git a/src/survive_process.c b/src/survive_process.c index 1df24e9..0f19007 100644 --- a/src/survive_process.c +++ b/src/survive_process.c @@ -66,45 +66,45 @@ void survive_default_angle_process( SurviveObject * so, int sensor_id, int acode void survive_default_button_process(SurviveObject * so, uint8_t eventType, uint8_t buttonId, uint8_t axis1Id, uint16_t axis1Val, uint8_t axis2Id, uint16_t axis2Val) { // do nothing. - printf("ButtonEntry: eventType:%x, buttonId:%d, axis1:%d, axis1Val:%8.8x, axis2:%d, axis2Val:%8.8x\n", - eventType, - buttonId, - axis1Id, - axis1Val, - axis2Id, - axis2Val); - if (buttonId == 24 && eventType == 1) // trigger engage - { - for (int j = 0; j < 6; j++) - { - for (int i = 0; i < 0x5; i++) - { - survive_haptic(so, 0, 0xf401, 0xb5a2, 0x0100); - //survive_haptic(so, 0, 0xf401, 0xb5a2, 0x0100); - OGUSleep(1000); - } - OGUSleep(20000); - } - } - if (buttonId == 2 && eventType == 1) // trigger engage - { - for (int j = 0; j < 6; j++) - { - for (int i = 0; i < 0x1; i++) - { - survive_haptic(so, 0, 0xf401, 0x05a2, 0xf100); - //survive_haptic(so, 0, 0xf401, 0xb5a2, 0x0100); - OGUSleep(5000); - } - OGUSleep(20000); - } - } + //printf("ButtonEntry: eventType:%x, buttonId:%d, axis1:%d, axis1Val:%8.8x, axis2:%d, axis2Val:%8.8x\n", + // eventType, + // buttonId, + // axis1Id, + // axis1Val, + // axis2Id, + // axis2Val); + //if (buttonId == 24 && eventType == 1) // trigger engage + //{ + // for (int j = 0; j < 6; j++) + // { + // for (int i = 0; i < 0x5; i++) + // { + // survive_haptic(so, 0, 0xf401, 0xb5a2, 0x0100); + // //survive_haptic(so, 0, 0xf401, 0xb5a2, 0x0100); + // OGUSleep(1000); + // } + // OGUSleep(20000); + // } + //} + //if (buttonId == 2 && eventType == 1) // trigger engage + //{ + // for (int j = 0; j < 6; j++) + // { + // for (int i = 0; i < 0x1; i++) + // { + // survive_haptic(so, 0, 0xf401, 0x05a2, 0xf100); + // //survive_haptic(so, 0, 0xf401, 0xb5a2, 0x0100); + // OGUSleep(5000); + // } + // OGUSleep(20000); + // } + //} } void survive_default_raw_pose_process(SurviveObject * so, uint8_t lighthouse, FLT *pos, FLT *quat) { // print the pose; - printf("Pose: [%1.1x][%s][% 08.8f,% 08.8f,% 08.8f] [% 08.8f,% 08.8f,% 08.8f,% 08.8f]\n", lighthouse, so->codename, pos[0], pos[1], pos[2], quat[0], quat[1], quat[2], quat[3]); + //printf("Pose: [%1.1x][%s][% 08.8f,% 08.8f,% 08.8f] [% 08.8f,% 08.8f,% 08.8f,% 08.8f]\n", lighthouse, so->codename, pos[0], pos[1], pos[2], quat[0], quat[1], quat[2], quat[3]); } diff --git a/test.c b/test.c index c9f03d7..5fc62af 100644 --- a/test.c +++ b/test.c @@ -37,6 +37,69 @@ void HandleDestroy() { } +void testprog_button_process(SurviveObject * so, uint8_t eventType, uint8_t buttonId, uint8_t axis1Id, uint16_t axis1Val, uint8_t axis2Id, uint16_t axis2Val) +{ + survive_default_button_process(so, eventType, buttonId, axis1Id, axis1Val, axis2Id, axis2Val); + + // do nothing. + printf("ButtonEntry: eventType:%x, buttonId:%d, axis1:%d, axis1Val:%8.8x, axis2:%d, axis2Val:%8.8x\n", + eventType, + buttonId, + axis1Id, + axis1Val, + axis2Id, + axis2Val); + + // Note: the code for haptic feedback is not yet written to support wired USB connections to the controller. + // Work is still needed to reverse engineer that USB protocol. + + // let's do some haptic feedback if the trigger is pushed + if (buttonId == 24 && eventType == 1) // trigger engage + { + for (int j = 0; j < 6; j++) + { + for (int i = 0; i < 0x5; i++) + { + survive_haptic(so, 0, 0xf401, 0xb5a2, 0x0100); + //survive_haptic(so, 0, 0xf401, 0xb5a2, 0x0100); + OGUSleep(1000); + } + OGUSleep(20000); + } + } + + // let's do some haptic feedback if the touchpad is pressed. + if (buttonId == 2 && eventType == 1) // trigger engage + { + for (int j = 0; j < 6; j++) + { + for (int i = 0; i < 0x1; i++) + { + survive_haptic(so, 0, 0xf401, 0x05a2, 0xf100); + //survive_haptic(so, 0, 0xf401, 0xb5a2, 0x0100); + OGUSleep(5000); + } + OGUSleep(20000); + } + } +} + +void testprog_raw_pose_process(SurviveObject * so, uint8_t lighthouse, FLT *pos, FLT *quat) +{ + survive_default_raw_pose_process(so, lighthouse, pos, quat); + + // print the pose; + printf("Pose: [%1.1x][%s][% 08.8f,% 08.8f,% 08.8f] [% 08.8f,% 08.8f,% 08.8f,% 08.8f]\n", lighthouse, so->codename, pos[0], pos[1], pos[2], quat[0], quat[1], quat[2], quat[3]); +} + +void testprog_imu_process(SurviveObject * so, int mask, FLT * accelgyromag, uint32_t timecode, int id) +{ + survive_default_imu_process(so, mask, accelgyromag, timecode, id); + + // so something here, like printing out the data... + +} + static void dump_iface( struct SurviveObject * so, const char * prefix ) { @@ -73,6 +136,10 @@ int main() ctx = survive_init( 0 ); + survive_install_button_fn(ctx, testprog_button_process); + survive_install_raw_pose_fn(ctx, testprog_raw_pose_process); + survive_install_imu_fn(ctx, testprog_imu_process); + if( !ctx ) { fprintf( stderr, "Fatal. Could not start\n" ); @@ -89,12 +156,12 @@ int main() while(survive_poll(ctx) == 0) { - double Now = OGGetAbsoluteTime(); - if( Now > (Start+1) && !magicon ) - { - survive_send_magic(ctx,1,0,0); - magicon = 1; - } + //double Now = OGGetAbsoluteTime(); + //if( Now > (Start+1) && !magicon ) + //{ + // survive_send_magic(ctx,1,0,0); + // magicon = 1; + //} //Do stuff. } } -- cgit v1.3.1