mavlink: new message sending API, includes fixed

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

@@ -102,6 +102,7 @@ static Mavlink *_mavlink_instances = nullptr;
 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
@@ -130,6 +131,7 @@ Mavlink::Mavlink() :
 	_subscriptions(nullptr),
 	_streams(nullptr),
 	_mission_manager(nullptr),
+	_parameters_manager(nullptr),
 	_mission_pub(-1),
 	_mission_result_sub(-1),
 	_mode(MAVLINK_MODE_NORMAL),
@@ -145,6 +147,7 @@ Mavlink::Mavlink() :
 	   _baudrate(57600),
 	   _datarate(1000),
 	   _datarate_events(500),
+	   _rate_mult(0.0f),
 	   _mavlink_param_queue_index(0),
 	   mavlink_link_termination_allowed(false),
 	   _subscribe_to_stream(nullptr),
@@ -687,18 +690,23 @@ Mavlink::set_hil_enabled(bool hil_enabled)
 	return ret;
 }
 
-bool
-Mavlink::check_can_send(const int prio, const unsigned packet_len)
+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;
+	}
+
 	/*
 	 * 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()
-	    && ioctl(_uart_fd, FIONWRITE, (unsigned long)&buf_free) == 0) {
-
+	if (get_flow_control_enabled() && 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
@@ -711,36 +719,22 @@ Mavlink::check_can_send(const int prio, const 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()) {
-		_last_write_try_time = hrt_absolute_time();
+	uint8_t payload_len = mavlink_message_lengths[msgid];
+	unsigned packet_len = payload_len + MAVLINK_NUM_NON_PAYLOAD_BYTES;
 
-		/* check if there is space in the buffer, let it overflow else */
-		ioctl(_uart_fd, FIONWRITE, (unsigned long)&buf_free);
+	_last_write_try_time = hrt_absolute_time();
 
-		if (buf_free >= (prio <= 0 ? packet_len : TX_BUFFER_GAP)) {
-			warnx("\t\t\tSKIP %i bytes prio %i", packet_len, (int)prio);
-			/* we don't want to send anything just in half, so return */
-			count_txerr();
-			count_txerrbytes(packet_len);
-			return true;
-		}
+	/* check if there is space in the buffer, let it overflow else */
+	if (buf_free >= TX_BUFFER_GAP) {
+		warnx("SKIP msgid %i, %i bytes", msgid, packet_len);
+		/* no enough space in buffer to send */
+		count_txerr();
+		count_txerrbytes(packet_len);
+		return;
 	}
-	return false;
-}
 
-void
-Mavlink::send_message(const uint8_t msgid, const void *msg, const int prio)
-{
 	uint8_t buf[MAVLINK_MAX_PACKET_LEN];
 
-	uint8_t payload_len = mavlink_message_lengths[msgid];
-	unsigned packet_len = payload_len + MAVLINK_NUM_NON_PAYLOAD_BYTES;
-	if (!check_can_send(prio, packet_len)) {
-		return;
-	}
-
 	/* header */
 	buf[0] = MAVLINK_STX;
 	buf[1] = payload_len;
@@ -755,9 +749,9 @@ Mavlink::send_message(const uint8_t msgid, const void *msg, const int prio)
 	/* checksum */
 	uint16_t checksum;
 	crc_init(&checksum);
-	crc_accumulate_buffer(&checksum, &buf[1], MAVLINK_CORE_HEADER_LEN + payload_len);
+	crc_accumulate_buffer(&checksum, (const char*) &buf[1], MAVLINK_CORE_HEADER_LEN + payload_len);
 #if MAVLINK_CRC_EXTRA
-	crc_accumulate(crc_extra, &checksum);
+	crc_accumulate(mavlink_message_crcs[msgid], &checksum);
 #endif
 
 	buf[MAVLINK_NUM_HEADER_BYTES + payload_len] = (uint8_t)(checksum & 0xFF);
@@ -766,7 +760,7 @@ Mavlink::send_message(const uint8_t msgid, const void *msg, const int prio)
 	/* send message to UART */
 	ssize_t ret = write(_uart_fd, buf, packet_len);
 
-	if (ret != packet_len) {
+	if (ret != (int) packet_len) {
 		count_txerr();
 		count_txerrbytes(packet_len);
 
@@ -783,7 +777,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 */
@@ -791,163 +785,6 @@ 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
 Mavlink::send_statustext_info(const char *string)
 {
@@ -1031,11 +868,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;
@@ -1066,10 +909,9 @@ 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);
+			stream->subscribe();
 			LL_APPEND(_streams, stream);
 
 			return OK;
@@ -1267,7 +1109,26 @@ 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();
+		}
+	}
+
+	_rate_mult = ((float)_datarate - const_rate) / (rate - const_rate);
 }
 
 int
@@ -1390,6 +1251,8 @@ Mavlink::task_main(int argc, char *argv[])
 		break;
 	}
 
+	update_rate_mult();
+
 	warnx("data rate: %d Bytes/s, port: %s, baud: %d", _datarate, _device_name, _baudrate);
 
 	/* flush stdout in case MAVLink is about to take it over */
