diff --git a/src/modules/mavlink/mavlink_bridge_header.h b/src/modules/mavlink/mavlink_bridge_header.h
index 97acfb908e..799df3da36 100644
--- a/src/modules/mavlink/mavlink_bridge_header.h
+++ b/src/modules/mavlink/mavlink_bridge_header.h
@@ -49,6 +49,8 @@ __BEGIN_DECLS
 /* use efficient approach, see mavlink_helpers.h */
 #define MAVLINK_SEND_UART_BYTES mavlink_send_uart_bytes
 
+#define MAVLINK_END_UART_SEND mavlink_end_uart_send
+
 #define MAVLINK_GET_CHANNEL_BUFFER mavlink_get_channel_buffer
 #define MAVLINK_GET_CHANNEL_STATUS mavlink_get_channel_status
 
@@ -76,6 +78,8 @@ extern mavlink_system_t mavlink_system;
  */
 void mavlink_send_uart_bytes(mavlink_channel_t chan, const uint8_t *ch, int length);
 
+void mavlink_end_uart_send(mavlink_channel_t chan, int length);
+
 extern mavlink_status_t *mavlink_get_channel_status(uint8_t chan);
 extern mavlink_message_t *mavlink_get_channel_buffer(uint8_t chan);
 
diff --git a/src/modules/mavlink/mavlink_ftp.cpp b/src/modules/mavlink/mavlink_ftp.cpp
index e8ea9c89b9..674fe66c83 100644
--- a/src/modules/mavlink/mavlink_ftp.cpp
+++ b/src/modules/mavlink/mavlink_ftp.cpp
@@ -1,6 +1,6 @@
 /****************************************************************************
  *
- *   Copyright (c) 2014, 2015 PX4 Development Team. All rights reserved.
+ *   Copyright (c) 2014-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
@@ -292,7 +292,7 @@ MavlinkFTP::_reply(mavlink_file_transfer_protocol_t* ftp_req)
 	// Unit test hook is set, call that instead
 	_utRcvMsgFunc(ftp_req, _worker_data);
 #else
-	_mavlink->send_message(MAVLINK_MSG_ID_FILE_TRANSFER_PROTOCOL, ftp_req);
+	mavlink_msg_file_transfer_protocol_send_struct(_mavlink->get_channel(), ftp_req);
 #endif
 
 }
diff --git a/src/modules/mavlink/mavlink_log_handler.cpp b/src/modules/mavlink/mavlink_log_handler.cpp
index 08197f51fe..a9dddb55ff 100644
--- a/src/modules/mavlink/mavlink_log_handler.cpp
+++ b/src/modules/mavlink/mavlink_log_handler.cpp
@@ -1,6 +1,6 @@
 /****************************************************************************
  *
- *   Copyright (c) 2014, 2015 PX4 Development Team. All rights reserved.
+ *   Copyright (c) 2014-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
@@ -257,7 +257,7 @@ MavlinkLogHandler::_log_send_listing()
 	response.id 		= _pLogHandlerHelper->next_entry;
 	response.num_logs	= _pLogHandlerHelper->log_count;
 	response.last_log_num	= _pLogHandlerHelper->last_entry;
-	_mavlink->send_message(MAVLINK_MSG_ID_LOG_ENTRY, &response);
+	mavlink_msg_log_entry_send_struct(_mavlink->get_channel(), &response);
         //-- If we're done listing, flag it.
         if (_pLogHandlerHelper->next_entry == _pLogHandlerHelper->last_entry) {
 		_pLogHandlerHelper->current_status = LogListHelper::LOG_HANDLER_IDLE;
@@ -289,7 +289,7 @@ MavlinkLogHandler::_log_send_data()
 	response.ofs   = _pLogHandlerHelper->current_log_data_offset;
 	response.id    = _pLogHandlerHelper->current_log_index;
 	response.count = read_size;
-	_mavlink->send_message(MAVLINK_MSG_ID_LOG_DATA, &response);
+	mavlink_msg_log_data_send_struct(_mavlink->get_channel(), &response);
 	_pLogHandlerHelper->current_log_data_offset    += read_size;
 	_pLogHandlerHelper->current_log_data_remaining -= read_size;
 	if (read_size < sizeof(response.data) || _pLogHandlerHelper->current_log_data_remaining == 0) {
diff --git a/src/modules/mavlink/mavlink_main.cpp b/src/modules/mavlink/mavlink_main.cpp
index 6d6da33a3a..7b1530e2f9 100644
--- a/src/modules/mavlink/mavlink_main.cpp
+++ b/src/modules/mavlink/mavlink_main.cpp
@@ -120,12 +120,17 @@ extern mavlink_system_t mavlink_system;
 void mavlink_send_uart_bytes(mavlink_channel_t chan, const uint8_t *ch, int length)
 {
 	Mavlink* m = Mavlink::get_instance((unsigned)chan);
-
-	if (m->get_free_tx_buf() < length) {
-		return;
+	if (m != nullptr) {
+		m->send_bytes(ch, length);
 	}
+}
 
-	m->send_bytes(ch, length);
+void mavlink_end_uart_send(mavlink_channel_t chan, int length)
+{
+	Mavlink* m = Mavlink::get_instance((unsigned)chan);
+	if (m != nullptr) {
+		m->send_packet();
+	}
 }
 
 static void usage(void);
@@ -191,6 +196,8 @@ Mavlink::Mavlink() :
 	_bcast_addr{},
 	_src_addr_initialized(false),
 	_broadcast_address_found(false),
+	_network_buf{},
+	_network_buf_len(0),
 #endif
 	_socket_fd(-1),
 	_protocol(SERIAL),
@@ -824,10 +831,61 @@ Mavlink::get_free_tx_buf()
 }
 
 void
-Mavlink::send_message(const uint8_t msgid, const void *msg, uint8_t component_ID)
+Mavlink::send_packet()
 {
-	uint8_t payload_len = mavlink_message_meta[msgid].msg_len;
-	unsigned packet_len = payload_len + MAVLINK_NUM_NON_PAYLOAD_BYTES;
+#ifdef __PX4_POSIX
+
+	int ret = -1;
+
+	if (get_protocol() == UDP) {
+		ret = sendto(_socket_fd, _network_buf, _network_buf_len, 0, (struct sockaddr *)&_src_addr, sizeof(_src_addr));
+
+		struct telemetry_status_s &tstatus = get_rx_status();
+		warnx("UDP count: %d", ret);
+
+		/* resend message via broadcast if no valid connection exists */
+		if ((_mode != MAVLINK_MODE_ONBOARD) &&
+			(!get_client_source_initialized()
+			|| (hrt_elapsed_time(&tstatus.heartbeat_time) > 3 * 1000 * 1000))) {
+
+			if (!_broadcast_address_found) {
+				find_broadcast_address();
+			}
+
+			if (_broadcast_address_found) {
+
+				int bret = sendto(_socket_fd, _network_buf, _network_buf_len, 0, (struct sockaddr *)&_bcast_addr, sizeof(_bcast_addr));
+
+				if (bret <= 0) {
+					PX4_WARN("sending broadcast failed, errno: %d: %s", errno, strerror(errno));
+				} else {
+					warnx("BROADCAST count: %d", bret);
+				}
+			}
+		}
+
+	} else if (get_protocol() == TCP) {
+		/* not implemented, but possible to do so */
+		warnx("TCP transport pending implementation");
+	}
+
+	_network_buf_len = 0;
+
+#endif
+}
+
+void
+Mavlink::send_bytes(const uint8_t *buf, unsigned packet_len)
+{
+	warnx("send bytes");
+	/* If the wait until transmit flag is on, only transmit after we've received messages.
+	   Otherwise, transmit all the time. */
+	if (!should_transmit()) {
+		warnx("not transmitting");
+		return;
+	}
+
+	pthread_mutex_lock(&_send_mutex);
 
 	_last_write_try_time = hrt_absolute_time();
 
@@ -847,45 +905,6 @@ Mavlink::send_message(const uint8_t msgid, const void *msg, uint8_t component_ID
 		}
 	}
 
