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 void
mavlink_send_uart_bytes(mavlink_channel_t channel, const uint8_t *ch, int length) mavlink_send_uart_bytes(mavlink_channel_t channel, const uint8_t *ch, int length)
{ {
int uart = -1; Mavlink *instance;
switch (channel) { switch (channel) {
case MAVLINK_COMM_0: case MAVLINK_COMM_0:
uart = Mavlink::get_uart_fd(0); instance = Mavlink::get_instance(0);
break; break;
case MAVLINK_COMM_1: case MAVLINK_COMM_1:
uart = Mavlink::get_uart_fd(1); instance = Mavlink::get_instance(1);
break; break;
case MAVLINK_COMM_2: case MAVLINK_COMM_2:
uart = Mavlink::get_uart_fd(2); instance = Mavlink::get_instance(2);
break; break;
case MAVLINK_COMM_3: case MAVLINK_COMM_3:
uart = Mavlink::get_uart_fd(3); instance = Mavlink::get_instance(3);
break; break;
#ifdef MAVLINK_COMM_4 #ifdef MAVLINK_COMM_4
case MAVLINK_COMM_4: case MAVLINK_COMM_4:
uart = Mavlink::get_uart_fd(4); instance = Mavlink::get_instance(4);
break; break;
#endif #endif
#ifdef MAVLINK_COMM_5 #ifdef MAVLINK_COMM_5
case MAVLINK_COMM_5: case MAVLINK_COMM_5:
uart = Mavlink::get_uart_fd(5); instance = Mavlink::get_instance(5);
break; break;
#endif #endif
#ifdef MAVLINK_COMM_6 #ifdef MAVLINK_COMM_6
case MAVLINK_COMM_6: case MAVLINK_COMM_6:
uart = Mavlink::get_uart_fd(6); instance = Mavlink::get_instance(6);
break; break;
#endif #endif
} }
/* no valid instance, bail */
if (!instance)
return;
int uart = instance->get_uart_fd();
ssize_t desired = (sizeof(uint8_t) * length); 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; 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 (buf_free == 0) {
if (last_write_times[(unsigned)channel] != 0 && if (last_write_times[(unsigned)channel] != 0 &&
hrt_elapsed_time(&last_write_times[(unsigned)channel]) > 500*1000UL) { 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"); warnx("DISABLING HARDWARE FLOW CONTROL");
instance->enable_flow_control(false);
} }
} else { } else {
@ -204,6 +208,7 @@ Mavlink::Mavlink() :
_mavlink_param_queue_index(0), _mavlink_param_queue_index(0),
_subscribe_to_stream(nullptr), _subscribe_to_stream(nullptr),
_subscribe_to_stream_rate(0.0f), _subscribe_to_stream_rate(0.0f),
_flow_control_enabled(true),
/* performance counters */ /* performance counters */
_loop_perf(perf_alloc(PC_ELAPSED, "mavlink")) _loop_perf(perf_alloc(PC_ELAPSED, "mavlink"))
@ -600,6 +605,10 @@ Mavlink::enable_flow_control(bool enabled)
} }
ret = tcsetattr(_uart_fd, TCSANOW, &uart_config); ret = tcsetattr(_uart_fd, TCSANOW, &uart_config);
if (!ret) {
_flow_control_enabled = enabled;
}
return ret; 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_hil_enabled() { return _mavlink_hil_enabled; };
bool get_flow_control_enabled() { return _flow_control_enabled; }
/** /**
* Handle waypoint related messages. * Handle waypoint related messages.
*/ */
@ -248,6 +250,8 @@ private:
char *_subscribe_to_stream; char *_subscribe_to_stream;
float _subscribe_to_stream_rate; float _subscribe_to_stream_rate;
bool _flow_control_enabled;
/** /**
* Send one parameter. * Send one parameter.
* *