normalize gyro data

3 files changed, 10 insertions, 8 deletions

diff --git a/main.vala b/main.vala
index 98a1eea..60b1c17 100644
--- a/main.vala
+++ b/main.vala
@@ -27,13 +27,12 @@ Kinematics kinematics;
 bool loop() {
 	try {
 		/* get sensor data */
-		int16 gx, gy, gz, ax, ay, az, mx, my, mz;
+		double gx, gy, gz;
+		int16 ax, ay, az, mx, my, mz;
 		gyroscope.get_data(out gx, out gy, out gz);
 		accelerometer.get_data(out ax, out ay, out az);
 		compass.get_data(out mx, out my, out mz);
 
-		log("MainLoop", LogLevelFlags.LEVEL_DEBUG, @"sensors: GYRO: $gx $gy $gz, ACCEL: $ax $ay $az, COMPASS: $mx $my $mz");
-
 		/* calculate kinematics (TODO: calculate time diff) */
 		kinematics.update(gx, gy, gz, ax, ay, az, mx, my, mz, 0.1);
 
diff --git a/sensors/gyroscope.vala b/sensors/gyroscope.vala
index 8cdc0cd..07452ec 100644
--- a/sensors/gyroscope.vala
+++ b/sensors/gyroscope.vala
@@ -24,6 +24,6 @@ public interface Gyroscope : Device {
 	ulong gyroLastMesuredTime = 0;
 #endif
 	public abstract void init(KeyFile cfg) throws Error;
-	public abstract void get_data(out int16 x, out int16 y, out int16 z) throws I2CError;
+	public abstract void get_data(out double x, out double y, out double z) throws I2CError;
 	public abstract int[] rate { get; set; }
 }
diff --git a/sensors/gyroscope/itg3200.vala b/sensors/gyroscope/itg3200.vala
index d00c853..ac3e2f0 100644
--- a/sensors/gyroscope/itg3200.vala
+++ b/sensors/gyroscope/itg3200.vala
@@ -16,6 +16,9 @@
 public class ITG3200 : I2CDevice, Gyroscope {
 	public int[] rate { get; set; }
 
+	/* ITG3200 14.375 LSBs per °/sec in radians */
+	private static double scale_factor = (1.0 / 14.375) * (180.0 / Math.PI);
+
 	public ITG3200(uint8 dev, uint8 addr = 0x68) throws I2CError {
 			setup(dev, addr);
 	}
@@ -50,10 +53,10 @@ public class ITG3200 : I2CDevice, Gyroscope {
 		return 350 + ((raw + 13200) / 28);
 	}
 
-	public void get_data(out int16 x, out int16 y, out int16 z) throws I2CError {
-		x = (int16) get_big_word(0x1d);
-		y = (int16) get_big_word(0x1f);
-		z = (int16) get_big_word(0x21);
+	public void get_data(out double x, out double y, out double z) throws I2CError {
+		x = ((int16) get_big_word(0x1d) * scale_factor);
+		y = ((int16) get_big_word(0x1f) * scale_factor);
+		z = ((int16) get_big_word(0x21) * scale_factor);
 	}
 }