@@ -1452,32 +1315,37 @@ Mavlink::task_main(int argc, char *argv[])
 
 	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);
 
+	/* PARAM_VALUE stream */
+	_parameters_manager = (MavlinkParametersManager *) MavlinkParametersManager::new_instance(this);
+	_parameters_manager->set_interval(interval_from_rate(30.0f));
+	LL_APPEND(_streams, _parameters_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("NAMED_VALUE_FLOAT", 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;
 
@@ -1488,10 +1356,12 @@ Mavlink::task_main(int argc, char *argv[])
 	/* don't send parameters on startup without request */
 	_mavlink_param_queue_index = param_count();
 
-	MavlinkRateLimiter fast_rate_limiter(30000.0f / rate_mult);
+	float base_rate_mult = _datarate / 1000.0f;
+
+	MavlinkRateLimiter fast_rate_limiter(30000.0f / base_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 / base_rate_mult;
 
 	/* now the instance is fully initialized and we can bump the instance count */
 	LL_APPEND(_mavlink_instances, this);
@@ -1543,18 +1413,9 @@ Mavlink::task_main(int argc, char *argv[])
 		}
 
 		if (fast_rate_limiter.check(t)) {
+			_mission_manager->eventloop();
 
-			/* only send messages if they fit the buffer */
-			if (check_can_send(0, MAVLINK_MSG_ID_PARAM_VALUE_LEN + MAVLINK_NUM_NON_PAYLOAD_BYTES)) {
-				mavlink_pm_queued_send();
-			}
-
-			if (check_can_send(0, MAVLINK_MSG_ID_MISSION_ITEM_LEN + MAVLINK_NUM_NON_PAYLOAD_BYTES)) {
-				_mission_manager->eventloop();
-			}
-
-			if (!mavlink_logbuffer_is_empty(&_logbuffer) &&
-					check_can_send(0, MAVLINK_MSG_ID_STATUSTEXT_LEN + MAVLINK_NUM_NON_PAYLOAD_BYTES)) {
+			if (!mavlink_logbuffer_is_empty(&_logbuffer)) {
 				struct mavlink_logmessage msg;
 				int lb_ret = mavlink_logbuffer_read(&_logbuffer, &msg);
 
@@ -1630,6 +1491,7 @@ Mavlink::task_main(int argc, char *argv[])
 	}
 
 	delete _mission_manager;
+	delete _parameters_manager;
 
 	delete _subscribe_to_stream;
 	_subscribe_to_stream = nullptr;

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,11 +160,7 @@ public:
 	 */
 	int			set_hil_enabled(bool hil_enabled);
 
-	bool		check_can_send(const int prio, const unsigned packet_len);
-
-	void		send_message(const uint8_t msgid, const void *msg, const int prio);
-
-	void			send_message(const mavlink_message_t *msg);
+	void		send_message(const uint8_t msgid, const void *msg);
 
 	void			handle_message(const mavlink_message_t *msg);
 
@@ -285,6 +281,7 @@ private:
 	MavlinkStream		*_streams;
 
 	MavlinkMissionManager	*_mission_manager;
+	MavlinkParametersManager *_parameters_manager;
 
 	orb_advert_t		_mission_pub;
 	int			_mission_result_sub;
@@ -305,6 +302,7 @@ private:
 	int			_baudrate;
 	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
@@ -352,51 +350,12 @@ 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);
 
+	static unsigned int interval_from_rate(float rate);
+
 	int configure_stream(const char *stream_name, const float rate);
 
 	/**
@@ -420,6 +379,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

@@ -157,15 +157,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 +173,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 +195,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 +220,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 +244,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,13 +264,11 @@ 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); }
 }

src/modules/mavlink/mavlink_parameters.cpp

@@ -38,11 +38,11 @@
  * @author Anton Babushkin <anton.babushkin@me.com>
  */
 
-//#include <stdlib.h>
+#include <stdio.h>
 
 #include "mavlink_parameters.h"
+#include "mavlink_main.h"
 
-explicit
 MavlinkParametersManager::MavlinkParametersManager(Mavlink *mavlink) : MavlinkStream(mavlink),
 	_send_all_index(-1)
 {
@@ -51,7 +51,7 @@ MavlinkParametersManager::MavlinkParametersManager(Mavlink *mavlink) : MavlinkSt
 unsigned
 MavlinkParametersManager::get_size()
 {
-	if (_send_all_index() >= 0) {
+	if (_send_all_index >= 0) {
 		return MAVLINK_MSG_ID_PARAM_VALUE_LEN + MAVLINK_NUM_NON_PAYLOAD_BYTES;
 
 	} else {

src/modules/mavlink/mavlink_parameters.h

@@ -40,6 +40,9 @@
 
 #pragma once
 
+#include <systemlib/param/param.h>
+
+#include "mavlink_bridge_header.h"
 #include "mavlink_stream.h"
 
 class MavlinkParametersManager : public MavlinkStream

src/modules/mavlink/module.mk

@@ -40,6 +40,7 @@ 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 \