diff --git a/skid4_control.orogen b/skid4_control.orogen
index 179c74b..8aa9df3 100644
--- a/skid4_control.orogen
+++ b/skid4_control.orogen
@@ -26,6 +26,9 @@ task_context "SimpleController" do
 
     needs_configuration
 
+    property("timeout", "double",0.1).
+        doc("Timeout in seconds after a stop command is emitted if no new motion commands are available.")
+
     property("wheel_radius", "double").
         doc("the radius of a wheel, in meters")
     property("track_width", "double").
@@ -34,7 +37,7 @@ task_context "SimpleController" do
     property("left_wheel_names", "std::vector<std::string>")
     property("right_wheel_names", "std::vector<std::string>")
 
-    input_port('motion_command', 'base/MotionCommand2D').
+    input_port('motion_command', 'base/commands/Motion2D').
         doc('input command in rotation and translation. Requires a data connection type.')
 end
 
@@ -48,7 +51,7 @@ task_context "ConstantSpeedController" do
     property("speed", "double", 0.0).
         doc("speed that is applied to the motors")
 
-    input_port('motion_command', 'base/MotionCommand2D').
+    input_port('motion_command', 'base/commands/Motion2D').
         doc('input command in rotation and translation. Requires a data connection type.')
 end
 
@@ -75,7 +78,7 @@ task_context "SoftTurnController" do
     property("Tt", "double", -1.0)
 
 
-    input_port('motion_command', 'base/MotionCommand2D').
+    input_port('motion_command', 'base/commands/Motion2D').
         doc('input command in rotation and translation. Requires a data connection type.')
 end
 
diff --git a/tasks/SimpleController.cpp b/tasks/SimpleController.cpp
index e7cbcf3..621fe30 100644
--- a/tasks/SimpleController.cpp
+++ b/tasks/SimpleController.cpp
@@ -49,7 +49,9 @@ bool SimpleController::configureHook()
         m_rightIndexes.push_back(curIndex);
         curIndex++;        
     }
-    
+
+    // The output of this controller is a speed command.
+    m_cmd.mode[0] = m_cmd.mode[1] = m_cmd.mode[2] = m_cmd.mode[3] = base::actuators::DM_SPEED;
     return true;    
 }
 
@@ -59,15 +61,32 @@ void SimpleController::updateHook()
 
     // This is the user's command
     base::commands::Motion2D cmd_in;
-    if (_motion_command.readNewest(cmd_in) == RTT::NoData)
+    switch(_motion_command.readNewest(cmd_in))
     {
+    // no data connection
+    case RTT::NoData:
+        // send stop command
         cmd_in.translation = 0;
-        cmd_in.rotation    = 0;
-    }
+        cmd_in.rotation  = 0;
+        break;
 
-    // The output of this controller is a speed command.
-    m_cmd.mode[0] = m_cmd.mode[1] =
-        m_cmd.mode[2] = m_cmd.mode[3] = base::actuators::DM_SPEED;
+    // old data
+    case RTT::OldData:
+        if((base::Time::now()-m_cmd.time).toSeconds() > _timeout.value())
+        {
+            // send stop command
+            cmd_in.translation = 0;
+            cmd_in.rotation  = 0;
+        }
+        break;
+
+    // new data are available
+    case RTT::NewData:
+        // set timestamp
+        m_cmd.time = base::Time::now();
+        m_jointCmd.time = m_cmd.time;
+        break;
+    }
 
     double fwd_velocity = cmd_in.translation / m_radius;
     double differential = cmd_in.rotation * m_trackWidth / m_radius;
@@ -75,20 +94,12 @@ void SimpleController::updateHook()
     m_cmd.target[base::actuators::WHEEL4_REAR_LEFT]   = fwd_velocity - differential;
     m_cmd.target[base::actuators::WHEEL4_FRONT_RIGHT] = fwd_velocity + differential;
     m_cmd.target[base::actuators::WHEEL4_REAR_RIGHT]  = fwd_velocity + differential;
-    m_cmd.time = base::Time::now();
-
-    _simple_command.write(m_cmd);
-
-    for(std::vector<size_t>::const_iterator it = m_leftIndexes.begin(); it != m_leftIndexes.end();it++)
-    {
+    for(std::vector<size_t>::const_iterator it = m_leftIndexes.begin(); it != m_leftIndexes.end();++it)
         m_jointCmd.elements[*it].speed = fwd_velocity - differential;
-    }
-    for(std::vector<size_t>::const_iterator it = m_rightIndexes.begin(); it != m_rightIndexes.end();it++)
-    {
+    for(std::vector<size_t>::const_iterator it = m_rightIndexes.begin(); it != m_rightIndexes.end();++it)
         m_jointCmd.elements[*it].speed = fwd_velocity + differential;
-    }
-    
-    m_jointCmd.time = m_cmd.time;
-    
+
+    // write data to output ports
+    _simple_command.write(m_cmd);
     _command.write(m_jointCmd);
 }