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

1 files changed, 38 insertions, 4 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);