dark0707
/
px4-firmware
forked from Archive/PX4-Autopilot
1
0
Fork 0
Code Pull Requests Activity

mavlink: forwarding and FTP fixed, flow control detector fixed

This commit is contained in:
Anton Babushkin 2014-07-25 00:16:02 +02:00
parent c486aa536f
commit 8f0af1c5fe
2 changed files with 77 additions and 39 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

97
src/modules/mavlink/mavlink_main.cpp
View File

@ -93,7 +93,7 @@ static const int ERROR = -1;
#define MAX_DATA_RATE 10000 // max data rate in bytes/s
#define MAIN_LOOP_DELAY 10000 // 100 Hz @ 1000 bytes/s data rate
#define TX_BUFFER_GAP 256
#define TX_BUFFER_GAP MAVLINK_MAX_PACKET_LEN
static Mavlink *_mavlink_instances = nullptr;
@ -252,25 +252,6 @@ Mavlink::count_txerr()
perf_count(_txerr_perf);
}
unsigned
Mavlink::get_free_tx_buf()
{
unsigned buf_free;
if (!ioctl(_uart_fd, FIONWRITE, (unsigned long)&buf_free)) {
if (_rstatus.telem_time > 0 &&
(hrt_elapsed_time(&_rstatus.telem_time) < (2 * 1000 * 1000))) {
return (unsigned)(buf_free * 0.01f * _rstatus.txbuf);
} else {
return buf_free;
}
} else {
return 0;
}
}
void
Mavlink::set_mode(enum MAVLINK_MODE mode)
{
@ -686,15 +667,9 @@ Mavlink::set_hil_enabled(bool hil_enabled)
return ret;
}
void
Mavlink::send_message(const uint8_t msgid, const void *msg)
unsigned
Mavlink::get_free_tx_buf()
{
/* If the wait until transmit flag is on, only transmit after we've received messages.
Otherwise, transmit all the time. */
if (!should_transmit()) {
return;
}
/*
* Check if the OS buffer is full and disable HW
* flow control if it continues to be full
@ -702,7 +677,7 @@ Mavlink::send_message(const uint8_t msgid, const void *msg)
int buf_free = 0;
(void) ioctl(_uart_fd, FIONWRITE, (unsigned long)&buf_free);
if (get_flow_control_enabled() && buf_free == 0) {
if (get_flow_control_enabled() && buf_free < TX_BUFFER_GAP) {
/* Disable hardware flow control:
* if no successful write since a defined time
* and if the last try was not the last successful write
@ -714,6 +689,19 @@ Mavlink::send_message(const uint8_t msgid, const void *msg)
enable_flow_control(false);
}
}
return buf_free;
}
void
Mavlink::send_message(const uint8_t msgid, const void *msg)
{
/* If the wait until transmit flag is on, only transmit after we've received messages.
Otherwise, transmit all the time. */
if (!should_transmit()) {
return;
}
int buf_free = get_free_tx_buf();
uint8_t payload_len = mavlink_message_lengths[msgid];
unsigned packet_len = payload_len + MAVLINK_NUM_NON_PAYLOAD_BYTES;
@ -722,7 +710,7 @@ Mavlink::send_message(const uint8_t msgid, const void *msg)
/* check if there is space in the buffer, let it overflow else */
if (buf_free < TX_BUFFER_GAP) {
warnx("SKIP msgid %i, %i bytes", msgid, packet_len);
warnx("SKIP msgid %i, %i bytes, free %i", msgid, packet_len, buf_free);
/* no enough space in buffer to send */
count_txerr();
count_txerrbytes(packet_len);
@ -764,6 +752,52 @@ Mavlink::send_message(const uint8_t msgid, const void *msg)
}
}
void
Mavlink::resend_message(mavlink_message_t *msg)
{
/* If the wait until transmit flag is on, only transmit after we've received messages.
Otherwise, transmit all the time. */
if (!should_transmit()) {
return;
}
int buf_free = get_free_tx_buf();
_last_write_try_time = hrt_absolute_time();
unsigned packet_len = msg->len + MAVLINK_NUM_NON_PAYLOAD_BYTES;
/* check if there is space in the buffer, let it overflow else */
if (buf_free < TX_BUFFER_GAP) {
warnx("SKIP resent msgid %i, %i bytes, free %i", msg->msgid, packet_len, buf_free);
/* no enough space in buffer to send */
count_txerr();
count_txerrbytes(packet_len);
return;
}
uint8_t buf[MAVLINK_MAX_PACKET_LEN];
/* header and payload */
memcpy(&buf[0], &msg->magic, MAVLINK_NUM_HEADER_BYTES + msg->len);
/* checksum */
buf[MAVLINK_NUM_HEADER_BYTES + msg->len] = (uint8_t)(msg->checksum & 0xFF);
buf[MAVLINK_NUM_HEADER_BYTES + msg->len + 1] = (uint8_t)(msg->checksum >> 8);
/* send message to UART */
ssize_t ret = write(_uart_fd, buf, packet_len);
if (ret != (int) packet_len) {
count_txerr();
count_txerrbytes(packet_len);
} else {
_last_write_success_time = _last_write_try_time;
count_txbytes(packet_len);
}
}
void
Mavlink::handle_message(const mavlink_message_t *msg)
{
@ -1411,8 +1445,7 @@ Mavlink::task_main(int argc, char *argv[])
pthread_mutex_unlock(&_message_buffer_mutex);
// TODO implement message resending
//_mavlink_resend_uart(_channel, &msg);
resend_message(&msg);
}
}

19
src/modules/mavlink/mavlink_main.h
View File

@ -162,6 +162,11 @@ public:
void send_message(const uint8_t msgid, const void *msg);
/**
* Resend message as is, don't change sequence number and CRC.
*/
void resend_message(mavlink_message_t *msg);
void handle_message(const mavlink_message_t *msg);
MavlinkOrbSubscription *add_orb_subscription(const orb_id_t topic);
@ -251,13 +256,6 @@ public:
*/
void count_rxbytes(unsigned n) { _bytes_rx += n; };
/**
* Get the free space in the transmit buffer
*
* @return free space in the UART TX buffer
*/
unsigned get_free_tx_buf();
/**
* Get the receive status of this MAVLink link
*/
@ -358,6 +356,13 @@ private:
int mavlink_open_uart(int baudrate, const char *uart_name, struct termios *uart_config_original, bool *is_usb);
/**
* Get the free space in the transmit buffer
*
* @return free space in the UART TX buffer
*/
unsigned get_free_tx_buf();
static unsigned int interval_from_rate(float rate);
int configure_stream(const char *stream_name, const float rate);