MAVLink 2.0: Take a first stab at integration, enable heartbeat packets

src/modules/mavlink/mavlink_bridge_header.h

@@ -44,12 +44,7 @@
 
 __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
@@ -57,7 +52,7 @@ __BEGIN_DECLS
 #define MAVLINK_GET_CHANNEL_BUFFER mavlink_get_channel_buffer
 #define MAVLINK_GET_CHANNEL_STATUS mavlink_get_channel_status
 
-#include <v1.0/mavlink_types.h>
+#include <v2.0/mavlink_types.h>
 #include <unistd.h>
 
 
@@ -84,7 +79,7 @@ void mavlink_send_uart_bytes(mavlink_channel_t chan, const uint8_t *ch, int leng
 extern mavlink_status_t *mavlink_get_channel_status(uint8_t chan);
 extern mavlink_message_t *mavlink_get_channel_buffer(uint8_t chan);
 
-#include <v1.0/common/mavlink.h>
+#include <v2.0/common/mavlink.h>
 
 __END_DECLS
 

src/modules/mavlink/mavlink_log_handler.h

@@ -40,7 +40,7 @@
 #include <queue.h>
 #include <time.h>
 #include <stdio.h>
-#include <v1.0/mavlink_types.h>
+#include <v2.0/mavlink_types.h>
 #include "mavlink_stream.h"
 
 class Mavlink;

src/modules/mavlink/mavlink_main.cpp

@@ -106,8 +106,7 @@ static const int ERROR = -1;
 
 static Mavlink *_mavlink_instances = nullptr;
 
-static const uint8_t mavlink_message_lengths[256] = MAVLINK_MESSAGE_LENGTHS;
+static const mavlink_crc_entry_t mavlink_message_meta[] = MAVLINK_MESSAGE_CRCS;
-static const uint8_t mavlink_message_crcs[256] = MAVLINK_MESSAGE_CRCS;
 
 /**
  * mavlink app start / stop handling function
@@ -118,6 +117,17 @@ extern "C" __EXPORT int mavlink_main(int argc, char *argv[]);
 
 extern mavlink_system_t mavlink_system;
 
+void mavlink_send_uart_bytes(mavlink_channel_t chan, const uint8_t *ch, int length)
+{
+	Mavlink* m = Mavlink::get_instance((unsigned)chan);
+
+	if (m->get_free_tx_buf() < length) {
+		return;
+	}
+
+	m->send_bytes(ch, length);
+}
+
 static void usage(void);
 
 bool Mavlink::_boot_complete = false;
@@ -816,15 +826,7 @@ Mavlink::get_free_tx_buf()
 void
 Mavlink::send_message(const uint8_t msgid, const void *msg, uint8_t component_ID)
 {
-	/* If the wait until transmit flag is on, only transmit after we've received messages.
+	uint8_t payload_len = mavlink_message_meta[msgid].msg_len;
-	   Otherwise, transmit all the time. */
-	if (!should_transmit()) {
-		return;
-	}
-
-	pthread_mutex_lock(&_send_mutex);
-
-	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();
@@ -845,6 +847,8 @@ Mavlink::send_message(const uint8_t msgid, const void *msg, uint8_t component_ID
 		}
 	}
 
+	mavlink_get_channel_status(_channel)->flags |= MAVLINK_STATUS_FLAG_OUT_MAVLINK1;
+
 	uint8_t buf[MAVLINK_MAX_PACKET_LEN];
 
 	/* header */
@@ -863,11 +867,25 @@ Mavlink::send_message(const uint8_t msgid, const void *msg, uint8_t component_ID
 	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);
+	crc_accumulate(mavlink_message_meta[msgid].crc_extra, &checksum);
 
 	buf[MAVLINK_NUM_HEADER_BYTES + payload_len] = (uint8_t)(checksum & 0xFF);
 	buf[MAVLINK_NUM_HEADER_BYTES + payload_len + 1] = (uint8_t)(checksum >> 8);
 
+	send_bytes(&buf[0], packet_len);
+}
+
+void
+Mavlink::send_bytes(const uint8_t *buf, unsigned packet_len)
+{
+	/* If the wait until transmit flag is on, only transmit after we've received messages.
+	   Otherwise, transmit all the time. */
+	if (!should_transmit()) {
+		return;
+	}
+
+	pthread_mutex_lock(&_send_mutex);
+
 	size_t ret = -1;
 
 	/* send message to UART */
@@ -881,11 +899,10 @@ Mavlink::send_message(const uint8_t msgid, const void *msg, uint8_t component_ID
 
 		struct telemetry_status_s &tstatus = get_rx_status();
 
-		/* resend heartbeat via broadcast */
+		/* resend message via broadcast if no valid connection exists */
 		if ((_mode != MAVLINK_MODE_ONBOARD) &&
 			(!get_client_source_initialized()
-			|| (hrt_elapsed_time(&tstatus.heartbeat_time) > 3 * 1000 * 1000))
+			|| (hrt_elapsed_time(&tstatus.heartbeat_time) > 3 * 1000 * 1000))) {
-			&& (msgid == MAVLINK_MSG_ID_HEARTBEAT)) {
 
 			if (!_broadcast_address_found) {
 				find_broadcast_address();

src/modules/mavlink/mavlink_main.h

@@ -226,6 +226,8 @@ public:
 
 	void			send_message(const uint8_t msgid, const void *msg, uint8_t component_ID = 0);
 
+	void			send_bytes(const uint8_t *buf, unsigned packet_len);
+
 	/**
 	 * Resend message as is, don't change sequence number and CRC.
 	 */

src/modules/mavlink/mavlink_messages.cpp

@@ -333,16 +333,13 @@ protected:
 			memset(&pos_sp_triplet, 0, sizeof(pos_sp_triplet));
 		}
 
-		mavlink_heartbeat_t msg;
+		uint8_t base_mode = 0;
+		uint32_t custom_mode = 0;
+		uint8_t system_status = 0;
+		get_mavlink_mode_state(&status, &pos_sp_triplet, &system_status, &base_mode, &custom_mode);
 
-		msg.base_mode = 0;
+		mavlink_msg_heartbeat_send(_mavlink->get_channel(), _mavlink->get_system_type(), MAV_AUTOPILOT_PX4,
-		msg.custom_mode = 0;
+			base_mode, custom_mode, system_status);
-		get_mavlink_mode_state(&status, &pos_sp_triplet, &msg.system_status, &msg.base_mode, &msg.custom_mode);
-		msg.type = _mavlink->get_system_type();
-		msg.autopilot = MAV_AUTOPILOT_PX4;
-		msg.mavlink_version = 3;
-
-		_mavlink->send_message(MAVLINK_MSG_ID_HEARTBEAT, &msg);
 	}
 };