diff --git a/src/modules/mavlink/mavlink_bridge_header.h b/src/modules/mavlink/mavlink_bridge_header.h index 43e274a228..a35573db75 100644 --- a/src/modules/mavlink/mavlink_bridge_header.h +++ b/src/modules/mavlink/mavlink_bridge_header.h @@ -47,12 +47,12 @@ /* use efficient approach, see mavlink_helpers.h */ #define MAVLINK_SEND_UART_BYTES mavlink_send_uart_bytes +#define MAVLINK_START_UART_SEND mavlink_start_uart_send #define MAVLINK_END_UART_SEND mavlink_end_uart_send #define MAVLINK_GET_CHANNEL_BUFFER mavlink_get_channel_buffer #define MAVLINK_GET_CHANNEL_STATUS mavlink_get_channel_status -#include // Needs to be included before v2.0/mavlink_types.h until https://github.com/ArduPilot/pymavlink/pull/22 makes it through. #include #include @@ -78,6 +78,7 @@ extern mavlink_system_t mavlink_system; */ void mavlink_send_uart_bytes(mavlink_channel_t chan, const uint8_t *ch, int length); +void mavlink_start_uart_send(mavlink_channel_t chan, int length); void mavlink_end_uart_send(mavlink_channel_t chan, int length); extern mavlink_status_t *mavlink_get_channel_status(uint8_t chan); diff --git a/src/modules/mavlink/mavlink_main.cpp b/src/modules/mavlink/mavlink_main.cpp index 37846dba0a..ba9378f9bc 100644 --- a/src/modules/mavlink/mavlink_main.cpp +++ b/src/modules/mavlink/mavlink_main.cpp @@ -118,6 +118,15 @@ void mavlink_send_uart_bytes(mavlink_channel_t chan, const uint8_t *ch, int leng } } +void mavlink_start_uart_send(mavlink_channel_t chan, int length) +{ + Mavlink *m = Mavlink::get_instance((unsigned)chan); + + if (m != nullptr) { + (void)m->begin_send(); + } +} + void mavlink_end_uart_send(mavlink_channel_t chan, int length) { Mavlink *m = Mavlink::get_instance((unsigned)chan); @@ -901,6 +910,14 @@ Mavlink::get_free_tx_buf() return buf_free; } +void +Mavlink::begin_send() +{ + // must protect the network buffer so other calls from receive_thread do not + // mangle the message. + pthread_mutex_lock(&_send_mutex); +} + int Mavlink::send_packet() { @@ -910,6 +927,7 @@ Mavlink::send_packet() /* Only send packets if there is something in the buffer. */ if (_network_buf_len == 0) { + pthread_mutex_unlock(&_send_mutex); return 0; } @@ -955,6 +973,7 @@ Mavlink::send_packet() _network_buf_len = 0; #endif + pthread_mutex_unlock(&_send_mutex); return ret; } @@ -967,8 +986,6 @@ Mavlink::send_bytes(const uint8_t *buf, unsigned packet_len) return; } - pthread_mutex_lock(&_send_mutex); - _last_write_try_time = hrt_absolute_time(); if (_mavlink_start_time == 0) { @@ -983,7 +1000,6 @@ Mavlink::send_bytes(const uint8_t *buf, unsigned packet_len) /* not enough space in buffer to send */ count_txerr(); count_txerrbytes(packet_len); - pthread_mutex_unlock(&_send_mutex); return; } } @@ -1017,7 +1033,6 @@ Mavlink::send_bytes(const uint8_t *buf, unsigned packet_len) count_txbytes(packet_len); } - pthread_mutex_unlock(&_send_mutex); } void diff --git a/src/modules/mavlink/mavlink_main.h b/src/modules/mavlink/mavlink_main.h index 333842cab1..e4b206ad4e 100644 --- a/src/modules/mavlink/mavlink_main.h +++ b/src/modules/mavlink/mavlink_main.h @@ -171,11 +171,10 @@ public: BROADCAST_MODE_ON }; - static const char *mavlink_mode_str(enum MAVLINK_MODE mode) - { + static const char *mavlink_mode_str(enum MAVLINK_MODE mode) { switch (mode) { case MAVLINK_MODE_NORMAL: - return "Normal"; + return "Normal"; case MAVLINK_MODE_CUSTOM: return "Custom"; @@ -273,6 +272,12 @@ public: */ bool get_manual_input_mode_generation() { return _generate_rc; } + + /** + * This is the beginning of a MAVLINK_START_UART_SEND/MAVLINK_END_UART_SEND transaction + */ + void begin_send(); + /** * Send bytes out on the link. * @@ -285,7 +290,7 @@ public: * * @return the number of bytes sent or -1 in case of error */ - int send_packet(); + int send_packet(); /** * Resend message as is, don't change sequence number and CRC. @@ -440,6 +445,7 @@ public: MavlinkULog *get_ulog_streaming() { return _mavlink_ulog; } void try_start_ulog_streaming(uint8_t target_system, uint8_t target_component) { if (_mavlink_ulog) { return; } + _mavlink_ulog = MavlinkULog::try_start(_datarate, 0.7f, target_system, target_component); } void request_stop_ulog_streaming() {