diff --git a/include/model_car_egomotion.h b/include/model_car_egomotion.h
index b1e5e0ac97b35e4e509cfaeb19f49d6356381c0c..6ec120336645ac2fc75bbcad76697bdbcb4f63eb 100644
--- a/include/model_car_egomotion.h
+++ b/include/model_car_egomotion.h
@@ -14,19 +14,28 @@ class CModelCarEgomotion: public CModelCarDriverBase
   private:
     void process_data_frame(uint8_t id, TDataUnion data_union);
     TWheelData left_wheel;
-    uint32_t left_wheel_timestamp;
+    uint64_t left_wheel_timestamp;
     TWheelData right_wheel;
-    uint32_t right_wheel_timestamp;
+    uint64_t right_wheel_timestamp;
     TImuData imu;
-    uint32_t imu_timestamp;
+    uint64_t imu_timestamp;
+    double zero_gx;
+    double zero_gy;
+    double zero_gz;
+    bool gyro_cal_running;
+    bool gyro_cal_stop;
+    unsigned int gyro_cal_num_samples;
     
   public:
     CModelCarEgomotion(std::string name);
-    void get_left_wheel(int &tach, bool &dir,unsigned int &timestamp);
-    void get_right_wheel(int &tach, bool &dir,unsigned int &timestamp);
-    void get_imu_accelerometer(double &ax, double &ay, double &az,unsigned int &timestamp);
-    void get_imu_gyroscope(double &gx, double &gy, double &gz,unsigned int &timestamp);
-    void get_imu_magnetometer(double &mx, double &my, double &mz,unsigned int &timestamp);
+    void get_left_wheel(int &tach, bool &dir,unsigned long int &timestamp);
+    void get_right_wheel(int &tach, bool &dir,unsigned long int &timestamp);
+    void get_imu_accelerometer(double &ax, double &ay, double &az,unsigned long int &timestamp);
+    void get_imu_gyroscope(double &gx, double &gy, double &gz,unsigned long int &timestamp);
+    void get_imu_magnetometer(double &mx, double &my, double &mz,unsigned long int &timestamp);
+    void start_gyro_calibration(unsigned int num_samples=100);
+    void stop_gyro_calibration(void);
+    bool is_gyro_calibration_done(void);
     ~CModelCarEgomotion(void);
 };
 
diff --git a/src/examples/model_car_egomotion_test.cpp b/src/examples/model_car_egomotion_test.cpp
index 1e7125facf8efc2e04dedd0c3ff4c673bacee17b..429c742e0797c694f00c2decc95e6297f879e304 100644
--- a/src/examples/model_car_egomotion_test.cpp
+++ b/src/examples/model_car_egomotion_test.cpp
@@ -8,7 +8,7 @@ int main(int argc, char *argv[])
   std::string name = "model_car_driver_egomotion";
   CModelCarEgomotion *egomotion_driver;
   double ax,ay,az,gx,gy,gz,mx,my,mz;
-  unsigned int timestamp;
+  unsigned long int timestamp;
   int tach;
   bool dir;
 
@@ -70,5 +70,4 @@ int main(int argc, char *argv[])
       std::cout << e.what() << std::endl;
     }
   }
-  
 }
diff --git a/src/model_car_egomotion.cpp b/src/model_car_egomotion.cpp
index 7dea239f256c82141d9cffa973b975623a398053..c9f5a8ad7e3d54de8897d1653c62c74717c9ec47 100644
--- a/src/model_car_egomotion.cpp
+++ b/src/model_car_egomotion.cpp
@@ -1,13 +1,21 @@
 #include "model_car_egomotion.h"
