Revision 0:9a72d42c0da3, committed 2011-12-27

Comitter:

lpetre

Date:

Tue Dec 27 17:20:06 2011 +0000

Child:

1:e27c4c0b71d8

Commit message:

Changed in this revision

--- /dev/null	Thu Jan 01 00:00:00 1970 +0000
+++ b/Compass.cpp	Tue Dec 27 17:20:06 2011 +0000
@@ -0,0 +1,25 @@
+/* This file is part of the Razor AHRS Firmware */
+#include "Razor_AHRS.h"
+#include <math.h>
+
+void IMU::Compass_Heading()
+{
+  float mag_x;
+  float mag_y;
+  float cos_roll;
+  float sin_roll;
+  float cos_pitch;
+  float sin_pitch;
+  
+  cos_roll = cos(roll);
+  sin_roll = sin(roll);
+  cos_pitch = cos(pitch);
+  sin_pitch = sin(pitch);
+  
+  // Tilt compensated magnetic field X
+  mag_x = magnetom[0]*cos_pitch + magnetom[1]*sin_roll*sin_pitch + magnetom[2]*cos_roll*sin_pitch;
+  // Tilt compensated magnetic field Y
+  mag_y = magnetom[1]*cos_roll - magnetom[2]*sin_roll;
+  // Magnetic Heading
+  MAG_Heading = atan2(-mag_y, mag_x);
+}

--- /dev/null	Thu Jan 01 00:00:00 1970 +0000
+++ b/DCM.cpp	Tue Dec 27 17:20:06 2011 +0000
@@ -0,0 +1,129 @@
+/* This file is part of the Razor AHRS Firmware */
+#include "Razor_AHRS.h"
+#include <math.h>
+
+// DCM algorithm
+
+/**************************************************/
+void IMU::Normalize(void)
+{
+  float error=0;
+  float temporary[3][3];
+  float renorm=0;
+  
+  error= -Vector_Dot_Product(&DCM_Matrix[0][0],&DCM_Matrix[1][0])*.5; //eq.19
+
+  Vector_Scale(&temporary[0][0], &DCM_Matrix[1][0], error); //eq.19
+  Vector_Scale(&temporary[1][0], &DCM_Matrix[0][0], error); //eq.19
+  
+  Vector_Add(&temporary[0][0], &temporary[0][0], &DCM_Matrix[0][0]);//eq.19
+  Vector_Add(&temporary[1][0], &temporary[1][0], &DCM_Matrix[1][0]);//eq.19
+  
+  Vector_Cross_Product(&temporary[2][0],&temporary[0][0],&temporary[1][0]); // c= a x b //eq.20
+  
+  renorm= .5 *(3 - Vector_Dot_Product(&temporary[0][0],&temporary[0][0])); //eq.21
+  Vector_Scale(&DCM_Matrix[0][0], &temporary[0][0], renorm);
+  
+  renorm= .5 *(3 - Vector_Dot_Product(&temporary[1][0],&temporary[1][0])); //eq.21
+  Vector_Scale(&DCM_Matrix[1][0], &temporary[1][0], renorm);
+  
+  renorm= .5 *(3 - Vector_Dot_Product(&temporary[2][0],&temporary[2][0])); //eq.21
+  Vector_Scale(&DCM_Matrix[2][0], &temporary[2][0], renorm);
+}
+
+/**************************************************/
+void IMU::Drift_correction(void)
+{
+  float mag_heading_x;
+  float mag_heading_y;
+  float errorCourse;
+  //Compensation the Roll, Pitch and Yaw drift. 
+  static float Scaled_Omega_P[3];
+  static float Scaled_Omega_I[3];
+  float Accel_magnitude;
+  float Accel_weight;
+  
+  
+  //*****Roll and Pitch***************
+
+  // Calculate the magnitude of the accelerometer vector
+  Accel_magnitude = sqrt(Accel_Vector[0]*Accel_Vector[0] + Accel_Vector[1]*Accel_Vector[1] + Accel_Vector[2]*Accel_Vector[2]);
+  Accel_magnitude = Accel_magnitude / GRAVITY; // Scale to gravity.
+  // Dynamic weighting of accelerometer info (reliability filter)
+  // Weight for accelerometer info (<0.5G = 0.0, 1G = 1.0 , >1.5G = 0.0)
+  Accel_weight = constrain(1 - 2*abs(1 - Accel_magnitude),0,1);  //  
+
+  Vector_Cross_Product(&errorRollPitch[0],&Accel_Vector[0],&DCM_Matrix[2][0]); //adjust the ground of reference
+  Vector_Scale(&Omega_P[0],&errorRollPitch[0],Kp_ROLLPITCH*Accel_weight);
+  
+  Vector_Scale(&Scaled_Omega_I[0],&errorRollPitch[0],Ki_ROLLPITCH*Accel_weight);
+  Vector_Add(Omega_I,Omega_I,Scaled_Omega_I);     
+  
+  //*****YAW***************
+  // We make the gyro YAW drift correction based on compass magnetic heading
+ 
+  mag_heading_x = cos(MAG_Heading);
+  mag_heading_y = sin(MAG_Heading);
+  errorCourse=(DCM_Matrix[0][0]*mag_heading_y) - (DCM_Matrix[1][0]*mag_heading_x);  //Calculating YAW error
+  Vector_Scale(errorYaw,&DCM_Matrix[2][0],errorCourse); //Applys the yaw correction to the XYZ rotation of the aircraft, depeding the position.
+  
+  Vector_Scale(&Scaled_Omega_P[0],&errorYaw[0],Kp_YAW);//.01proportional of YAW.
+  Vector_Add(Omega_P,Omega_P,Scaled_Omega_P);//Adding  Proportional.
+  
+  Vector_Scale(&Scaled_Omega_I[0],&errorYaw[0],Ki_YAW);//.00001Integrator
+  Vector_Add(Omega_I,Omega_I,Scaled_Omega_I);//adding integrator to the Omega_I
+}
+
+void IMU::Matrix_update(void)
+{
+  Gyro_Vector[0]=GYRO_SCALED_RAD(gyro[0]); //gyro x roll
+  Gyro_Vector[1]=GYRO_SCALED_RAD(gyro[1]); //gyro y pitch
+  Gyro_Vector[2]=GYRO_SCALED_RAD(gyro[2]); //gyro z yaw
+  
+  Accel_Vector[0]=accel[0];
+  Accel_Vector[1]=accel[1];
+  Accel_Vector[2]=accel[2];
+    
+  Vector_Add(&Omega[0], &Gyro_Vector[0], &Omega_I[0]);  //adding proportional term
+  Vector_Add(&Omega_Vector[0], &Omega[0], &Omega_P[0]); //adding Integrator term
+  
+#if DEBUG__NO_DRIFT_CORRECTION == true // Do not use drift correction
+  Update_Matrix[0][0]=0;
+  Update_Matrix[0][1]=-G_Dt*Gyro_Vector[2];//-z
+  Update_Matrix[0][2]=G_Dt*Gyro_Vector[1];//y
+  Update_Matrix[1][0]=G_Dt*Gyro_Vector[2];//z
+  Update_Matrix[1][1]=0;
+  Update_Matrix[1][2]=-G_Dt*Gyro_Vector[0];
+  Update_Matrix[2][0]=-G_Dt*Gyro_Vector[1];
+  Update_Matrix[2][1]=G_Dt*Gyro_Vector[0];
+  Update_Matrix[2][2]=0;
+#else // Use drift correction
+  Update_Matrix[0][0]=0;
+  Update_Matrix[0][1]=-G_Dt*Omega_Vector[2];//-z
+  Update_Matrix[0][2]=G_Dt*Omega_Vector[1];//y
+  Update_Matrix[1][0]=G_Dt*Omega_Vector[2];//z
+  Update_Matrix[1][1]=0;
+  Update_Matrix[1][2]=-G_Dt*Omega_Vector[0];//-x
+  Update_Matrix[2][0]=-G_Dt*Omega_Vector[1];//-y
+  Update_Matrix[2][1]=G_Dt*Omega_Vector[0];//x
+  Update_Matrix[2][2]=0;
+#endif
+
+  Matrix_Multiply(DCM_Matrix,Update_Matrix,Temporary_Matrix); //a*b=c
+
+  for(int x=0; x<3; x++) //Matrix Addition (update)
+  {
+    for(int y=0; y<3; y++)
+    {
+      DCM_Matrix[x][y]+=Temporary_Matrix[x][y];
+    } 
+  }
+}
+
+void IMU::Euler_angles(void)
+{
+  pitch = -asin(DCM_Matrix[2][0]);
+  roll = atan2(DCM_Matrix[2][1],DCM_Matrix[2][2]);
+  yaw = atan2(DCM_Matrix[1][0],DCM_Matrix[0][0]);
+}
+

