Update code to permit wireless watchmen use.

3 files changed, 14 insertions, 12 deletions

diff --git a/src/poser_charlesrefine.c b/src/poser_charlesrefine.c
index ce14f70..cc188fb 100644
--- a/src/poser_charlesrefine.c
+++ b/src/poser_charlesrefine.c
@@ -197,7 +197,7 @@ int PoserCharlesRefine(SurviveObject *so, PoserData *pd) {
 #define CORRECT_LATERAL_POSITION_COEFFICIENT 0.7 // Explodes if you exceed 1.0
 #define CORRECT_TELESCOPTION_COEFFICIENT 7.00	 // Converges even as high as 10.0 and doesn't explode.
 #define CORRECT_ROTATION_COEFFICIENT                                                                                   \
-	1.0 // This starts to fall apart above 5.0, but for good reason.  It is amplified by the number of points seen.
+	0.2 // This starts to fall apart above 5.0, but for good reason.  It is amplified by the number of points seen.
 #define ROTATIONAL_CORRECTION_MAXFORCE 0.01
 
 			// Step 1: Determine standard of deviation, and average in order to
diff --git a/src/survive_default_devices.c b/src/survive_default_devices.c
index 3c102b2..954c47a 100644
--- a/src/survive_default_devices.c
+++ b/src/survive_default_devices.c
@@ -190,7 +190,14 @@ int survive_load_htc_config_format(SurviveObject *so, char *ct0conf, int len) {
 	}
 	else if( memcmp( so->codename, "WM", 2 ) == 0 )
 	{
-		//??!!?? No one has yet decoded the watchman accelerometer data.
+		if( so->acc_scale ) scale3d( so->acc_scale, so->acc_scale, 2./8192.0 );
+		if( so->acc_bias )	scale3d( so->acc_bias, so->acc_bias, 2./1000. );	//Need to verify.
+		if( so->gyro_scale ) scale3d( so->gyro_scale, so->gyro_scale, 3.14159 / 1800. / 1.8 ); //??! 1.8 feels right but why?!
+		int j;
+		for (j = 0; j < so->sensor_ct; j++) {
+			so->sensor_locations[j * 3 + 0] *= 1.0;
+		}
+
 	}
 	else //Verified on WW, Need to verify on Tracker.
 	{
diff --git a/src/survive_vive.c b/src/survive_vive.c
index 60b3dc9..83bc977 100755
--- a/src/survive_vive.c
+++ b/src/survive_vive.c
@@ -1233,20 +1233,15 @@ static void handle_watchman( SurviveObject * w, uint8_t * readdata )
 	if( ( ( type & 0xe8 ) == 0xe8 ) || doimu ) //Hmm, this looks kind of yucky... we can get e8's that are accelgyro's but, cleared by first propset.
 	{
 		propset |= 2;
-		//XXX XXX BIG TODO!!! Actually recal gyro data.
-		FLT agm[9] = { readdata[1], readdata[2], readdata[3],
-						readdata[4], readdata[5], readdata[6],
-						0,0,0 };
-
-//		if (w->acc_scale) printf("%f %f %f\n",w->acc_scale[0],w->acc_scale[1],w->acc_scale[2]);
-		//I don't understand where these numbers come from but the data from the WMD seems to max out at 255...
+		FLT agm[9] = { 0, 0, 0, 0, 0, 0, 0, 0, 0 };
+		int j;
+		for( j = 0; j < 6; j++ )
+			agm[j] = (int16_t)(readdata[j*2+1] | (readdata[j*2+2]<<8));
 		calibrate_acc(w, agm);
 		calibrate_gyro(w, agm+3);
 		w->ctx->imuproc( w, 3, agm, (time1<<24)|(time2<<16)|readdata[0], 0 );
-
-		int16_t * k = (int16_t *)readdata+1;
-		//printf( "Match8 %d %d %d %d %d %3d %3d\n", qty, k[0], k[1], k[2], k[3], k[4], k[5] );
 		readdata += 13; qty -= 13;
+
 		type &= ~0xe8;
 		if( qty )
 		{