Merge pull request #1217 from PX4/mavlinkrates2

Mavlink: don't use mavlink helpers

ROMFS/px4fmu_common/init.d/rc.usb

@@ -5,6 +5,10 @@
 
 mavlink start -r 10000 -d /dev/ttyACM0 -x
 # Enable a number of interesting streams we want via USB
+mavlink stream -d /dev/ttyACM0 -s PARAM_VALUE -r 200
+usleep 100000
+mavlink stream -d /dev/ttyACM0 -s MISSION_ITEM -r 50
+usleep 100000
 mavlink stream -d /dev/ttyACM0 -s NAMED_VALUE_FLOAT -r 10
 usleep 100000
 mavlink stream -d /dev/ttyACM0 -s OPTICAL_FLOW -r 10
@@ -15,6 +19,8 @@ mavlink stream -d /dev/ttyACM0 -s ATTITUDE -r 20
 usleep 100000
 mavlink stream -d /dev/ttyACM0 -s ATTITUDE_CONTROLS -r 30
 usleep 100000
+mavlink stream -d /dev/ttyACM0 -s RC_CHANNELS_RAW -r 5
+usleep 100000
 mavlink stream -d /dev/ttyACM0 -s SERVO_OUTPUT_RAW_0 -r 20
 usleep 100000
 mavlink stream -d /dev/ttyACM0 -s GLOBAL_POSITION_SETPOINT_INT -r 20

src/modules/mavlink/mavlink_bridge_header.h

@@ -44,7 +44,12 @@
 
 __BEGIN_DECLS
 
-#define MAVLINK_USE_CONVENIENCE_FUNCTIONS
+/*
+ * We are NOT using convenience functions,
+ * but instead send messages with a custom function.
+ * So we do NOT do this:
+ * #define MAVLINK_USE_CONVENIENCE_FUNCTIONS
+ */
 
 /* use efficient approach, see mavlink_helpers.h */
 #define MAVLINK_SEND_UART_BYTES mavlink_send_uart_bytes

src/modules/mavlink/mavlink_commands.cpp

@@ -1,73 +0,0 @@
-/****************************************************************************
- *
- *   Copyright (c) 2014 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
- * are met:
- *
- * 1. Redistributions of source code must retain the above copyright
- *    notice, this list of conditions and the following disclaimer.
- * 2. Redistributions in binary form must reproduce the above copyright
- *    notice, this list of conditions and the following disclaimer in
- *    the documentation and/or other materials provided with the
- *    distribution.
- * 3. Neither the name PX4 nor the names of its contributors may be
- *    used to endorse or promote products derived from this software
- *    without specific prior written permission.
- *
- * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
- * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
- * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
- * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
- * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
- * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
- * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS
- * OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED
- * AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
- * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
- * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
- * POSSIBILITY OF SUCH DAMAGE.
- *
- ****************************************************************************/
-
-/**
- * @file mavlink_commands.cpp
- * Mavlink commands stream implementation.
- *
- * @author Anton Babushkin <anton.babushkin@me.com>
- */
-
-#include "mavlink_commands.h"
-
-MavlinkCommandsStream::MavlinkCommandsStream(Mavlink *mavlink, mavlink_channel_t channel) :
-	_cmd_sub(mavlink->add_orb_subscription(ORB_ID(vehicle_command))),
-	_cmd{},
-	_channel(channel),
-	_cmd_time(0)
-{
-}
-
-void
-MavlinkCommandsStream::update(const hrt_abstime t)
-{
-	struct vehicle_command_s cmd;
-
-	if (_cmd_sub->update(&_cmd_time, &cmd)) {
-		/* only send commands for other systems/components */
-		if (cmd.target_system != mavlink_system.sysid || cmd.target_component != mavlink_system.compid) {
-			mavlink_msg_command_long_send(_channel,
-						      cmd.target_system,
-						      cmd.target_component,
-						      cmd.command,
-						      cmd.confirmation,
-						      cmd.param1,
-						      cmd.param2,
-						      cmd.param3,
-						      cmd.param4,
-						      cmd.param5,
-						      cmd.param6,
-						      cmd.param7);
-		}
-	}
-}

src/modules/mavlink/mavlink_ftp.h

@@ -55,6 +55,7 @@
 #include <systemlib/err.h>
 
 #include "mavlink_messages.h"
