From 2b700c8f2ddf5759e75cf7c24d70cbd0436b22ac Mon Sep 17 00:00:00 2001
From: Sn0MaN <clayoung@hotmail.com>
Date: Tue, 24 Jun 2014 23:53:50 -0600
Subject: [PATCH 1/2] Add Readme.txt

---
 Readme.txt | 0
 1 file changed, 0 insertions(+), 0 deletions(-)
 create mode 100644 Readme.txt

diff --git a/Readme.txt b/Readme.txt
new file mode 100644
index 0000000..e69de29

From d1327e6828496b0529fcfe0f2b93862bc41c6363 Mon Sep 17 00:00:00 2001
From: Sn0MaN <clayoung@hotmail.com>
Date: Wed, 25 Jun 2014 00:32:22 -0600
Subject: [PATCH 2/2] Add Readme.txt

---
 Arduino/Razor_AHRS/Sensors.ino | 38 ++++++++++++++++++++++++++--------
 Readme.txt                     |  4 ++++
 2 files changed, 33 insertions(+), 9 deletions(-)

diff --git a/Arduino/Razor_AHRS/Sensors.ino b/Arduino/Razor_AHRS/Sensors.ino
index 4c1aa4f..fd04a95 100644
--- a/Arduino/Razor_AHRS/Sensors.ino
+++ b/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
   {
diff --git a/Readme.txt b/Readme.txt
index e69de29..ba7a630 100644
--- a/Readme.txt
+++ b/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.
\ No newline at end of file