diff --git a/src/drivers/device/cdev.cpp b/src/drivers/device/cdev.cpp
index b157b3f181..6e2ebb1f7b 100644
--- a/src/drivers/device/cdev.cpp
+++ b/src/drivers/device/cdev.cpp
@@ -124,7 +124,7 @@ CDev::register_class_devname(const char *class_devname)
 			if (ret == OK) break;
 		} else {
 			char name[32];
-			snprintf(name, sizeof(name), "%s%u", class_devname, class_instance);
+			snprintf(name, sizeof(name), "%s%d", class_devname, class_instance);
 			ret = register_driver(name, &fops, 0666, (void *)this);
 			if (ret == OK) break;
 		}
diff --git a/src/drivers/gps/gps.cpp b/src/drivers/gps/gps.cpp
index a736cbdf6b..c72f692d73 100644
--- a/src/drivers/gps/gps.cpp
+++ b/src/drivers/gps/gps.cpp
@@ -232,6 +232,11 @@ GPS::ioctl(struct file *filp, int cmd, unsigned long arg)
 	case SENSORIOCRESET:
 		cmd_reset();
 		break;
+
+	default:
+		/* give it to parent if no one wants it */
+		ret = CDev::ioctl(filp, cmd, arg);
+		break;
 	}
 
 	unlock();
diff --git a/src/modules/attitude_estimator_so3/attitude_estimator_so3_main.cpp b/src/modules/attitude_estimator_so3/attitude_estimator_so3_main.cpp
index e2d69cd615..3653defa66 100755
--- a/src/modules/attitude_estimator_so3/attitude_estimator_so3_main.cpp
+++ b/src/modules/attitude_estimator_so3/attitude_estimator_so3_main.cpp
@@ -445,7 +445,6 @@ int attitude_estimator_so3_thread_main(int argc, char *argv[])
 	// XXX write this out to perf regs
 
 	/* keep track of sensor updates */
-	uint32_t sensor_last_count[3] = {0, 0, 0};
 	uint64_t sensor_last_timestamp[3] = {0, 0, 0};
 
 	struct attitude_estimator_so3_params so3_comp_params;
diff --git a/src/modules/commander/state_machine_helper.cpp b/src/modules/commander/state_machine_helper.cpp
index 31955d3e5d..e6f245cf93 100644
--- a/src/modules/commander/state_machine_helper.cpp
+++ b/src/modules/commander/state_machine_helper.cpp
@@ -44,6 +44,7 @@
 #include <stdbool.h>
 #include <dirent.h>
 #include <fcntl.h>
+#include <string.h>
 
 #include <uORB/uORB.h>
 #include <uORB/topics/vehicle_status.h>
