mavlink: use send_message() method of Mavlink class instead of using helpers

src/modules/mavlink/mavlink_main.cpp

@@ -94,11 +94,15 @@ 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 256
+
 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;
+
 /**
  * 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() :
@@ -253,6 +150,8 @@ Mavlink::Mavlink() :
 	   _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),
@@ -359,30 +258,10 @@ Mavlink::get_free_tx_buf()
 	unsigned buf_free;
 
 	if (!ioctl(_uart_fd, FIONWRITE, (unsigned long)&buf_free)) {
-		if (_rstatus.timestamp > 0 &&
+		if (_rstatus.telem_time > 0 &&
-			(hrt_elapsed_time(&_rstatus.timestamp) < (2 * 1000 * 1000))) {
+			(hrt_elapsed_time(&_rstatus.telem_time) < (2 * 1000 * 1000))) {
 
-			unsigned low_buf;
+			return (unsigned)(buf_free * 0.01f * _rstatus.txbuf);
-
-			switch (_rstatus.type) {
-				case TELEMETRY_STATUS_RADIO_TYPE_3DR_RADIO:
-					low_buf = 50;
-					break;
-				default:
-					low_buf = 50;
-					break;
-			}
-
-			if (_rstatus.txbuf < low_buf) {
-				/*
-				 * If the TX buf measure is initialized and up to date and low
-				 * return 0 to slow event based transmission
-				 */
-				return 0;
-			} else {
-				/* there is no SW flow control option, just use what our buffer tells us */
-				return buf_free;
-			}
 
 		} else {
 			return buf_free;
@@ -808,13 +687,93 @@ Mavlink::set_hil_enabled(bool hil_enabled)
 	return ret;
 }
 
+bool
+Mavlink::check_can_send(const int prio, const unsigned packet_len)
+{
+	/*
+	 * Check if the OS buffer is full and disable HW
+	 * flow control if it continues to be full
+	 */
+	int buf_free = 0;
+
+	if (get_flow_control_enabled()
+	    && ioctl(_uart_fd, 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_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);
+		}
+	}
+
+	/* 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();
+
+		/* check if there is space in the buffer, let it overflow else */
+		ioctl(_uart_fd, FIONWRITE, (unsigned long)&buf_free);
+
+		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;
+		}
+	}
+	return false;
+}
+
 void
-Mavlink::send_message(const mavlink_message_t *msg)
+Mavlink::send_message(const uint8_t msgid, const void *msg, const int prio)
 {
 	uint8_t buf[MAVLINK_MAX_PACKET_LEN];
 
-	uint16_t len = mavlink_msg_to_send_buffer(buf, msg);
+	uint8_t payload_len = mavlink_message_lengths[msgid];
-	mavlink_send_uart_bytes(_channel, buf, len);
+	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;
+	buf[2] = mavlink_get_channel_status(_channel)->current_tx_seq++;	// TODO use internal seq counter?
+	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, &buf[1], MAVLINK_CORE_HEADER_LEN + payload_len);
+#if MAVLINK_CRC_EXTRA
+	crc_accumulate(crc_extra, &checksum);
+#endif
+
+	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 != packet_len) {
+		count_txerr();
+		count_txerrbytes(packet_len);
+
+	} else {
+		_last_write_success_time = _last_write_try_time;
+		count_txbytes(packet_len);
+	}
 }
 
 void