--- /dev/null	Thu Jan 01 00:00:00 1970 +0000
+++ b/MODSERIAL.lib	Tue Dec 27 17:20:06 2011 +0000
@@ -0,0 +1,1 @@
+http://mbed.org/users/AjK/code/MODSERIAL/#af2af4c61c2f

--- /dev/null	Thu Jan 01 00:00:00 1970 +0000
+++ b/Math.cpp	Tue Dec 27 17:20:06 2011 +0000
@@ -0,0 +1,93 @@
+/* This file is part of the Razor AHRS Firmware */
+#include <math.h>
+
+// Computes the dot product of two vectors
+float Vector_Dot_Product(float vector1[3], float vector2[3])
+{
+  float op=0;
+  
+  for(int c=0; c<3; c++)
+  {
+    op+=vector1[c]*vector2[c];
+  }
+  
+  return op; 
+}
+
+// Computes the cross product of two vectors
+void Vector_Cross_Product(float vectorOut[3], float v1[3], float v2[3])
+{
+  vectorOut[0]= (v1[1]*v2[2]) - (v1[2]*v2[1]);
+  vectorOut[1]= (v1[2]*v2[0]) - (v1[0]*v2[2]);
+  vectorOut[2]= (v1[0]*v2[1]) - (v1[1]*v2[0]);
+}
+
+// Multiply the vector by a scalar. 
+void Vector_Scale(float vectorOut[3], float vectorIn[3], float scale2)
+{
+  for(int c=0; c<3; c++)
+  {
+    vectorOut[c]=vectorIn[c]*scale2; 
+  }
+}
+
+// Adds two vectors
+void Vector_Add(float vectorOut[3], float vectorIn1[3], float vectorIn2[3])
+{
+  for(int c=0; c<3; c++)
+  {
+    vectorOut[c]=vectorIn1[c]+vectorIn2[c];
+  }
+}
+
+//Multiply two 3x3 matrixs. This function developed by Jordi can be easily adapted to multiple n*n matrix's. (Pero me da flojera!). 
+void Matrix_Multiply(float a[3][3], float b[3][3],float mat[3][3])
+{
+  float op[3]; 
+  for(int x=0; x<3; x++)
+  {
+    for(int y=0; y<3; y++)
+    {
+      for(int w=0; w<3; w++)
+      {
+       op[w]=a[x][w]*b[w][y];
+      } 
+      mat[x][y]=0;
+      mat[x][y]=op[0]+op[1]+op[2];
+      
+      float test=mat[x][y];
+    }
+  }
+}
+
+// Init rotation matrix using euler angles
+void init_rotation_matrix(float m[3][3], float yaw, float pitch, float roll)
+{
+  float c1 = cos(roll);
+  float s1 = sin(roll);
+  float c2 = cos(pitch);
+  float s2 = sin(pitch);
+  float c3 = cos(yaw);
+  float s3 = sin(yaw);
+
+  // Euler angles, right-handed, intrinsic, XYZ convention
+  // (which means: rotate around body axes Z, Y', X'') 
+  m[0][0] = c2 * c3;
+  m[0][1] = c3 * s1 * s2 - c1 * s3;
+  m[0][2] = s1 * s3 + c1 * c3 * s2;
+
+  m[1][0] = c2 * s3;
+  m[1][1] = c1 * c3 + s1 * s2 * s3;
+  m[1][2] = c1 * s2 * s3 - c3 * s1;
+
+  m[2][0] = -s2;
+  m[2][1] = c2 * s1;
+  m[2][2] = c1 * c2;
+}
+
+float constrain(float in, float min, float max)
+{
+    in = in > max ? max : in;
+    in = in < min ? min : in;
+    return in;
+}
\ No newline at end of file