@@ -309,10 +310,7 @@ int hil_state_transition(hil_state_t new_state, int status_pub, struct vehicle_s
 	bool valid_transition = false;
 	int ret = ERROR;
 
-	warnx("Current state: %d, requested state: %d", current_status->hil_state, new_state);
-
 	if (current_status->hil_state == new_state) {
-		warnx("Hil state not changed");
 		valid_transition = true;
 
 	} else {
@@ -340,23 +338,60 @@ int hil_state_transition(hil_state_t new_state, int status_pub, struct vehicle_s
 
 				/* list directory */
 				DIR		*d;
-				struct dirent	*direntry;
 				d = opendir("/dev");
 				if (d) {
 
+					struct dirent	*direntry;
+					char devname[24];
+
 					while ((direntry = readdir(d)) != NULL) {
 
-						int sensfd = ::open(direntry->d_name, 0);
-						int block_ret = ::ioctl(sensfd, DEVIOCSPUBBLOCK, 0);
+						/* skip serial ports */
+						if (!strncmp("tty", direntry->d_name, 3)) {
+							continue;
+						}
+						/* skip mtd devices */
+						if (!strncmp("mtd", direntry->d_name, 3)) {
+							continue;
+						}
+						/* skip ram devices */
+						if (!strncmp("ram", direntry->d_name, 3)) {
+							continue;
+						}
+						/* skip MMC devices */
+						if (!strncmp("mmc", direntry->d_name, 3)) {
+							continue;
+						}
+						/* skip mavlink */
+						if (!strcmp("mavlink", direntry->d_name)) {
+							continue;
+						}
+						/* skip console */
+						if (!strcmp("console", direntry->d_name)) {
+							continue;
+						}
+						/* skip null */
+						if (!strcmp("null", direntry->d_name)) {
+							continue;
+						}
+
+						snprintf(devname, sizeof(devname), "/dev/%s", direntry->d_name);
+
+						int sensfd = ::open(devname, 0);
+
+						if (sensfd < 0) {
+							warn("failed opening device %s", devname);
+							continue;
+						}
+
+						int block_ret = ::ioctl(sensfd, DEVIOCSPUBBLOCK, 1);
 						close(sensfd);
 
-						printf("Disabling %s\n: %s", direntry->d_name, (!block_ret) ? "OK" : "FAIL");
+						printf("Disabling %s: %s\n", devname, (block_ret == OK) ? "OK" : "ERROR");
 					}
 
 					closedir(d);
 
-					warnx("directory listing ok (FS mounted and readable)");
-
 				} else {
 					/* failed opening dir */
 					warnx("FAILED LISTING DEVICE ROOT DIRECTORY");
diff --git a/src/modules/mavlink/mavlink_main.cpp b/src/modules/mavlink/mavlink_main.cpp
index 11882f0f40..9c2e0178a3 100644
--- a/src/modules/mavlink/mavlink_main.cpp
+++ b/src/modules/mavlink/mavlink_main.cpp
@@ -201,6 +201,7 @@ Mavlink::Mavlink() :
 	_mavlink_fd(-1),
 	_task_running(false),
 	_hil_enabled(false),
+	_is_usb_uart(false),
 	_main_loop_delay(1000),
 	_subscriptions(nullptr),
 	_streams(nullptr),
@@ -577,17 +578,19 @@ int Mavlink::mavlink_open_uart(int baud, const char *uart_name, struct termios *
 		return -1;
 	}
 
-	/*
-	 * Setup hardware flow control. If the port has no RTS pin this call will fail,
-	 * which is not an issue, but requires a separate call so we can fail silently.
-	 */
-	(void)tcgetattr(_uart_fd, &uart_config);
-	uart_config.c_cflag |= CRTS_IFLOW;
-	(void)tcsetattr(_uart_fd, TCSANOW, &uart_config);
+	if (!_is_usb_uart) {
+		/*
+		 * Setup hardware flow control. If the port has no RTS pin this call will fail,
+		 * which is not an issue, but requires a separate call so we can fail silently.
+		 */
+		(void)tcgetattr(_uart_fd, &uart_config);
+		uart_config.c_cflag |= CRTS_IFLOW;
+		(void)tcsetattr(_uart_fd, TCSANOW, &uart_config);
 
-	/* setup output flow control */
-	if (enable_flow_control(true)) {
-		warnx("ERR FLOW CTRL EN");
+		/* setup output flow control */
+		if (enable_flow_control(true)) {
+			warnx("ERR FLOW CTRL EN");
+		}
 	}
 
 	return _uart_fd;
@@ -596,6 +599,11 @@ int Mavlink::mavlink_open_uart(int baud, const char *uart_name, struct termios *
 int
 Mavlink::enable_flow_control(bool enabled)
 {
+	// We can't do this on USB - skip
+	if (_is_usb_uart) {
+		return OK;
+	}
+
 	struct termios uart_config;
 	int ret = tcgetattr(_uart_fd, &uart_config);
 	if (enabled) {
@@ -1701,10 +1709,9 @@ Mavlink::task_main(int argc, char *argv[])
 	fflush(stdout);
 
 	struct termios uart_config_original;
-	bool usb_uart;
 
 	/* default values for arguments */
-	_uart_fd = mavlink_open_uart(_baudrate, _device_name, &uart_config_original, &usb_uart);
+	_uart_fd = mavlink_open_uart(_baudrate, _device_name, &uart_config_original, &_is_usb_uart);
 
 	if (_uart_fd < 0) {
 		warn("could not open %s", _device_name);
diff --git a/src/modules/mavlink/mavlink_main.h b/src/modules/mavlink/mavlink_main.h
index e6e20eb938..f743c25044 100644
--- a/src/modules/mavlink/mavlink_main.h
+++ b/src/modules/mavlink/mavlink_main.h
@@ -208,6 +208,7 @@ private:
 
 	/* states */
 	bool		_hil_enabled;		/**< Hardware In the Loop mode */
+	bool		_is_usb_uart;		/**< Port is USB */
 
 	unsigned	_main_loop_delay;		/**< mainloop delay, depends on data rate */
 
diff --git a/src/modules/mavlink/mavlink_messages.cpp b/src/modules/mavlink/mavlink_messages.cpp
index a2bc28bad0..7d388f88da 100644
--- a/src/modules/mavlink/mavlink_messages.cpp
+++ b/src/modules/mavlink/mavlink_messages.cpp
@@ -216,8 +216,8 @@ protected:
 
 	void send(const hrt_abstime t)
 	{
-		status_sub->update(t);
-		pos_sp_triplet_sub->update(t);
+		(void)status_sub->update(t);
+		(void)pos_sp_triplet_sub->update(t);
 
 		uint8_t mavlink_state = 0;
 		uint8_t mavlink_base_mode = 0;
@@ -261,22 +261,23 @@ protected:
 
 	void send(const hrt_abstime t)
 	{
-		status_sub->update(t);
+		if (status_sub->update(t)) {
 
-		mavlink_msg_sys_status_send(_channel,
-					    status->onboard_control_sensors_present,
-					    status->onboard_control_sensors_enabled,
-					    status->onboard_control_sensors_health,
-					    status->load * 1000.0f,
-					    status->battery_voltage * 1000.0f,
-					    status->battery_current * 1000.0f,
-					    status->battery_remaining,
-					    status->drop_rate_comm,
-					    status->errors_comm,
-					    status->errors_count1,
-					    status->errors_count2,
-					    status->errors_count3,
-					    status->errors_count4);
+			mavlink_msg_sys_status_send(_channel,
+						    status->onboard_control_sensors_present,
+						    status->onboard_control_sensors_enabled,
+						    status->onboard_control_sensors_health,
+						    status->load * 1000.0f,
+						    status->battery_voltage * 1000.0f,
+						    status->battery_current * 1000.0f,
+						    status->battery_remaining,
+						    status->drop_rate_comm,
+						    status->errors_comm,
+						    status->errors_count1,
+						    status->errors_count2,
+						    status->errors_count3,
+						    status->errors_count4);
+		}
 	}
 };
 
@@ -316,42 +317,43 @@ protected:
 
 	void send(const hrt_abstime t)
 	{
-		sensor_sub->update(t);
+		if (sensor_sub->update(t)) {
 
-		uint16_t fields_updated = 0;
+			uint16_t fields_updated = 0;
 
-		if (accel_timestamp != sensor->accelerometer_timestamp) {
-			/* mark first three dimensions as changed */
-			fields_updated |= (1 << 0) | (1 << 1) | (1 << 2);
-			accel_timestamp = sensor->accelerometer_timestamp;
+			if (accel_timestamp != sensor->accelerometer_timestamp) {
+				/* mark first three dimensions as changed */
+				fields_updated |= (1 << 0) | (1 << 1) | (1 << 2);
+				accel_timestamp = sensor->accelerometer_timestamp;
+			}
+
+			if (gyro_timestamp != sensor->timestamp) {
+				/* mark second group dimensions as changed */
+				fields_updated |= (1 << 3) | (1 << 4) | (1 << 5);
+				gyro_timestamp = sensor->timestamp;
+			}
+
+			if (mag_timestamp != sensor->magnetometer_timestamp) {
+				/* mark third group dimensions as changed */
+				fields_updated |= (1 << 6) | (1 << 7) | (1 << 8);
+				mag_timestamp = sensor->magnetometer_timestamp;
+			}
+
+			if (baro_timestamp != sensor->baro_timestamp) {
+				/* mark last group dimensions as changed */
+				fields_updated |= (1 << 9) | (1 << 11) | (1 << 12);
+				baro_timestamp = sensor->baro_timestamp;
+			}
+
+			mavlink_msg_highres_imu_send(_channel,
+						     sensor->timestamp,
+						     sensor->accelerometer_m_s2[0], sensor->accelerometer_m_s2[1], sensor->accelerometer_m_s2[2],
+						     sensor->gyro_rad_s[0], sensor->gyro_rad_s[1], sensor->gyro_rad_s[2],
+						     sensor->magnetometer_ga[0], sensor->magnetometer_ga[1], sensor->magnetometer_ga[2],
+						     sensor->baro_pres_mbar, sensor->differential_pressure_pa,
+						     sensor->baro_alt_meter, sensor->baro_temp_celcius,
+						     fields_updated);
 		}
-
-		if (gyro_timestamp != sensor->timestamp) {
-			/* mark second group dimensions as changed */
-			fields_updated |= (1 << 3) | (1 << 4) | (1 << 5);
-			gyro_timestamp = sensor->timestamp;
-		}
-
-		if (mag_timestamp != sensor->magnetometer_timestamp) {
-			/* mark third group dimensions as changed */
-			fields_updated |= (1 << 6) | (1 << 7) | (1 << 8);
-			mag_timestamp = sensor->magnetometer_timestamp;
-		}
-
-		if (baro_timestamp != sensor->baro_timestamp) {
-			/* mark last group dimensions as changed */
-			fields_updated |= (1 << 9) | (1 << 11) | (1 << 12);
-			baro_timestamp = sensor->baro_timestamp;
-		}
-
-		mavlink_msg_highres_imu_send(_channel,
-					     sensor->timestamp,
-					     sensor->accelerometer_m_s2[0], sensor->accelerometer_m_s2[1], sensor->accelerometer_m_s2[2],
-					     sensor->gyro_rad_s[0], sensor->gyro_rad_s[1], sensor->gyro_rad_s[2],
-					     sensor->magnetometer_ga[0], sensor->magnetometer_ga[1], sensor->magnetometer_ga[2],
-					     sensor->baro_pres_mbar, sensor->differential_pressure_pa,
-					     sensor->baro_alt_meter, sensor->baro_temp_celcius,
-					     fields_updated);
 	}
 };
 
@@ -382,12 +384,13 @@ protected:
 
 	void send(const hrt_abstime t)
 	{
-		att_sub->update(t);
+		if (att_sub->update(t)) {
 
-		mavlink_msg_attitude_send(_channel,
-					  att->timestamp / 1000,
-					  att->roll, att->pitch, att->yaw,
-					  att->rollspeed, att->pitchspeed, att->yawspeed);
+			mavlink_msg_attitude_send(_channel,
+						  att->timestamp / 1000,
+						  att->roll, att->pitch, att->yaw,
+						  att->rollspeed, att->pitchspeed, att->yawspeed);
+		}
 	}
 };
 
@@ -418,17 +421,18 @@ protected:
 
 	void send(const hrt_abstime t)
 	{
-		att_sub->update(t);
+		if (att_sub->update(t)) {
 
-		mavlink_msg_attitude_quaternion_send(_channel,
-						     att->timestamp / 1000,
-						     att->q[0],
-						     att->q[1],
-						     att->q[2],
-						     att->q[3],
-						     att->rollspeed,
-						     att->pitchspeed,
-						     att->yawspeed);
+			mavlink_msg_attitude_quaternion_send(_channel,
+							     att->timestamp / 1000,
+							     att->q[0],
+							     att->q[1],
+							     att->q[2],
+							     att->q[3],
+							     att->rollspeed,
+							     att->pitchspeed,
+							     att->yawspeed);
+		}
 	}
 };
 
@@ -483,23 +487,26 @@ protected:
 
 	void send(const hrt_abstime t)
 	{
-		att_sub->update(t);
-		pos_sub->update(t);
-		armed_sub->update(t);
-		act_sub->update(t);
-		airspeed_sub->update(t);
+		bool updated = att_sub->update(t);
+		updated |= pos_sub->update(t);
+		updated |= armed_sub->update(t);
+		updated |= act_sub->update(t);
+		updated |= airspeed_sub->update(t);
 
-		float groundspeed = sqrtf(pos->vel_n * pos->vel_n + pos->vel_e * pos->vel_e);
-		uint16_t heading = _wrap_2pi(att->yaw) * M_RAD_TO_DEG_F;
-		float throttle = armed->armed ? act->control[3] * 100.0f : 0.0f;
+		if (updated) {
 
-		mavlink_msg_vfr_hud_send(_channel,
-					 airspeed->true_airspeed_m_s,
-					 groundspeed,
-					 heading,
-					 throttle,
-					 pos->alt,
-					 -pos->vel_d);
+			float groundspeed = sqrtf(pos->vel_n * pos->vel_n + pos->vel_e * pos->vel_e);
+			uint16_t heading = _wrap_2pi(att->yaw) * M_RAD_TO_DEG_F;
+			float throttle = armed->armed ? act->control[3] * 100.0f : 0.0f;
+
+			mavlink_msg_vfr_hud_send(_channel,
+						 airspeed->true_airspeed_m_s,
+						 groundspeed,
+						 heading,
+						 throttle,
+						 pos->alt,
+						 -pos->vel_d);
+		}
 	}
 };
 
@@ -530,19 +537,20 @@ protected:
 
 	void send(const hrt_abstime t)
 	{
-		gps_sub->update(t);
+		if (gps_sub->update(t)) {
 
-		mavlink_msg_gps_raw_int_send(_channel,
-					     gps->timestamp_position,
-					     gps->fix_type,
-					     gps->lat,
-					     gps->lon,
-					     gps->alt,
-					     cm_uint16_from_m_float(gps->eph_m),
-					     cm_uint16_from_m_float(gps->epv_m),
-					     gps->vel_m_s * 100.0f,
-					     _wrap_2pi(gps->cog_rad) * M_RAD_TO_DEG_F * 1e2f,
-					     gps->satellites_visible);
+			mavlink_msg_gps_raw_int_send(_channel,
+						     gps->timestamp_position,
+						     gps->fix_type,
+						     gps->lat,
+						     gps->lon,
+						     gps->alt,
+						     cm_uint16_from_m_float(gps->eph_m),
+						     cm_uint16_from_m_float(gps->epv_m),
+						     gps->vel_m_s * 100.0f,
+						     _wrap_2pi(gps->cog_rad) * M_RAD_TO_DEG_F * 1e2f,
+						     gps->satellites_visible);
+		}
 	}
 };
 
@@ -579,10 +587,11 @@ protected:
 
 	void send(const hrt_abstime t)
 	{
-		pos_sub->update(t);
-		home_sub->update(t);
+		bool updated = pos_sub->update(t);
+		updated |= home_sub->update(t);
 
-		mavlink_msg_global_position_int_send(_channel,
+		if (updated) {
+			mavlink_msg_global_position_int_send(_channel,
 						     pos->timestamp / 1000,
 						     pos->lat * 1e7,
 						     pos->lon * 1e7,
@@ -592,6 +601,7 @@ protected:
 						     pos->vel_e * 100.0f,
 						     pos->vel_d * 100.0f,
 						     _wrap_2pi(pos->yaw) * M_RAD_TO_DEG_F * 100.0f);
+		}
 	}
 };
 
@@ -622,16 +632,17 @@ protected:
 
 	void send(const hrt_abstime t)
 	{
-		pos_sub->update(t);
+		if (pos_sub->update(t)) {
 
-		mavlink_msg_local_position_ned_send(_channel,
-						    pos->timestamp / 1000,
-						    pos->x,
-						    pos->y,
-						    pos->z,
-						    pos->vx,
-						    pos->vy,
-						    pos->vz);
+			mavlink_msg_local_position_ned_send(_channel,
+							    pos->timestamp / 1000,
+							    pos->x,
+							    pos->y,
+							    pos->z,
+							    pos->vx,
+							    pos->vy,
+							    pos->vz);
+		}
 	}
 };
 
@@ -662,12 +673,17 @@ protected:
 
 	void send(const hrt_abstime t)
 	{
-		home_sub->update(t);
 
-		mavlink_msg_gps_global_origin_send(_channel,
-						   (int32_t)(home->lat * 1e7),
-						   (int32_t)(home->lon * 1e7),
-						   (int32_t)(home->alt) * 1000.0f);
+		/* we're sending the GPS home periodically to ensure the
+		 * the GCS does pick it up at one point */
+		if (home_sub->is_published()) {
+			home_sub->update(t);
+
+			mavlink_msg_gps_global_origin_send(_channel,
+							   (int32_t)(home->lat * 1e7),
+							   (int32_t)(home->lon * 1e7),
+							   (int32_t)(home->alt) * 1000.0f);
+		}
 	}
 };
 
@@ -713,19 +729,20 @@ protected:
 
 	void send(const hrt_abstime t)
 	{
-		act_sub->update(t);
+		if (act_sub->update(t)) {
 
-		mavlink_msg_servo_output_raw_send(_channel,
-						  act->timestamp / 1000,
-						  _n,
-						  act->output[0],
-						  act->output[1],
-						  act->output[2],
-						  act->output[3],
-						  act->output[4],
-						  act->output[5],
-						  act->output[6],
-						  act->output[7]);
+			mavlink_msg_servo_output_raw_send(_channel,
+							  act->timestamp / 1000,
+							  _n,
+							  act->output[0],
+							  act->output[1],
+							  act->output[2],
+							  act->output[3],
+							  act->output[4],
+							  act->output[5],
+							  act->output[6],
+							  act->output[7]);
+		}
 	}
 };
 
@@ -768,57 +785,60 @@ protected:
 
 	void send(const hrt_abstime t)
 	{
-		status_sub->update(t);
-		pos_sp_triplet_sub->update(t);
-		act_sub->update(t);
+		bool updated = status_sub->update(t);
+		updated |= pos_sp_triplet_sub->update(t);
+		updated |= act_sub->update(t);
 
-		/* translate the current syste state to mavlink state and mode */
-		uint8_t mavlink_state;
-		uint8_t mavlink_base_mode;
-		uint32_t mavlink_custom_mode;
-		get_mavlink_mode_state(status, pos_sp_triplet, &mavlink_state, &mavlink_base_mode, &mavlink_custom_mode);
+		if (updated) {
 
-		/* set number of valid outputs depending on vehicle type */
-		unsigned n;
+			/* translate the current syste state to mavlink state and mode */
+			uint8_t mavlink_state;
+			uint8_t mavlink_base_mode;
+			uint32_t mavlink_custom_mode;
+			get_mavlink_mode_state(status, pos_sp_triplet, &mavlink_state, &mavlink_base_mode, &mavlink_custom_mode);
 
-		switch (mavlink_system.type) {
-		case MAV_TYPE_QUADROTOR:
-			n = 4;
-			break;
+			/* set number of valid outputs depending on vehicle type */
+			unsigned n;
 
-		case MAV_TYPE_HEXAROTOR:
-			n = 6;
-			break;
+			switch (mavlink_system.type) {
+			case MAV_TYPE_QUADROTOR:
+				n = 4;
+				break;
 
-		default:
-			n = 8;
-			break;
-		}
+			case MAV_TYPE_HEXAROTOR:
+				n = 6;
+				break;
 
-		/* scale / assign outputs depending on system type */
-		float out[8];
+			default:
+				n = 8;
+				break;
+			}
 
-		for (unsigned i = 0; i < 8; i++) {
-			if (i < n) {
-				if (mavlink_base_mode & MAV_MODE_FLAG_SAFETY_ARMED) {
-					/* scale fake PWM out 900..1200 us to 0..1*/
-					out[i] = (act->output[i] - 900.0f) / 1200.0f;
+			/* scale / assign outputs depending on system type */
+			float out[8];
+
+			for (unsigned i = 0; i < 8; i++) {
+				if (i < n) {
+					if (mavlink_base_mode & MAV_MODE_FLAG_SAFETY_ARMED) {
+						/* scale fake PWM out 900..1200 us to 0..1*/
+						out[i] = (act->output[i] - 900.0f) / 1200.0f;
+
+					} else {
+						/* send 0 when disarmed */
+						out[i] = 0.0f;
+					}
 
 				} else {
-					/* send 0 when disarmed */
-					out[i] = 0.0f;
+					out[i] = -1.0f;
 				}
-
-			} else {
-				out[i] = -1.0f;
 			}
-		}
 
-		mavlink_msg_hil_controls_send(_channel,
-					      hrt_absolute_time(),
-					      out[0], out[1], out[2], out[3], out[4], out[5], out[6], out[7],
-					      mavlink_base_mode,
-					      0);
+			mavlink_msg_hil_controls_send(_channel,
+						      hrt_absolute_time(),
+						      out[0], out[1], out[2], out[3], out[4], out[5], out[6], out[7],
+						      mavlink_base_mode,
+						      0);
+		}
 	}
 };
 
@@ -849,14 +869,15 @@ protected:
 
 	void send(const hrt_abstime t)
 	{
-		pos_sp_triplet_sub->update(t);
+		if (pos_sp_triplet_sub->update(t)) {
 
-		mavlink_msg_global_position_setpoint_int_send(_channel,
-				MAV_FRAME_GLOBAL,
-				(int32_t)(pos_sp_triplet->current.lat * 1e7),
-				(int32_t)(pos_sp_triplet->current.lon * 1e7),
-				(int32_t)(pos_sp_triplet->current.alt * 1000),
-				(int16_t)(pos_sp_triplet->current.yaw * M_RAD_TO_DEG_F * 100.0f));
+			mavlink_msg_global_position_setpoint_int_send(_channel,
+					MAV_FRAME_GLOBAL,
+					(int32_t)(pos_sp_triplet->current.lat * 1e7),
+					(int32_t)(pos_sp_triplet->current.lon * 1e7),
+					(int32_t)(pos_sp_triplet->current.alt * 1000),
+					(int16_t)(pos_sp_triplet->current.yaw * M_RAD_TO_DEG_F * 100.0f));
+		}
 	}
 };
 
@@ -925,14 +946,15 @@ protected:
 
 	void send(const hrt_abstime t)
 	{
-		att_sp_sub->update(t);
+		if (att_sp_sub->update(t)) {
 
-		mavlink_msg_roll_pitch_yaw_thrust_setpoint_send(_channel,
-				att_sp->timestamp / 1000,
-				att_sp->roll_body,
-				att_sp->pitch_body,
-				att_sp->yaw_body,
-				att_sp->thrust);
+			mavlink_msg_roll_pitch_yaw_thrust_setpoint_send(_channel,
+					att_sp->timestamp / 1000,
+					att_sp->roll_body,
+					att_sp->pitch_body,
+					att_sp->yaw_body,
+					att_sp->thrust);
+		}
 	}
 };
 
@@ -963,14 +985,15 @@ protected:
 
 	void send(const hrt_abstime t)
 	{
-		att_rates_sp_sub->update(t);
+		if (att_rates_sp_sub->update(t)) {
 
-		mavlink_msg_roll_pitch_yaw_rates_thrust_setpoint_send(_channel,
-				att_rates_sp->timestamp / 1000,
-				att_rates_sp->roll,
-				att_rates_sp->pitch,
-				att_rates_sp->yaw,
-				att_rates_sp->thrust);
+			mavlink_msg_roll_pitch_yaw_rates_thrust_setpoint_send(_channel,
+					att_rates_sp->timestamp / 1000,
+					att_rates_sp->roll,
+					att_rates_sp->pitch,
+					att_rates_sp->yaw,
+					att_rates_sp->thrust);
+		}
 	}
 };
 
