diff --git a/src/ardrone_driver.cpp b/src/ardrone_driver.cpp
index 2d87067..dca469a 100644
--- a/src/ardrone_driver.cpp
+++ b/src/ardrone_driver.cpp
@@ -138,14 +138,8 @@ void ARDroneDriver::run()
                          shared_raw_navdata.navdata_demo.vbat_flying_percentage);
                 ROS_INFO("Navdata Publish Settings:");
                 ROS_INFO("    Legacy Mode: %s", enabled_legacy_navdata ? "On" : "Off");
-                if (fullspeed_navdata)
-                {
-                    ROS_INFO("    Nominal update rate: Full Speed");
-                }
-                else
-                {
-                    ROS_INFO("    Nominal update rate: %d Hz", looprate);
-                }
+                ROS_INFO("    ROS Loop Rate: %d", looprate);
+                ROS_INFO("    Instant New Navdata Publish: %s", fullspeed_navdata ? "On" : "Off");
                 // TODO: Enabled Navdata Demo
                 vp_os_mutex_unlock(&navdata_lock);
                 if (ardrone_control_config.num_version_soft[0] == '0')
@@ -154,15 +148,22 @@ void ARDroneDriver::run()
                 }
             }
         } else {
-            if (current_frame_id != last_frame_id)
+            vp_os_mutex_lock(&video_lock);
+            copy_current_frame_id = current_frame_id;
+            vp_os_mutex_unlock(&video_lock);
+            if (copy_current_frame_id != last_frame_id)
             {
                 publish_video();                                
-                last_frame_id = current_frame_id;
+                last_frame_id = copy_current_frame_id;
             }
-            if (current_navdata_id != last_navdata_id)
+
+            vp_os_mutex_lock(&navdata_lock);
+            copy_current_navdata_id = current_navdata_id;
+            vp_os_mutex_unlock(&navdata_lock);
+            if (copy_current_navdata_id != last_navdata_id)
             {
                 publish_navdata();
-                last_navdata_id = current_navdata_id;
+                last_navdata_id = copy_current_navdata_id;
             }
             if (freq_dev == 0) publish_tf();
 
diff --git a/src/ardrone_driver.h b/src/ardrone_driver.h
index 30ad167..f4ae22b 100644
--- a/src/ardrone_driver.h
+++ b/src/ardrone_driver.h
@@ -102,6 +102,9 @@ private:
 
     long int last_frame_id;
     long int last_navdata_id;
+    long int copy_current_frame_id;
+    long int copy_current_navdata_id;
+
 	int flying_state;
 
     bool inited;
diff --git a/src/video.cpp b/src/video.cpp
index 0eb5e74..0f9c55f 100644
--- a/src/video.cpp
+++ b/src/video.cpp
@@ -17,9 +17,9 @@ extern "C" C_RESULT export_stage_transform( void *cfg, vp_api_io_data_t *in, vp_
 //        printf("The size of buffer is %d\n", in->size);
     vp_os_mutex_lock(&video_lock);
 	memcpy(buffer, in->buffers[0], in->size);
+    current_frame_id++;
     vp_os_mutex_unlock(&video_lock);
-//    vp_os_mutex_unlock(&video_update_lock);
-	current_frame_id++;
+//    vp_os_mutex_unlock(&video_update_lock);	
  	return (SUCCESS);
 }