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(-) 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.2.3