--- /dev/null	Thu Jan 01 00:00:00 1970 +0000
+++ b/Output.cpp	Tue Dec 27 17:20:06 2011 +0000
@@ -0,0 +1,75 @@
+/* This file is part of the Razor AHRS Firmware */
+#include "Razor_AHRS.h"
+
+#if 1
+void WriteBytes(Serial& s, char* b, int count)
+{
+    for ( int i = 0; i < count; ++i )
+        s.putc(b[i]);
+}
+
+// Output angles: yaw, pitch, roll
+void IMU::output_angles()
+{
+  if (output_mode == OUTPUT__MODE_ANGLES_BINARY)
+  {
+    float ypr[3];  
+    ypr[0] = TO_DEG(yaw);
+    ypr[1] = TO_DEG(pitch);
+    ypr[2] = TO_DEG(roll);
+    WriteBytes(pc, (char*)ypr, 12); // No new-line
+  }
+  else if (output_mode == OUTPUT__MODE_ANGLES_TEXT)
+  {
+      pc.printf("#YPR=%f,%f,%f" NEW_LINE,TO_DEG(yaw),TO_DEG(pitch),TO_DEG(roll));
+  }
+}
+
+void IMU::output_calibration(int calibration_sensor)
+{
+  if (calibration_sensor == 0)  // Accelerometer
+  {
+    // Output MIN/MAX values
+    pc.printf("accel x,y,z (min/max) = ");
+    for (int i = 0; i < 3; i++) {
+      if (accel[i] < accel_min[i]) accel_min[i] = accel[i];
+      if (accel[i] > accel_max[i]) accel_max[i] = accel[i];
+      pc.printf("%+5i/%+5i%s", accel_min[i], accel_max[i], (i < 2 ? "  " : "                    "));
+    }
+    pc.printf("%c" ,13,10);
+  }
+  else if (calibration_sensor == 1)  // Magnetometer
+  {
+    // Output MIN/MAX values
+    pc.printf("magn x,y,z (min/max) = ");
+    for (int i = 0; i < 3; i++) {
+      if (magnetom[i] < magnetom_min[i]) magnetom_min[i] = magnetom[i];
+      if (magnetom[i] > magnetom_max[i]) magnetom_max[i] = magnetom[i];
+      pc.printf("%+4i/%+4i%s", magnetom_min[i], magnetom_max[i], (i < 2 ? "  " :  "                    "));
+    }
+    pc.printf("%c" ,13,10);
+  }
+  else if (calibration_sensor == 2)  // Gyroscope
+  {
+    // Average gyro values
+    for (int i = 0; i < 3; i++)
+      gyro_average[i] += gyro[i];
+    gyro_num_samples++;
+      
+    // Output current and averaged gyroscope values
+    pc.printf("gyro x,y,z (current/average) = ");
+    for (int i = 0; i < 3; i++) {
+      pc.printf("%+5i/%+5i%s",gyro[i], (int16_t)(gyro_average[i] / gyro_num_samples), (i < 2 ? "  " : "                    "));
+    }
+    pc.printf("%c" ,13,10);
+  }
+}
+
+void IMU::output_sensors()
+{
+  pc.printf("#ACC=%+5i %+5i %+5i ", accel[0],    accel[1],    accel[2]);
+  pc.printf("#MAG=%+4i %+4i %+4i ", magnetom[0], magnetom[1], magnetom[2]);
+  pc.printf("#GYR=%+5i %+5i %+5i",  gyro[0],     gyro[1],     gyro[2]);
+  pc.printf("%c" ,13,10);
+}
+#endif

