MAVLink: Update complete app to support MAVLink 1 & MAVLink 2. Add MAV_PROTO_VER param to switch between them

src/modules/mavlink/mavlink.c

@@ -50,21 +50,3 @@ mavlink_system_t mavlink_system = {
 	1,
 	1
 }; // System ID, 1-255, Component/Subsystem ID, 1-255
-
-/*
- * Internal function to give access to the channel status for each channel
- */
-extern mavlink_status_t *mavlink_get_channel_status(uint8_t channel)
-{
-	static mavlink_status_t m_mavlink_status[MAVLINK_COMM_NUM_BUFFERS];
-	return &m_mavlink_status[channel];
-}
-
-/*
- * Internal function to give access to the channel buffer for each channel
- */
-extern mavlink_message_t *mavlink_get_channel_buffer(uint8_t channel)
-{
-	static mavlink_message_t m_mavlink_buffer[MAVLINK_COMM_NUM_BUFFERS];
-	return &m_mavlink_buffer[channel];
-}

src/modules/mavlink/mavlink_main.cpp

@@ -106,8 +106,6 @@ static const int ERROR = -1;
 
 static Mavlink *_mavlink_instances = nullptr;
 
-static const mavlink_crc_entry_t mavlink_message_meta[] = MAVLINK_MESSAGE_CRCS;
-
 /**
  * mavlink app start / stop handling function
  *
@@ -133,10 +131,28 @@ void mavlink_end_uart_send(mavlink_channel_t chan, int length)
 	}
 }
 
+/*
+ * Internal function to give access to the channel status for each channel
+ */
+mavlink_status_t *mavlink_get_channel_status(uint8_t channel)
+{
+	return Mavlink::get_status_for_instance(channel);
+}
+
+/*
+ * Internal function to give access to the channel buffer for each channel
+ */
+mavlink_message_t *mavlink_get_channel_buffer(uint8_t channel)
+{
+	return Mavlink::get_buffer_for_instance(channel);
+}
+
 static void usage(void);
 
 bool Mavlink::_boot_complete = false;
 bool Mavlink::_config_link_on = false;
+mavlink_message_t Mavlink::_mavlink_buffer[MAVLINK_COMM_NUM_BUFFERS] = {};
+mavlink_status_t Mavlink::_mavlink_status[MAVLINK_COMM_NUM_BUFFERS] = {};
 
 Mavlink::Mavlink() :
 	_device_name("/dev/ttyS1"),
@@ -183,6 +199,7 @@ Mavlink::Mavlink() :
 	_last_write_success_time(0),
 	_last_write_try_time(0),
 	_mavlink_start_time(0),
+	_protocol_version(0),
 	_bytes_tx(0),
 	_bytes_txerr(0),
 	_bytes_rx(0),
@@ -297,6 +314,18 @@ Mavlink::~Mavlink()
 	}
 }
 
