summaryrefslogtreecommitdiffstats
diff options
context:
space:
mode:
authorSebastian Reichel <sre@ring0.de>2012-10-14 22:43:16 +0200
committerSebastian Reichel <sre@ring0.de>2012-10-14 22:43:16 +0200
commiteb440fbdd480af0c071b43a3bbc46c23ff40a6dd (patch)
tree95522b064fdb9f133eba0ddcb588af29bfda5a1a
parent25ac6a9d3096ec740db3102a6a2ca33b20d6b1c5 (diff)
downloadmicrocopterd-eb440fbdd480af0c071b43a3bbc46c23ff40a6dd.tar.bz2
normalize gyro data
-rw-r--r--main.vala5
-rw-r--r--sensors/gyroscope.vala2
-rw-r--r--sensors/gyroscope/itg3200.vala11
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);
}
}