--- /dev/null	Thu Jan 01 00:00:00 1970 +0000
+++ b/Razor_AHRS.cpp	Tue Dec 27 17:20:06 2011 +0000
@@ -0,0 +1,236 @@
+/* This file is part of the Razor AHRS Firmware */
+#include "Razor_AHRS.h"
+#include <math.h>
+
+void IMU::read_sensors() {
+    Read_Gyro(); // Read gyroscope
+    Read_Accel(); // Read accelerometer
+    Read_Magn(); // Read magnetometer
+}
+
+// Read every sensor and record a time stamp
+// Init DCM with unfiltered orientation
+// TODO re-init global vars?
+void IMU::reset_sensor_fusion() {
+    float temp0[3] = { accel[0], accel[1], accel[2] };
+    float temp1[3];
+    float temp2[3];
+    float xAxis[] = {1.0f, 0.0f, 0.0f};
+
+    read_sensors();
+    timestamp = timer.read_ms();
+
+    // GET PITCH
+    // Using y-z-plane-component/x-component of gravity vector
+    pitch = -atan2(accel[0], sqrt((double)(accel[1] * accel[1] + accel[2] * accel[2])));
+
+    // GET ROLL
+    // Compensate pitch of gravity vector
+        
+    Vector_Cross_Product(temp1, temp0, xAxis);
+    Vector_Cross_Product(temp2, xAxis, temp1);
+    // Normally using x-z-plane-component/y-component of compensated gravity vector
+    // roll = atan2(temp2[1], sqrt(temp2[0] * temp2[0] + temp2[2] * temp2[2]));
+    // Since we compensated for pitch, x-z-plane-component equals z-component:
+    roll = atan2(temp2[1], temp2[2]);
+
+    // GET YAW
+    Compass_Heading();
+    yaw = MAG_Heading;
+
+    // Init rotation matrix
+    init_rotation_matrix(DCM_Matrix, yaw, pitch, roll);
+}
+
+// Apply calibration to raw sensor readings
+void IMU::compensate_sensor_errors() {
+    // Compensate accelerometer error
+    accel[0] = (accel[0] - ACCEL_X_OFFSET) * ACCEL_X_SCALE;
+    accel[1] = (accel[1] - ACCEL_Y_OFFSET) * ACCEL_Y_SCALE;
+    accel[2] = (accel[2] - ACCEL_Z_OFFSET) * ACCEL_Z_SCALE;
+
+    // Compensate magnetometer error
+    magnetom[0] = (magnetom[0] - MAGN_X_OFFSET) * MAGN_X_SCALE;
+    magnetom[1] = (magnetom[1] - MAGN_Y_OFFSET) * MAGN_Y_SCALE;
+    magnetom[2] = (magnetom[2] - MAGN_Z_OFFSET) * MAGN_Z_SCALE;
+
+    // Compensate gyroscope error
+    gyro[0] -= GYRO_AVERAGE_OFFSET_X;
+    gyro[1] -= GYRO_AVERAGE_OFFSET_Y;
+    gyro[2] -= GYRO_AVERAGE_OFFSET_Z;
+}
+
+// Reset calibration session if reset_calibration_session_flag is set
+void IMU::check_reset_calibration_session() {
+    // Raw sensor values have to be read already, but no error compensation applied
+
+    // Reset this calibration session?
+    if (!reset_calibration_session_flag) return;
+
+    // Reset acc and mag calibration variables
+    for (int i = 0; i < 3; i++) {
+        accel_min[i] = accel_max[i] = accel[i];
+        magnetom_min[i] = magnetom_max[i] = magnetom[i];
+    }
+
+    // Reset gyro calibration variables
+    gyro_num_samples = 0;  // Reset gyro calibration averaging
+    gyro_average[0] = gyro_average[1] = gyro_average[2] = 0;
+
+    reset_calibration_session_flag = false;
+}
+
+void IMU::turn_output_stream_on() {
+    output_stream_on = true;
+    statusLed = 1;
+}
+
+void IMU::turn_output_stream_off() {
+    output_stream_on = false;
+    statusLed = 0;
+}
+
+// Blocks until another byte is available on serial port
+char IMU::readChar() {
+    while (pc.rxBufferGetCount() < 1) { } // Block
+    return pc.getc();
+}
+
+void IMU::setup() {
+    timer.start();
+    // Init serial output
+    pc.baud(OUTPUT__BAUD_RATE);
+
+    // Init status LED
+    statusLed = 0;
+
+    // Init sensors
+    wait_ms(50);  // Give sensors enough time to start
+    I2C_Init();
+    Accel_Init();
+    Magn_Init();
+    Gyro_Init();
+
+    // Read sensors, init DCM algorithm
+    wait_ms(20);  // Give sensors enough time to collect data
+    reset_sensor_fusion();
+
+    // Init output
+#if (OUTPUT__HAS_RN_BLUETOOTH == true) || (OUTPUT__STARTUP_STREAM_ON == false)
+    turn_output_stream_off();
+#else
+    turn_output_stream_on();
+#endif
+}
+
+// Main loop
+void IMU::loop() {
+
+    // Read incoming control messages
+    if (pc.rxBufferGetCount() >= 2) {
+        if (pc.getc() == '#') { // Start of new control message
+            int command = pc.getc(); // Commands
+            if (command == 'f') // request one output _f_rame
+                output_single_on = true;
+            else if (command == 's') { // _s_ynch request
+                // Read ID
+                char id[2];
+                id[0] = readChar();
+                id[1] = readChar();
+
+                // Reply with synch message
+                pc.printf("#SYNCH");
+                pc.putc(id[0]);
+                pc.putc(id[1]);
+                pc.printf(NEW_LINE);
+            } else if (command == 'o') { // Set _o_utput mode
+                char output_param = readChar();
+                if (output_param == 'n') { // Calibrate _n_ext sensor
+                    curr_calibration_sensor = (curr_calibration_sensor + 1) % 3;
+                    reset_calibration_session_flag = true;
+                } else if (output_param == 't') // Output angles as _t_ext
+                    output_mode = OUTPUT__MODE_ANGLES_TEXT;
+                else if (output_param == 'b') // Output angles in _b_inary form
+                    output_mode = OUTPUT__MODE_ANGLES_BINARY;
+                else if (output_param == 'c') { // Go to _c_alibration mode
+                    output_mode = OUTPUT__MODE_CALIBRATE_SENSORS;
+                    reset_calibration_session_flag = true;
+                } else if (output_param == 's') // Output _s_ensor values as text
+                    output_mode = OUTPUT__MODE_SENSORS_TEXT;
+                else if (output_param == '0') { // Disable continuous streaming output
+                    turn_output_stream_off();
+                    reset_calibration_session_flag = true;
+                } else if (output_param == '1') { // Enable continuous streaming output
+                    reset_calibration_session_flag = true;
+                    turn_output_stream_on();
+                } else if (output_param == 'e') { // _e_rror output settings
+                    char error_param = readChar();
+                    if (error_param == '0') output_errors = false;
+                    else if (error_param == '1') output_errors = true;
+                    else if (error_param == 'c') { // get error count
+                        pc.printf("#AMG-ERR:%d,%d,%d" NEW_LINE,num_accel_errors,num_magn_errors,num_gyro_errors);
+                    }
+                }
+            }
+#if OUTPUT__HAS_RN_BLUETOOTH == true
+            // Read messages from bluetooth module
+            // For this to work, the connect/disconnect message prefix of the module has to be set to "#".
+            else if (command == 'C') // Bluetooth "#CONNECT" message (does the same as "#o1")
+                turn_output_stream_on();
+            else if (command == 'D') // Bluetooth "#DISCONNECT" message (does the same as "#o0")
+                turn_output_stream_off();
+#endif // OUTPUT__HAS_RN_BLUETOOTH == true
+        } else { } // Skip character
+    }
+
+    // Time to read the sensors again?
+    int now = timer.read_ms();
+    if ((now - timestamp) >= OUTPUT__DATA_INTERVAL) {
+        timestamp_old = timestamp;
+        timestamp = now;
+        if (timestamp > timestamp_old)
+            G_Dt = (float) (timestamp - timestamp_old) / 1000.0f; // Real time of loop run. We use this on the DCM algorithm (gyro integration time)
+        else
+            G_Dt = 0;
+
+        // Update sensor readings
+        read_sensors();
+
+        if (output_mode == OUTPUT__MODE_CALIBRATE_SENSORS) { // We're in calibration mode
+            check_reset_calibration_session();  // Check if this session needs a reset
+            if (output_stream_on || output_single_on)
+                output_calibration(curr_calibration_sensor);
+        } else if (output_mode == OUTPUT__MODE_SENSORS_TEXT) {
+            // Apply sensor calibration
+            compensate_sensor_errors();
+
+            if (output_stream_on || output_single_on)
+                output_sensors();
+        } else if (output_mode == OUTPUT__MODE_ANGLES_TEXT || output_mode == OUTPUT__MODE_ANGLES_BINARY) {
+            // Apply sensor calibration
+            compensate_sensor_errors();
+
+            // Run DCM algorithm
+            Compass_Heading(); // Calculate magnetic heading
+            Matrix_update();
+            Normalize();
+            Drift_correction();
+            Euler_angles();
+
+            if (output_stream_on || output_single_on) 
+                output_angles();
+        }
+
+        output_single_on = false;
+
+#if DEBUG__PRINT_LOOP_TIME == true
+        Serial.print("loop time (ms) = ");
+        Serial.println(millis() - timestamp);
+#endif
+    }
+#if DEBUG__PRINT_LOOP_TIME == true
+    else {
+        Serial.println("waiting...");
+    }
+#endif
+}

