improve itg3200 driver

1 files changed, 37 insertions, 6 deletions

diff --git a/sensors/gyroscope/itg3200.vala b/sensors/gyroscope/itg3200.vala
index 9c33463..e616848 100644
--- a/sensors/gyroscope/itg3200.vala
+++ b/sensors/gyroscope/itg3200.vala
@@ -15,12 +15,25 @@
 
 public class ITG3200 : I2CDevice, Gyroscope {
 	public int[] rate { get; set; }
+	private int[] zero = new int[3];
 
 	/* ITG3200 14.375 LSBs per °/sec in radians */
-	private static const double scale_factor = (1.0 / 14.375) * (180.0 / Math.PI);
+	private static const double scale = 14.375;
 
 	public ITG3200(uint8 dev, uint8 addr = 0x68) throws I2CError {
 			setup(dev, addr);
+			devsetup();
+	}
+
+	private void devsetup() throws I2CError {
+		if((get_byte(0x00) & 0x7e) != 0x68)
+			log("ITG3200", LogLevelFlags.LEVEL_ERROR, @"ITG3200: invalid device id");
+		else
+			log("ITG3200", LogLevelFlags.LEVEL_DEBUG, @"ITG3200: correct device id");
+
+		/* setup FS_SEL for "proper operation" */
+		set_byte(0x16, 0x18);
+		calibrate();
 	}
 
 	public void init(KeyFile cfg) throws KeyFileError, I2CError {
@@ -29,12 +42,30 @@ public class ITG3200 : I2CDevice, Gyroscope {
 		var adapter = cfg.get_uint64("ITG3200", "i2c-adapter");
 		var address = cfg.get_uint64("ITG3200", "i2c-address");
 		setup((uint8) adapter, (uint8) address);
+		devsetup();
 	}
 
 	public uint8 get_address() throws I2CError {
 		return (uint8) get_byte(0x00);
 	}
 
+	public void calibrate() throws I2CError {
+		uint8 samples = 10;
+		zero[AXIS.X] = 0;
+		zero[AXIS.Y] = 0;
+		zero[AXIS.Z] = 0;
+
+		for(int i=0; i<samples; i++) {
+			zero[AXIS.X] += ((int16) get_big_word(0x1d));
+			zero[AXIS.Y] += ((int16) get_big_word(0x1f));
+			zero[AXIS.Z] += ((int16) get_big_word(0x21));
+		}
+
+		zero[AXIS.X] /= samples;
+		zero[AXIS.Y] /= samples;
+		zero[AXIS.Z] /= samples;
+	}
+
 	/* TODO: add method for register 0x16 (frequency + range) */
 	/* TODO: add method for register 0x17 (interrupt config) */
 	/* TODO: add method for register 0x3E (power management) */
@@ -54,13 +85,13 @@ public class ITG3200 : I2CDevice, Gyroscope {
 	}
 
 	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);
+		x = ((int16) get_big_word(0x1d)-zero[AXIS.X]) / scale;
+		y = ((int16) get_big_word(0x1f)-zero[AXIS.Y]) / scale;
+		z = ((int16) get_big_word(0x21)-zero[AXIS.Z]) / scale;
 	}
 }
 
 public Type register_plugin(Module module) {
-	// types are registered automatically
-	return typeof(ITG3200);
+       // types are registered automatically
+       return typeof(ITG3200);
 }