diff --git a/src/modules/mavlink/mavlink_main.cpp b/src/modules/mavlink/mavlink_main.cpp
index 68696fb778e2d9f66222b05ff9c626be9a65e081..506ef4903e7f812d97faef91ec5dfda41627cba3 100644
--- a/src/modules/mavlink/mavlink_main.cpp
+++ b/src/modules/mavlink/mavlink_main.cpp
@@ -1758,9 +1758,7 @@ Mavlink::task_main(int argc, char *argv[])
 	mavlink_update_system();
 
 	/* start the MAVLink receiver */
-	if (_mode != MAVLINK_MODE_OSD) {
-		MavlinkReceiver::receive_start(&_receive_thread, this);
-	}
+	MavlinkReceiver::receive_start(&_receive_thread, this);
 
 	MavlinkOrbSubscription *param_sub = add_orb_subscription(ORB_ID(parameter_update));
 	uint64_t param_time = 0;
@@ -2125,9 +2123,7 @@ Mavlink::task_main(int argc, char *argv[])
 	}
 
 	/* first wait for threads to complete before tearing down anything */
-	if (_mode != MAVLINK_MODE_OSD) {
-		pthread_join(_receive_thread, NULL);
-	}
+	pthread_join(_receive_thread, NULL);
 
 	delete _subscribe_to_stream;
 	_subscribe_to_stream = nullptr;
diff --git a/src/modules/mavlink/mavlink_main.h b/src/modules/mavlink/mavlink_main.h
index 249e36816ee640bed176f554ddccee870bac9885..682d644a484147726298153bb041ba7656a7d32f 100644
--- a/src/modules/mavlink/mavlink_main.h
+++ b/src/modules/mavlink/mavlink_main.h
@@ -1,6 +1,6 @@
 /****************************************************************************
  *
- *   Copyright (c) 2012-2014 PX4 Development Team. All rights reserved.
+ *   Copyright (c) 2012-2016 PX4 Development Team. All rights reserved.
  *
  * Redistribution and use in source and binary forms, with or without
  * modification, are permitted provided that the following conditions