forked from Archive/PX4-Autopilot
Merge pull request #727 from PX4/beta_mavlink2_flow_control
UART flow control
This commit is contained in:
Lorenz Meier
commit
e3b80d6e9d
|
|
@ -104,55 +104,90 @@ static struct file_operations fops;
|
|||
*/
|
||||
extern "C" __EXPORT int mavlink_main(int argc, char *argv[]);
|
||||
|
||||
static uint64_t last_write_times[6] = {0};
|
||||
|
||||
/*
|
||||
* Internal function to send the bytes through the right serial port
|
||||
*/
|
||||
void
|
||||
mavlink_send_uart_bytes(mavlink_channel_t channel, const uint8_t *ch, int length)
|
||||
{
|
||||
int uart = -1;
|
||||
Mavlink *instance;
|
||||
|
||||
switch (channel) {
|
||||
case MAVLINK_COMM_0:
|
||||
uart = Mavlink::get_uart_fd(0);
|
||||
instance = Mavlink::get_instance(0);
|
||||
break;
|
||||
|
||||
case MAVLINK_COMM_1:
|
||||
uart = Mavlink::get_uart_fd(1);
|
||||
instance = Mavlink::get_instance(1);
|
||||
break;
|
||||
|
||||
case MAVLINK_COMM_2:
|
||||
uart = Mavlink::get_uart_fd(2);
|
||||
instance = Mavlink::get_instance(2);
|
||||
break;
|
||||
|
||||
case MAVLINK_COMM_3:
|
||||
uart = Mavlink::get_uart_fd(3);
|
||||
instance = Mavlink::get_instance(3);
|
||||
break;
|
||||
#ifdef MAVLINK_COMM_4
|
||||
|
||||
case MAVLINK_COMM_4:
|
||||
uart = Mavlink::get_uart_fd(4);
|
||||
instance = Mavlink::get_instance(4);
|
||||
break;
|
||||
#endif
|
||||
#ifdef MAVLINK_COMM_5
|
||||
|
||||
case MAVLINK_COMM_5:
|
||||
uart = Mavlink::get_uart_fd(5);
|
||||
instance = Mavlink::get_instance(5);
|
||||
break;
|
||||
#endif
|
||||
#ifdef MAVLINK_COMM_6
|
||||
|
||||
case MAVLINK_COMM_6:
|
||||
uart = Mavlink::get_uart_fd(6);
|
||||
instance = Mavlink::get_instance(6);
|
||||
break;
|
||||
#endif
|
||||
}
|
||||
|
||||
/* no valid instance, bail */
|
||||
if (!instance)
|
||||
return;
|
||||
|
||||
int uart = instance->get_uart_fd();
|
||||
|
||||
ssize_t desired = (sizeof(uint8_t) * length);
|
||||
|
||||
/*
|
||||
* Check if the OS buffer is full and disable HW
|
||||
* flow control if it continues to be full
|
||||
*/
|
||||
int buf_free = 0;
|
||||
|
||||
if (instance->get_flow_control_enabled()
|
||||
&& ioctl(uart, FIONWRITE, (unsigned long)&buf_free) == 0) {
|
||||
|
||||
if (buf_free == 0) {
|
||||
|
||||
if (last_write_times[(unsigned)channel] != 0 &&
|
||||
hrt_elapsed_time(&last_write_times[(unsigned)channel]) > 500*1000UL) {
|
||||
|
||||
warnx("DISABLING HARDWARE FLOW CONTROL");
|
||||
instance->enable_flow_control(false);
|
||||
}
|
||||
|
||||
} else {
|
||||
|
||||
/* apparently there is space left, although we might be
|
||||
* partially overflooding the buffer already */
|
||||
last_write_times[(unsigned)channel] = hrt_absolute_time();
|
||||
}
|
||||
}
|
||||
|
||||
ssize_t ret = write(uart, ch, desired);
|
||||
|
||||
if (ret != desired) {
|
||||
warn("write err");
|
||||
// XXX do something here, but change to using FIONWRITE and OS buf size for detection
|
||||
}
|
||||
|
||||
}
|
||||
|
|
@ -173,6 +208,7 @@ Mavlink::Mavlink() :
|
|||
_mavlink_param_queue_index(0),
|
||||
_subscribe_to_stream(nullptr),
|
||||
_subscribe_to_stream_rate(0.0f),
|
||||
_flow_control_enabled(true),
|
||||
|
||||
/* performance counters */
|
||||
_loop_perf(perf_alloc(PC_ELAPSED, "mavlink"))
|
||||
|
|
@ -512,7 +548,7 @@ int Mavlink::mavlink_open_uart(int baud, const char *uart_name, struct termios *
|
|||
|
||||
/* Back up the original uart configuration to restore it after exit */
|
||||
if ((termios_state = tcgetattr(_uart_fd, uart_config_original)) < 0) {
|
||||
warnx("ERROR get termios config %s: %d\n", uart_name, termios_state);
|
||||
warnx("ERR GET CONF %s: %d\n", uart_name, termios_state);
|
||||
close(_uart_fd);
|
||||
return -1;
|
||||
}
|
||||
|
|
@ -528,7 +564,7 @@ int Mavlink::mavlink_open_uart(int baud, const char *uart_name, struct termios *
|
|||
|
||||
/* Set baud rate */
|
||||
if (cfsetispeed(&uart_config, speed) < 0 || cfsetospeed(&uart_config, speed) < 0) {
|
||||
warnx("ERROR setting baudrate / termios config for %s: %d (cfsetispeed, cfsetospeed)\n", uart_name, termios_state);
|
||||
warnx("ERR SET BAUD %s: %d\n", uart_name, termios_state);
|
||||
close(_uart_fd);
|
||||
return -1;
|
||||
}
|
||||
|
|
@ -536,14 +572,46 @@ int Mavlink::mavlink_open_uart(int baud, const char *uart_name, struct termios *
|
|||
}
|
||||
|
||||
if ((termios_state = tcsetattr(_uart_fd, TCSANOW, &uart_config)) < 0) {
|
||||
warnx("ERROR setting baudrate / termios config for %s (tcsetattr)\n", uart_name);
|
||||
warnx("ERR SET CONF %s\n", uart_name);
|
||||
close(_uart_fd);
|
||||
return -1;
|
||||
}
|
||||
|
||||
/*
|
||||
* Setup hardware flow control. If the port has no RTS pin this call will fail,
|
||||
* which is not an issue, but requires a separate call so we can fail silently.
|
||||
*/
|
||||
(void)tcgetattr(_uart_fd, &uart_config);
|
||||
uart_config.c_cflag |= CRTS_IFLOW;
|
||||
(void)tcsetattr(_uart_fd, TCSANOW, &uart_config);
|
||||
|
||||
/* setup output flow control */
|
||||
if (enable_flow_control(true)) {
|
||||
warnx("ERR FLOW CTRL EN");
|
||||
}
|
||||
|
||||
return _uart_fd;
|
||||
}
|
||||
|
||||
int
|
||||
Mavlink::enable_flow_control(bool enabled)
|
||||
{
|
||||
struct termios uart_config;
|
||||
int ret = tcgetattr(_uart_fd, &uart_config);
|
||||
if (enabled) {
|
||||
uart_config.c_cflag |= CRTSCTS;
|
||||
} else {
|
||||
uart_config.c_cflag &= ~CRTSCTS;
|
||||
}
|
||||
ret = tcsetattr(_uart_fd, TCSANOW, &uart_config);
|
||||
|
||||
if (!ret) {
|
||||
_flow_control_enabled = enabled;
|
||||
}
|
||||
|
||||
return ret;
|
||||
}
|
||||
|
||||
int
|
||||
Mavlink::set_hil_enabled(bool hil_enabled)
|
||||
{
|
||||
|
|
@ -1554,10 +1622,6 @@ Mavlink::configure_stream_threadsafe(const char *stream_name, const float rate)
|
|||
int
|
||||
Mavlink::task_main(int argc, char *argv[])
|
||||
{
|
||||
/* inform about start */
|
||||
warnx("start");
|
||||
fflush(stdout);
|
||||
|
||||
int ch;
|
||||
_baudrate = 57600;
|
||||
_datarate = 0;
|
||||
|
|
@ -1686,8 +1750,7 @@ Mavlink::task_main(int argc, char *argv[])
|
|||
break;
|
||||
}
|
||||
|
||||
warnx("data rate: %d bytes/s", _datarate);
|
||||
warnx("port: %s, baudrate: %d", _device_name, _baudrate);
|
||||
warnx("data rate: %d bps, port: %s, baud: %d", _datarate, _device_name, (int)_baudrate);
|
||||
|
||||
/* flush stdout in case MAVLink is about to take it over */
|
||||
fflush(stdout);
|
||||
|
|
@ -1732,9 +1795,6 @@ Mavlink::task_main(int argc, char *argv[])
|
|||
|
||||
struct vehicle_status_s *status = (struct vehicle_status_s *) status_sub->get_data();
|
||||
|
||||
warnx("started");
|
||||
mavlink_log_info(_mavlink_fd, "[mavlink] started");
|
||||
|
||||
/* add default streams depending on mode and intervals depending on datarate */
|
||||
float rate_mult = _datarate / 1000.0f;
|
||||
|
||||
|
|
|
|||
|
|
@ -156,6 +156,8 @@ public:
|
|||
|
||||
bool get_hil_enabled() { return _mavlink_hil_enabled; };
|
||||
|
||||
bool get_flow_control_enabled() { return _flow_control_enabled; }
|
||||
|
||||
/**
|
||||
* Handle waypoint related messages.
|
||||
*/
|
||||
|
|
@ -184,6 +186,13 @@ public:
|
|||
|
||||
int get_instance_id();
|
||||
|
||||
/**
|
||||
* Enable / disable hardware flow control.
|
||||
*
|
||||
* @param enabled True if hardware flow control should be enabled
|
||||
*/
|
||||
int enable_flow_control(bool enabled);
|
||||
|
||||
mavlink_channel_t get_channel();
|
||||
|
||||
bool _task_should_exit; /**< if true, mavlink task should exit */
|
||||
|
|
@ -241,6 +250,8 @@ private:
|
|||
char *_subscribe_to_stream;
|
||||
float _subscribe_to_stream_rate;
|
||||
|
||||
bool _flow_control_enabled;
|
||||
|
||||
/**
|
||||
* Send one parameter.
|
||||
*
|
||||
|
|
|
|||
Loading…
Reference in New Issue