diff --git a/src/modules/mavlink/mavlink_bridge_header.h b/src/modules/mavlink/mavlink_bridge_header.h index 0cd08769e1..97acfb908e 100644 --- a/src/modules/mavlink/mavlink_bridge_header.h +++ b/src/modules/mavlink/mavlink_bridge_header.h @@ -44,12 +44,7 @@ __BEGIN_DECLS -/* - * 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 - */ +#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 +#include #include @@ -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 +#include __END_DECLS diff --git a/src/modules/mavlink/mavlink_log_handler.h b/src/modules/mavlink/mavlink_log_handler.h index ebbbe116a8..8742d8e2f4 100644 --- a/src/modules/mavlink/mavlink_log_handler.h +++ b/src/modules/mavlink/mavlink_log_handler.h @@ -40,7 +40,7 @@ #include #include #include -#include +#include #include "mavlink_stream.h" class Mavlink; diff --git a/src/modules/mavlink/mavlink_main.cpp b/src/modules/mavlink/mavlink_main.cpp index f35541c1e4..6d6da33a3a 100644 --- a/src/modules/mavlink/mavlink_main.cpp +++ b/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 uint8_t mavlink_message_crcs[256] = MAVLINK_MESSAGE_CRCS; +static const mavlink_crc_entry_t mavlink_message_meta[] = 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. - Otherwise, transmit all the time. */ - if (!should_transmit()) { - return; - } - - pthread_mutex_lock(&_send_mutex); - - uint8_t payload_len = mavlink_message_lengths[msgid]; + uint8_t payload_len = mavlink_message_meta[msgid].msg_len; 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)) - && (msgid == MAVLINK_MSG_ID_HEARTBEAT)) { + || (hrt_elapsed_time(&tstatus.heartbeat_time) > 3 * 1000 * 1000))) { if (!_broadcast_address_found) { find_broadcast_address(); diff --git a/src/modules/mavlink/mavlink_main.h b/src/modules/mavlink/mavlink_main.h index 9372c53f0f..90110d0f54 100644 --- a/src/modules/mavlink/mavlink_main.h +++ b/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. */ diff --git a/src/modules/mavlink/mavlink_messages.cpp b/src/modules/mavlink/mavlink_messages.cpp index e88188a8f3..5c2677bc46 100644 --- a/src/modules/mavlink/mavlink_messages.cpp +++ b/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; - msg.custom_mode = 0; - 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); + mavlink_msg_heartbeat_send(_mavlink->get_channel(), _mavlink->get_system_type(), MAV_AUTOPILOT_PX4, + base_mode, custom_mode, system_status); } };