--- /dev/null	Thu Jan 01 00:00:00 1970 +0000
+++ b/Razor_AHRS.h	Tue Dec 27 17:20:06 2011 +0000
@@ -0,0 +1,420 @@
+/***************************************************************************************************************
+* Razor AHRS Firmware v1.4.0
+* 9 Degree of Measurement Attitude and Heading Reference System
+* for Sparkfun "9DOF Razor IMU" (SEN-10125 and SEN-10736)
+* and "9DOF Sensor Stick" (SEN-10183, 10321 and SEN-10724)
+*
+* Released under GNU GPL (General Public License) v3.0
+* Copyright (C) 2011 Quality & Usability Lab, Deutsche Telekom Laboratories, TU Berlin
+*
+* Infos, updates, bug reports and feedback:
+*     http://dev.qu.tu-berlin.de/projects/sf-razor-9dof-ahrs
+*
+*
+* History:
+*   * Original code (http://code.google.com/p/sf9domahrs/) by Doug Weibel and Jose Julio,
+*     based on ArduIMU v1.5 by Jordi Munoz and William Premerlani, Jose Julio and Doug Weibel. Thank you!
+*
+*   * Updated code (http://groups.google.com/group/sf_9dof_ahrs_update) by David Malik (david.zsolt.malik@gmail.com)
+*     for new Sparkfun 9DOF Razor hardware (SEN-10125).
+*
+*   * Updated and extended by Peter Bartz (peter-bartz@gmx.de):
+*     * v1.3.0
+*       * Cleaned up, streamlined and restructured most of the code to make it more comprehensible.
+*       * Added sensor calibration (improves precision and responsiveness a lot!).
+*       * Added binary yaw/pitch/roll output.
+*       * Added basic serial command interface to set output modes/calibrate sensors/synch stream/etc.
+*       * Added support to synch automatically when using Rovering Networks Bluetooth modules (and compatible).
+*       * Wrote new easier to use test program (using Processing).
+*       * Added support for new version of "9DOF Razor IMU": SEN-10736.
+*       --> The output of this code is not compatible with the older versions!
+*       --> A Processing sketch to test the tracker is available.
+*     * v1.3.1
+*       * Initializing rotation matrix based on start-up sensor readings -> orientation OK right away.
+*       * Adjusted gyro low-pass filter and output rate settings.
+*     * v1.3.2
+*       * Adapted code to work with new Arduino 1.0 (and older versions still).
+*     * v1.3.3
+*       * Improved synching.
+*     * v1.4.0
+*       * Added support for SparkFun "9DOF Sensor Stick" (versions SEN-10183, SEN-10321 and SEN-10724).
+*
+* TODOs:
+*   * Allow optional use of EEPROM for storing and reading calibration values.
+*   * Use self-test and temperature-compensation features of the sensors.
+*   * Add binary output of unfused sensor data for all 9 axes.
+***************************************************************************************************************/
+
+/*
+  "9DOF Razor IMU" hardware versions: SEN-10125 and SEN-10736
+
+  ATMega328@3.3V, 8MHz
+
+  ADXL345  : Accelerometer
+  HMC5843  : Magnetometer on SEN-10125
+  HMC5883L : Magnetometer on SEN-10736
+  ITG-3200 : Gyro
+
+  Arduino IDE : Select board "Arduino Pro or Pro Mini (3.3v, 8Mhz) w/ATmega328"
+*/
+
+/*
+  "9DOF Sensor Stick" hardware versions: SEN-10183, SEN-10321 and SEN-10724
+
+  ADXL345  : Accelerometer
+  HMC5843  : Magnetometer on SEN-10183 and SEN-10321
+  HMC5883L : Magnetometer on SEN-10724
+  ITG-3200 : Gyro
+*/
+
+/*
+  Axis definition (differs from definition printed on the board!):
+    X axis pointing forward (towards the short edge with the connector holes)
+    Y axis pointing to the right
+    and Z axis pointing down.
+    
+  Positive yaw   : clockwise
+  Positive roll  : right wing down
+  Positive pitch : nose up
+  
+  Transformation order: first yaw then pitch then roll.
+*/
+
+/*
+  Commands that the firmware understands:
+  
+  "#o<param>" - Set output parameter. The available options are:
+      "#o0" - Disable continuous streaming output.
+      "#o1" - Enable continuous streaming output.
+      "#ob" - Output angles in binary format (yaw/pitch/roll as binary float, so one output frame
+              is 3x4 = 12 bytes long).
+      "#ot" - Output angles in text format (Output frames have form like "#YPR=-142.28,-5.38,33.52",
+              followed by carriage return and line feed [\r\n]).
+      "#os" - Output (calibrated) sensor data of all 9 axes in text format. One frame consist of
+              three lines - one for each sensor.
+      "#oc" - Go to calibration output mode.
+      "#on" - When in calibration mode, go on to calibrate next sensor.
+      "#oe0" - Disable error message output.
+      "#oe1" - Enable error message output.
+    
+  "#f" - Request one output frame - useful when continuous output is disabled and updates are
+         required in larger intervals only.
+  "#s<xy>" - Request synch token - useful to find out where the frame boundaries are in a continuous
+         binary stream or to see if tracker is present and answering. The tracker will send
+         "#SYNCH<xy>\r\n" in response (so it's possible to read using a readLine() function).
+         x and y are two mandatory but arbitrary bytes that can be used to find out which request
+         the answer belongs to.
+          
+  ("#C" and "#D" - Reserved for communication with optional Bluetooth module.)
+  
+  Newline characters are not required. So you could send "#ob#o1#s", which
+  would set binary output mode, enable continuous streaming output and request
+  a synch token all at once.
+  
+  The status LED will be on if streaming output is enabled and off otherwise.
+  
+  Byte order of binary output is little-endian: least significant byte comes first.
+*/
+#include <mbed.h>
+#include <MODSERIAL.h>
+
+
+/*****************************************************************/
+/*********** USER SETUP AREA! Set your options here! *************/
+/*****************************************************************/
+
+// HARDWARE OPTIONS
+/*****************************************************************/
+// Select your hardware here by uncommenting one line!
+//#define HW__VERSION_CODE 10125 // SparkFun "9DOF Razor IMU" version "SEN-10125" (HMC5843 magnetometer)
+#define HW__VERSION_CODE 10736 // SparkFun "9DOF Razor IMU" version "SEN-10736" (HMC5883L magnetometer)
+//#define HW__VERSION_CODE 10183 // SparkFun "9DOF Sensor Stick" version "SEN-10183" (HMC5843 magnetometer)
+//#define HW__VERSION_CODE 10321 // SparkFun "9DOF Sensor Stick" version "SEN-10321" (HMC5843 magnetometer)
+//#define HW__VERSION_CODE 10724 // SparkFun "9DOF Sensor Stick" version "SEN-10724" (HMC5883L magnetometer)
+
+
+// OUTPUT OPTIONS
+/*****************************************************************/
+// Set your serial port baud rate used to send out data here!
+#define OUTPUT__BAUD_RATE 57600
+
+// Sensor data output interval in milliseconds
+// This may not work, if faster than 20ms (=50Hz)
+// Code is tuned for 20ms, so better leave it like that
+#define OUTPUT__DATA_INTERVAL 20  // in milliseconds
+
+// Output mode
+#define OUTPUT__MODE_CALIBRATE_SENSORS 0 // Outputs sensor min/max values as text for manual calibration
+#define OUTPUT__MODE_ANGLES_TEXT 1 // Outputs yaw/pitch/roll in degrees as text
+#define OUTPUT__MODE_ANGLES_BINARY 2 // Outputs yaw/pitch/roll in degrees as binary float
+#define OUTPUT__MODE_SENSORS_TEXT 3 // Outputs (calibrated) sensor values for all 9 axes as text
+
+// Select if serial continuous streaming output is enabled per default on startup.
+#define OUTPUT__STARTUP_STREAM_ON false  // true or false
+
+// Bluetooth
+// You can set this to true, if you have a Rovering Networks Bluetooth Module attached.
+// The connect/disconnect message prefix of the module has to be set to "#".
+// (Refer to manual, it can be set like this: SO,#)
+// When using this, streaming output will only be enabled as long as we're connected. That way
+// receiver and sender are synchronzed easily just by connecting/disconnecting.
+// It is not necessary to set this! It just makes life easier when writing code for
+// the receiving side. The Processing test sketch also works without setting this.
+// NOTE: When using this, OUTPUT__STARTUP_STREAM_ON has no effect!
+#define OUTPUT__HAS_RN_BLUETOOTH false  // true or false
+
+
+// SENSOR CALIBRATION
+/*****************************************************************/
+// How to calibrate? Read the tutorial at http://dev.qu.tu-berlin.de/projects/sf-razor-9dof-ahrs
+// Put MIN/MAX and OFFSET readings for your board here!
+// Accelerometer
+// "accel x,y,z (min/max) = X_MIN/X_MAX  Y_MIN/Y_MAX  Z_MIN/Z_MAX"
+#define ACCEL_X_MIN ((float) -250)
+#define ACCEL_X_MAX ((float) 250)
+#define ACCEL_Y_MIN ((float) -250)
+#define ACCEL_Y_MAX ((float) 250)
+#define ACCEL_Z_MIN ((float) -250)
+#define ACCEL_Z_MAX ((float) 250)
+
+// Magnetometer
+// "magn x,y,z (min/max) = X_MIN/X_MAX  Y_MIN/Y_MAX  Z_MIN/Z_MAX"
+#define MAGN_X_MIN ((float) -600)
+#define MAGN_X_MAX ((float) 600)
+#define MAGN_Y_MIN ((float) -600)
+#define MAGN_Y_MAX ((float) 600)
+#define MAGN_Z_MIN ((float) -600)
+#define MAGN_Z_MAX ((float) 600)
+
+// Gyroscope
+// "gyro x,y,z (current/average) = .../OFFSET_X  .../OFFSET_Y  .../OFFSET_Z
+#define GYRO_AVERAGE_OFFSET_X ((float) 0.0)
+#define GYRO_AVERAGE_OFFSET_Y ((float) 0.0)
+#define GYRO_AVERAGE_OFFSET_Z ((float) 0.0)
+
+/*
+// Calibration example:
+// "accel x,y,z (min/max) = -278.00/270.00  -254.00/284.00  -294.00/235.00"
+#define ACCEL_X_MIN ((float) -278)
+#define ACCEL_X_MAX ((float) 270)
+#define ACCEL_Y_MIN ((float) -254)
+#define ACCEL_Y_MAX ((float) 284)
+#define ACCEL_Z_MIN ((float) -294)
+#define ACCEL_Z_MAX ((float) 235)
+
+// "magn x,y,z (min/max) = -511.00/581.00  -516.00/568.00  -489.00/486.00"
+#define MAGN_X_MIN ((float) -511)
+#define MAGN_X_MAX ((float) 581)
+#define MAGN_Y_MIN ((float) -516)
+#define MAGN_Y_MAX ((float) 568)
+#define MAGN_Z_MIN ((float) -489)
+#define MAGN_Z_MAX ((float) 486)
+
+//"gyro x,y,z (current/average) = -32.00/-34.82  102.00/100.41  -16.00/-16.38"
+#define GYRO_AVERAGE_OFFSET_X ((float) -34.82)
+#define GYRO_AVERAGE_OFFSET_Y ((float) 100.41)
+#define GYRO_AVERAGE_OFFSET_Z ((float) -16.38)
+*/
+
+
+// DEBUG OPTIONS
+/*****************************************************************/
+// When set to true, gyro drift correction will not be applied
+#define DEBUG__NO_DRIFT_CORRECTION false
+// Print elapsed time after each I/O loop
+#define DEBUG__PRINT_LOOP_TIME false
+
+
+/*****************************************************************/
+/****************** END OF USER SETUP AREA!  *********************/
+/*****************************************************************/
+
+
+// Check if hardware version code is defined
+#ifndef HW__VERSION_CODE
+  // Generate compile error
+  #error YOU HAVE TO SELECT THE HARDWARE YOU ARE USING! See "HARDWARE OPTIONS" in "USER SETUP AREA" at top of Razor_AHRS.pde!
+#endif
+
+//#include <Wire.h>
+
+// Sensor calibration scale and offset values
+#define ACCEL_X_OFFSET ((ACCEL_X_MIN + ACCEL_X_MAX) / 2.0f)
+#define ACCEL_Y_OFFSET ((ACCEL_Y_MIN + ACCEL_Y_MAX) / 2.0f)
+#define ACCEL_Z_OFFSET ((ACCEL_Z_MIN + ACCEL_Z_MAX) / 2.0f)
+#define ACCEL_X_SCALE (GRAVITY / (ACCEL_X_MAX - ACCEL_X_OFFSET))
+#define ACCEL_Y_SCALE (GRAVITY / (ACCEL_Y_MAX - ACCEL_Y_OFFSET))
+#define ACCEL_Z_SCALE (GRAVITY / (ACCEL_Z_MAX - ACCEL_Z_OFFSET))
+
+#define MAGN_X_OFFSET ((MAGN_X_MIN + MAGN_X_MAX) / 2.0f)
+#define MAGN_Y_OFFSET ((MAGN_Y_MIN + MAGN_Y_MAX) / 2.0f)
+#define MAGN_Z_OFFSET ((MAGN_Z_MIN + MAGN_Z_MAX) / 2.0f)
+#define MAGN_X_SCALE (100.0f / (MAGN_X_MAX - MAGN_X_OFFSET))
+#define MAGN_Y_SCALE (100.0f / (MAGN_Y_MAX - MAGN_Y_OFFSET))
+#define MAGN_Z_SCALE (100.0f / (MAGN_Z_MAX - MAGN_Z_OFFSET))
+
+
+// Gain for gyroscope (ITG-3200)
+#define GYRO_GAIN 0.06957 // Same gain on all axes
+#define GYRO_SCALED_RAD(x) (x * TO_RAD(GYRO_GAIN)) // Calculate the scaled gyro readings in radians per second
+
+// DCM parameters
+#define Kp_ROLLPITCH 0.02f
+#define Ki_ROLLPITCH 0.00002f
+#define Kp_YAW 1.2f
+#define Ki_YAW 0.00002f
+
+// Stuff
+#define GRAVITY 256.0f // "1G reference" used for DCM filter and accelerometer calibration
+#define TO_RAD(x) (x * 0.01745329252)  // *pi/180
+#define TO_DEG(x) (x * 57.2957795131)  // *180/pi
+#define NEW_LINE "\r\n"
+
+class IMU {
+public:
+    // Sensor variables
+    int16_t accel[3];  // Actually stores the NEGATED acceleration (equals gravity, if board not moving).
+    int16_t accel_min[3];
+    int16_t accel_max[3];
+
+    int16_t magnetom[3];
+    int16_t magnetom_min[3];
+    int16_t magnetom_max[3];
+
+    int16_t gyro[3];
+    int16_t gyro_average[3];
+    int gyro_num_samples;
+
+    // Euler angles
+    float yaw;
+    float pitch;
+    float roll;
+
+    // DCM variables
+    float MAG_Heading;
+    float Accel_Vector[3]; // Store the acceleration in a vector
+    float Gyro_Vector[3];  // Store the gyros turn rate in a vector
+    float Omega_Vector[3]; // Corrected Gyro_Vector data
+    float Omega_P[3];//= {0, 0, 0}; // Omega Proportional correction
+    float Omega_I[3];//= {0, 0, 0}; // Omega Integrator
+    float Omega[3];//= {0, 0, 0};
+    float errorRollPitch[3];// = {0, 0, 0};
+    float errorYaw[3];// = {0, 0, 0};
+    float DCM_Matrix[3][3];// = {{1, 0, 0}, {0, 1, 0}, {0, 0, 1}};
+    float Update_Matrix[3][3];// = {{0, 1, 2}, {3, 4, 5}, {6, 7, 8}};
+    float Temporary_Matrix[3][3];// = {{0, 0, 0}, {0, 0, 0}, {0, 0, 0}};
+
+    // DCM timing in the main loop
+    long timestamp;
+    long timestamp_old;
+    float G_Dt; // Integration time for DCM algorithm
+
+    // More output-state variables
+    int output_mode;
+    bool output_stream_on;
+    bool output_single_on;
+    int curr_calibration_sensor;
+    bool reset_calibration_session_flag;
+    int num_accel_errors;
+    int num_magn_errors;
+    int num_gyro_errors;
+
+    // If set true, an error message will be output if we fail to read sensor data.
+    // Message format: "!ERR: reading <sensor>", followed by "\r\n".
+    bool output_errors;
+
+    DigitalOut statusLed;
+    MODSERIAL pc;
+    I2C Wire;
+    Timer timer;
+
+public:
+    IMU()
+        : gyro_num_samples(0)
+        , yaw(0)
+        , pitch(0)
+        , roll(0)
+        , MAG_Heading(0)
+        , timestamp(0)
+        , timestamp_old(0)
+        , G_Dt(0)
+        , output_mode(-1) // Select your startup output mode here!// Select your startup output mode here!
+        , output_stream_on(false)
+        , output_single_on(true)
+        , curr_calibration_sensor(0)
+        , reset_calibration_session_flag(true)
+        , num_accel_errors(0)
+        , num_magn_errors(0)
+        , num_gyro_errors(0)
+        , output_errors(true)
+        , statusLed(LED1)
+        , pc(USBTX, USBRX)
+        , Wire(p28, p27)
+    {
+        accel[0] = accel_min[0] = accel_max[0] = magnetom[0] = magnetom_min[0] = magnetom_max[0] = gyro[0] = gyro_average[0] = 0;
+        accel[1] = accel_min[1] = accel_max[1] = magnetom[1] = magnetom_min[1] = magnetom_max[1] = gyro[1] = gyro_average[1] = 0;
+        accel[2] = accel_min[2] = accel_max[2] = magnetom[2] = magnetom_min[2] = magnetom_max[2] = gyro[2] = gyro_average[2] = 0;
+
+        Accel_Vector[0] = Gyro_Vector[0] = Omega_Vector[0] = Omega_P[0] = Omega_I[0] = Omega[0] = errorRollPitch[0] = errorYaw[0] = 0;
+        Accel_Vector[1] = Gyro_Vector[1] = Omega_Vector[1] = Omega_P[1] = Omega_I[1] = Omega[1] = errorRollPitch[1] = errorYaw[1] = 0;
+        Accel_Vector[2] = Gyro_Vector[2] = Omega_Vector[2] = Omega_P[2] = Omega_I[2] = Omega[2] = errorRollPitch[2] = errorYaw[2] = 0;
+
+        DCM_Matrix[0][0] = 1; DCM_Matrix[0][1] = 0; DCM_Matrix[0][2] = 0;
+        DCM_Matrix[1][0] = 0; DCM_Matrix[1][1] = 1; DCM_Matrix[1][2] = 0;
+        DCM_Matrix[2][0] = 0; DCM_Matrix[2][1] = 0; DCM_Matrix[2][2] = 1;
+  
+        Update_Matrix[0][0] = 0; Update_Matrix[0][1] = 1; Update_Matrix[0][2] = 2;
+        Update_Matrix[1][0] = 3; Update_Matrix[1][1] = 4; Update_Matrix[1][2] = 5;
+        Update_Matrix[2][0] = 6; Update_Matrix[2][1] = 7; Update_Matrix[2][2] = 8;
+  
+        Temporary_Matrix[0][0] = 0; Temporary_Matrix[0][1] = 0; Temporary_Matrix[0][2] = 0;
+        Temporary_Matrix[1][0] = 0; Temporary_Matrix[1][1] = 0; Temporary_Matrix[1][2] = 0;
+        Temporary_Matrix[2][0] = 0; Temporary_Matrix[2][1] = 0; Temporary_Matrix[2][2] = 0;
+   }
+    
+    // Compass.cpp
+    void Compass_Heading();
+ 
+    // DCM.cpp
+    void Normalize();
+    void Drift_correction();
+    void Matrix_update();
+    void Euler_angles();
+    
+    // Output.cpp
+    void output_angles();
+    void output_calibration(int calibration_sensor);
+    void output_sensors();
+    
+    
+    // Razor_AHRS.cpp
+    void read_sensors();
+    void reset_sensor_fusion();
+    void compensate_sensor_errors();
+    void check_reset_calibration_session();
+    void turn_output_stream_on();
+    void turn_output_stream_off();
+    char readChar();
+    void setup();
+    void loop();
+    
+    
+    // Sensors.cpp
+    void I2C_Init();
+    void Accel_Init();
+    void Read_Accel();
+    void Magn_Init();
+    void Read_Magn();
+    void Gyro_Init();
+    void Read_Gyro();
+    
+};
+
+float Vector_Dot_Product(float vector1[3], float vector2[3]);
+void Vector_Cross_Product(float vectorOut[3], float v1[3], float v2[3]);
+void Vector_Scale(float vectorOut[3], float vectorIn[3], float scale2);
+void Vector_Add(float vectorOut[3], float vectorIn1[3], float vectorIn2[3]);
+void Matrix_Multiply(float a[3][3], float b[3][3],float mat[3][3]);
+void init_rotation_matrix(float m[3][3], float yaw, float pitch, float roll);
+float constrain(float in, float min, float max);
+