-	mavlink_get_channel_status(_channel)->flags |= MAVLINK_STATUS_FLAG_OUT_MAVLINK1;
-
-	uint8_t buf[MAVLINK_MAX_PACKET_LEN];
-
-	/* header */
-	buf[0] = MAVLINK_STX;
-	buf[1] = payload_len;
-	/* use mavlink's internal counter for the TX seq */
-	buf[2] = mavlink_get_channel_status(_channel)->current_tx_seq++;
-	buf[3] = mavlink_system.sysid;
-	buf[4] = (component_ID == 0) ? mavlink_system.compid : component_ID;
-	buf[5] = msgid;
-
-	/* payload */
-	memcpy(&buf[MAVLINK_NUM_HEADER_BYTES], msg, payload_len);
-
-	/* checksum */
-	uint16_t checksum;
-	crc_init(&checksum);
-	crc_accumulate_buffer(&checksum, (const char *) &buf[1], MAVLINK_CORE_HEADER_LEN + payload_len);
-	crc_accumulate(mavlink_message_meta[msgid].crc_extra, &checksum);
-
-	buf[MAVLINK_NUM_HEADER_BYTES + payload_len] = (uint8_t)(checksum & 0xFF);
-	buf[MAVLINK_NUM_HEADER_BYTES + payload_len + 1] = (uint8_t)(checksum >> 8);
-
-	send_bytes(&buf[0], packet_len);
-}
-
-void
-Mavlink::send_bytes(const uint8_t *buf, unsigned packet_len)
-{
-	/* If the wait until transmit flag is on, only transmit after we've received messages.
-	   Otherwise, transmit all the time. */
-	if (!should_transmit()) {
-		return;
-	}
-
-	pthread_mutex_lock(&_send_mutex);
-
 	size_t ret = -1;
 
 	/* send message to UART */
@@ -894,33 +913,14 @@ Mavlink::send_bytes(const uint8_t *buf, unsigned packet_len)
 	}
 
 #ifdef __PX4_POSIX
