Unify the location of the updates to the IMU to use G's and rads/sec

2 files changed, 39 insertions, 32 deletions

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 );
 			}
 		}