diff --git a/components/Odometry.cpp b/components/Odometry.cpp
index 071654ea2d40281c1213ebffaa0c7f2f4ede30a5..2083bd1d4cd10d44c892b2e584d34907548fbd4b 100644
--- a/components/Odometry.cpp
+++ b/components/Odometry.cpp
@@ -19,7 +19,7 @@ extern Global global;
 
 
 Odometry::Odometry(MotorIncrements* mi, L3G4200D* gyroscope)
-    : BaseStaticThread<1024>(),
+    : BaseStaticThread<512>(),
       motorIncrements(mi),
       gyro(gyroscope),
       eventSource(),
@@ -82,15 +82,11 @@ void Odometry::resetPosition() {
 
 void Odometry::setError(float* Cp3x3) {
   chSysLock();
-//   float** test;
     Matrix::copy<float>(Cp3x3,3,3, &(this->Cp3x3[0]),3,3);
-//     Matrix::copy<float>(Cp3x3,3,3, test,3,3);
   chSysUnlock();
 }
 
 void Odometry::resetError() {
-//   float zeroMatrix[9] = {};
-//   this->setError(&(zeroMatrix[0]));
   Matrix::init<float>(&(this->Cp3x3[0]),3,3,0.0f);
 }
 
@@ -105,7 +101,7 @@ msg_t Odometry::main(void) {
   while (!this->shouldTerminate()) {
     time += MS2ST(this->period);
 
-    // Update the base distance, because it may change after a calibration
+    // Update the base distance, because it may have changed after a calibration
     this->updateWheelBaseDistance();
 
     // Get the actual speed
@@ -120,11 +116,11 @@ msg_t Odometry::main(void) {
 //     chprintf((BaseSequentialStream*) &global.sercanmux1, "distance_left:%f distance_right:%f", this->distance[0],this->distance[1]);
 //     chprintf((BaseSequentialStream*) &global.sercanmux1, "\r\n");
 
-  if (time >= System::getTime()) {
-      chThdSleepUntil(time);
+    if (time >= System::getTime()) {
+        chThdSleepUntil(time);
     } else {
-      chprintf((BaseSequentialStream*) &global.sercanmux1, "WARNING Odometry: Unable to keep track\r\n");
-  }
+        chprintf((BaseSequentialStream*) &global.sercanmux1, "WARNING Odometry: Unable to keep track\r\n");
+    }
   }
 
   return true;
@@ -134,15 +130,16 @@ void Odometry::updateOdometry() {
 
   // Get the temporary position and error
   float Cp3x3[9];
-  uint32_t angular_ud;
+  int32_t angular_ud;
+  int32_t angularRate_udps;
   chSysLock();
     float pX = this->pX;
     float pY = this->pY;
     float pPhi = this->pPhi;
     Matrix::copy<float>(this->Cp3x3,3,3,Cp3x3,3,3);
-    // Get the differentail gyro information and reset it
-    angular_ud = gyro->getAngular_ud(L3G4200D::AXIS_Z);
-  gyro->angularReset();
+    // TODO Get the gyro (or gyro rate) information and do something with it
+    // angular_ud = gyro->getAngular_ud(L3G4200D::AXIS_Z);
+    // angularRate_udps = gyro->getAngularRate_udps(L3G4200D::AXIS_Z);
   chSysUnlock();
 
   ////////////////
@@ -150,8 +147,11 @@ void Odometry::updateOdometry() {
   ////////////////
 
   // TMP: Rotated angular
-  // float dPhi = (this->distance[RIGHT_WHEEL] - this->distance[LEFT_WHEEL]) / this->wheelBaseDistanceSI;
-  float dPhi = ((float(angular_ud * 1e-3) * M_PI ) * 1e-3) / 180.0f;
+  float dPhi = (this->distance[RIGHT_WHEEL] - this->distance[LEFT_WHEEL]) / this->wheelBaseDistanceSI;
+  // TODO Calculate the differential angel dPhi from either the angular (1. line) or angular rate (2.+3. line)
+  // float dPhi = ((float(angular_ud * 1e-3) * M_PI ) * 1e-3) / 180.0f;
+  // const float angular_md = float((angularRate_udps * this->period / constants::millisecondsPerSecond) * 1e-3);
+  // float dPhi = ((angular_md * M_PI) * 1e-3) / 180.0f;
 
   // TMP: Moved distance
   float dDistance = (this->distance[RIGHT_WHEEL] + this->distance[LEFT_WHEEL]) / 2.0f;
diff --git a/components/gyro/l3g4200d.cpp b/components/gyro/l3g4200d.cpp
index 2201677fc4d9672e2f32d710505f4d635f74dc63..a8e43c1727c95808b96e75be31fd607fc6e314c9 100644
--- a/components/gyro/l3g4200d.cpp
+++ b/components/gyro/l3g4200d.cpp
@@ -37,7 +37,7 @@ msg_t L3G4200D::main() {
   this->setName("l3g4200d");
 
   while (!this->shouldTerminate()) {
-  time += this->period_st;
+    time += this->period_st;
 
     updateSensorData();
     calcAngular();
@@ -48,7 +48,7 @@ msg_t L3G4200D::main() {
       chThdSleepUntil(time);
     } else {
       chprintf((BaseSequentialStream*) &global.sercanmux1, "WARNING l3g4200d: Unable to keep track\r\n");
-  }
+    }
   }
   return RDY_OK;
 }
@@ -62,7 +62,7 @@ getAngularRate(const uint8_t axis) {
 int32_t
 L3G4200D::
 getAngularRate_udps(const uint8_t axis) {
-  return uint32_t(this->angularRate[axis]) * this->udpsPerTic;
+  return int32_t(this->angularRate[axis]) * this->udpsPerTic;
 }
 
 int32_t
@@ -160,7 +160,8 @@ msg_t L3G4200D::configure(const L3G4200DConfig *config) {
     case L3G4200D::DR_400_HZ: this->period_us =  2500; break;
     case L3G4200D::DR_800_HZ: this->period_us =  1250; break;
   }
-  this->period_st = US2ST(this->period_st);
+  this->period_st = US2ST(this->period_us);
+  this->period_ms = this->period_us * 1e-3;
 
   // Handle the new full scale
   switch(config->ctrl1 & L3G4200D::FS_MASK) {
diff --git a/devices/DiWheelDrive/main.cpp b/devices/DiWheelDrive/main.cpp
index 4cc08215341eae4082af6ac20e036c8b6db97248..ee7dad3212dc3f30977104b4ea7cba5f81d6068b 100644
--- a/devices/DiWheelDrive/main.cpp
+++ b/devices/DiWheelDrive/main.cpp
@@ -849,6 +849,10 @@ int main(void) {
 
   global.increments.start();  // Start the qei driver
 
+  // Start the three axes gyroscope
+  global.l3g4200d.configure(&global.gyro_run_config);
+  global.l3g4200d.start(NORMALPRIO+5);
+
   global.odometry.start(NORMALPRIO + 20);
 
   global.robot.start(HIGHPRIO - 1);
@@ -867,10 +871,6 @@ int main(void) {
   global.lis331dlh.configure(&global.accel_run_config);
   global.lis331dlh.start(NORMALPRIO+4);
 
-  // Start the three axes gyroscope
-  global.l3g4200d.configure(&global.gyro_run_config);
-  global.l3g4200d.start(NORMALPRIO+5);
-
   // Start the user thread
   global.userThread.start(NORMALPRIO);
 
diff --git a/include/amiro/Odometry.h b/include/amiro/Odometry.h
index 0f44ddb9fb337f6ede79ab1693697983be7205cd..af4a513d088445866dce7e1813bc268efce6ba0b 100644
--- a/include/amiro/Odometry.h
+++ b/include/amiro/Odometry.h
@@ -8,7 +8,7 @@
 
 namespace amiro {
 
-  class Odometry : public chibios_rt::BaseStaticThread<1024> {
+  class Odometry : public chibios_rt::BaseStaticThread<512> {
   public:
     /**
      * Constructor