+#include "mavlink_main.h"
 
 class MavlinkFTP
 {

src/modules/mavlink/mavlink_main.cpp

@@ -78,7 +78,6 @@
 #include "mavlink_messages.h"
 #include "mavlink_receiver.h"
 #include "mavlink_rate_limiter.h"
-#include "mavlink_commands.h"
 
 #ifndef MAVLINK_CRC_EXTRA
 #error MAVLINK_CRC_EXTRA has to be defined on PX4 systems
@@ -94,11 +93,16 @@ static const int ERROR = -1;
 #define MAX_DATA_RATE	10000	// max data rate in bytes/s
 #define MAIN_LOOP_DELAY 10000	// 100 Hz @ 1000 bytes/s data rate
 
+#define TX_BUFFER_GAP MAVLINK_MAX_PACKET_LEN
+
 static Mavlink *_mavlink_instances = nullptr;
 
 /* TODO: if this is a class member it crashes */
 static struct file_operations fops;
 
+static const uint8_t mavlink_message_lengths[256] = MAVLINK_MESSAGE_LENGTHS;
+static const uint8_t mavlink_message_crcs[256] = MAVLINK_MESSAGE_CRCS;
+
 /**
  * mavlink app start / stop handling function
  *
@@ -108,113 +112,6 @@ extern "C" __EXPORT int mavlink_main(int argc, char *argv[]);
 
 extern mavlink_system_t mavlink_system;
 
-static uint64_t last_write_success_times[6] = {0};
-static uint64_t last_write_try_times[6] = {0};
-
-/*
- * Internal function to send the bytes through the right serial port
- */
-void
-mavlink_send_uart_bytes(mavlink_channel_t channel, const uint8_t *ch, int length)
-{
-
-	Mavlink *instance;
-
-	switch (channel) {
-	case MAVLINK_COMM_0:
-		instance = Mavlink::get_instance(0);
-		break;
-
-	case MAVLINK_COMM_1:
-		instance = Mavlink::get_instance(1);
-		break;
-
-	case MAVLINK_COMM_2:
-		instance = Mavlink::get_instance(2);
-		break;
-
-	case MAVLINK_COMM_3:
-		instance = Mavlink::get_instance(3);
-		break;
-#ifdef MAVLINK_COMM_4
-
-	case MAVLINK_COMM_4:
-		instance = Mavlink::get_instance(4);
-		break;
-#endif
-#ifdef MAVLINK_COMM_5
-
-	case MAVLINK_COMM_5:
-		instance = Mavlink::get_instance(5);
-		break;
-#endif
-#ifdef MAVLINK_COMM_6
-
-	case MAVLINK_COMM_6:
-		instance = Mavlink::get_instance(6);
-		break;
-#endif
-
-	default:
-		return;
-	}
-
-	int uart = instance->get_uart_fd();
-
-	ssize_t desired = (sizeof(uint8_t) * length);
-
-	/*
-	 * Check if the OS buffer is full and disable HW
-	 * flow control if it continues to be full
-	 */
-	int buf_free = 0;
-
-	if (instance->get_flow_control_enabled()
-	    && ioctl(uart, FIONWRITE, (unsigned long)&buf_free) == 0) {
-
-		/* Disable hardware flow control:
-		 * if no successful write since a defined time
-		 * and if the last try was not the last successful write
-		 */
-		if (last_write_try_times[(unsigned)channel] != 0 &&
-		    hrt_elapsed_time(&last_write_success_times[(unsigned)channel]) > 500 * 1000UL &&
-		    last_write_success_times[(unsigned)channel] !=
-		    last_write_try_times[(unsigned)channel]) {
-			warnx("DISABLING HARDWARE FLOW CONTROL");
-			instance->enable_flow_control(false);
-		}
-
-	}
-
-	/* If the wait until transmit flag is on, only transmit after we've received messages.
-	   Otherwise, transmit all the time. */
-	if (instance->should_transmit()) {
-		last_write_try_times[(unsigned)channel] = hrt_absolute_time();
-
-		/* check if there is space in the buffer, let it overflow else */
-		if (!ioctl(uart, FIONWRITE, (unsigned long)&buf_free)) {
-
-			if (buf_free < desired) {
-				/* we don't want to send anything just in half, so return */
-				instance->count_txerr();
-				instance->count_txerrbytes(desired);
-				return;
-			}
-		}
-
-		ssize_t ret = write(uart, ch, desired);
-
-		if (ret != desired) {
-			instance->count_txerr();
-			instance->count_txerrbytes(desired);
-
-		} else {
-			last_write_success_times[(unsigned)channel] = last_write_try_times[(unsigned)channel];
-			instance->count_txbytes(desired);
-		}
-	}
-}
-
 static void usage(void);
 
 Mavlink::Mavlink() :
@@ -233,8 +130,7 @@ Mavlink::Mavlink() :
 	_subscriptions(nullptr),
 	_streams(nullptr),
 	_mission_manager(nullptr),
-	_mission_pub(-1),
-	_mission_result_sub(-1),
+	_parameters_manager(nullptr),
 	_mode(MAVLINK_MODE_NORMAL),
 	_channel(MAVLINK_COMM_0),
 	_logbuffer {},
@@ -246,12 +142,16 @@ Mavlink::Mavlink() :
 	   _ftp_on(false),
 	   _uart_fd(-1),
 	   _baudrate(57600),
-	   _datarate(10000),
+	   _datarate(1000),
+	   _datarate_events(500),
+	   _rate_mult(1.0f),
 	   _mavlink_param_queue_index(0),
 	   mavlink_link_termination_allowed(false),
 	   _subscribe_to_stream(nullptr),
 	   _subscribe_to_stream_rate(0.0f),
 	   _flow_control_enabled(true),
+	   _last_write_success_time(0),
+	   _last_write_try_time(0),
 	   _bytes_tx(0),
 	   _bytes_txerr(0),
 	   _bytes_rx(0),
@@ -262,6 +162,7 @@ Mavlink::Mavlink() :
 	   _rstatus {},
 	   _message_buffer {},
 	   _message_buffer_mutex {},
+	   _send_mutex {},
 	   _param_initialized(false),
 	   _param_system_id(0),
 	   _param_component_id(0),
@@ -486,7 +387,7 @@ Mavlink::forward_message(const mavlink_message_t *msg, Mavlink *self)
 
 			/* if not in normal mode, we are an onboard link
 			 * onboard links should only pass on messages from the same system ID */
-			if(!(self->_mode != MAVLINK_MODE_NORMAL && msg->sysid != mavlink_system.sysid)) {
+			if (!(self->_mode != MAVLINK_MODE_NORMAL && msg->sysid != mavlink_system.sysid)) {
 				inst->pass_message(msg);
 			}
 		}
@@ -536,10 +437,26 @@ Mavlink::mavlink_dev_ioctl(struct file *filep, int cmd, unsigned long arg)
 	case (int)MAVLINK_IOC_SEND_TEXT_EMERGENCY: {
 
 			const char *txt = (const char *)arg;
-//		printf("logmsg: %s\n", txt);
 			struct mavlink_logmessage msg;
 			strncpy(msg.text, txt, sizeof(msg.text));
-			msg.severity = (unsigned char)cmd;
+
+			switch (cmd) {
+			case MAVLINK_IOC_SEND_TEXT_INFO:
+				msg.severity = MAV_SEVERITY_INFO;
+				break;
+
+			case MAVLINK_IOC_SEND_TEXT_CRITICAL:
+				msg.severity = MAV_SEVERITY_CRITICAL;
+				break;
+
+			case MAVLINK_IOC_SEND_TEXT_EMERGENCY:
+				msg.severity = MAV_SEVERITY_EMERGENCY;
+				break;
+
+			default:
+				msg.severity = MAV_SEVERITY_INFO;
+				break;
+			}
 
 			Mavlink *inst;
 			LL_FOREACH(_mavlink_instances, inst) {
@@ -733,6 +650,9 @@ int Mavlink::mavlink_open_uart(int baud, const char *uart_name, struct termios *
 		if (enable_flow_control(true)) {
 			warnx("hardware flow control not supported");
 		}
+
+	} else {
+		_flow_control_enabled = false;
 	}
 
 	return _uart_fd;
@@ -774,8 +694,7 @@ Mavlink::set_hil_enabled(bool hil_enabled)
 	/* enable HIL */
 	if (hil_enabled && !_hil_enabled) {
 		_hil_enabled = true;
-		float rate_mult = _datarate / 1000.0f;
-		configure_stream("HIL_CONTROLS", 15.0f * rate_mult);
+		configure_stream("HIL_CONTROLS", 150.0f);
 	}
 
 	/* disable HIL */
@@ -790,13 +709,145 @@ Mavlink::set_hil_enabled(bool hil_enabled)
 	return ret;
 }
 
-void
-Mavlink::send_message(const mavlink_message_t *msg)
+unsigned
+Mavlink::get_free_tx_buf()
 {
+	/*
+	 * Check if the OS buffer is full and disable HW
+	 * flow control if it continues to be full
+	 */
+	int buf_free = 0;
+	(void) ioctl(_uart_fd, FIONWRITE, (unsigned long)&buf_free);
+
+	if (get_flow_control_enabled() && buf_free < TX_BUFFER_GAP) {
+		/* Disable hardware flow control:
+		 * if no successful write since a defined time
+		 * and if the last try was not the last successful write
+		 */
+		if (_last_write_try_time != 0 &&
+		    hrt_elapsed_time(&_last_write_success_time) > 500 * 1000UL &&
+		    _last_write_success_time != _last_write_try_time) {
+			warnx("DISABLING HARDWARE FLOW CONTROL");
+			enable_flow_control(false);
+		}
+	}
+
+	return buf_free;
+}
+
+void
+Mavlink::send_message(const uint8_t msgid, const void *msg)
+{
+	/* 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);
+
+	int buf_free = get_free_tx_buf();
+
+	uint8_t payload_len = mavlink_message_lengths[msgid];
+	unsigned packet_len = payload_len + MAVLINK_NUM_NON_PAYLOAD_BYTES;
+
+	_last_write_try_time = hrt_absolute_time();
+
+	/* check if there is space in the buffer, let it overflow else */
+	if (buf_free < TX_BUFFER_GAP) {
+		/* no enough space in buffer to send */
+		count_txerr();
+		count_txerrbytes(packet_len);
+		pthread_mutex_unlock(&_send_mutex);
+		return;
+	}
+
 	uint8_t buf[MAVLINK_MAX_PACKET_LEN];
 
-	uint16_t len = mavlink_msg_to_send_buffer(buf, msg);
-	mavlink_send_uart_bytes(_channel, buf, 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] = mavlink_system.compid;
+	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_crcs[msgid], &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 message to UART */
+	ssize_t ret = write(_uart_fd, buf, packet_len);
+
+	if (ret != (int) packet_len) {
+		count_txerr();
+		count_txerrbytes(packet_len);
+
+	} else {
+		_last_write_success_time = _last_write_try_time;
+		count_txbytes(packet_len);
+	}
+
+	pthread_mutex_unlock(&_send_mutex);
+}
+
+void
+Mavlink::resend_message(mavlink_message_t *msg)
+{
+	/* 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);
+
+	int buf_free = get_free_tx_buf();
+
+	_last_write_try_time = hrt_absolute_time();
+
+	unsigned packet_len = msg->len + MAVLINK_NUM_NON_PAYLOAD_BYTES;
+
+	/* check if there is space in the buffer, let it overflow else */
+	if (buf_free < TX_BUFFER_GAP) {
+		/* no enough space in buffer to send */
+		count_txerr();
+		count_txerrbytes(packet_len);
+		pthread_mutex_unlock(&_send_mutex);
+		return;
+	}
+
+	uint8_t buf[MAVLINK_MAX_PACKET_LEN];
+
+	/* header and payload */
+	memcpy(&buf[0], &msg->magic, MAVLINK_NUM_HEADER_BYTES + msg->len);
+
+	/* checksum */
+	buf[MAVLINK_NUM_HEADER_BYTES + msg->len] = (uint8_t)(msg->checksum & 0xFF);
+	buf[MAVLINK_NUM_HEADER_BYTES + msg->len + 1] = (uint8_t)(msg->checksum >> 8);
+
+	/* send message to UART */
+	ssize_t ret = write(_uart_fd, buf, packet_len);
+
+	if (ret != (int) packet_len) {
+		count_txerr();
+		count_txerrbytes(packet_len);
+
+	} else {
+		_last_write_success_time = _last_write_try_time;
+		count_txbytes(packet_len);
+	}
+
+	pthread_mutex_unlock(&_send_mutex);
 }
 
 void
@@ -806,7 +857,7 @@ Mavlink::handle_message(const mavlink_message_t *msg)
 	_mission_manager->handle_message(msg);
 
 	/* handle packet with parameter component */
-	mavlink_pm_message_handler(_channel, msg);
+	_parameters_manager->handle_message(msg);
 
 	if (get_forwarding_on()) {
 		/* forward any messages to other mavlink instances */
@@ -814,224 +865,32 @@ Mavlink::handle_message(const mavlink_message_t *msg)
 	}
 }
 
-int
-Mavlink::mavlink_pm_queued_send()
-{
-	if (_mavlink_param_queue_index < param_count()) {
-		mavlink_pm_send_param(param_for_index(_mavlink_param_queue_index));
-		_mavlink_param_queue_index++;
-		return 0;
-
-	} else {
-		return 1;
-	}
-}
-
-void Mavlink::mavlink_pm_start_queued_send()
-{
-	_mavlink_param_queue_index = 0;
-}
-
-int Mavlink::mavlink_pm_send_param_for_index(uint16_t index)
-{
-	return mavlink_pm_send_param(param_for_index(index));
-}
-
-int Mavlink::mavlink_pm_send_param_for_name(const char *name)
-{
-	return mavlink_pm_send_param(param_find(name));
-}
-
-int Mavlink::mavlink_pm_send_param(param_t param)
-{
-	if (param == PARAM_INVALID) { return 1; }
-
-	/* buffers for param transmission */
-	char name_buf[MAVLINK_MSG_PARAM_VALUE_FIELD_PARAM_ID_LEN];
-	float val_buf;
-	mavlink_message_t tx_msg;
-
-	/* query parameter type */
-	param_type_t type = param_type(param);
-	/* copy parameter name */
-	strncpy((char *)name_buf, param_name(param), MAVLINK_MSG_PARAM_VALUE_FIELD_PARAM_ID_LEN);
-
-	/*
-	 * Map onboard parameter type to MAVLink type,
-	 * endianess matches (both little endian)
-	 */
-	uint8_t mavlink_type;
-
-	if (type == PARAM_TYPE_INT32) {
-		mavlink_type = MAVLINK_TYPE_INT32_T;
-
-	} else if (type == PARAM_TYPE_FLOAT) {
-		mavlink_type = MAVLINK_TYPE_FLOAT;
-
-	} else {
-		mavlink_type = MAVLINK_TYPE_FLOAT;
-	}
-
-	/*
-	 * get param value, since MAVLink encodes float and int params in the same
-	 * space during transmission, copy param onto float val_buf
-	 */
-
-	int ret;
-
-	if ((ret = param_get(param, &val_buf)) != OK) {
-		return ret;
-	}
-
-	mavlink_msg_param_value_pack_chan(mavlink_system.sysid,
-					  mavlink_system.compid,
-					  _channel,
-					  &tx_msg,
-					  name_buf,
-					  val_buf,
-					  mavlink_type,
-					  param_count(),
-					  param_get_index(param));
-	send_message(&tx_msg);
-	return OK;
-}
-
-void Mavlink::mavlink_pm_message_handler(const mavlink_channel_t chan, const mavlink_message_t *msg)
-{
-	switch (msg->msgid) {
-	case MAVLINK_MSG_ID_PARAM_REQUEST_LIST: {
-			mavlink_param_request_list_t req;
-			mavlink_msg_param_request_list_decode(msg, &req);
-
-			if (req.target_system == mavlink_system.sysid &&
-			    (req.target_component == mavlink_system.compid || req.target_component == MAV_COMP_ID_ALL)) {
-				/* Start sending parameters */
-				mavlink_pm_start_queued_send();
-				send_statustext_info("[pm] sending list");
-			}
-		} break;
-
-	case MAVLINK_MSG_ID_PARAM_SET: {
-
-			/* Handle parameter setting */
-
-			if (msg->msgid == MAVLINK_MSG_ID_PARAM_SET) {
-				mavlink_param_set_t mavlink_param_set;
-				mavlink_msg_param_set_decode(msg, &mavlink_param_set);
-
-				if (mavlink_param_set.target_system == mavlink_system.sysid
-				    && ((mavlink_param_set.target_component == mavlink_system.compid)
-					|| (mavlink_param_set.target_component == MAV_COMP_ID_ALL))) {
-					/* local name buffer to enforce null-terminated string */
-					char name[MAVLINK_MSG_PARAM_VALUE_FIELD_PARAM_ID_LEN + 1];
-					strncpy(name, mavlink_param_set.param_id, MAVLINK_MSG_PARAM_VALUE_FIELD_PARAM_ID_LEN);
-					/* enforce null termination */
-					name[MAVLINK_MSG_PARAM_VALUE_FIELD_PARAM_ID_LEN] = '\0';
-					/* attempt to find parameter, set and send it */
-					param_t param = param_find(name);
-
-					if (param == PARAM_INVALID) {
-						char buf[MAVLINK_MSG_STATUSTEXT_FIELD_TEXT_LEN];
-						sprintf(buf, "[pm] unknown: %s", name);
-						send_statustext_info(buf);
-
-					} else {
-						/* set and send parameter */
-						param_set(param, &(mavlink_param_set.param_value));
-						mavlink_pm_send_param(param);
-					}
-				}
-			}
-		} break;
-
-	case MAVLINK_MSG_ID_PARAM_REQUEST_READ: {
-			mavlink_param_request_read_t mavlink_param_request_read;
-			mavlink_msg_param_request_read_decode(msg, &mavlink_param_request_read);
-
-			if (mavlink_param_request_read.target_system == mavlink_system.sysid
-			    && ((mavlink_param_request_read.target_component == mavlink_system.compid)
-				|| (mavlink_param_request_read.target_component == MAV_COMP_ID_ALL))) {
-				/* when no index is given, loop through string ids and compare them */
-				if (mavlink_param_request_read.param_index == -1) {
-					/* local name buffer to enforce null-terminated string */
-					char name[MAVLINK_MSG_PARAM_VALUE_FIELD_PARAM_ID_LEN + 1];
-					strncpy(name, mavlink_param_request_read.param_id, MAVLINK_MSG_PARAM_VALUE_FIELD_PARAM_ID_LEN);
-					/* enforce null termination */
-					name[MAVLINK_MSG_PARAM_VALUE_FIELD_PARAM_ID_LEN] = '\0';
-					/* attempt to find parameter and send it */
-					mavlink_pm_send_param_for_name(name);
-
-				} else {
-					/* when index is >= 0, send this parameter again */
-					mavlink_pm_send_param_for_index(mavlink_param_request_read.param_index);
-				}
-			}
-
-		} break;
-	}
-}
-
-int
+void
 Mavlink::send_statustext_info(const char *string)
 {
-	return send_statustext(MAVLINK_IOC_SEND_TEXT_INFO, string);
+	send_statustext(MAV_SEVERITY_INFO, string);
 }
 
-int
+void
 Mavlink::send_statustext_critical(const char *string)
 {
-	return send_statustext(MAVLINK_IOC_SEND_TEXT_CRITICAL, string);
+	send_statustext(MAV_SEVERITY_CRITICAL, string);
 }
 
-int
+void
 Mavlink::send_statustext_emergency(const char *string)
 {
-	return send_statustext(MAVLINK_IOC_SEND_TEXT_EMERGENCY, string);
+	send_statustext(MAV_SEVERITY_EMERGENCY, string);
 }
 
-int
-Mavlink::send_statustext(unsigned severity, const char *string)
+void
+Mavlink::send_statustext(unsigned char severity, const char *string)
 {
-	const int len = MAVLINK_MSG_STATUSTEXT_FIELD_TEXT_LEN;
-	mavlink_statustext_t statustext;
+	struct mavlink_logmessage logmsg;
+	strncpy(logmsg.text, string, sizeof(logmsg.text));
+	logmsg.severity = severity;
 
-	int i = 0;
-
-	while (i < len - 1) {
-		statustext.text[i] = string[i];
-
-		if (string[i] == '\0') {
-			break;
-		}
-
-		i++;
-	}
-
-	if (i > 1) {
-		/* Enforce null termination */
-		statustext.text[i] = '\0';
-
-		/* Map severity */
-		switch (severity) {
-		case MAVLINK_IOC_SEND_TEXT_INFO:
-			statustext.severity = MAV_SEVERITY_INFO;
-			break;
-
-		case MAVLINK_IOC_SEND_TEXT_CRITICAL:
-			statustext.severity = MAV_SEVERITY_CRITICAL;
-			break;
-
-		case MAVLINK_IOC_SEND_TEXT_EMERGENCY:
-			statustext.severity = MAV_SEVERITY_EMERGENCY;
-			break;
-		}
-
-		mavlink_msg_statustext_send(_channel, statustext.severity, statustext.text);
-		return OK;
-
-	} else {
-		return ERROR;
-	}
+	mavlink_logbuffer_write(&_logbuffer, &logmsg);
 }
 
 MavlinkOrbSubscription *Mavlink::add_orb_subscription(const orb_id_t topic)
@@ -1054,11 +913,17 @@ MavlinkOrbSubscription *Mavlink::add_orb_subscription(const orb_id_t topic)
 	return sub_new;
 }
 
