use calibration numbers from device json files

2 files changed, 83 insertions, 0 deletions

diff --git a/include/libsurvive/survive.h b/include/libsurvive/survive.h
index e85fe48..1532d62 100644
--- a/include/libsurvive/survive.h
+++ b/include/libsurvive/survive.h
@@ -62,6 +62,12 @@ struct SurviveObject
 	uint32_t last_lighttime;  //May be a 24- or 32- bit number depending on what device.
 
 
+	FLT* acc_bias; // size is FLT*3. contains x,y,z
+	FLT* acc_scale; // size is FLT*3. contains x,y,z
+	FLT* gyro_bias; // size is FLT*3. contains x,y,z
+	FLT* gyro_scale; // size is FLT*3. contains x,y,z
+
+
 	//Debug
 	int tsl;
 };
diff --git a/src/survive_vive.c b/src/survive_vive.c
index 1f6a975..720df77 100755
--- a/src/survive_vive.c
+++ b/src/survive_vive.c
@@ -22,6 +22,8 @@
 #include <malloc.h> // for alloca
 #endif
 
+#include "json_helpers.h"
+
 #ifdef HIDAPI
 #if defined(WINDOWS) || defined(WIN32) || defined (_WIN32)
 #include <windows.h>
@@ -1057,6 +1059,34 @@ static void handle_watchman( SurviveObject * w, uint8_t * readdata )
 	}
 }
 
+void calibrate_acc(SurviveObject* so, FLT* agm) {
+	if (so->acc_bias != NULL) {
+		agm[0] -= so->acc_bias[0];
+		agm[1] -= so->acc_bias[1];
+		agm[2] -= so->acc_bias[2];
+	}
+
+	if (so->acc_scale != NULL) {
+		agm[0] *= so->acc_scale[0];
+		agm[1] *= so->acc_scale[1];
+		agm[2] *= so->acc_scale[2];
+	}
+}
+
+void calibrate_gyro(SurviveObject* so, FLT* agm) {
+	if (so->gyro_bias != NULL) {
+		agm[0] -= so->gyro_bias[0];
+		agm[1] -= so->gyro_bias[1];
+		agm[2] -= so->gyro_bias[2];
+	}
+
+	if (so->gyro_bias != NULL) {
+		agm[0] *= so->gyro_scale[0];
+		agm[1] *= so->gyro_scale[1];
+		agm[2] *= so->gyro_scale[2];
+	}
+}
+
 
 void survive_data_cb( SurviveUSBInterface * si )
 {
@@ -1127,12 +1157,16 @@ void survive_data_cb( SurviveUSBInterface * si )
 								acceldata[3], acceldata[4], acceldata[5],
 								0,0,0 };
 
+				calibrate_acc(obj, agm);
+
 				//1G for accelerometer, from MPU6500 datasheet
 				//this can change if the firmware changes the sensitivity.
 				agm[0]/=8192.0f;
 				agm[1]/=8192.0f;
 				agm[2]/=8192.0f;
 
+				calibrate_gyro(obj, agm+3);
+
 				//65.5 deg/s for gyroscope, from MPU6500 datasheet
 				//1000 deg/s for gyroscope, from MPU6500 datasheet
 				agm[3]/=65.5f;
@@ -1322,6 +1356,37 @@ printf( "Loading config: %d\n", len );
 					break;
 				}
 			}
+
+
+			if (jsoneq(ct0conf, tk, "acc_bias") == 0) {
+				int32_t count = (tk+1)->size;
+				FLT* values = NULL;
+				if ( parse_float_array(ct0conf, tk+2, &values, count) >0 ) {
+					so->acc_bias = values;
+				}
+			}
+			if (jsoneq(ct0conf, tk, "acc_scale") == 0) {
+				int32_t count = (tk+1)->size;
+				FLT* values = NULL;
+				if ( parse_float_array(ct0conf, tk+2, &values, count) >0 ) {
+					so->acc_scale = values;
+				}
+			}
+
+			if (jsoneq(ct0conf, tk, "gyro_bias") == 0) {
+				int32_t count = (tk+1)->size;
+				FLT* values = NULL;
+				if ( parse_float_array(ct0conf, tk+2, &values, count) >0 ) {
+					so->gyro_bias = values;
+				}
+			}
+			if (jsoneq(ct0conf, tk, "gyro_scale") == 0) {
+				int32_t count = (tk+1)->size;
+				FLT* values = NULL;
+				if ( parse_float_array(ct0conf, tk+2, &values, count) >0 ) {
+					so->gyro_scale = values;
+				}
+			}
 		}
 	}
 	else
@@ -1361,6 +1426,12 @@ int survive_vive_close( SurviveContext * ctx, void * driver )
 	return 0;
 }
 
+void init_SurviveObject(SurviveObject* so) {
+	so->acc_scale = NULL;
+	so->acc_bias = NULL;
+	so->gyro_scale = NULL;
+	so->gyro_bias = NULL;
+}
 
 int DriverRegHTCVive( SurviveContext * ctx )
 {
@@ -1372,6 +1443,12 @@ int DriverRegHTCVive( SurviveContext * ctx )
 	SurviveObject * ww0 = calloc( 1, sizeof( SurviveObject ) );
 	SurviveViveData * sv = calloc( 1, sizeof( SurviveViveData ) );
 
+	init_SurviveObject(hmd);
+	init_SurviveObject(wm0);
+	init_SurviveObject(wm1);
+	init_SurviveObject(tr0);
+	init_SurviveObject(ww0);
+
 	sv->ctx = ctx;
 	
 	#ifdef _WIN32