+void
+Mavlink::set_proto_version(unsigned version)
+{
+	_protocol_version = version;
+
+	if (version == 1 || ((version == 0) && !_received_messages)) {
+		get_status()->flags |= MAVLINK_STATUS_FLAG_OUT_MAVLINK1;
+	} else if (version == 2) {
+		get_status()->flags &= ~(MAVLINK_STATUS_FLAG_OUT_MAVLINK1);
+	}
+}
+
 void
 Mavlink::count_txerr()
 {
@@ -493,6 +522,7 @@ void Mavlink::mavlink_update_system(void)
 	if (!_param_initialized) {
 		_param_system_id = param_find("MAV_SYS_ID");
 		_param_component_id = param_find("MAV_COMP_ID");
+		_param_proto_ver = param_find("MAV_PROTO_VER");
 		_param_radio_id = param_find("MAV_RADIO_ID");
 		_param_system_type = param_find("MAV_TYPE");
 		_param_use_hil_gps = param_find("MAV_USEHILGPS");
@@ -509,6 +539,10 @@ void Mavlink::mavlink_update_system(void)
 	int32_t component_id;
 	param_get(_param_component_id, &component_id);
 
+	int32_t proto;
+	param_get(_param_proto_ver, &proto);
+	set_proto_version(proto);
+
 	param_get(_param_radio_id, &_radio_id);
 
 	/* only allow system ID and component ID updates
@@ -841,7 +875,6 @@ Mavlink::send_packet()
 		ret = sendto(_socket_fd, _network_buf, _network_buf_len, 0, (struct sockaddr *)&_src_addr, sizeof(_src_addr));
 
 		struct telemetry_status_s &tstatus = get_rx_status();
-		warnx("UDP count: %d", ret);
 
 		/* resend message via broadcast if no valid connection exists */
 		if ((_mode != MAVLINK_MODE_ONBOARD) &&
@@ -858,8 +891,6 @@ Mavlink::send_packet()
 
 				if (bret <= 0) {
 					PX4_WARN("sending broadcast failed, errno: %d: %s", errno, strerror(errno));
-				} else {
-					warnx("BROADCAST count: %d", bret);
 				}
 			}
 		}
@@ -877,7 +908,6 @@ Mavlink::send_packet()
 void
 Mavlink::send_bytes(const uint8_t *buf, unsigned packet_len)
 {
-	warnx("send bytes");
 	/* If the wait until transmit flag is on, only transmit after we've received messages.
 	   Otherwise, transmit all the time. */
 	if (!should_transmit()) {

src/modules/mavlink/mavlink_main.h

@@ -114,6 +114,23 @@ public:
 
 	static Mavlink 		*get_instance_for_network_port(unsigned long port);
 
+	static mavlink_message_t *get_buffer_for_instance(unsigned instance) { return &_mavlink_buffer[instance]; }
+
+	mavlink_message_t 	*get_buffer() { return Mavlink::get_buffer_for_instance(_instance_id); }
+
+	static mavlink_status_t *get_status_for_instance(unsigned instance) { return &_mavlink_status[instance]; }
+
+	mavlink_status_t 	*get_status() { return Mavlink::get_status_for_instance(_instance_id); }
+
+	/**
+	 * Set the MAVLink version
+	 *
+	 * Currently supporting v1 and v2
+	 *
+	 * @param version MAVLink version
+	 */
+	void			set_proto_version(unsigned version);
+
 	static int		destroy_all_instances();
 
 	static int		get_status_all_instances();
@@ -385,6 +402,8 @@ private:
 	orb_advert_t		_mavlink_log_pub;
 	bool			_task_running;
 	static bool		_boot_complete;
+	static mavlink_message_t _mavlink_buffer[MAVLINK_COMM_NUM_BUFFERS];
+	static mavlink_status_t _mavlink_status[MAVLINK_COMM_NUM_BUFFERS];
 
 	/* states */
 	bool			_hil_enabled;		/**< Hardware In the Loop mode */
@@ -444,6 +463,7 @@ private:
 	uint64_t		_last_write_success_time;
 	uint64_t		_last_write_try_time;
 	uint64_t		_mavlink_start_time;
+	int32_t			_protocol_version;
 
 	unsigned		_bytes_tx;
 	unsigned		_bytes_txerr;
@@ -486,6 +506,7 @@ private:
 
 	param_t			_param_system_id;
 	param_t			_param_component_id;
+	param_t			_param_proto_ver;
 	param_t			_param_radio_id;
 	param_t			_param_system_type;
 	param_t			_param_use_hil_gps;

src/modules/mavlink/mavlink_params.c

@@ -47,6 +47,15 @@ PARAM_DEFINE_INT32(MAV_SYS_ID, 1);
  */
 PARAM_DEFINE_INT32(MAV_COMP_ID, 1);
 
+/**
+ * MAVLink protocol version
+ * @group MAVLink
+ * @value 0 Default to 1, switch to 2 if GCS sends version 2
+ * @value 1 Always use version 1
+ * @value 2 Always use version 2
+ */
+PARAM_DEFINE_INT32(MAV_PROTO_VER, 1);
+
 /**
  * MAVLink Radio ID
  *

src/modules/mavlink/mavlink_receiver.cpp

@@ -2084,6 +2084,10 @@ MavlinkReceiver::receive_thread(void *arg)
 				/* if read failed, this loop won't execute */
 				for (ssize_t i = 0; i < nread; i++) {
 					if (mavlink_parse_char(_mavlink->get_channel(), buf[i], &msg, &status)) {
+
+						/* check if we received version 2 */
+						// XXX todo _mavlink->set_proto_version(2);
+
 						/* handle generic messages and commands */
 						handle_message(&msg);