-	if (get_protocol() == UDP) {
-		ret = sendto(_socket_fd, buf, packet_len, 0, (struct sockaddr *)&_src_addr, sizeof(_src_addr));
 
-		struct telemetry_status_s &tstatus = get_rx_status();
+	else {
+		if (_network_buf_len + packet_len < sizeof(_network_buf) / sizeof(_network_buf[0])) {
+			memcpy(&_network_buf[_network_buf_len], buf, packet_len);
+			_network_buf_len += packet_len;
 
-		/* resend message via broadcast if no valid connection exists */
-		if ((_mode != MAVLINK_MODE_ONBOARD) &&
-			(!get_client_source_initialized()
-			|| (hrt_elapsed_time(&tstatus.heartbeat_time) > 3 * 1000 * 1000))) {
-
-			if (!_broadcast_address_found) {
-				find_broadcast_address();
-			}
-
-			if (_broadcast_address_found) {
-
-				int bret = sendto(_socket_fd, buf, packet_len, 0, (struct sockaddr *)&_bcast_addr, sizeof(_bcast_addr));
-
-				if (bret <= 0) {
-					PX4_WARN("sending broadcast failed, errno: %d: %s", errno, strerror(errno));
-				}
-			}
+			ret = packet_len;
 		}
-
-	} else if (get_protocol() == TCP) {
-		/* not implemented, but possible to do so */
-		warnx("TCP transport pending implementation");
 	}
 #endif
 
@@ -1231,7 +1231,7 @@ void Mavlink::send_autopilot_capabilites()
 		mcu_unique_id(uid);
 		msg.uid = (((uint64_t)uid[1]) << 32) | uid[2];
 
-		this->send_message(MAVLINK_MSG_ID_AUTOPILOT_VERSION, &msg);
+		mavlink_msg_autopilot_version_send_struct(get_channel(), &msg);
 	}
 }
 
@@ -2044,7 +2044,7 @@ Mavlink::task_main(int argc, char *argv[])
 			msg.result = command_ack.result;
 			msg.command = command_ack.command;
 
-			send_message(MAVLINK_MSG_ID_COMMAND_ACK, &msg);
+			mavlink_msg_command_ack_send_struct(get_channel(), &msg);
 		}
 
 		struct mavlink_log_s mavlink_log;
diff --git a/src/modules/mavlink/mavlink_main.h b/src/modules/mavlink/mavlink_main.h
index 90110d0f54..b0b682d5cb 100644
--- a/src/modules/mavlink/mavlink_main.h
+++ b/src/modules/mavlink/mavlink_main.h
@@ -224,10 +224,18 @@ public:
 	 */
 	bool			get_manual_input_mode_generation() { return _generate_rc; }
 
-	void			send_message(const uint8_t msgid, const void *msg, uint8_t component_ID = 0);
-
+	/**
+	 * Send bytes out on the link.
+	 *
+	 * On a network port these might actually get buffered to form a packet.
+	 */
 	void			send_bytes(const uint8_t *buf, unsigned packet_len);
 
+	/**
+	 * Flush the transmit buffer and send one MAVLink packet
+	 */
+	void			send_packet();
+
 	/**
 	 * Resend message as is, don't change sequence number and CRC.
 	 */
@@ -451,7 +459,8 @@ private:
 	struct sockaddr_in _bcast_addr;
 	bool _src_addr_initialized;
 	bool _broadcast_address_found;
-
+	uint8_t _network_buf[MAVLINK_MAX_PACKET_LEN];
+	unsigned _network_buf_len;
 #endif
 	int _socket_fd;
 	Protocol	_protocol;
