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

Fine tuning for flow control disable to stop firing after change is effective

This commit is contained in:
Lorenz Meier 2014-03-12 17:53:25 +01:00
parent 7604518db4
commit 45a18587fc
2 changed files with 26 additions and 13 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

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

@ -112,44 +112,50 @@ static uint64_t last_write_times[6] = {0};
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);
/*
@ -158,18 +164,16 @@ mavlink_send_uart_bytes(mavlink_channel_t channel, const uint8_t *ch, int length
*/
int buf_free = 0;
if (ioctl(uart, FIONWRITE, (unsigned long)&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) {
struct termios uart_config;
(void)tcgetattr(uart, &uart_config);
uart_config.c_cflag &= ~CRTSCTS;
(void)tcsetattr(uart, TCSANOW, &uart_config);
warnx("DISABLING HARDWARE FLOW CONTROL");
instance->enable_flow_control(false);
}
} else {
@ -204,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"))
@ -600,6 +605,10 @@ Mavlink::enable_flow_control(bool enabled)
}
ret = tcsetattr(_uart_fd, TCSANOW, &uart_config);
if (!ret) {
_flow_control_enabled = enabled;
}
return ret;
}

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

@ -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.
*/
@ -248,6 +250,8 @@ private:
char *_subscribe_to_stream;
float _subscribe_to_stream_rate;
bool _flow_control_enabled;
/**
* Send one parameter.
*