psvr: improve scaling

It is still unclear how to interpret the calibration data
from the device, but these scaling factors should be closer
to the truth.

src/drv_psvr/psvr.c

@@ -37,11 +37,18 @@ typedef struct {
 
 } psvr_priv;
 
-void vec3f_from_psvr_vec(const int16_t* smp, vec3f* out_vec)
+void accel_from_psvr_vec(const int16_t* smp, vec3f* out_vec)
 {
-	out_vec->x = (float)smp[1] * 0.001f;
-	out_vec->y = (float)smp[0] * 0.001f;
-	out_vec->z = (float)smp[2] * 0.001f * -1.0f;
+	out_vec->x = (float)smp[1] *  (9.81 / 16384);
+	out_vec->y = (float)smp[0] *  (9.81 / 16384);
+	out_vec->z = (float)smp[2] * -(9.81 / 16384);
+}
+
+void gyro_from_psvr_vec(const int16_t* smp, vec3f* out_vec)
+{
+	out_vec->x = (float)smp[1] * 0.00105f;
+	out_vec->y = (float)smp[0] * 0.00105f;
+	out_vec->z = (float)smp[2] * 0.00105f * -1.0f;
 }
 
 static void handle_tracker_sensor_msg(psvr_priv* priv, unsigned char* buffer, int size)
@@ -69,8 +76,8 @@ static void handle_tracker_sensor_msg(psvr_priv* priv, unsigned char* buffer, in
 	vec3f mag = {{0.0f, 0.0f, 0.0f}};
 
 	for(int i = 0; i < 2; i++){
-		vec3f_from_psvr_vec(s->samples[i].accel, &priv->raw_accel);
-		vec3f_from_psvr_vec(s->samples[i].gyro, &priv->raw_gyro);
+		accel_from_psvr_vec(s->samples[0].accel, &priv->raw_accel);
+		gyro_from_psvr_vec(s->samples[0].gyro, &priv->raw_gyro);
 
 		ofusion_update(&priv->sensor_fusion, dt, &priv->raw_gyro, &priv->raw_accel, &mag);