diff --git a/src/modules/mavlink/mavlink_messages.cpp b/src/modules/mavlink/mavlink_messages.cpp
index 5c2677bc46..5bafb057ce 100644
--- a/src/modules/mavlink/mavlink_messages.cpp
+++ b/src/modules/mavlink/mavlink_messages.cpp
@@ -1,6 +1,6 @@
 /****************************************************************************
  *
- *   Copyright (c) 2012-2015 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
@@ -409,7 +409,7 @@ protected:
 				strncpy(msg.text, (const char *)mavlink_log.text, sizeof(msg.text));
 				msg.text[sizeof(msg.text) - 1] = '\0';
 
-				_mavlink->send_message(MAVLINK_MSG_ID_STATUSTEXT, &msg);
+				mavlink_msg_statustext_send_struct(_mavlink->get_channel(), &msg);
 
 // TODO: the logging doesn't work on Snapdragon yet because of file paths.
 #ifndef __PX4_POSIX_EAGLE
@@ -537,7 +537,7 @@ protected:
 				msg.param6 = cmd.param6;
 				msg.param7 = cmd.param7;
 
-				_mavlink->send_message(MAVLINK_MSG_ID_COMMAND_LONG, &msg);
+				mavlink_msg_command_long_send_struct(_mavlink->get_channel(), &msg);
 			}
 		}
 	}
@@ -623,7 +623,7 @@ protected:
 			msg.errors_count3 = 0;
 			msg.errors_count4 = 0;
 
-			_mavlink->send_message(MAVLINK_MSG_ID_SYS_STATUS, &msg);
+			mavlink_msg_sys_status_send_struct(_mavlink->get_channel(), &msg);
 
 			/* battery status message with higher resolution */
 			mavlink_battery_status_t bat_msg = {};
@@ -643,7 +643,7 @@ protected:
 				}
 			}
 
-			_mavlink->send_message(MAVLINK_MSG_ID_BATTERY_STATUS, &bat_msg);
+			mavlink_msg_battery_status_send_struct(_mavlink->get_channel(), &bat_msg);
 		}
 	}
 };
@@ -749,7 +749,7 @@ protected:
 			msg.temperature = sensor.baro_temp_celcius[0];
 			msg.fields_updated = fields_updated;
 
-			_mavlink->send_message(MAVLINK_MSG_ID_HIGHRES_IMU, &msg);
+			mavlink_msg_highres_imu_send_struct(_mavlink->get_channel(), &msg);
 		}
 	}
 };
@@ -813,7 +813,7 @@ protected:
 			msg.pitchspeed = att.pitchspeed;
 			msg.yawspeed = att.yawspeed;
 
-			_mavlink->send_message(MAVLINK_MSG_ID_ATTITUDE, &msg);
+			mavlink_msg_attitude_send_struct(_mavlink->get_channel(), &msg);
 		}
 	}
 };
@@ -877,7 +877,7 @@ protected:
 			msg.pitchspeed = att.pitchspeed;
 			msg.yawspeed = att.yawspeed;
 
-			_mavlink->send_message(MAVLINK_MSG_ID_ATTITUDE_QUATERNION, &msg);
+			mavlink_msg_attitude_quaternion_send_struct(_mavlink->get_channel(), &msg);
 		}
 	}
 };
@@ -983,7 +983,7 @@ protected:
 			}
 			msg.climb = -pos.vel_d;
 
-			_mavlink->send_message(MAVLINK_MSG_ID_VFR_HUD, &msg);
+			mavlink_msg_vfr_hud_send_struct(_mavlink->get_channel(), &msg);
 		}
 	}
 };
@@ -1049,7 +1049,7 @@ protected:
 			msg.cog = _wrap_2pi(gps.cog_rad) * M_RAD_TO_DEG_F * 1e2f,
 			msg.satellites_visible = gps.satellites_used;
 
-			_mavlink->send_message(MAVLINK_MSG_ID_GPS_RAW_INT, &msg);
+			mavlink_msg_gps_raw_int_send_struct(_mavlink->get_channel(), &msg);
 		}
 	}
 };
@@ -1095,7 +1095,7 @@ protected:
 		msg.time_boot_ms = hrt_absolute_time() / 1000;
 		msg.time_unix_usec = (uint64_t)tv.tv_sec * 1000000 + tv.tv_nsec / 1000;
 
-		_mavlink->send_message(MAVLINK_MSG_ID_SYSTEM_TIME, &msg);
+		mavlink_msg_system_time_send_struct(_mavlink->get_channel(), &msg);
 	}
 };
 
@@ -1137,7 +1137,7 @@ protected:
 		msg.tc1 = 0;
 		msg.ts1 = hrt_absolute_time() * 1000; // boot time in nanoseconds
 