--- /dev/null	Thu Jan 01 00:00:00 1970 +0000
+++ b/Sensors.cpp	Tue Dec 27 17:20:06 2011 +0000
@@ -0,0 +1,166 @@
+/* This file is part of the Razor AHRS Firmware */
+#include "Razor_AHRS.h"
+
+// I2C code to read the sensors
+
+// Sensor I2C addresses
+#define ACCEL_READ  0xA7
+#define ACCEL_WRITE 0xA6
+#define MAGN_WRITE  0x3C 
+#define MAGN_READ   0x3D 
+#define GYRO_WRITE  0xD0
+#define GYRO_READ   0xD1
+
+void IMU::I2C_Init()
+{
+  Wire.frequency(100000);
+}
+
+void IMU::Accel_Init()
+{
+  char tx[2];
+  tx[0] = 0x2D; // Power register
+  tx[1] = 0x08; // Power register
+  Wire.write(ACCEL_WRITE, tx, 2);
+  wait_ms(5);
+  tx[0] = 0x31; // Data format register
+  tx[1] = 0x08; // Set to full resolution
+  Wire.write(ACCEL_WRITE, tx, 2);
+  wait_ms(5);
+  
+  // Because our main loop runs at 50Hz we adjust the output data rate to 50Hz (25Hz bandwidth)
+  tx[0] = 0x2C;  // Rate
+  tx[1] = 0x09;  // Set to 50Hz, normal operation
+  Wire.write(ACCEL_WRITE, tx, 2);
+  wait_ms(5);
+}
+
+// Reads x, y and z accelerometer registers
+void IMU::Read_Accel()
+{
+  char buff[6];
+  char tx = 0x32;  // Send address to read from
+ 
+  Wire.write(ACCEL_WRITE, &tx, 1);
+  
+  if (Wire.read(ACCEL_READ, buff, 6) == 0) // All bytes received?
+  {
+    // 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 | (int)buff[2]; // X axis (internal sensor y axis)
+    accel[1] = (int)buff[1] << 8 | (int)buff[0]; // Y axis (internal sensor x axis)
+    accel[2] = (int)buff[5] << 8 | (int)buff[4]; // Z axis (internal sensor z axis)
+  }
+  else
+  {
+    num_accel_errors++;
+    if (output_errors) pc.printf("!ERR: reading accelerometer" NEW_LINE);
+  }
+}
+
+void IMU::Magn_Init()
+{
+  char tx[2];
+  tx[0] = 0x02; // Mode
+  tx[1] = 0x00; // Set continuous mode (default 10Hz)
+  Wire.write(MAGN_WRITE, tx, 2);
+  wait_ms(5);
+
+  tx[0] = 0x00; // CONFIG_A 
+  tx[1] = 0x18; // Set 50Hz
+  Wire.write(MAGN_WRITE, tx, 2);
+  wait_ms(5);
+}
+
+void IMU::Read_Magn()
+{
+  char buff[6];
+  char tx = 0x03;  // Send address to read from 
+
+  Wire.write(MAGN_WRITE, &tx, 1);
+  
+  if (Wire.read(MAGN_READ, buff, 6) == 0)  // All bytes received?
+  {
+// 9DOF Razor IMU SEN-10125 using HMC5843 magnetometer
+#if HW__VERSION_CODE == 10125
+    // MSB byte first, then LSB; X, Y, Z
+    magnetom[0] = -1 * (((int) buff[2]) << 8) | buff[3];  // X axis (internal sensor -y axis)
+    magnetom[1] = -1 * (((int) buff[0]) << 8) | buff[1];  // Y axis (internal sensor -x axis)
+    magnetom[2] = -1 * (((int) buff[4]) << 8) | buff[5];  // Z axis (internal sensor -z axis)
+// 9DOF Razor IMU SEN-10736 using HMC5883L magnetometer
+#elif HW__VERSION_CODE == 10736
+    // MSB byte first, then LSB; Y and Z reversed: X, Z, Y
+    magnetom[0] = -1 * (((int) buff[4]) << 8) | buff[5];  // X axis (internal sensor -y axis)
+    magnetom[1] = -1 * (((int) buff[0]) << 8) | buff[1];  // Y axis (internal sensor -x axis)
+    magnetom[2] = -1 * (((int) buff[2]) << 8) | buff[3];  // Z axis (internal sensor -z axis)
+// 9DOF Sensor Stick SEN-10183 and SEN-10321 using HMC5843 magnetometer
+#elif (HW__VERSION_CODE == 10183) || (HW__VERSION_CODE == 10321)
+    // MSB byte first, then LSB; X, Y, Z
+    magnetom[0] = (((int) buff[0]) << 8) | buff[1];       // X axis (internal sensor x axis)
+    magnetom[1] = -1 * (((int) buff[2]) << 8) | buff[3];  // Y axis (internal sensor -y axis)
+    magnetom[2] = -1 * (((int) buff[4]) << 8) | buff[5];  // Z axis (internal sensor -z axis)
+// 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] =  1 * ((int)buff[0] << 8 | (int)buff[1]); // X axis (internal sensor x axis)
+    magnetom[1] = -1 * ((int)buff[4] << 8 | (int)buff[5]); // Y axis (internal sensor -y axis)
+    magnetom[2] = -1 * ((int)buff[2] << 8 | (int)buff[3]); // Z axis (internal sensor -z axis)
+#endif
+  }
+  else
+  {
+    num_magn_errors++;
+    if (output_errors) pc.printf("!ERR: reading magnetometer" NEW_LINE);
+  }
+}
+
+void IMU::Gyro_Init()
+{
+  char tx[2];
+  
+  // Power up reset defaults
+  tx[0] = 0x3E; // Power management
+  tx[1] = 0x80; // ?
+  Wire.write(GYRO_WRITE, tx, 2);
+  wait_ms(5);
+  
+  // Select full-scale range of the gyro sensors
+  // Set LP filter bandwidth to 42Hz
+  tx[0] = 0x16; // 
+  tx[1] = 0x1B; // DLPF_CFG = 3, FS_SEL = 3
+  Wire.write(GYRO_WRITE, tx, 2);
+  wait_ms(5);
+  
+  // Set sample rato to 50Hz
+  tx[0] = 0x15; //
+  tx[1] = 0x0A; //  SMPLRT_DIV = 10 (50Hz)
+  Wire.write(GYRO_WRITE, tx, 2);
+  wait_ms(5);
+
+  // Set clock to PLL with z gyro reference
+  tx[0] = 0x3E;
+  tx[1] = 0x00;
+  Wire.write(GYRO_WRITE, tx, 2);
+  wait_ms(5);
+}
+
+// Reads x, y and z gyroscope registers
+void IMU::Read_Gyro()
+{
+  char buff[6];
+  char tx = 0x1D; // Sends address to read from
+  
+  Wire.write(GYRO_WRITE, &tx, 1);
+  
+  if (Wire.read(GYRO_READ, buff, 6) == 0)  // All bytes received?
+  {
+    gyro[0] = -1 * ((int)buff[2] << 8 | (int)buff[3]);   // X axis (internal sensor -y axis)
+    gyro[1] = -1 * ((int)buff[0] << 8 | (int)buff[1]);   // Y axis (internal sensor -x axis)
+    gyro[2] = -1 * ((int)buff[4] << 8 | (int)buff[5]);   // Z axis (internal sensor -z axis)
+  }
+  else
+  {
+    num_gyro_errors++;
+    if (output_errors) pc.printf("!ERR: reading gyroscope" NEW_LINE);
+  }
+}

--- /dev/null	Thu Jan 01 00:00:00 1970 +0000
+++ b/main.cpp	Tue Dec 27 17:20:06 2011 +0000
@@ -0,0 +1,11 @@
+#include "Razor_AHRS.h"
+
+int main()
+{
+    IMU imu;
+    imu.setup();
+    while(1)
+        imu.loop();
+        
+    return 0;
+}

--- /dev/null	Thu Jan 01 00:00:00 1970 +0000
+++ b/mbed.bld	Tue Dec 27 17:20:06 2011 +0000
@@ -0,0 +1,1 @@
+http://mbed.org/users/mbed_official/code/mbed/builds/078e4b97a13e