Modified Sensors.ino to work with Teensy 3.1

Arduino/Razor_AHRS/Sensors.ino

@@ -66,9 +66,16 @@ void Read_Accel()
   {
     // No multiply by -1 for coordinate system transformation here, because of double negation:
     // We want the gravity vector, which is negated acceleration vector.
-    accel[0] = (((int) buff[3]) << 8) | buff[2];  // X axis (internal sensor y axis)
-    accel[1] = (((int) buff[1]) << 8) | buff[0];  // Y axis (internal sensor x axis)
-    accel[2] = (((int) buff[5]) << 8) | buff[4];  // Z axis (internal sensor z axis)
+
+    // ORIGINAL CODE HERE:
+//    accel[0] = (((int) buff[3]) << 8) | buff[2];  // X axis (internal sensor y axis)
+//    accel[1] = (((int) buff[1]) << 8) | buff[0];  // Y axis (internal sensor x axis)
+//    accel[2] = (((int) buff[5]) << 8) | buff[4];  // Z axis (internal sensor z axis)
+
+    // MODIFIED CODE HERE:
+    accel[0] = (int16_t)((((int) buff[3]) << 8) | buff[2]);  // X axis (internal sensor y axis)
+    accel[1] = (int16_t)((((int) buff[1]) << 8) | buff[0]);  // Y axis (internal sensor x axis)
+    accel[2] = (int16_t)((((int) buff[5]) << 8) | buff[4]);  // Z axis (internal sensor z axis)
   }
   else
   {
@@ -133,9 +140,16 @@ void Read_Magn()
 // 9DOF Sensor Stick SEN-10724 using HMC5883L magnetometer
 #elif HW__VERSION_CODE == 10724
     // MSB byte first, then LSB; Y and Z reversed: X, Z, Y
-    magnetom[0] = (((int) buff[0]) << 8) | buff[1];         // X axis (internal sensor x axis)
-    magnetom[1] = -1 * ((((int) buff[4]) << 8) | buff[5]);  // Y axis (internal sensor -y axis)
-    magnetom[2] = -1 * ((((int) buff[2]) << 8) | buff[3]);  // Z axis (internal sensor -z axis)
+
+    // ORIGINAL CODE HERE:
+//    magnetom[0] = (((int) buff[0]) << 8) | buff[1];         // X axis (internal sensor x axis)
+//    magnetom[1] = -1 * ((((int) buff[4]) << 8) | buff[5]);  // Y axis (internal sensor -y axis)
+//    magnetom[2] = -1 * ((((int) buff[2]) << 8) | buff[3]);  // Z axis (internal sensor -z axis)
+
+    // MODIFIED CODE HERE:
+    magnetom[0] = (int16_t)(((int) buff[0]) << 8) | buff[1];         // X axis (internal sensor x axis)
+    magnetom[1] = -1 * (int16_t)((((int) buff[4]) << 8) | buff[5]);  // Y axis (internal sensor -y axis)
+    magnetom[2] = -1 * (int16_t)((((int) buff[2]) << 8) | buff[3]);  // Z axis (internal sensor -z axis)
 #endif
   }
   else
@@ -198,9 +212,15 @@ void Read_Gyro()
 
   if (i == 6)  // All bytes received?
   {
-    gyro[0] = -1 * ((((int) buff[2]) << 8) | buff[3]);    // X axis (internal sensor -y axis)
-    gyro[1] = -1 * ((((int) buff[0]) << 8) | buff[1]);    // Y axis (internal sensor -x axis)
-    gyro[2] = -1 * ((((int) buff[4]) << 8) | buff[5]);    // Z axis (internal sensor -z axis)
+    // ORIGINAL CODE HERE:
+//    gyro[0] = -1 * ((((int) buff[2]) << 8) | buff[3]);    // X axis (internal sensor -y axis)
+//    gyro[1] = -1 * ((((int) buff[0]) << 8) | buff[1]);    // Y axis (internal sensor -x axis)
+//    gyro[2] = -1 * ((((int) buff[4]) << 8) | buff[5]);    // Z axis (internal sensor -z axis)
+
+    // MODIFIED CODE HERE:
+    gyro[0] = -1 * (int16_t)((((int) buff[2]) << 8) | buff[3]);    // X axis (internal sensor -y axis)
+    gyro[1] = -1 * (int16_t)((((int) buff[0]) << 8) | buff[1]);    // Y axis (internal sensor -x axis)
+    gyro[2] = -1 * (int16_t)((((int) buff[4]) << 8) | buff[5]);    // Z axis (internal sensor -z axis)
   }
   else
   {

Readme.txt

@@ -0,0 +1,4 @@
+
+Modified Sensors.ino to work with Teensy 3.1
+
+Type casted the Output x,y,z Values for the Read_"Sensor" Functions for each Sensor to int16_t.