-		_mavlink->send_message(MAVLINK_MSG_ID_TIMESYNC, &msg);
+		mavlink_msg_timesync_send_struct(_mavlink->get_channel(), &msg);
 	}
 };
 
@@ -1204,7 +1204,7 @@ protected:
 			msg.flags = pos.flags;
 			msg.squawk = pos.squawk;
 
-			_mavlink->send_message(MAVLINK_MSG_ID_ADSB_VEHICLE, &msg);
+			mavlink_msg_adsb_vehicle_send_struct(_mavlink->get_channel(), &msg);
 		}
 	}
 };
@@ -1263,7 +1263,7 @@ protected:
 
 			/* ensure that only active trigger events are sent */
 			if (trigger.timestamp > 0) {
-				_mavlink->send_message(MAVLINK_MSG_ID_CAMERA_TRIGGER, &msg);
+				mavlink_msg_camera_trigger_send_struct(_mavlink->get_channel(), &msg);
 			}
 		}
 	}
@@ -1337,7 +1337,7 @@ protected:
 			msg.vz = pos.vel_d * 100.0f;
 			msg.hdg = _wrap_2pi(pos.yaw) * M_RAD_TO_DEG_F * 100.0f;
 
-			_mavlink->send_message(MAVLINK_MSG_ID_GLOBAL_POSITION_INT, &msg);
+			mavlink_msg_global_position_int_send_struct(_mavlink->get_channel(), &msg);
 		}
 	}
 };
@@ -1401,7 +1401,7 @@ protected:
             vmsg.pitch = rpy(1);
             vmsg.yaw = rpy(2);
 
-            _mavlink->send_message(MAVLINK_MSG_ID_VISION_POSITION_ESTIMATE, &vmsg);
+            mavlink_msg_vision_position_estimate_send_struct(_mavlink->get_channel(), &vmsg);
         }
     }
 };
@@ -1463,7 +1463,7 @@ protected:
 			msg.vy = pos.vy;
 			msg.vz = pos.vz;
 
-			_mavlink->send_message(MAVLINK_MSG_ID_LOCAL_POSITION_NED, &msg);
+			mavlink_msg_local_position_ned_send_struct(_mavlink->get_channel(), &msg);
 		}
 	}
 };
@@ -1536,7 +1536,7 @@ protected:
 			msg.covariance[11] = est.timeout_flags;
 			memcpy(msg.covariance, est.covariances, sizeof(est.covariances));
 
-			_mavlink->send_message(MAVLINK_MSG_ID_LOCAL_POSITION_NED_COV, &msg);
+			mavlink_msg_local_position_ned_cov_send_struct(_mavlink->get_channel(), &msg);
 		}
 	}
 };
@@ -1591,7 +1591,7 @@ protected:
 
 			// To be added and filled
 			// mavlink_estimator_status_t msg = {};
-			//_mavlink->send_message(MAVLINK_MSG_ID_ESTIMATOR_STATUS, &msg);
+			//mavlink_msg_statustext_send_struct(_mavlink->get_channel(), &msg);
 
 			mavlink_vibration_t msg = {};
 
@@ -1599,7 +1599,7 @@ protected:
 			msg.vibration_y = est.vibe[1];
 			msg.vibration_z = est.vibe[2];
 
-			_mavlink->send_message(MAVLINK_MSG_ID_VIBRATION, &msg);
+			mavlink_msg_vibration_send_struct(_mavlink->get_channel(), &msg);
 		}
 	}
 };
@@ -1662,7 +1662,7 @@ protected:
 			msg.y = mocap.y;
 			msg.z = mocap.z;
 
-			_mavlink->send_message(MAVLINK_MSG_ID_ATT_POS_MOCAP, &msg);
+			mavlink_msg_att_pos_mocap_send_struct(_mavlink->get_channel(), &msg);
 		}
 	}
 };
@@ -1735,7 +1735,7 @@ protected:
 				msg.approach_y = 0.0f;
 				msg.approach_z = 0.0f;
 
-				_mavlink->send_message(MAVLINK_MSG_ID_HOME_POSITION, &msg);
+				mavlink_msg_home_position_send_struct(_mavlink->get_channel(), &msg);
 			}
 		}
 	}
@@ -1817,7 +1817,7 @@ protected:
 			msg.servo7_raw = act.output[6];
 			msg.servo8_raw = act.output[7];
 
-			_mavlink->send_message(MAVLINK_MSG_ID_SERVO_OUTPUT_RAW, &msg);
+			mavlink_msg_servo_output_raw_send_struct(_mavlink->get_channel(), &msg);
 		}
 	}
 };
@@ -1910,7 +1910,7 @@ protected:
 				msg.controls[i] = att_ctrl.control[i];
 			}
 