+unsigned int
+Mavlink::interval_from_rate(float rate)
+{
+	return (rate > 0.0f) ? (1000000.0f / rate) : 0;
+}
+
 int
 Mavlink::configure_stream(const char *stream_name, const float rate)
 {
 	/* calculate interval in us, 0 means disabled stream */
-	unsigned int interval = (rate > 0.0f) ? (1000000.0f / rate) : 0;
+	unsigned int interval = interval_from_rate(rate);
 
 	/* search if stream exists */
 	MavlinkStream *stream;
@@ -1089,10 +954,8 @@ Mavlink::configure_stream(const char *stream_name, const float rate)
 
 		if (strcmp(stream_name, streams_list[i]->get_name()) == 0) {
 			/* create new instance */
-			stream = streams_list[i]->new_instance();
-			stream->set_channel(get_channel());
+			stream = streams_list[i]->new_instance(this);
 			stream->set_interval(interval);
-			stream->subscribe(this);
 			LL_APPEND(_streams, stream);
 
 			return OK;
@@ -1105,6 +968,31 @@ Mavlink::configure_stream(const char *stream_name, const float rate)
 	return ERROR;
 }
 
+void
+Mavlink::adjust_stream_rates(const float multiplier)
+{
+	/* do not allow to push us to zero */
+	if (multiplier < 0.01f) {
+		return;
+	}
+
+	/* search if stream exists */
+	MavlinkStream *stream;
+	LL_FOREACH(_streams, stream) {
+		/* set new interval */
+		unsigned interval = stream->get_interval();
+		interval /= multiplier;
+
+		/* allow max ~600 Hz */
+		if (interval < 1600) {
+			interval = 1600;
+		}
+
+		/* set new interval */
+		stream->set_interval(interval * multiplier);
+	}
+}
+
 void
 Mavlink::configure_stream_threadsafe(const char *stream_name, const float rate)
 {
@@ -1265,7 +1153,27 @@ Mavlink::pass_message(const mavlink_message_t *msg)
 float
 Mavlink::get_rate_mult()
 {
-	return _datarate / 1000.0f;
+	return _rate_mult;
+}
+
+void
+Mavlink::update_rate_mult()
+{
+	float const_rate = 0.0f;
+	float rate = 0.0f;
+
+	MavlinkStream *stream;
+	LL_FOREACH(_streams, stream) {
+		if (stream->const_rate()) {
+			const_rate += stream->get_size() * 1000000.0f / stream->get_interval();
+
+		} else {
+			rate += stream->get_size() * 1000000.0f / stream->get_interval();
+		}
+	}
+
+	/* don't scale up rates, only scale down if needed */
+	_rate_mult = fminf(1.0f, ((float)_datarate - const_rate) / rate);
 }
 
 int
@@ -1403,6 +1311,9 @@ Mavlink::task_main(int argc, char *argv[])
 		return ERROR;
 	}
 
+	/* initialize send mutex */
+	pthread_mutex_init(&_send_mutex, NULL);
+
 	/* initialize mavlink text message buffering */
 	mavlink_logbuffer_init(&_logbuffer, 5);
 
@@ -1412,7 +1323,7 @@ Mavlink::task_main(int argc, char *argv[])
 		 * make space for two messages plus off-by-one space as we use the empty element
 		 * marker ring buffer approach.
 		 */
-		if (OK != message_buffer_init(2 * MAVLINK_MAX_PACKET_LEN + 2)) {
+		if (OK != message_buffer_init(2 * sizeof(mavlink_message_t) + 1)) {
 			errx(1, "can't allocate message buffer, exiting");
 		}
 
@@ -1432,12 +1343,6 @@ Mavlink::task_main(int argc, char *argv[])
 	/* start the MAVLink receiver */
 	_receive_thread = MavlinkReceiver::receive_start(this);
 
-	_mission_result_sub = orb_subscribe(ORB_ID(mission_result));
-
-	/* create mission manager */
-	_mission_manager = new MavlinkMissionManager(this);
-	_mission_manager->set_verbose(_verbose);
-
 	_task_running = true;
 
 	MavlinkOrbSubscription *param_sub = add_orb_subscription(ORB_ID(parameter_update));
@@ -1448,34 +1353,50 @@ Mavlink::task_main(int argc, char *argv[])
 	struct vehicle_status_s status;
 	status_sub->update(&status_time, &status);
 
-	MavlinkCommandsStream commands_stream(this, _channel);
-
-	/* add default streams depending on mode and intervals depending on datarate */
-	float rate_mult = get_rate_mult();
+	/* add default streams depending on mode */
 
+	/* HEARTBEAT is constant rate stream, rate never adjusted */
 	configure_stream("HEARTBEAT", 1.0f);
 
+	/* STATUSTEXT stream is like normal stream but gets messages from logbuffer instead of uORB */
+	configure_stream("STATUSTEXT", 20.0f);
+
+	/* COMMAND_LONG stream: use high rate to avoid commands skipping */
+	configure_stream("COMMAND_LONG", 100.0f);
+
+	/* PARAM_VALUE stream */
+	_parameters_manager = (MavlinkParametersManager *) MavlinkParametersManager::new_instance(this);
+	_parameters_manager->set_interval(interval_from_rate(30.0f));
+	LL_APPEND(_streams, _parameters_manager);
+
+	/* MISSION_STREAM stream, actually sends all MISSION_XXX messages at some rate depending on
+	 * remote requests rate. Rate specified here controls how much bandwidth we will reserve for
+	 * mission messages. */
+	_mission_manager = (MavlinkMissionManager *) MavlinkMissionManager::new_instance(this);
+	_mission_manager->set_interval(interval_from_rate(10.0f));
+	_mission_manager->set_verbose(_verbose);
+	LL_APPEND(_streams, _mission_manager);
+
 	switch (_mode) {
 	case MAVLINK_MODE_NORMAL:
 		configure_stream("SYS_STATUS", 1.0f);
 		configure_stream("GPS_GLOBAL_ORIGIN", 0.5f);
-		configure_stream("HIGHRES_IMU", 1.0f * rate_mult);
-		configure_stream("ATTITUDE", 10.0f * rate_mult);
-		configure_stream("VFR_HUD", 8.0f * rate_mult);
-		configure_stream("GPS_RAW_INT", 1.0f * rate_mult);
-		configure_stream("GLOBAL_POSITION_INT", 3.0f * rate_mult);
-		configure_stream("LOCAL_POSITION_NED", 3.0f * rate_mult);
-		configure_stream("RC_CHANNELS_RAW", 1.0f * rate_mult);
-		configure_stream("NAMED_VALUE_FLOAT", 1.0f * rate_mult);
-		configure_stream("GLOBAL_POSITION_SETPOINT_INT", 3.0f * rate_mult);
-		configure_stream("ROLL_PITCH_YAW_THRUST_SETPOINT", 3.0f * rate_mult);
+		configure_stream("HIGHRES_IMU", 1.0f);
+		configure_stream("ATTITUDE", 10.0f);
+		configure_stream("VFR_HUD", 8.0f);
+		configure_stream("GPS_RAW_INT", 1.0f);
+		configure_stream("GLOBAL_POSITION_INT", 3.0f);
+		configure_stream("LOCAL_POSITION_NED", 3.0f);
+		configure_stream("RC_CHANNELS_RAW", 1.0f);
+		configure_stream("GLOBAL_POSITION_SETPOINT_INT", 3.0f);
+		configure_stream("ROLL_PITCH_YAW_THRUST_SETPOINT", 3.0f);
 		configure_stream("DISTANCE_SENSOR", 0.5f);
 		break;
 
 	case MAVLINK_MODE_CAMERA:
 		configure_stream("SYS_STATUS", 1.0f);
-		configure_stream("ATTITUDE", 15.0f * rate_mult);
-		configure_stream("GLOBAL_POSITION_INT", 15.0f * rate_mult);
+		configure_stream("ATTITUDE", 15.0f);
+		configure_stream("GLOBAL_POSITION_INT", 15.0f);
 		configure_stream("CAMERA_CAPTURE", 1.0f);
 		break;
 
@@ -1483,13 +1404,8 @@ Mavlink::task_main(int argc, char *argv[])
 		break;
 	}
 
-	/* don't send parameters on startup without request */
-	_mavlink_param_queue_index = param_count();
-
-	MavlinkRateLimiter fast_rate_limiter(30000.0f / rate_mult);
-
 	/* set main loop delay depending on data rate to minimize CPU overhead */
-	_main_loop_delay = MAIN_LOOP_DELAY / rate_mult;
+	_main_loop_delay = MAIN_LOOP_DELAY * 1000 / _datarate;
 
 	/* now the instance is fully initialized and we can bump the instance count */
 	LL_APPEND(_mavlink_instances, this);
@@ -1502,6 +1418,8 @@ Mavlink::task_main(int argc, char *argv[])
 
 		hrt_abstime t = hrt_absolute_time();
 
+		update_rate_mult();
+
 		if (param_sub->update(&param_time, nullptr)) {
 			/* parameters updated */
 			mavlink_update_system();
@@ -1512,9 +1430,6 @@ Mavlink::task_main(int argc, char *argv[])
 			set_hil_enabled(status.hil_state == HIL_STATE_ON);
 		}
 
-		/* update commands stream */
-		commands_stream.update(t);
-
 		/* check for requested subscriptions */
 		if (_subscribe_to_stream != nullptr) {
 			if (OK == configure_stream(_subscribe_to_stream, _subscribe_to_stream_rate)) {
@@ -1540,20 +1455,6 @@ Mavlink::task_main(int argc, char *argv[])
 			stream->update(t);
 		}
 
-		if (fast_rate_limiter.check(t)) {
-			mavlink_pm_queued_send();
-			_mission_manager->eventloop();
-
-			if (!mavlink_logbuffer_is_empty(&_logbuffer)) {
-				struct mavlink_logmessage msg;
-				int lb_ret = mavlink_logbuffer_read(&_logbuffer, &msg);
-
-				if (lb_ret == OK) {
-					send_statustext(msg.severity, msg.text);
-				}
-			}
-		}
-
 		/* pass messages from other UARTs or FTP worker */
 		if (_passing_on || _ftp_on) {
 
@@ -1598,7 +1499,7 @@ Mavlink::task_main(int argc, char *argv[])
 
 				pthread_mutex_unlock(&_message_buffer_mutex);
 
-				_mavlink_resend_uart(_channel, &msg);
+				resend_message(&msg);
 			}
 		}
 
@@ -1613,14 +1514,13 @@ Mavlink::task_main(int argc, char *argv[])
 				_bytes_txerr = 0;
 				_bytes_rx = 0;
 			}
+
 			_bytes_timestamp = t;
 		}
 
 		perf_end(_loop_perf);
 	}
 