@@ -1585,21 +1544,17 @@ Mavlink::task_main(int argc, char *argv[])
 
 		if (fast_rate_limiter.check(t)) {
 
-			unsigned buf_free = get_free_tx_buf();
-
 			/* only send messages if they fit the buffer */
-			if (buf_free >= (MAVLINK_MSG_ID_PARAM_VALUE_LEN + MAVLINK_NUM_NON_PAYLOAD_BYTES)) {
+			if (check_can_send(0, MAVLINK_MSG_ID_PARAM_VALUE_LEN + MAVLINK_NUM_NON_PAYLOAD_BYTES)) {
 				mavlink_pm_queued_send();
 			}
 
-			buf_free = get_free_tx_buf();
+			if (check_can_send(0, MAVLINK_MSG_ID_MISSION_ITEM_LEN + MAVLINK_NUM_NON_PAYLOAD_BYTES)) {
-			if (buf_free >= (MAVLINK_MSG_ID_MISSION_ITEM_LEN + MAVLINK_NUM_NON_PAYLOAD_BYTES)) {
 				_mission_manager->eventloop();
 			}
 
-			buf_free = get_free_tx_buf();
+			if (!mavlink_logbuffer_is_empty(&_logbuffer) &&
-			if (buf_free >= (MAVLINK_MSG_ID_STATUSTEXT_LEN + MAVLINK_NUM_NON_PAYLOAD_BYTES) &&
+					check_can_send(0, MAVLINK_MSG_ID_STATUSTEXT_LEN + MAVLINK_NUM_NON_PAYLOAD_BYTES)) {
-				(!mavlink_logbuffer_is_empty(&_logbuffer))) {
 				struct mavlink_logmessage msg;
 				int lb_ret = mavlink_logbuffer_read(&_logbuffer, &msg);
 

src/modules/mavlink/mavlink_main.h

@@ -160,6 +160,10 @@ 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			handle_message(const mavlink_message_t *msg);
@@ -315,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;

src/modules/mavlink/mavlink_messages.cpp

@@ -249,9 +249,9 @@ public:
 		return MAVLINK_MSG_ID_HEARTBEAT;
 	}
 
-	static MavlinkStream *new_instance()
+	static MavlinkStream *new_instance(Mavlink *mavlink)
 	{
-		return new MavlinkStreamHeartbeat();
+		return new MavlinkStreamHeartbeat(mavlink);
 	}
 
 private:
@@ -263,15 +263,15 @@ private:
 	MavlinkStreamHeartbeat& operator = (const MavlinkStreamHeartbeat &);
 
 protected:
-	explicit MavlinkStreamHeartbeat() : MavlinkStream(),
+	explicit MavlinkStreamHeartbeat(Mavlink *mavlink) : MavlinkStream(mavlink),
 		status_sub(nullptr),
 		pos_sp_triplet_sub(nullptr)
 	{}
 
-	void subscribe(Mavlink *mavlink)
+	void subscribe()
 	{
-		status_sub = mavlink->add_orb_subscription(ORB_ID(vehicle_status));
+		status_sub = _mavlink->add_orb_subscription(ORB_ID(vehicle_status));
-		pos_sp_triplet_sub = mavlink->add_orb_subscription(ORB_ID(position_setpoint_triplet));
+		pos_sp_triplet_sub = _mavlink->add_orb_subscription(ORB_ID(position_setpoint_triplet));
 	}
 
 	void send(const hrt_abstime t)
@@ -290,17 +290,15 @@ protected:
 			memset(&pos_sp_triplet, 0, sizeof(pos_sp_triplet));
 		}
 
-		uint8_t mavlink_state = 0;
+		mavlink_heartbeat_t msg;
-		uint8_t mavlink_base_mode = 0;
+		msg.base_mode = 0;
-		uint32_t mavlink_custom_mode = 0;
+		msg.custom_mode = 0;
-		get_mavlink_mode_state(&status, &pos_sp_triplet, &mavlink_state, &mavlink_base_mode, &mavlink_custom_mode);
+		get_mavlink_mode_state(&status, &pos_sp_triplet, &msg.system_status, &msg.base_mode, &msg.custom_mode);
+		msg.type = mavlink_system.type;
+		msg.autopilot = mavlink_system.type;
+		msg.mavlink_version = 3;
 
-		mavlink_msg_heartbeat_send(_channel,
+		_mavlink->send_message(MAVLINK_MSG_ID_HEARTBEAT, &msg, 1);
-					   mavlink_system.type,
-					   MAV_AUTOPILOT_PX4,
-					   mavlink_base_mode,
-					   mavlink_custom_mode,
-					   mavlink_state);
 	}
 };
 

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
  */

src/modules/mavlink/mavlink_stream.h

@@ -54,7 +54,7 @@ class MavlinkStream
 public:
 	MavlinkStream *next;
 
-	MavlinkStream();
+	MavlinkStream(Mavlink *mavlink);
 	virtual ~MavlinkStream();
 
 	/**
@@ -76,14 +76,14 @@ public:
 	 * @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 void subscribe() = 0;
 	virtual const char *get_name() const = 0;
 	virtual uint8_t get_id() = 0;
 
 protected:
-	mavlink_channel_t _channel;
+	Mavlink *    _mavlink;
 	unsigned int _interval;
 
 	virtual void send(const hrt_abstime t) = 0;