-			_mavlink->send_message(MAVLINK_MSG_ID_ACTUATOR_CONTROL_TARGET, &msg);
+			mavlink_msg_actuator_control_target_send_struct(_mavlink->get_channel(), &msg);
 		}
 	}
 };
@@ -2076,7 +2076,7 @@ protected:
 			msg.mode = mavlink_base_mode;
 			msg.nav_mode = 0;
 
-			_mavlink->send_message(MAVLINK_MSG_ID_HIL_CONTROLS, &msg);
+			mavlink_msg_hil_controls_send_struct(_mavlink->get_channel(), &msg);
 		}
 	}
 };
@@ -2135,7 +2135,7 @@ protected:
 			msg.lon_int = pos_sp_triplet.current.lon * 1e7;
 			msg.alt = pos_sp_triplet.current.alt;
 
-			_mavlink->send_message(MAVLINK_MSG_ID_POSITION_TARGET_GLOBAL_INT, &msg);
+			mavlink_msg_position_target_global_int_send_struct(_mavlink->get_channel(), &msg);
 		}
 	}
 };
@@ -2203,7 +2203,7 @@ protected:
 			msg.afy = pos_sp.acc_y;
 			msg.afz = pos_sp.acc_z;
 
-			_mavlink->send_message(MAVLINK_MSG_ID_POSITION_TARGET_LOCAL_NED, &msg);
+			mavlink_msg_position_target_local_ned_send_struct(_mavlink->get_channel(), &msg);
 		}
 	}
 };
@@ -2275,7 +2275,7 @@ protected:
 
 			msg.thrust = att_sp.thrust;
 
-			_mavlink->send_message(MAVLINK_MSG_ID_ATTITUDE_TARGET, &msg);
+			mavlink_msg_attitude_target_send_struct(_mavlink->get_channel(), &msg);
 		}
 	}
 };
@@ -2355,7 +2355,7 @@ protected:
 
 			msg.rssi = (rc.channel_count > 0) ? rc.rssi : 0;
 
-			_mavlink->send_message(MAVLINK_MSG_ID_RC_CHANNELS, &msg);
+			mavlink_msg_rc_channels_send_struct(_mavlink->get_channel(), &msg);
 
 			/* send override message - harmless if connected to GCS, allows to connect a board to a Linux system */
 			/* http://mavlink.org/messages/common#RC_CHANNELS_OVERRIDE */
@@ -2371,7 +2371,7 @@ protected:
 			over.chan7_raw = msg.chan7_raw;
 			over.chan8_raw = msg.chan8_raw;
 
-			_mavlink->send_message(MAVLINK_MSG_ID_RC_CHANNELS_OVERRIDE, &over);
+			mavlink_msg_rc_channels_override_send_struct(_mavlink->get_channel(), &over);
 		}
 	}
 };
@@ -2440,7 +2440,7 @@ protected:
 			msg.buttons |= (manual.acro_switch << (shift * 4));
 			msg.buttons |= (manual.offboard_switch << (shift * 5));
 
-			_mavlink->send_message(MAVLINK_MSG_ID_MANUAL_CONTROL, &msg);
+			mavlink_msg_manual_control_send_struct(_mavlink->get_channel(), &msg);
 		}
 	}
 };
@@ -2508,7 +2508,7 @@ protected:
 			msg.time_delta_distance_us = flow.time_since_last_sonar_update;
 			msg.temperature = flow.gyro_temperature;
 
-			_mavlink->send_message(MAVLINK_MSG_ID_OPTICAL_FLOW_RAD, &msg);
+			mavlink_msg_optical_flow_rad_send_struct(_mavlink->get_channel(), &msg);
 		}
 	}
 };
@@ -2568,7 +2568,7 @@ protected:
 			msg.name[sizeof(msg.name) - 1] = '\0';
 			msg.value = debug.value;
 
-			_mavlink->send_message(MAVLINK_MSG_ID_NAMED_VALUE_FLOAT, &msg);
+			mavlink_msg_named_value_float_send_struct(_mavlink->get_channel(), &msg);
 		}
 	}
 };
@@ -2635,7 +2635,7 @@ protected:
 			msg.alt_error = _tecs_status.altitude_filtered - _tecs_status.altitudeSp;
 			msg.aspd_error = _tecs_status.airspeed_filtered - _tecs_status.airspeedSp;
 
-			_mavlink->send_message(MAVLINK_MSG_ID_NAV_CONTROLLER_OUTPUT, &msg);
+			mavlink_msg_nav_controller_output_send_struct(_mavlink->get_channel(), &msg);
 		}
 	}
 };
@@ -2700,7 +2700,7 @@ protected:
 		msg.param6 = 0;
 		msg.param7 = 0;
 