-	delete _mission_manager;
-
 	delete _subscribe_to_stream;
 	_subscribe_to_stream = nullptr;
 
@@ -1778,10 +1678,12 @@ Mavlink::display_status()
 	} else {
 		printf("\tno telem status.\n");
 	}
+
 	printf("\trates:\n");
 	printf("\ttx: %.3f kB/s\n", (double)_rate_tx);
 	printf("\ttxerr: %.3f kB/s\n", (double)_rate_txerr);
 	printf("\trx: %.3f kB/s\n", (double)_rate_rx);
+	printf("\trate mult: %.3f\n", (double)_rate_mult);
 }
 
 int

src/modules/mavlink/mavlink_main.h

@@ -58,7 +58,7 @@
 #include "mavlink_stream.h"
 #include "mavlink_messages.h"
 #include "mavlink_mission.h"
-
+#include "mavlink_parameters.h"
 
 class Mavlink
 {
@@ -160,7 +160,12 @@ public:
 	 */
 	int			set_hil_enabled(bool hil_enabled);
 
-	void			send_message(const mavlink_message_t *msg);
+	void		send_message(const uint8_t msgid, const void *msg);
+
+	/**
+	 * Resend message as is, don't change sequence number and CRC.
+	 */
+	void		resend_message(mavlink_message_t *msg);
 
 	void			handle_message(const mavlink_message_t *msg);
 
@@ -188,29 +193,33 @@ public:
 	 *
 	 * @param string the message to send (will be capped by mavlink max string length)
 	 */
-	int			send_statustext_info(const char *string);
+	void			send_statustext_info(const char *string);
 
 	/**
 	 * Send a status text with loglevel CRITICAL
 	 *
 	 * @param string the message to send (will be capped by mavlink max string length)
 	 */
-	int			send_statustext_critical(const char *string);
+	void			send_statustext_critical(const char *string);
 
 	/**
 	 * Send a status text with loglevel EMERGENCY
 	 *
 	 * @param string the message to send (will be capped by mavlink max string length)
 	 */
-	int			send_statustext_emergency(const char *string);
+	void			send_statustext_emergency(const char *string);
 
 	/**
-	 * Send a status text with loglevel
+	 * Send a status text with loglevel, the difference from mavlink_log_xxx() is that message sent
+	 * only on this mavlink connection. Useful for reporting communication specific, not system-wide info
+	 * only to client interested in it. Message will be not sent immediately but queued in buffer as
+	 * for mavlink_log_xxx().
 	 *
 	 * @param string the message to send (will be capped by mavlink max string length)
-	 * @param severity the log level, one of 
+	 * @param severity the log level
 	 */
-	int			send_statustext(unsigned severity, const char *string);
+	void			send_statustext(unsigned char severity, const char *string);
+
 	MavlinkStream *		get_streams() const { return _streams; }
 
 	float			get_rate_mult();
@@ -252,6 +261,8 @@ public:
 	 */
 	struct telemetry_status_s&	get_rx_status() { return _rstatus; }
 
+	struct mavlink_logbuffer	*get_logbuffer() { return &_logbuffer; }
+
 protected:
 	Mavlink			*next;
 
@@ -274,9 +285,8 @@ private:
 	MavlinkStream		*_streams;
 
 	MavlinkMissionManager	*_mission_manager;
+	MavlinkParametersManager *_parameters_manager;
 
-	orb_advert_t		_mission_pub;
-	int			_mission_result_sub;
 	MAVLINK_MODE 		_mode;
 
 	mavlink_channel_t	_channel;
@@ -292,7 +302,9 @@ private:
 	bool			_ftp_on;
 	int			_uart_fd;
 	int			_baudrate;
-	int			_datarate;
+	int			_datarate;		///< data rate for normal streams (attitude, position, etc.)
+	int			_datarate_events;	///< data rate for params, waypoints, text messages
+	float		_rate_mult;
 
 	/**
 	 * If the queue index is not at 0, the queue sending
@@ -307,6 +319,8 @@ private:
 	float			_subscribe_to_stream_rate;
 
 	bool			_flow_control_enabled;
+	uint64_t		_last_write_success_time;
+	uint64_t		_last_write_try_time;
 
 	unsigned		_bytes_tx;
 	unsigned		_bytes_txerr;
@@ -328,6 +342,7 @@ private:
 	mavlink_message_buffer	_message_buffer;
 
 	pthread_mutex_t		_message_buffer_mutex;
+	pthread_mutex_t		_send_mutex;
 
 	bool			_param_initialized;
 	param_t			_param_system_id;
@@ -338,53 +353,28 @@ private:
 	perf_counter_t		_loop_perf;			/**< loop performance counter */
 	perf_counter_t		_txerr_perf;			/**< TX error counter */
 
-	/**
-	 * Send one parameter.
-	 *
-	 * @param param		The parameter id to send.
-	 * @return		zero on success, nonzero on failure.
-	 */
-	int			mavlink_pm_send_param(param_t param);
-
-	/**
-	 * Send one parameter identified by index.
-	 *
-	 * @param index		The index of the parameter to send.
-	 * @return		zero on success, nonzero else.
-	 */
-	int			mavlink_pm_send_param_for_index(uint16_t index);
-
-	/**
-	 * Send one parameter identified by name.
-	 *
-	 * @param name		The index of the parameter to send.
-	 * @return		zero on success, nonzero else.
-	 */
-	int			mavlink_pm_send_param_for_name(const char *name);
-
-	/**
-	 * Send a queue of parameters, one parameter per function call.
-	 *
-	 * @return		zero on success, nonzero on failure
-	 */
-	int			mavlink_pm_queued_send(void);
-
-	/**
-	 * Start sending the parameter queue.
-	 *
-	 * This function will not directly send parameters, but instead
-	 * activate the sending of one parameter on each call of
-	 * mavlink_pm_queued_send().
-	 * @see 		mavlink_pm_queued_send()
-	 */
-	void			mavlink_pm_start_queued_send();
-
 	void			mavlink_update_system();
 
 	int mavlink_open_uart(int baudrate, const char *uart_name, struct termios *uart_config_original, bool *is_usb);
 
+	/**
+	 * Get the free space in the transmit buffer
+	 *
+	 * @return free space in the UART TX buffer
+	 */
+	unsigned			get_free_tx_buf();
+
+	static unsigned int interval_from_rate(float rate);
+
 	int configure_stream(const char *stream_name, const float rate);
 
+	/**
+	 * Adjust the stream rates based on the current rate
+	 *
+	 * @param multiplier if greater than 1, the transmission rate will increase, if smaller than one decrease
+	 */
+	void adjust_stream_rates(const float multiplier);
+
 	int message_buffer_init(int size);
 
 	void message_buffer_destroy();
@@ -399,6 +389,11 @@ private:
 
 	void pass_message(const mavlink_message_t *msg);
 
+	/**
+	 * Update rate mult so total bitrate will be equal to _datarate.
+	 */
+	void update_rate_mult();
+
 	static int	mavlink_dev_ioctl(struct file *filep, int cmd, unsigned long arg);
 
 	/**

src/modules/mavlink/mavlink_messages.h

@@ -46,10 +46,10 @@
 class StreamListItem {
 
 public:
-	MavlinkStream* (*new_instance)();
+	MavlinkStream* (*new_instance)(Mavlink *mavlink);
 	const char* (*get_name)();
 
-	StreamListItem(MavlinkStream* (*inst)(), const char* (*name)()) :
+	StreamListItem(MavlinkStream* (*inst)(Mavlink *mavlink), const char* (*name)()) :
 		new_instance(inst),
 		get_name(name) {};
 

src/modules/mavlink/mavlink_mission.cpp

@@ -59,8 +59,7 @@
 static const int ERROR = -1;
 
 
-MavlinkMissionManager::MavlinkMissionManager(Mavlink *mavlink) :
-	_mavlink(mavlink),
+MavlinkMissionManager::MavlinkMissionManager(Mavlink *mavlink) : MavlinkStream(mavlink),
 	_state(MAVLINK_WPM_STATE_IDLE),
 	_time_last_recv(0),
 	_time_last_sent(0),
@@ -79,9 +78,8 @@ MavlinkMissionManager::MavlinkMissionManager(Mavlink *mavlink) :
 	_offboard_mission_sub(-1),
 	_mission_result_sub(-1),
 	_offboard_mission_pub(-1),
-	_slow_rate_limiter(2000000.0f / mavlink->get_rate_mult()),
+	_slow_rate_limiter(_interval / 10.0f),
 	_verbose(false),
-	_channel(mavlink->get_channel()),
 	_comp_id(MAV_COMP_ID_MISSIONPLANNER)
 {
 	_offboard_mission_sub = orb_subscribe(ORB_ID(offboard_mission));
@@ -96,6 +94,20 @@ MavlinkMissionManager::~MavlinkMissionManager()
 	close(_mission_result_sub);
 }
 
+unsigned
+MavlinkMissionManager::get_size()
+{
+	if (_state == MAVLINK_WPM_STATE_SENDLIST) {
+		return MAVLINK_MSG_ID_MISSION_ITEM_LEN + MAVLINK_NUM_NON_PAYLOAD_BYTES;
+
+	} else if (_state == MAVLINK_WPM_STATE_GETLIST) {
+		return MAVLINK_MSG_ID_MISSION_REQUEST + MAVLINK_NUM_NON_PAYLOAD_BYTES;
+
+	} else {
+		return 0;
+	}
+}
+
 void
 MavlinkMissionManager::init_offboard_mission()
 {
@@ -157,15 +169,13 @@ MavlinkMissionManager::update_active_mission(int dataman_id, unsigned count, int
 void
 MavlinkMissionManager::send_mission_ack(uint8_t sysid, uint8_t compid, uint8_t type)
 {
-	mavlink_message_t msg;
 	mavlink_mission_ack_t wpa;
 
 	wpa.target_system = sysid;
 	wpa.target_component = compid;
 	wpa.type = type;
 
-	mavlink_msg_mission_ack_encode_chan(mavlink_system.sysid, _comp_id, _channel, &msg, &wpa);
-	_mavlink->send_message(&msg);
+	_mavlink->send_message(MAVLINK_MSG_ID_MISSION_ACK, &wpa);
 
 	if (_verbose) { warnx("WPM: Send MISSION_ACK type %u to ID %u", wpa.type, wpa.target_system); }
 }
@@ -175,13 +185,11 @@ void
 MavlinkMissionManager::send_mission_current(uint16_t seq)
 {
 	if (seq < _count) {
-		mavlink_message_t msg;
 		mavlink_mission_current_t wpc;
 
 		wpc.seq = seq;
 
-		mavlink_msg_mission_current_encode_chan(mavlink_system.sysid, _comp_id, _channel, &msg, &wpc);
-		_mavlink->send_message(&msg);
+		_mavlink->send_message(MAVLINK_MSG_ID_MISSION_CURRENT, &wpc);
 
 	} else if (seq == 0 && _count == 0) {
 		/* don't broadcast if no WPs */
@@ -199,15 +207,13 @@ MavlinkMissionManager::send_mission_count(uint8_t sysid, uint8_t compid, uint16_
 {
 	_time_last_sent = hrt_absolute_time();
 
-	mavlink_message_t msg;
 	mavlink_mission_count_t wpc;
 
 	wpc.target_system = sysid;
 	wpc.target_component = compid;
 	wpc.count = _count;
 
-	mavlink_msg_mission_count_encode_chan(mavlink_system.sysid, _comp_id, _channel, &msg, &wpc);
-	_mavlink->send_message(&msg);
+	_mavlink->send_message(MAVLINK_MSG_ID_MISSION_COUNT, &wpc);
 
 	if (_verbose) { warnx("WPM: Send MISSION_COUNT %u to ID %u", wpc.count, wpc.target_system); }
 }
@@ -226,13 +232,12 @@ MavlinkMissionManager::send_mission_item(uint8_t sysid, uint8_t compid, uint16_t
 		mavlink_mission_item_t wp;
 		format_mavlink_mission_item(&mission_item, &wp);
 
-		mavlink_message_t msg;
 		wp.target_system = sysid;
 		wp.target_component = compid;
 		wp.seq = seq;
 		wp.current = (_current_seq == seq) ? 1 : 0;
-		mavlink_msg_mission_item_encode_chan(mavlink_system.sysid, _comp_id, _channel, &msg, &wp);
-		_mavlink->send_message(&msg);
+
+		_mavlink->send_message(MAVLINK_MSG_ID_MISSION_ITEM, &wp);
 
 		if (_verbose) { warnx("WPM: Send MISSION_ITEM seq %u to ID %u", wp.seq, wp.target_system); }
 
@@ -251,13 +256,12 @@ MavlinkMissionManager::send_mission_request(uint8_t sysid, uint8_t compid, uint1
 	if (seq < _max_count) {
 		_time_last_sent = hrt_absolute_time();
 
-		mavlink_message_t msg;
 		mavlink_mission_request_t wpr;
 		wpr.target_system = sysid;
 		wpr.target_component = compid;
 		wpr.seq = seq;
-		mavlink_msg_mission_request_encode_chan(mavlink_system.sysid, _comp_id, _channel, &msg, &wpr);
-		_mavlink->send_message(&msg);
+
+		_mavlink->send_message(MAVLINK_MSG_ID_MISSION_REQUEST, &wpr);
 
 		if (_verbose) { warnx("WPM: Send MISSION_REQUEST seq %u to ID %u", wpr.seq, wpr.target_system); }
 
@@ -272,22 +276,21 @@ MavlinkMissionManager::send_mission_request(uint8_t sysid, uint8_t compid, uint1
 void
 MavlinkMissionManager::send_mission_item_reached(uint16_t seq)
 {
-	mavlink_message_t msg;
 	mavlink_mission_item_reached_t wp_reached;
 
 	wp_reached.seq = seq;
 
-	mavlink_msg_mission_item_reached_encode_chan(mavlink_system.sysid, _comp_id, _channel, &msg, &wp_reached);
-	_mavlink->send_message(&msg);
+	_mavlink->send_message(MAVLINK_MSG_ID_MISSION_ITEM_REACHED, &wp_reached);
 
 	if (_verbose) { warnx("WPM: Send MISSION_ITEM_REACHED reached_seq %u", wp_reached.seq); }
 }
 
 
 void
-MavlinkMissionManager::eventloop()
+MavlinkMissionManager::send(const hrt_abstime now)
 {
-	hrt_abstime now = hrt_absolute_time();
+	/* update interval for slow rate limiter */
+	_slow_rate_limiter.set_interval(_interval * 10 / _mavlink->get_rate_mult());
 
 	bool updated = false;
 	orb_check(_mission_result_sub, &updated);

src/modules/mavlink/mavlink_mission.h

@@ -42,11 +42,12 @@
 
 #pragma once
 
-#include "mavlink_bridge_header.h"
-#include "mavlink_rate_limiter.h"
 #include <uORB/uORB.h>
 
-// FIXME XXX - TO BE MOVED TO XML
+#include "mavlink_bridge_header.h"
+#include "mavlink_rate_limiter.h"
+#include "mavlink_stream.h"
+
 enum MAVLINK_WPM_STATES {
 	MAVLINK_WPM_STATE_IDLE = 0,
 	MAVLINK_WPM_STATE_SENDLIST,
@@ -66,20 +67,75 @@ enum MAVLINK_WPM_CODES {
 #define MAVLINK_MISSION_PROTOCOL_TIMEOUT_DEFAULT 5000000    ///< Protocol communication action timeout in useconds
 #define MAVLINK_MISSION_RETRY_TIMEOUT_DEFAULT 500000        ///< Protocol communication retry timeout in useconds
 
-class Mavlink;
-
-class MavlinkMissionManager {
+class MavlinkMissionManager : public MavlinkStream {
 public:
-	MavlinkMissionManager(Mavlink *parent);
-
 	~MavlinkMissionManager();
 
+	const char *get_name() const
+	{
+		return MavlinkMissionManager::get_name_static();
+	}
+
+	static const char *get_name_static()
+	{
+		return "MISSION_ITEM";
+	}
+
+	uint8_t get_id()
+	{
+		return MAVLINK_MSG_ID_MISSION_ITEM;
+	}
+
+	static MavlinkStream *new_instance(Mavlink *mavlink)
+	{
+		return new MavlinkMissionManager(mavlink);
+	}
+
+	unsigned get_size();
+
+	void handle_message(const mavlink_message_t *msg);
+
+	void set_verbose(bool v) { _verbose = v; }
+
+private:
+	enum MAVLINK_WPM_STATES _state;					///< Current state
+
+	uint64_t		_time_last_recv;
+	uint64_t		_time_last_sent;
+
+	uint32_t		_action_timeout;
+	uint32_t		_retry_timeout;
+	unsigned		_max_count;				///< Maximum number of mission items
+
+	int			_dataman_id;				///< Dataman storage ID for active mission
+	unsigned		_count;					///< Count of items in active mission
+	int			_current_seq;				///< Current item sequence in active mission
+
+	int			_transfer_dataman_id;			///< Dataman storage ID for current transmission
+	unsigned		_transfer_count;			///< Items count in current transmission
+	unsigned		_transfer_seq;				///< Item sequence in current transmission
+	unsigned		_transfer_current_seq;			///< Current item ID for current transmission (-1 means not initialized)
+	unsigned		_transfer_partner_sysid;		///< Partner system ID for current transmission
+	unsigned		_transfer_partner_compid;		///< Partner component ID for current transmission
+
+	int			_offboard_mission_sub;
+	int			_mission_result_sub;
+	orb_advert_t		_offboard_mission_pub;
+
+	MavlinkRateLimiter	_slow_rate_limiter;
+
+	bool _verbose;
+
+	uint8_t			_comp_id;
+
+	/* do not allow top copying this class */
+	MavlinkMissionManager(MavlinkMissionManager &);
+	MavlinkMissionManager& operator = (const MavlinkMissionManager &);
+
 	void init_offboard_mission();
 
 	int update_active_mission(int dataman_id, unsigned count, int seq);
 
-	void send_message(mavlink_message_t *msg);
-
 	/**
 	 *  @brief Sends an waypoint ack message
 	 */
@@ -111,10 +167,6 @@ public:
 	 */
 	void send_mission_item_reached(uint16_t seq);
 
-	void eventloop();
-
-	void handle_message(const mavlink_message_t *msg);
-
 	void handle_mission_ack(const mavlink_message_t *msg);
 
 	void handle_mission_set_current(const mavlink_message_t *msg);
@@ -139,43 +191,8 @@ public:
 	 */
 	int format_mavlink_mission_item(const struct mission_item_s *mission_item, mavlink_mission_item_t *mavlink_mission_item);
 
-	void set_verbose(bool v) { _verbose = v; }
+protected:
+	explicit MavlinkMissionManager(Mavlink *mavlink);
 
-private:
-	Mavlink*		_mavlink;
-
-	enum MAVLINK_WPM_STATES _state;					///< Current state
-
-	uint64_t		_time_last_recv;
-	uint64_t		_time_last_sent;
-
-	uint32_t		_action_timeout;
-	uint32_t		_retry_timeout;
-	unsigned		_max_count;				///< Maximum number of mission items
-
-	int			_dataman_id;				///< Dataman storage ID for active mission
-	unsigned		_count;					///< Count of items in active mission
-	int			_current_seq;				///< Current item sequence in active mission
-
-	int			_transfer_dataman_id;			///< Dataman storage ID for current transmission
-	unsigned		_transfer_count;			///< Items count in current transmission
-	unsigned		_transfer_seq;				///< Item sequence in current transmission
-	unsigned		_transfer_current_seq;			///< Current item ID for current transmission (-1 means not initialized)
-	unsigned		_transfer_partner_sysid;		///< Partner system ID for current transmission
-	unsigned		_transfer_partner_compid;		///< Partner component ID for current transmission
-
-	int			_offboard_mission_sub;
-	int			_mission_result_sub;
-	orb_advert_t		_offboard_mission_pub;
-
-	MavlinkRateLimiter	_slow_rate_limiter;
-
-	bool _verbose;
-
-	mavlink_channel_t	_channel;
-	uint8_t			_comp_id;
-
-	/* do not allow copying this class */
-	MavlinkMissionManager(const MavlinkMissionManager&);
-	MavlinkMissionManager operator=(const MavlinkMissionManager&);
+	void send(const hrt_abstime t);
 };

src/modules/mavlink/mavlink_parameters.cpp

@@ -0,0 +1,197 @@
+/****************************************************************************
+ *
+ *   Copyright (c) 2014 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
+ * are met:
+ *
+ * 1. Redistributions of source code must retain the above copyright
+ *    notice, this list of conditions and the following disclaimer.
+ * 2. Redistributions in binary form must reproduce the above copyright
+ *    notice, this list of conditions and the following disclaimer in
+ *    the documentation and/or other materials provided with the
+ *    distribution.
+ * 3. Neither the name PX4 nor the names of its contributors may be
+ *    used to endorse or promote products derived from this software
+ *    without specific prior written permission.
+ *
+ * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
+ * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
+ * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
+ * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
+ * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
+ * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
+ * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS
+ * OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED
+ * AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
+ * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
+ * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
+ * POSSIBILITY OF SUCH DAMAGE.
+ *
+ ****************************************************************************/
+
+/**
+ * @file mavlink_parameters.cpp
+ * Mavlink parameters manager implementation.
+ *
+ * @author Anton Babushkin <anton.babushkin@me.com>
+ */
+
+#include <stdio.h>
+
+#include "mavlink_parameters.h"
+#include "mavlink_main.h"
+
+MavlinkParametersManager::MavlinkParametersManager(Mavlink *mavlink) : MavlinkStream(mavlink),
+	_send_all_index(-1)
+{
+}
+
+unsigned
+MavlinkParametersManager::get_size()
+{
+	if (_send_all_index >= 0) {
+		return MAVLINK_MSG_ID_PARAM_VALUE_LEN + MAVLINK_NUM_NON_PAYLOAD_BYTES;
+
+	} else {
+		return 0;
+	}
+}
+
+void
+MavlinkParametersManager::handle_message(const mavlink_message_t *msg)
+{
+	switch (msg->msgid) {
+	case MAVLINK_MSG_ID_PARAM_REQUEST_LIST: {
+			/* request all parameters */
+			mavlink_param_request_list_t req_list;
+			mavlink_msg_param_request_list_decode(msg, &req_list);
+
+			if (req_list.target_system == mavlink_system.sysid &&
+			    (req_list.target_component == mavlink_system.compid || req_list.target_component == MAV_COMP_ID_ALL)) {
+
+				_send_all_index = 0;
+				_mavlink->send_statustext_info("[pm] sending list");
+			}
+			break;
+		}
+
+	case MAVLINK_MSG_ID_PARAM_SET: {
+			/* set parameter */
+			if (msg->msgid == MAVLINK_MSG_ID_PARAM_SET) {
+				mavlink_param_set_t set;
+				mavlink_msg_param_set_decode(msg, &set);
+
+				if (set.target_system == mavlink_system.sysid &&
+				    (set.target_component == mavlink_system.compid || set.target_component == MAV_COMP_ID_ALL)) {
+
+					/* local name buffer to enforce null-terminated string */
+					char name[MAVLINK_MSG_PARAM_VALUE_FIELD_PARAM_ID_LEN + 1];
+					strncpy(name, set.param_id, MAVLINK_MSG_PARAM_VALUE_FIELD_PARAM_ID_LEN);
+					/* enforce null termination */
+					name[MAVLINK_MSG_PARAM_VALUE_FIELD_PARAM_ID_LEN] = '\0';
+					/* attempt to find parameter, set and send it */
+					param_t param = param_find(name);
+
+					if (param == PARAM_INVALID) {
+						char buf[MAVLINK_MSG_STATUSTEXT_FIELD_TEXT_LEN];
+						sprintf(buf, "[pm] unknown param: %s", name);
+						_mavlink->send_statustext_info(buf);
+
+					} else {
+						/* set and send parameter */
+						param_set(param, &(set.param_value));
+						send_param(param);
+					}
+				}
+			}
+			break;
+		}
+
+	case MAVLINK_MSG_ID_PARAM_REQUEST_READ: {
+			/* request one parameter */
+			mavlink_param_request_read_t req_read;
+			mavlink_msg_param_request_read_decode(msg, &req_read);
+
+			if (req_read.target_system == mavlink_system.sysid &&
+			    (req_read.target_component == mavlink_system.compid || req_read.target_component == MAV_COMP_ID_ALL)) {
+
+				/* when no index is given, loop through string ids and compare them */
+				if (req_read.param_index < 0) {
+					/* local name buffer to enforce null-terminated string */
+					char name[MAVLINK_MSG_PARAM_VALUE_FIELD_PARAM_ID_LEN + 1];
+					strncpy(name, req_read.param_id, MAVLINK_MSG_PARAM_VALUE_FIELD_PARAM_ID_LEN);
+					/* enforce null termination */
+					name[MAVLINK_MSG_PARAM_VALUE_FIELD_PARAM_ID_LEN] = '\0';
+					/* attempt to find parameter and send it */
+					send_param(param_find(name));
+
+				} else {
+					/* when index is >= 0, send this parameter again */
+					send_param(param_for_index(req_read.param_index));
+				}
+			}
+			break;
+		}
+
+	default:
+		break;
+	}
+}
+
+void
+MavlinkParametersManager::send(const hrt_abstime t)
+{
+	/* send all parameters if requested */
+	if (_send_all_index >= 0) {
+		send_param(param_for_index(_send_all_index));
+		_send_all_index++;
+		if (_send_all_index >= (int) param_count()) {
+			_send_all_index = -1;
+		}
+	}
+}
+
+void
+MavlinkParametersManager::send_param(param_t param)
+{
+	if (param == PARAM_INVALID) {
+		return;
+	}
+
+	mavlink_param_value_t msg;
+
+	/*
+	 * get param value, since MAVLink encodes float and int params in the same
+	 * space during transmission, copy param onto float val_buf
+	 */
+	if (param_get(param, &msg.param_value) != OK) {
+		return;
+	}
+
+	msg.param_count = param_count();
+	msg.param_index = param_get_index(param);
+
+	/* copy parameter name */
+	strncpy(msg.param_id, param_name(param), MAVLINK_MSG_PARAM_VALUE_FIELD_PARAM_ID_LEN);
+
+	/* query parameter type */
+	param_type_t type = param_type(param);
+
+	/*
+	 * Map onboard parameter type to MAVLink type,
+	 * endianess matches (both little endian)
+	 */
+	if (type == PARAM_TYPE_INT32) {
+		msg.param_type = MAVLINK_TYPE_INT32_T;
+
+	} else if (type == PARAM_TYPE_FLOAT) {
+		msg.param_type = MAVLINK_TYPE_FLOAT;
+
+	} else {
+		msg.param_type = MAVLINK_TYPE_FLOAT;
+	}
+
+	_mavlink->send_message(MAVLINK_MSG_ID_PARAM_VALUE, &msg);
+}

src/modules/mavlink/mavlink_parameters.h

@@ -1,6 +1,6 @@
 /****************************************************************************
  *
- *   Copyright (c) 2014 PX4 Development Team. All rights reserved.
+ *   Copyright (c) 2012-2014 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
@@ -32,34 +32,84 @@
  ****************************************************************************/
 
 /**
- * @file mavlink_commands.h
- * Mavlink commands stream definition.
+ * @file mavlink_parameters.h
+ * Mavlink parameters manager definition.
  *
  * @author Anton Babushkin <anton.babushkin@me.com>
  */
 
-#ifndef MAVLINK_COMMANDS_H_
-#define MAVLINK_COMMANDS_H_
+#pragma once
 
-#include <uORB/uORB.h>
-#include <uORB/topics/vehicle_command.h>
+#include <systemlib/param/param.h>
 
-class Mavlink;
-class MavlinkCommansStream;
+#include "mavlink_bridge_header.h"
+#include "mavlink_stream.h"
 
-#include "mavlink_main.h"
-
-class MavlinkCommandsStream
+class MavlinkParametersManager : public MavlinkStream
 {
-private:
-	MavlinkOrbSubscription *_cmd_sub;
-	struct vehicle_command_s *_cmd;
-	mavlink_channel_t _channel;
-	uint64_t _cmd_time;
-
 public:
-	MavlinkCommandsStream(Mavlink *mavlink, mavlink_channel_t channel);
-	void update(const hrt_abstime t);
-};
+	const char *get_name() const
+	{
+		return MavlinkParametersManager::get_name_static();
+	}
 
-#endif /* MAVLINK_COMMANDS_H_ */
+	static const char *get_name_static()
+	{
+		return "PARAM_VALUE";
+	}
+
+	uint8_t get_id()
+	{
+		return MAVLINK_MSG_ID_PARAM_VALUE;
+	}
+
+	static MavlinkStream *new_instance(Mavlink *mavlink)
+	{
+		return new MavlinkParametersManager(mavlink);
+	}
+
+	unsigned get_size();
+
+	void handle_message(const mavlink_message_t *msg);
+
+	/**
+	 * Send one parameter identified by index.
+	 *
+	 * @param index		The index of the parameter to send.
+	 * @return		zero on success, nonzero else.
+	 */
+	void		start_send_one(int index);
+
+
+	/**
+	 * Send one parameter identified by name.
+	 *
+	 * @param name		The index of the parameter to send.
+	 * @return		zero on success, nonzero else.
+	 */
+	int			start_send_for_name(const char *name);
+
+	/**
+	 * Start sending the parameter queue.
+	 *
+	 * This function will not directly send parameters, but instead
+	 * activate the sending of one parameter on each call of
+	 * mavlink_pm_queued_send().
+	 * @see 		mavlink_pm_queued_send()
+	 */
+	void		start_send_all();
+
+private:
+	int		_send_all_index;
+
+	/* do not allow top copying this class */
+	MavlinkParametersManager(MavlinkParametersManager &);
+	MavlinkParametersManager& operator = (const MavlinkParametersManager &);
+
+protected:
+	explicit MavlinkParametersManager(Mavlink *mavlink);
+
+	void send(const hrt_abstime t);
+
+	void send_param(param_t param);
+};

src/modules/mavlink/mavlink_rate_limiter.cpp

@@ -64,7 +64,7 @@ MavlinkRateLimiter::check(hrt_abstime t)
 	uint64_t dt = t - _last_sent;
 
 	if (dt > 0 && dt >= _interval) {
-		_last_sent = (t / _interval) * _interval;
+		_last_sent = t;
 		return true;
 	}
 

src/modules/mavlink/mavlink_stream.cpp

@@ -43,9 +43,9 @@
 #include "mavlink_stream.h"
 #include "mavlink_main.h"
 
-MavlinkStream::MavlinkStream() :
+MavlinkStream::MavlinkStream(Mavlink *mavlink) :
 	next(nullptr),
-	_channel(MAVLINK_COMM_0),
+	_mavlink(mavlink),
 	_interval(1000000),
 	_last_sent(0)
 {
@@ -64,15 +64,6 @@ MavlinkStream::set_interval(const unsigned int interval)
 	_interval = interval;
 }
 
-/**
- * Set mavlink channel
- */
-void
-MavlinkStream::set_channel(mavlink_channel_t channel)
-{
-	_channel = channel;
-}
-
 /**
  * Update subscriptions and send message if necessary
  */
@@ -80,11 +71,21 @@ int
 MavlinkStream::update(const hrt_abstime t)
 {
 	uint64_t dt = t - _last_sent;
+	unsigned int interval = _interval;
 
-	if (dt > 0 && dt >= _interval) {
+	if (!const_rate()) {
+		interval /= _mavlink->get_rate_mult();
+	}
+
+	if (dt > 0 && dt >= interval) {
 		/* interval expired, send message */
 		send(t);
-		_last_sent = (t / _interval) * _interval;
+		if (const_rate()) {
+			_last_sent = (t / _interval) * _interval;
+
+		} else {
+			_last_sent = t;
+		}
 
 		return 0;
 	}

src/modules/mavlink/mavlink_stream.h

@@ -46,31 +46,50 @@
 class Mavlink;
 class MavlinkStream;
 
-#include "mavlink_main.h"
-
 class MavlinkStream
 {
 
 public:
 	MavlinkStream *next;
 
-	MavlinkStream();
+	MavlinkStream(Mavlink *mavlink);
 	virtual ~MavlinkStream();
+
+	/**
+	 * Get the interval
+	 *
+	 * @param interval the inveral in microseconds (us) between messages
+	 */
 	void set_interval(const unsigned int interval);
-	void set_channel(mavlink_channel_t channel);
+
+	/**
+	 * Get the interval
+	 *
+	 * @return the inveral in microseconds (us) between messages
+	 */
+	unsigned get_interval() { return _interval; }
 
 	/**
 	 * @return 0 if updated / sent, -1 if unchanged
 	 */
 	int update(const hrt_abstime t);
-	static MavlinkStream *new_instance();
+	static MavlinkStream *new_instance(const Mavlink *mavlink);
 	static const char *get_name_static();
-	virtual void subscribe(Mavlink *mavlink) = 0;
 	virtual const char *get_name() const = 0;
 	virtual uint8_t get_id() = 0;
 
+	/**
+	 * @return true if steam rate shouldn't be adjusted
+	 */
+	virtual bool const_rate() { return false; }
+
+	/**
+	 * Get maximal total messages size on update
+	 */
+	virtual unsigned get_size() = 0;
+
 protected:
-	mavlink_channel_t _channel;
+	Mavlink *    _mavlink;
 	unsigned int _interval;
 
 	virtual void send(const hrt_abstime t) = 0;

src/modules/mavlink/module.mk

@@ -40,11 +40,11 @@ SRCS		 += mavlink_main.cpp \
 			mavlink.c \
 			mavlink_receiver.cpp \
 			mavlink_mission.cpp \
+			mavlink_parameters.cpp \
 			mavlink_orb_subscription.cpp \
 			mavlink_messages.cpp \
 			mavlink_stream.cpp \
 			mavlink_rate_limiter.cpp \
-			mavlink_commands.cpp \
 			mavlink_ftp.cpp
 
 INCLUDE_DIRS	 += $(MAVLINK_SRC)/include/mavlink