Skip to content
New issue

Have a question about this project? Sign up for a free GitHub account to open an issue and contact its maintainers and the community.

Sign up for GitHub

By clicking “Sign up for GitHub”, you agree to our terms of service and privacy statement. We’ll occasionally send you account related emails.

Already on GitHub? Sign in to your account

mavlink send helpers refactor to prevent writing partial messages #12967

Merged
merged 2 commits into from
Apr 15, 2020
Merged

mavlink send helpers refactor to prevent writing partial messages #12967

Show file tree
Hide file tree
Changes from 1 commit
Commits
Show all changes
2 commits
Select commit Hold shift + click to select a range
6484ec8
WIP: mavlink send hacks
dagar Apr 14, 2020
0d6251d
perf counters fix perf free and cleanup naming
dagar Apr 14, 2020
File filter

Filter by extension

Filter by extension
Conversations
Failed to load comments.
Loading
Jump to
Jump to file
Failed to load files.
Loading
Diff view
Diff view
144 changes: 70 additions & 74 deletions src/modules/mavlink/mavlink_main.cpp
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -106,7 +106,7 @@ void mavlink_start_uart_send(mavlink_channel_t chan, int length)
Mavlink *m = Mavlink::get_instance(chan);

if (m != nullptr) {
(void)m->begin_send();
m->send_start(length);
#ifdef MAVLINK_PRINT_PACKETS
printf("START PACKET (%u): ", (unsigned)chan);
#endif
Expand All @@ -118,7 +118,7 @@ void mavlink_end_uart_send(mavlink_channel_t chan, int length)
Mavlink *m = Mavlink::get_instance(chan);

if (m != nullptr) {
(void)m->send_packet();
m->send_finish();
#ifdef MAVLINK_PRINT_PACKETS
printf("\n");
#endif
Expand Down Expand Up @@ -187,6 +187,11 @@ Mavlink::Mavlink() :

Mavlink::~Mavlink()
{
perf_free(_loop_perf);
perf_free(_loop_interval_perf);
perf_free(_send_byte_error_perf);
perf_free(_send_start_tx_buf_low);

if (_task_running) {
/* task wakes up every 10ms or so at the longest */
_task_should_exit = true;
Expand Down Expand Up @@ -749,12 +754,13 @@ Mavlink::get_free_tx_buf()
} else
#endif // MAVLINK_UDP
{

#if defined(__PX4_NUTTX)
(void) ioctl(_uart_fd, FIONSPACE, (unsigned long)&buf_free);
#else
// No FIONSPACE on Linux todo:use SIOCOUTQ and queue size to emulate FIONSPACE
#if defined(__PX4_LINUX) || defined(__PX4_DARWIN) || defined(__PX4_CYGWIN)
//Linux cp210x does not support TIOCOUTQ
buf_free = 256;
#else
(void) ioctl(_uart_fd, FIONSPACE, (unsigned long)&buf_free);
buf_free = MAVLINK_MAX_PACKET_LEN;
#endif

if (_flow_control_mode == FLOW_CONTROL_AUTO && buf_free < FLOW_CONTROL_DISABLE_THRESHOLD) {
Expand All @@ -774,45 +780,64 @@ Mavlink::get_free_tx_buf()
return buf_free;
}

int
Mavlink::send_packet()
void Mavlink::send_start(int length)
{
int ret = -1;
pthread_mutex_lock(&_send_mutex);
_last_write_try_time = hrt_absolute_time();

#if defined(MAVLINK_UDP)
// check if there is space in the buffer
if (length > (int)get_free_tx_buf()) {
// not enough space in buffer to send
count_txerrbytes(length);

// prevent writes
_tx_buffer_low = true;

perf_count(_send_start_tx_buf_low);

} else {
_tx_buffer_low = false;
}
}

/* Only send packets if there is something in the buffer. */
if (_network_buf_len == 0) {
void Mavlink::send_finish()
{
if (_tx_buffer_low || (_buf_fill == 0)) {
pthread_mutex_unlock(&_send_mutex);
return 0;
return;
}

if (get_protocol() == Protocol::UDP) {
int ret = -1;

#ifdef CONFIG_NET
// send message to UART
if (get_protocol() == Protocol::SERIAL) {
ret = ::write(_uart_fd, _buf, _buf_fill);
}

#if defined(MAVLINK_UDP)

else if (get_protocol() == Protocol::UDP) {

# if defined(CONFIG_NET)

if (_src_addr_initialized) {
#endif
ret = sendto(_socket_fd, _network_buf, _network_buf_len, 0,
(struct sockaddr *)&_src_addr, sizeof(_src_addr));
#ifdef CONFIG_NET
# endif // CONFIG_NET
ret = sendto(_socket_fd, _buf, _buf_fill, 0, (struct sockaddr *)&_src_addr, sizeof(_src_addr));
# if defined(CONFIG_NET)
}

#endif
# endif // CONFIG_NET

/* resend message via broadcast if no valid connection exists */
if ((_mode != MAVLINK_MODE_ONBOARD) && broadcast_enabled() &&
(!get_client_source_initialized()
|| (hrt_elapsed_time(&_tstatus.heartbeat_time) > 3_s))) {
(!get_client_source_initialized() || (hrt_elapsed_time(&_tstatus.heartbeat_time) > 3_s))) {

if (!_broadcast_address_found) {
find_broadcast_address();
}

if (_broadcast_address_found && _network_buf_len > 0) {
if (_broadcast_address_found && _buf_fill > 0) {

int bret = sendto(_socket_fd, _network_buf, _network_buf_len, 0,
(struct sockaddr *)&_bcast_addr, sizeof(_bcast_addr));
int bret = sendto(_socket_fd, _buf, _buf_fill, 0, (struct sockaddr *)&_bcast_addr, sizeof(_bcast_addr));

if (bret <= 0) {
if (!_broadcast_failed_warned) {
Expand All @@ -825,69 +850,38 @@ Mavlink::send_packet()
}
}
}

}

_network_buf_len = 0;

#endif // MAVLINK_UDP

pthread_mutex_unlock(&_send_mutex);
return ret;
}

void
Mavlink::send_bytes(const uint8_t *buf, unsigned packet_len)
{
_last_write_try_time = hrt_absolute_time();

if (_mavlink_start_time == 0) {
_mavlink_start_time = _last_write_try_time;
}

if (get_protocol() == Protocol::SERIAL) {
/* check if there is space in the buffer, let it overflow else */
unsigned buf_free = get_free_tx_buf();
if (ret == (int)_buf_fill) {
count_txbytes(_buf_fill);
_last_write_success_time = _last_write_try_time;

if (buf_free < packet_len) {
/* not enough space in buffer to send */
count_txerrbytes(packet_len);
return;
}
} else {
count_txerrbytes(_buf_fill);
}

size_t ret = -1;
_buf_fill = 0;

/* send message to UART */
if (get_protocol() == Protocol::SERIAL) {
ret = ::write(_uart_fd, buf, packet_len);
}

#if defined(MAVLINK_UDP)
pthread_mutex_unlock(&_send_mutex);
}

else {
if (_network_buf_len + packet_len < sizeof(_network_buf) / sizeof(_network_buf[0])) {
memcpy(&_network_buf[_network_buf_len], buf, packet_len);
_network_buf_len += packet_len;
void Mavlink::send_bytes(const uint8_t *buf, unsigned packet_len)
{
if (!_tx_buffer_low) {
if (_buf_fill + packet_len < sizeof(_buf)) {
memcpy(&_buf[_buf_fill], buf, packet_len);
_buf_fill += packet_len;

ret = packet_len;
} else {
perf_count(_send_byte_error_perf);
}
}

#endif // MAVLINK_UDP

if (ret != (size_t) packet_len) {
count_txerrbytes(packet_len);

} else {
_last_write_success_time = _last_write_try_time;
count_txbytes(packet_len);
}
}

#ifdef MAVLINK_UDP
void
Mavlink::find_broadcast_address()
void Mavlink::find_broadcast_address()
{
#if defined(__PX4_LINUX) || defined(__PX4_DARWIN) || defined(__PX4_CYGWIN)
struct ifconf ifconf;
Expand Down Expand Up @@ -2210,6 +2204,8 @@ Mavlink::task_main(int argc, char *argv[])
/* start the MAVLink receiver last to avoid a race */
MavlinkReceiver::receive_start(&_receive_thread, this);

_mavlink_start_time = hrt_absolute_time();

while (!_task_should_exit) {
/* main loop */
px4_usleep(_main_loop_delay);
Expand Down
19 changes: 10 additions & 9 deletions src/modules/mavlink/mavlink_main.h
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -318,21 +318,17 @@ class Mavlink : public ModuleParams
/**
* This is the beginning of a MAVLINK_START_UART_SEND/MAVLINK_END_UART_SEND transaction
*/
void begin_send() { pthread_mutex_lock(&_send_mutex); }
void send_start(int length);

/**
* Send bytes out on the link.
*
* On a network port these might actually get buffered to form a packet.
* Buffer bytes to send out on the link.
*/
void send_bytes(const uint8_t *buf, unsigned packet_len);

/**
* Flush the transmit buffer and send one MAVLink packet
*
* @return the number of bytes sent or -1 in case of error
*/
int send_packet();
void send_finish();

/**
* Resend message as is, don't change sequence number and CRC.
Expand Down Expand Up @@ -619,13 +615,16 @@ class Mavlink : public ModuleParams
bool _broadcast_address_found{false};
bool _broadcast_address_not_found_warned{false};
bool _broadcast_failed_warned{false};
uint8_t _network_buf[MAVLINK_MAX_PACKET_LEN] {};
unsigned _network_buf_len{0};

unsigned short _network_port{14556};
unsigned short _remote_port{DEFAULT_REMOTE_PORT_UDP};
#endif // MAVLINK_UDP

uint8_t _buf[MAVLINK_MAX_PACKET_LEN] {};
unsigned _buf_fill{0};

bool _tx_buffer_low{false};

const char *_interface_name{nullptr};

int _socket_fd{-1};
Expand Down Expand Up @@ -668,6 +667,8 @@ class Mavlink : public ModuleParams

perf_counter_t _loop_perf{perf_alloc(PC_ELAPSED, "mavlink_el")}; /**< loop performance counter */
perf_counter_t _loop_interval_perf{perf_alloc(PC_INTERVAL, "mavlink_int")}; /**< loop interval performance counter */
perf_counter_t _send_byte_error_perf{perf_alloc(PC_COUNT, "mavlink_send_bytes_error")}; /**< send bytes error count */
perf_counter_t _send_start_tx_buf_low{perf_alloc(PC_COUNT, "mavlink_tx_buf_low")}; /**< available tx buffer smaller than message */

void mavlink_update_parameters();

Expand Down