Mirrors_Framework/PX4-Autopilot
1
0
Fork 0
mirror of https://github.com/PX4/PX4-Autopilot.git synced 2026-05-24 15:40:31 +08:00
Code Issues Actions 8 Packages Projects Releases Wiki Activity

Add 'forced on' flow control mode to mavlink UART (#8776)

Browse Source
This commit is contained in:
Vasily Evseenko
2018-03-10 22:54:57 +03:00
committed by Kabir Mohammed
parent fd0be3c412
commit ebe74ccb2e
2 changed files with 37 additions and 31 deletions
Download Patch File Download Diff File Expand all files Collapse all files
src/modules/mavlink/mavlink_main.cpp
+26 -26
View File
@@ -1,6 +1,6 @@
/****************************************************************************
*
* Copyright (c) 2012-2017 PX4 Development Team. All rights reserved.
* Copyright (c) 2012-2018 PX4 Development Team. All rights reserved.
*
* Redistribution and use in source and binary forms, with or without
* modification, are permitted provided that the following conditions
@@ -228,7 +228,7 @@ Mavlink::Mavlink() :
_subscribe_to_stream(nullptr),
_subscribe_to_stream_rate(0.0f),
_udp_initialised(false),
_flow_control_enabled(false),
_flow_control_mode(Mavlink::FLOW_CONTROL_OFF),
_last_write_success_time(0),
_last_write_try_time(0),
_mavlink_start_time(0),
@@ -634,7 +634,7 @@ int Mavlink::get_component_id()
return mavlink_system.compid;
}
int Mavlink::mavlink_open_uart(int baud, const char *uart_name)
int Mavlink::mavlink_open_uart(int baud, const char *uart_name, bool force_flow_control)
{
#ifndef B460800
#define B460800 460800
@@ -808,30 +808,25 @@ int Mavlink::mavlink_open_uart(int baud, const char *uart_name)
return -1;
}
if (!_is_usb_uart) {
/*
* 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.
*/
/*
* 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.
*/
/* setup output flow control */
if (enable_flow_control(true)) {
PX4_WARN("hardware flow control not supported");
}
} else {
_flow_control_enabled = false;
/* setup output flow control */
if (enable_flow_control(force_flow_control ? FLOW_CONTROL_ON : FLOW_CONTROL_AUTO)) {
PX4_WARN("hardware flow control not supported");
}
return _uart_fd;
}
int
Mavlink::enable_flow_control(bool enabled)
Mavlink::enable_flow_control(enum FLOW_CONTROL_MODE mode)
{
// We can't do this on USB - skip
if (_is_usb_uart) {
_flow_control_enabled = false;
_flow_control_mode = FLOW_CONTROL_OFF;
return OK;
}
@@ -839,7 +834,7 @@ Mavlink::enable_flow_control(bool enabled)
int ret = tcgetattr(_uart_fd, &uart_config);
if (enabled) {
if (mode) {
uart_config.c_cflag |= CRTSCTS;
} else {
@@ -850,7 +845,7 @@ Mavlink::enable_flow_control(bool enabled)
ret = tcsetattr(_uart_fd, TCSANOW, &uart_config);
if (!ret) {
_flow_control_enabled = enabled;
_flow_control_mode = mode;
}
return ret;
@@ -901,15 +896,15 @@ Mavlink::get_free_tx_buf()
(void) ioctl(_uart_fd, FIONSPACE, (unsigned long)&buf_free);
#endif
if (get_flow_control_enabled() && buf_free < FLOW_CONTROL_DISABLE_THRESHOLD) {
/* Disable hardware flow control:
if (_flow_control_mode == FLOW_CONTROL_AUTO && buf_free < FLOW_CONTROL_DISABLE_THRESHOLD) {
/* Disable hardware flow control in FLOW_CONTROL_AUTO mode:
* if no successful write since a defined time
* and if the last try was not the last successful write
*/
if (_last_write_try_time != 0 &&
hrt_elapsed_time(&_last_write_success_time) > 500 * 1000UL &&
_last_write_success_time != _last_write_try_time) {
enable_flow_control(false);
enable_flow_control(FLOW_CONTROL_OFF);
}
}
}
@@ -1737,6 +1732,7 @@ Mavlink::task_main(int argc, char *argv[])
_baudrate = 57600;
_datarate = 0;
_mode = MAVLINK_MODE_NORMAL;
bool _force_flow_control = false;
#ifdef __PX4_NUTTX
/* the NuttX optarg handler does not
@@ -1759,7 +1755,7 @@ Mavlink::task_main(int argc, char *argv[])
int temp_int_arg;
#endif
while ((ch = px4_getopt(argc, argv, "b:r:d:u:o:m:t:fwx", &myoptind, &myoptarg)) != EOF) {
while ((ch = px4_getopt(argc, argv, "b:r:d:u:o:m:t:fwxz", &myoptind, &myoptarg)) != EOF) {
switch (ch) {
case 'b':
_baudrate = strtoul(myoptarg, nullptr, 10);
@@ -1884,6 +1880,10 @@ Mavlink::task_main(int argc, char *argv[])
_ftp_on = true;
break;
case 'z':
_force_flow_control = true;
break;
default:
err_flag = true;
break;
@@ -1917,7 +1917,7 @@ Mavlink::task_main(int argc, char *argv[])
fflush(stdout);
/* default values for arguments */
_uart_fd = mavlink_open_uart(_baudrate, _device_name);
_uart_fd = mavlink_open_uart(_baudrate, _device_name, _force_flow_control);
if (_uart_fd < 0 && _mode != MAVLINK_MODE_CONFIG) {
PX4_ERR("could not open %s", _device_name);
@@ -2582,7 +2582,7 @@ Mavlink::display_status()
printf("\tno telem status.\n");
}
printf("\tflow control:\t%s\n", (_flow_control_enabled) ? "ON" : "OFF");
printf("\tflow control:\t%s\n", (_flow_control_mode) ? "ON" : "OFF");
printf("\trates:\n");
printf("\ttx: %.3f kB/s\n", (double)_rate_tx);
printf("\ttxerr: %.3f kB/s\n", (double)_rate_txerr);
@@ -2625,7 +2625,6 @@ Mavlink::stream_command(int argc, char *argv[])
int temp_int_arg;
bool provided_device = false;
bool provided_network_port = false;
/*
* Called via main with original argv
* mavlink start
@@ -2788,6 +2787,7 @@ $ mavlink stream -u 14556 -s HIGHRES_IMU -r 50
PRINT_MODULE_USAGE_PARAM_FLAG('f', "Enable message forwarding to other Mavlink instances", true);
PRINT_MODULE_USAGE_PARAM_FLAG('w', "Wait to send, until first message received", true);
PRINT_MODULE_USAGE_PARAM_FLAG('x', "Enable FTP", true);
PRINT_MODULE_USAGE_PARAM_FLAG('z', "Force flow control always on", true);
PRINT_MODULE_USAGE_ARG("on|off", "Enable/disable", true);
src/modules/mavlink/mavlink_main.h
+11 -5
View File
@@ -1,6 +1,6 @@
/****************************************************************************
*
* Copyright (c) 2012-2017 PX4 Development Team. All rights reserved.
* Copyright (c) 2012-2018 PX4 Development Team. All rights reserved.
*
* Redistribution and use in source and binary forms, with or without
* modification, are permitted provided that the following conditions
@@ -178,6 +178,12 @@ public:
BROADCAST_MODE_ON
};
enum FLOW_CONTROL_MODE {
FLOW_CONTROL_OFF = 0,
FLOW_CONTROL_AUTO,
FLOW_CONTROL_ON
};
static const char *mavlink_mode_str(enum MAVLINK_MODE mode)
{
switch (mode) {
@@ -218,7 +224,7 @@ public:
bool get_forward_externalsp() { return _forward_externalsp; }
bool get_flow_control_enabled() { return _flow_control_enabled; }
bool get_flow_control_enabled() { return _flow_control_mode; }
bool get_forwarding_on() { return _forwarding_on; }
@@ -316,7 +322,7 @@ public:
*
* @param enabled True if hardware flow control should be enabled
*/
int enable_flow_control(bool enabled);
int enable_flow_control(enum FLOW_CONTROL_MODE enabled);
#endif
mavlink_channel_t get_channel();
@@ -542,7 +548,7 @@ private:
float _subscribe_to_stream_rate;
bool _udp_initialised;
bool _flow_control_enabled;
enum FLOW_CONTROL_MODE _flow_control_mode;
uint64_t _last_write_success_time;
uint64_t _last_write_try_time;
uint64_t _mavlink_start_time;
@@ -608,7 +614,7 @@ private:
void mavlink_update_system();
#ifndef __PX4_QURT
int mavlink_open_uart(int baudrate, const char *uart_name);
int mavlink_open_uart(int baudrate, const char *uart_name, bool force_flow_control);
#endif
static int interval_from_rate(float rate);