-		_mavlink->send_message(MAVLINK_MSG_ID_COMMAND_LONG, &msg);
+		mavlink_msg_command_long_send_struct(_mavlink->get_channel(), &msg);
 	}
 };
 
@@ -2783,7 +2783,7 @@ protected:
 			msg.current_distance = dist_sensor.current_distance * 100.0f; /* m to cm */
 			msg.covariance = dist_sensor.covariance;
 
-			_mavlink->send_message(MAVLINK_MSG_ID_DISTANCE_SENSOR, &msg);
+			mavlink_msg_distance_sensor_send_struct(_mavlink->get_channel(), &msg);
 		}
 	}
 };
@@ -2877,7 +2877,7 @@ protected:
 		}
 
 		if (updated) {
-			_mavlink->send_message(MAVLINK_MSG_ID_EXTENDED_SYS_STATE, &_msg);
+			mavlink_msg_extended_sys_state_send_struct(_mavlink->get_channel(), &_msg);
 		}
 	}
 };
@@ -2971,7 +2971,7 @@ protected:
 				msg.bottom_clearance = NAN;
 			}
 
-			_mavlink->send_message(MAVLINK_MSG_ID_ALTITUDE, &msg);
+			mavlink_msg_altitude_send_struct(_mavlink->get_channel(), &msg);
 		}
 	}
 };
diff --git a/src/modules/mavlink/mavlink_mission.cpp b/src/modules/mavlink/mavlink_mission.cpp
index 929e67a0c1..05c62e02e3 100644
--- a/src/modules/mavlink/mavlink_mission.cpp
+++ b/src/modules/mavlink/mavlink_mission.cpp
@@ -1,6 +1,6 @@
 /****************************************************************************
  *
- *   Copyright (c) 2012-2015 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
@@ -188,7 +188,7 @@ MavlinkMissionManager::send_mission_ack(uint8_t sysid, uint8_t compid, uint8_t t
 	wpa.target_component = compid;
 	wpa.type = type;
 
-	_mavlink->send_message(MAVLINK_MSG_ID_MISSION_ACK, &wpa);
+	mavlink_msg_mission_ack_send_struct(_mavlink->get_channel(), &wpa);
 
 	if (_verbose) { warnx("WPM: Send MISSION_ACK type %u to ID %u", wpa.type, wpa.target_system); }
 }
@@ -202,7 +202,7 @@ MavlinkMissionManager::send_mission_current(uint16_t seq)
 
 		wpc.seq = seq;
 
-		_mavlink->send_message(MAVLINK_MSG_ID_MISSION_CURRENT, &wpc);
+		mavlink_msg_mission_current_send_struct(_mavlink->get_channel(), &wpc);
 
 	} else if (seq == 0 && _count == 0) {
 		/* don't broadcast if no WPs */
@@ -226,7 +226,7 @@ MavlinkMissionManager::send_mission_count(uint8_t sysid, uint8_t compid, uint16_
 	wpc.target_component = compid;
 	wpc.count = _count;
 
-	_mavlink->send_message(MAVLINK_MSG_ID_MISSION_COUNT, &wpc);
+	mavlink_msg_mission_count_send_struct(_mavlink->get_channel(), &wpc);
 
 	if (_verbose) { warnx("WPM: Send MISSION_COUNT %u to ID %u", wpc.count, wpc.target_system); }
 }