+#include <list>
 
 CModelCarEgomotion::CModelCarEgomotion(std::string name) : CModelCarDriverBase(name, ARDUINO_REAR_IMU_WHEELENC)
 {
-
+  this->zero_gx=0.0;
+  this->zero_gy=0.0;
+  this->zero_gz=0.0;
+  this->gyro_cal_running=false;
+  this->gyro_cal_stop=false;
+  this->gyro_cal_num_samples=100;
 }
   
 void CModelCarEgomotion::process_data_frame(uint8_t id, TDataUnion data_union)
 {
   static bool left_done=false,right_done=false,imu_done=false;
+  static double cal_gx=0.0,cal_gy=0.0,cal_gz=0.0;
+  static unsigned int cal_count=0;
 
   switch((SENSOR_ID)id)
   {
@@ -32,6 +40,36 @@ void CModelCarEgomotion::process_data_frame(uint8_t id, TDataUnion data_union)
       this->imu.my = data_union.imu.my*1e-7; //miliGauss(mG) to Tesla(T)
       this->imu.mz = data_union.imu.mz*1e-7; //miliGauss(mG) to Tesla(T)
       this->imu_timestamp=this->get_last_timestamp();
+      if(this->gyro_cal_running)
+      {
+        cal_gx+=this->imu.gx;
+        cal_gy+=this->imu.gy;
+        cal_gz+=this->imu.gz;
+        cal_count++;
+        if(this->gyro_cal_num_samples==cal_count)
+        {
+          cal_count=0;
+          this->zero_gx=cal_gx/this->gyro_cal_num_samples;
+          this->zero_gy=cal_gy/this->gyro_cal_num_samples;
+          this->zero_gz=cal_gz/this->gyro_cal_num_samples;
+          cal_gx=0.0;
+          cal_gy=0.0;
+          cal_gz=0.0;
+          this->gyro_cal_running=false;
+        }
+        else if(this->gyro_cal_stop)
+        {
+          cal_count=0;
+          this->zero_gx=cal_gx/this->gyro_cal_num_samples;
+          this->zero_gy=cal_gy/this->gyro_cal_num_samples;
+          this->zero_gz=cal_gz/this->gyro_cal_num_samples;
+          cal_gx=0.0;
+          cal_gy=0.0;
+          cal_gz=0.0;
+          this->gyro_cal_running=false;
+          this->gyro_cal_stop=false;
+        }
+      }
       imu_done=true;
       break;
     default:
@@ -47,7 +85,7 @@ void CModelCarEgomotion::process_data_frame(uint8_t id, TDataUnion data_union)
   }
 }
 
-void CModelCarEgomotion::get_left_wheel(int &tach, bool &dir,unsigned int &timestamp)
+void CModelCarEgomotion::get_left_wheel(int &tach, bool &dir,unsigned long int &timestamp)
 {
   this->data_mutex.enter();
   tach=this->left_wheel.wheel_tach;
@@ -56,7 +94,7 @@ void CModelCarEgomotion::get_left_wheel(int &tach, bool &dir,unsigned int &times
   this->data_mutex.exit();
 }
 
-void CModelCarEgomotion::get_right_wheel(int &tach, bool &dir,unsigned int &timestamp)
+void CModelCarEgomotion::get_right_wheel(int &tach, bool &dir,unsigned long int &timestamp)
 {
   this->data_mutex.enter();
   tach = this->right_wheel.wheel_tach;
@@ -65,7 +103,7 @@ void CModelCarEgomotion::get_right_wheel(int &tach, bool &dir,unsigned int &time
   this->data_mutex.exit();
 }
 
-void CModelCarEgomotion::get_imu_accelerometer(double &ax, double &ay, double &az,unsigned int &timestamp)
+void CModelCarEgomotion::get_imu_accelerometer(double &ax, double &ay, double &az,unsigned long int &timestamp)
 {
   this->data_mutex.enter();
   ax = this->imu.ax;
@@ -75,17 +113,17 @@ void CModelCarEgomotion::get_imu_accelerometer(double &ax, double &ay, double &a
   this->data_mutex.exit();
 }
 
-void CModelCarEgomotion::get_imu_gyroscope(double &gx, double &gy, double &gz,unsigned int &timestamp)
+void CModelCarEgomotion::get_imu_gyroscope(double &gx, double &gy, double &gz,unsigned long int &timestamp)
 {
   this->data_mutex.enter();
-  gx = this->imu.gx;
-  gy = this->imu.gy;
-  gz = this->imu.gz;
+  gx = this->imu.gx-this->zero_gx;
+  gy = this->imu.gy-this->zero_gy;
+  gz = this->imu.gz-this->zero_gz;
   timestamp=this->imu_timestamp;
   this->data_mutex.exit();
 }
 
-void CModelCarEgomotion::get_imu_magnetometer(double &mx, double &my, double &mz,unsigned int &timestamp)
+void CModelCarEgomotion::get_imu_magnetometer(double &mx, double &my, double &mz,unsigned long int &timestamp)
 {
   this->data_mutex.enter();
   mx = this->imu.mx;
@@ -95,6 +133,29 @@ void CModelCarEgomotion::get_imu_magnetometer(double &mx, double &my, double &mz
   this->data_mutex.exit();
 }
 
+void CModelCarEgomotion::start_gyro_calibration(unsigned int num_samples)
+{
+  this->data_mutex.enter();
+  this->zero_gx=0.0;
+  this->zero_gy=0.0;
+  this->zero_gy=0.0;
+  this->gyro_cal_running=true;
+  this->gyro_cal_num_samples=num_samples;
+  this->data_mutex.exit();
+}
+
+void CModelCarEgomotion::stop_gyro_calibration(void)
+{
+  this->data_mutex.enter();
+  this->gyro_cal_stop=true;
+  this->data_mutex.exit();
+}
+
+bool CModelCarEgomotion::is_gyro_calibration_done(void)
+{
+  return !this->gyro_cal_running;
+}
+
 CModelCarEgomotion::~CModelCarEgomotion()
 {