@@ -1001,24 +1024,25 @@ protected:
 
 	void send(const hrt_abstime t)
 	{
-		rc_sub->update(t);
+		if (rc_sub->update(t)) {
 
-		const unsigned port_width = 8;
+			const unsigned port_width = 8;
 
-		for (unsigned i = 0; (i * port_width) < rc->channel_count; i++) {
-			/* Channels are sent in MAVLink main loop at a fixed interval */
-			mavlink_msg_rc_channels_raw_send(_channel,
-							 rc->timestamp_publication / 1000,
-							 i,
-							 (rc->channel_count > (i * port_width) + 0) ? rc->values[(i * port_width) + 0] : UINT16_MAX,
-							 (rc->channel_count > (i * port_width) + 1) ? rc->values[(i * port_width) + 1] : UINT16_MAX,
-							 (rc->channel_count > (i * port_width) + 2) ? rc->values[(i * port_width) + 2] : UINT16_MAX,
-							 (rc->channel_count > (i * port_width) + 3) ? rc->values[(i * port_width) + 3] : UINT16_MAX,
-							 (rc->channel_count > (i * port_width) + 4) ? rc->values[(i * port_width) + 4] : UINT16_MAX,
-							 (rc->channel_count > (i * port_width) + 5) ? rc->values[(i * port_width) + 5] : UINT16_MAX,
-							 (rc->channel_count > (i * port_width) + 6) ? rc->values[(i * port_width) + 6] : UINT16_MAX,
-							 (rc->channel_count > (i * port_width) + 7) ? rc->values[(i * port_width) + 7] : UINT16_MAX,
-							 rc->rssi);
+			for (unsigned i = 0; (i * port_width) < rc->channel_count; i++) {
+				/* Channels are sent in MAVLink main loop at a fixed interval */
+				mavlink_msg_rc_channels_raw_send(_channel,
+								 rc->timestamp_publication / 1000,
+								 i,
+								 (rc->channel_count > (i * port_width) + 0) ? rc->values[(i * port_width) + 0] : UINT16_MAX,
+								 (rc->channel_count > (i * port_width) + 1) ? rc->values[(i * port_width) + 1] : UINT16_MAX,
+								 (rc->channel_count > (i * port_width) + 2) ? rc->values[(i * port_width) + 2] : UINT16_MAX,
+								 (rc->channel_count > (i * port_width) + 3) ? rc->values[(i * port_width) + 3] : UINT16_MAX,
+								 (rc->channel_count > (i * port_width) + 4) ? rc->values[(i * port_width) + 4] : UINT16_MAX,
+								 (rc->channel_count > (i * port_width) + 5) ? rc->values[(i * port_width) + 5] : UINT16_MAX,
+								 (rc->channel_count > (i * port_width) + 6) ? rc->values[(i * port_width) + 6] : UINT16_MAX,
+								 (rc->channel_count > (i * port_width) + 7) ? rc->values[(i * port_width) + 7] : UINT16_MAX,
+								 rc->rssi);
+			}
 		}
 	}
 };