@@ -250,7 +250,7 @@ MavlinkMissionManager::send_mission_item(uint8_t sysid, uint8_t compid, uint16_t
 		wp.seq = seq;
 		wp.current = (_current_seq == seq) ? 1 : 0;
 
-		_mavlink->send_message(MAVLINK_MSG_ID_MISSION_ITEM, &wp);
+		mavlink_msg_mission_item_send_struct(_mavlink->get_channel(), &wp);
 
 		if (_verbose) { warnx("WPM: Send MISSION_ITEM seq %u to ID %u", wp.seq, wp.target_system); }
 
@@ -276,7 +276,7 @@ MavlinkMissionManager::send_mission_request(uint8_t sysid, uint8_t compid, uint1
 		wpr.target_component = compid;
 		wpr.seq = seq;
 
-		_mavlink->send_message(MAVLINK_MSG_ID_MISSION_REQUEST, &wpr);
+		mavlink_msg_mission_request_send_struct(_mavlink->get_channel(), &wpr);
 
 		if (_verbose) { warnx("WPM: Send MISSION_REQUEST seq %u to ID %u", wpr.seq, wpr.target_system); }
 
@@ -295,7 +295,7 @@ MavlinkMissionManager::send_mission_item_reached(uint16_t seq)
 
 	wp_reached.seq = seq;
 
-	_mavlink->send_message(MAVLINK_MSG_ID_MISSION_ITEM_REACHED, &wp_reached);
+	mavlink_msg_mission_item_reached_send_struct(_mavlink->get_channel(), &wp_reached);
 
 	if (_verbose) { warnx("WPM: Send MISSION_ITEM_REACHED reached_seq %u", wp_reached.seq); }
 }
diff --git a/src/modules/mavlink/mavlink_parameters.cpp b/src/modules/mavlink/mavlink_parameters.cpp
index 50fc0429be..99f887a5b9 100644
--- a/src/modules/mavlink/mavlink_parameters.cpp
+++ b/src/modules/mavlink/mavlink_parameters.cpp
@@ -186,7 +186,7 @@ MavlinkParametersManager::handle_message(const mavlink_message_t *msg)
 						strncpy(param_value.param_id, HASH_PARAM, MAVLINK_MSG_PARAM_VALUE_FIELD_PARAM_ID_LEN);
 						param_value.param_type = MAV_PARAM_TYPE_UINT32;
 						memcpy(&param_value.param_value, &hash, sizeof(hash));
-						_mavlink->send_message(MAVLINK_MSG_ID_PARAM_VALUE, &param_value);
+						mavlink_msg_param_value_send_struct(_mavlink->get_channel(), &param_value);
 					} else {
 						/* local name buffer to enforce null-terminated string */
 						char name[MAVLINK_MSG_PARAM_VALUE_FIELD_PARAM_ID_LEN + 1];
@@ -302,7 +302,7 @@ MavlinkParametersManager::send(const hrt_abstime t)
 			memcpy(&msg.param_value, &val, sizeof(int32_t));
 			msg.param_type = MAVLINK_TYPE_INT32_T;
 		}
-		_mavlink->send_message(MAVLINK_MSG_ID_PARAM_VALUE, &msg, value.node_id);
+		mavlink_msg_param_value_send_struct(_mavlink->get_channel(), &msg);
 	} else if (_send_all_index >= 0 && _mavlink->boot_complete()) {
 		/* send all parameters if requested, but only after the system has booted */
 
@@ -325,7 +325,7 @@ MavlinkParametersManager::send(const hrt_abstime t)
 			strncpy(msg.param_id, HASH_PARAM, MAVLINK_MSG_PARAM_VALUE_FIELD_PARAM_ID_LEN);
 			msg.param_type = MAV_PARAM_TYPE_UINT32;
 			memcpy(&msg.param_value, &hash, sizeof(hash));
-			_mavlink->send_message(MAVLINK_MSG_ID_PARAM_VALUE, &msg);
+			mavlink_msg_param_value_send_struct(_mavlink->get_channel(), &msg);
 
 			/* after this we should start sending all params */
 			_send_all_index = 0;
@@ -396,7 +396,7 @@ MavlinkParametersManager::send_param(param_t param)
 		msg.param_type = MAVLINK_TYPE_FLOAT;
 	}
 
-	_mavlink->send_message(MAVLINK_MSG_ID_PARAM_VALUE, &msg);
+	mavlink_msg_param_value_send_struct(_mavlink->get_channel(), &msg);
 
 	return 0;
 }
diff --git a/src/modules/mavlink/mavlink_receiver.cpp b/src/modules/mavlink/mavlink_receiver.cpp
index 7a72c24ded..d31b7e0ad1 100644
--- a/src/modules/mavlink/mavlink_receiver.cpp
+++ b/src/modules/mavlink/mavlink_receiver.cpp
@@ -1353,9 +1353,7 @@ MavlinkReceiver::handle_message_ping(mavlink_message_t *msg)
 
 	if ((mavlink_system.sysid == ping.target_system) &&
 	    (mavlink_system.compid == ping.target_component)) {
-		mavlink_message_t msg_out;
-		mavlink_msg_ping_encode(_mavlink->get_system_id(), _mavlink->get_component_id(), &msg_out, &ping);
-		_mavlink->send_message(MAVLINK_MSG_ID_PING, &msg_out);
+		mavlink_msg_ping_send_struct(_mavlink->get_channel(), &ping);
 	}
 }
 
@@ -1423,7 +1421,7 @@ MavlinkReceiver::handle_message_timesync(mavlink_message_t *msg)
 		rsync.tc1 = now_ns;
 		rsync.ts1 = tsync.ts1;
 
-		_mavlink->send_message(MAVLINK_MSG_ID_TIMESYNC, &rsync);
+		mavlink_msg_timesync_send_struct(_mavlink->get_channel(), &rsync);
 
 		return;