From 55f498db296ff353a0cf870c51bffd92cc360484 Mon Sep 17 00:00:00 2001 From: cnlohr Date: Fri, 6 Apr 2018 19:39:27 -0400 Subject: Unify the location of the updates to the IMU to use G's and rads/sec --- src/survive_default_devices.c | 42 ++++++++++++++++++++++++++++++++++++++---- src/survive_vive.c | 29 +---------------------------- 2 files changed, 39 insertions(+), 32 deletions(-) (limited to 'src') diff --git a/src/survive_default_devices.c b/src/survive_default_devices.c index 2e47b9e..3c102b2 100644 --- a/src/survive_default_devices.c +++ b/src/survive_default_devices.c @@ -144,10 +144,6 @@ int survive_load_htc_config_format(SurviveObject *so, char *ct0conf, int len) { FLT *values = NULL; if (parse_float_array(ct0conf, tk + 2, &values, count) > 0) { so->acc_bias = values; - const FLT bias_units = 1. / 1000.; // I deeply suspect bias is in milligravities -JB - so->acc_bias[0] *= bias_units; - so->acc_bias[1] *= bias_units; - so->acc_bias[2] *= bias_units; } } if (jsoneq(ct0conf, tk, "acc_scale") == 0) { @@ -174,6 +170,44 @@ int survive_load_htc_config_format(SurviveObject *so, char *ct0conf, int len) { } } + + //Handle device-specific sacling. + if( strcmp( so->codename, "HMD" ) == 0 ) + { + if( so->acc_scale ) + { + so->acc_scale[0] *= -1./8192.0; + so->acc_scale[1] *= -1./8192.0; + so->acc_scale[2] *= 1./8192.0; + } + if( so->acc_bias ) scale3d( so->acc_bias, so->acc_bias, 2./1000. ); //Odd but seems right. + if( so->gyro_scale ) + { + so->gyro_scale[0] *= -0.000065665; + so->gyro_scale[1] *= -0.000065665; + so->gyro_scale[2] *= 0.000065665; + } + } + else if( memcmp( so->codename, "WM", 2 ) == 0 ) + { + //??!!?? No one has yet decoded the watchman accelerometer data. + } + else //Verified on WW, Need to verify on Tracker. + { + //1G for accelerometer, from MPU6500 datasheet + //this can change if the firmware changes the sensitivity. + // When coming off of USB, these values are in units of .5g -JB + if( so->acc_scale ) scale3d( so->acc_scale, so->acc_scale, 2./8192.0 ); + + //If any other device, we know we at least need this. + // I deeply suspect bias is in milligravities -JB + if( so->acc_bias ) scale3d( so->acc_bias, so->acc_bias, 1./1000. ); + + // From datasheet, can be 250, 500, 1000, 2000 deg/s range over 16 bits + // FLT deg_per_sec = 250; + if( so->gyro_scale )scale3d( so->gyro_scale, so->gyro_scale, 3.14159 / 1800. ); + } + char fname[64]; sprintf(fname, "calinfo/%s_points.csv", so->codename); diff --git a/src/survive_vive.c b/src/survive_vive.c index 1ffb737..60b3dc9 100755 --- a/src/survive_vive.c +++ b/src/survive_vive.c @@ -1239,20 +1239,9 @@ static void handle_watchman( SurviveObject * w, uint8_t * readdata ) 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]*=(1.0f/255.0f); - agm[1]*=(1.0f/255.0f); - agm[2]*=(1.0f/255.0f); - + calibrate_acc(w, agm); 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]*=(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 ); int16_t * k = (int16_t *)readdata+1; @@ -1495,24 +1484,8 @@ void survive_data_cb( SurviveUSBInterface * si ) 0, 0}; - //1G for accelerometer, from MPU6500 datasheet - //this can change if the firmware changes the sensitivity. - // When coming off of USB, these values are in units of .5g -JB - agm[0] *= (float)(2. / 8192.0); - agm[1] *= (float)(2. / 8192.0); - agm[2] *= (float)(2. / 8192.0); calibrate_acc(obj, agm); - - // From datasheet, can be 250, 500, 1000, 2000 deg/s range over 16 bits - // FLT deg_per_sec = 250; - // FLT conv = (float)((1./deg_per_sec)*(3.14159/180.)) / 8192.; - FLT DEGREES_TO_RADS = 3.14159 / 180.; - FLT conv = 1. / 10. * DEGREES_TO_RADS; calibrate_gyro(obj, agm + 3); - agm[3] *= conv; - agm[4] *= conv; - agm[5] *= conv; - ctx->imuproc( obj, 3, agm, timecode, code ); } } -- cgit v1.2.3