@@ -1050,15 +1074,16 @@ protected:
 
 	void send(const hrt_abstime t)
 	{
-		manual_sub->update(t);
+		if (manual_sub->update(t)) {
 
-		mavlink_msg_manual_control_send(_channel,
-						mavlink_system.sysid,
-						manual->roll * 1000,
-						manual->pitch * 1000,
-						manual->yaw * 1000,
-						manual->throttle * 1000,
-						0);
+			mavlink_msg_manual_control_send(_channel,
+							mavlink_system.sysid,
+							manual->roll * 1000,
+							manual->pitch * 1000,
+							manual->yaw * 1000,
+							manual->throttle * 1000,
+							0);
+		}
 	}
 };
 
@@ -1089,15 +1114,16 @@ protected:
 
 	void send(const hrt_abstime t)
 	{
-		flow_sub->update(t);
+		if (flow_sub->update(t)) {
 
-		mavlink_msg_optical_flow_send(_channel,
-					      flow->timestamp,
-					      flow->sensor_id,
-					      flow->flow_raw_x, flow->flow_raw_y,
-					      flow->flow_comp_x_m, flow->flow_comp_y_m,
-					      flow->quality,
-					      flow->ground_distance_m);
+			mavlink_msg_optical_flow_send(_channel,
+						      flow->timestamp,
+						      flow->sensor_id,
+						      flow->flow_raw_x, flow->flow_raw_y,
+						      flow->flow_comp_x_m, flow->flow_comp_y_m,
+						      flow->quality,
+						      flow->ground_distance_m);
+		}
 	}
 };
 
