diff --git a/include/model_car_egomotion.h b/include/model_car_egomotion.h
index c3da983a4d2cf7f1ac1aaaa982b095238a0d73a8..b1e5e0ac97b35e4e509cfaeb19f49d6356381c0c 100644
--- a/include/model_car_egomotion.h
+++ b/include/model_car_egomotion.h
@@ -1,19 +1,7 @@
 #ifndef _MODEL_CAR_EGOMOTION_H
 #define _MODEL_CAR_EGOMOTION_H
 
-#include "rs232.h"
-#include "commexceptions.h"
-#include "threadserver.h"
 #include "model_car_driver_base.h"
-#include "model_car_exceptions.h"
-#include "model_car_protocol.h"
-#include <iostream>
-#include <iomanip>
-#include <vector>
-#include <dirent.h>
-#include <chrono>
-#include <thread>
-#include <map>
 
 /**
   * @class CModelCarEgomotion
@@ -24,20 +12,21 @@
 class CModelCarEgomotion: public CModelCarDriverBase
 {
   private:
-    void process_data_frame(uint8_t id,uint32_t timestamp, TDataUnion data_union);
+    void process_data_frame(uint8_t id, TDataUnion data_union);
     TWheelData left_wheel;
+    uint32_t left_wheel_timestamp;
     TWheelData right_wheel;
+    uint32_t right_wheel_timestamp;
     TImuData imu;
-    
-  protected:
+    uint32_t imu_timestamp;
     
   public:
     CModelCarEgomotion(std::string name);
-    bool get_left_wheel(int &tach, bool &dir);
-    bool get_right_wheel(int &tach, bool &dir);
-    bool get_imu_accel(float &ax, float &ay, float &az);
-    bool get_imu_gyro( float &gx, float &gy, float &gz);
-    bool get_imu_magn( float &mx, float &my, float &mz);
+    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);
     ~CModelCarEgomotion(void);
 };
 
diff --git a/src/model_car_egomotion.cpp b/src/model_car_egomotion.cpp
index 7b7386210a57c01548a23cc8e02b8ed72e57c28c..7dea239f256c82141d9cffa973b975623a398053 100644
--- a/src/model_car_egomotion.cpp
+++ b/src/model_car_egomotion.cpp
@@ -5,99 +5,94 @@ CModelCarEgomotion::CModelCarEgomotion(std::string name) : CModelCarDriverBase(n
 
 }
   
-void CModelCarEgomotion::process_data_frame(uint8_t id,uint32_t timestamp, TDataUnion data_union)
+void CModelCarEgomotion::process_data_frame(uint8_t id, TDataUnion data_union)
 {
-  //std::cout << "CModelCarEgomotion::process_data_frame: id=" << unsigned(id) << std::endl;
+  static bool left_done=false,right_done=false,imu_done=false;
 
-  const SENSOR_ID currentSensor = static_cast<SENSOR_ID>(id);
-
-  switch(currentSensor)
+  switch((SENSOR_ID)id)
   {
     case ID_ARD_SENS_WHEEL_RIGHT:
       this->right_wheel = data_union.wheel;
+      this->right_wheel_timestamp=this->get_last_timestamp();
+      right_done=true;
       break;
     case ID_ARD_SENS_WHEEL_LEFT:
       this->left_wheel = data_union.wheel;
+      this->left_wheel_timestamp=this->get_last_timestamp();
+      left_done=true;
       break;
     case ID_ARD_SENS_IMU:
-      this->imu = data_union.imu;
-      this->imu.ax = this->imu.ax*9.81; // G to m/s2
-      this->imu.ay = this->imu.ay*9.81; // G to m/s2
-      this->imu.az = this->imu.az*9.81; // G to m/s2
-      this->imu.gx = this->imu.gx*3.14159/180; // º/s to rad/s
-      this->imu.gy = this->imu.gy*3.14159/180; // º/s to rad/s
-      this->imu.gz = this->imu.gz*3.14159/180; // º/s to rad/s
-      this->imu.mx = this->imu.mx*1e-7; //miliGauss(mG) to Tesla(T)
-      this->imu.my = this->imu.my*1e-7; //miliGauss(mG) to Tesla(T)
-      this->imu.mz = this->imu.mz*1e-7; //miliGauss(mG) to Tesla(T)
+      this->imu.ax = data_union.imu.ax*9.81; // G to m/s2
+      this->imu.ay = data_union.imu.ay*9.81; // G to m/s2
+      this->imu.az = data_union.imu.az*9.81; // G to m/s2
+      this->imu.gx = data_union.imu.gx*3.14159/180; // º/s to rad/s
+      this->imu.gy = data_union.imu.gy*3.14159/180; // º/s to rad/s
+      this->imu.gz = data_union.imu.gz*3.14159/180; // º/s to rad/s
+      this->imu.mx = data_union.imu.mx*1e-7; //miliGauss(mG) to Tesla(T)
+      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();
+      imu_done=true;
       break;
     default:
-      std::cout << "CModelCarEgomotion::process_data_frame: error, incorrect id: " << (unsigned int)id << std::endl;
+      break;
+  }
+  if(left_done && right_done && imu_done)
+  {
+    left_done=false;
+    right_done=false;
+    imu_done=false;
+    if(!this->event_server->event_is_set(this->new_data_event_id))
+      this->event_server->set_event(this->new_data_event_id);
   }
 }
 
-bool CModelCarEgomotion::get_left_wheel(int &tach, bool &dir)
+void CModelCarEgomotion::get_left_wheel(int &tach, bool &dir,unsigned int &timestamp)
 {
-  bool ok=false;
   this->data_mutex.enter();
-  tach = this->left_wheel.wheel_tach;
-  dir  = this->left_wheel.wheel_dir;
-  ok=true;
+  tach=this->left_wheel.wheel_tach;
+  dir=this->left_wheel.wheel_dir;
+  timestamp=this->left_wheel_timestamp;
   this->data_mutex.exit();
-
-  return ok;
 }
 
-bool CModelCarEgomotion::get_right_wheel(int &tach, bool &dir)
+void CModelCarEgomotion::get_right_wheel(int &tach, bool &dir,unsigned int &timestamp)
 {
-  bool ok=false;
   this->data_mutex.enter();
   tach = this->right_wheel.wheel_tach;
   dir  = this->right_wheel.wheel_dir;
-  ok=true;
+  timestamp=this->right_wheel_timestamp;
   this->data_mutex.exit();
-
-  return ok;
-
 }
 
-bool CModelCarEgomotion::get_imu_accel(float &ax, float &ay, float &az)
+void CModelCarEgomotion::get_imu_accelerometer(double &ax, double &ay, double &az,unsigned int &timestamp)
 {
-  bool ok=false;
   this->data_mutex.enter();
   ax = this->imu.ax;
   ay = this->imu.ay;
   az = this->imu.az;
-  ok=true;
+  timestamp=this->imu_timestamp;
   this->data_mutex.exit();
-
-  return ok;
 }
 
-bool CModelCarEgomotion::get_imu_gyro(float &gx, float &gy, float &gz)
+void CModelCarEgomotion::get_imu_gyroscope(double &gx, double &gy, double &gz,unsigned int &timestamp)
 {
-  bool ok=false;
   this->data_mutex.enter();
   gx = this->imu.gx;
   gy = this->imu.gy;
   gz = this->imu.gz;
-  ok=true;
+  timestamp=this->imu_timestamp;
   this->data_mutex.exit();
-
-  return ok;
 }
 
-bool CModelCarEgomotion::get_imu_magn(float &mx, float &my, float &mz)
+void CModelCarEgomotion::get_imu_magnetometer(double &mx, double &my, double &mz,unsigned int &timestamp)
 {
-  bool ok=false;
   this->data_mutex.enter();
   mx = this->imu.mx;
   my = this->imu.my;
   mz = this->imu.mz;
-  ok=true;
+  timestamp=this->imu_timestamp;
   this->data_mutex.exit();
-
-  return ok;
 }
 
 CModelCarEgomotion::~CModelCarEgomotion()