Diff: imu.cpp

Revision:

4:8f63393d49fb

Parent:

3:f04d3d10d518

--- a/imu.cpp	Sat Nov 05 01:02:59 2011 +0000
+++ b/imu.cpp	Mon Jul 08 04:04:21 2013 +0000
@@ -6,7 +6,7 @@
  * 20.138444, -15.096182, 232.355290, -29.691927, -14.679685, 45.097056, -75.690469, 0.737977, -159.505392
  */
 
-IMU::IMU(int baud, PinName tx, PinName rx, Serial* pc){
+IMU::IMU(int baud, PinName tx, PinName rx, Serial* pc) {
     p_pc = pc;
     p_pc->printf("Initializing IMU...\n");
     p_device = new Serial(tx, rx);
@@ -15,7 +15,7 @@
     readable = false;
     i_IMU = 0;
     accX = accY = accZ = gyrX = gyrY = gyrZ = magX = magY = magZ = 0;
-    p_device->putc('6'); //Tell the IMU to start sending binary data, if it isn't already
+    p_device->putc('4'); //Tell the IMU to start sending binary data, if it isn't already
     newData = false;
 }
 
@@ -80,7 +80,7 @@
 // in unsigned form so all of the bits are switched and we switch it back and center
 // it around 512 which we are using as out zero value
 inline void IMU::makeCorrect(short* i) {
-    if ((*i)>>15) *i = 0-(~(*i));
+    if ((*i)>>15) *i = 0-(~(*i)+1);
     else *i = 0+*i;
 }