diff --git a/src/modules/mavlink/mavlink_orb_subscription.cpp b/src/modules/mavlink/mavlink_orb_subscription.cpp
index 6279e5366a..9963184683 100644
--- a/src/modules/mavlink/mavlink_orb_subscription.cpp
+++ b/src/modules/mavlink/mavlink_orb_subscription.cpp
@@ -46,11 +46,15 @@
 
 #include "mavlink_orb_subscription.h"
 
-MavlinkOrbSubscription::MavlinkOrbSubscription(const orb_id_t topic) : _topic(topic), _last_check(0), next(nullptr)
+MavlinkOrbSubscription::MavlinkOrbSubscription(const orb_id_t topic) :
+	_fd(orb_subscribe(_topic)),
+	_published(false),
+	_topic(topic),
+	_last_check(0),
+	next(nullptr)
 {
 	_data = malloc(topic->o_size);
 	memset(_data, 0, topic->o_size);
-	_fd = orb_subscribe(_topic);
 }
 
 MavlinkOrbSubscription::~MavlinkOrbSubscription()
@@ -87,3 +91,16 @@ MavlinkOrbSubscription::update(const hrt_abstime t)
 
 	return false;
 }
+
+bool
+MavlinkOrbSubscription::is_published()
+{
+	bool updated;
+	orb_check(_fd, &updated);
+
+	if (updated) {
+		_published = true;
+	}
+
+	return _published;
+}
diff --git a/src/modules/mavlink/mavlink_orb_subscription.h b/src/modules/mavlink/mavlink_orb_subscription.h
index eacc270349..42d47e96ed 100644
--- a/src/modules/mavlink/mavlink_orb_subscription.h
+++ b/src/modules/mavlink/mavlink_orb_subscription.h
@@ -54,12 +54,21 @@ public:
 	~MavlinkOrbSubscription();
 
 	bool update(const hrt_abstime t);
+
+	/**
+	 * Check if the topic has been published.
+	 *
+	 * This call will return true if the topic was ever published.
+	 * @param true if the topic has been published at least once.
+	 */
+	bool is_published();
 	void *get_data();
 	const orb_id_t get_topic();
 
 private:
 	const orb_id_t _topic;
 	int _fd;
+	bool _published;
 	void *_data;
 	hrt_abstime _last_check;
 };