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

Fixed HW flow control enable / disable scheme, console output cleanup on start

This commit is contained in:
Lorenz Meier 2014-03-12 10:29:39 +01:00
parent 069cba6338
commit 7604518db4
1 changed files with 23 additions and 22 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

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

@ -160,23 +160,30 @@ mavlink_send_uart_bytes(mavlink_channel_t channel, const uint8_t *ch, int length
if (ioctl(uart, FIONWRITE, (unsigned long)&buf_free) == 0) { if (ioctl(uart, FIONWRITE, (unsigned long)&buf_free) == 0) {
if (buf_free == 0 && last_write_times[(unsigned)channel] != 0 && if (buf_free == 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; struct termios uart_config;
(void)tcgetattr(uart, &uart_config); (void)tcgetattr(uart, &uart_config);
uart_config.c_cflag &= ~CRTSCTS; uart_config.c_cflag &= ~CRTSCTS;
(void)tcsetattr(uart, TCSANOW, &uart_config); (void)tcsetattr(uart, TCSANOW, &uart_config);
warnx("DISABLING HARDWARE FLOW CONTROL"); warnx("DISABLING HARDWARE FLOW CONTROL");
}
} 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); ssize_t ret = write(uart, ch, desired);
if (ret != desired) { if (ret != desired) {
warn("write err"); // XXX do something here, but change to using FIONWRITE and OS buf size for detection
} else {
last_write_times[(unsigned)channel] = hrt_absolute_time();
} }
} }
@ -536,7 +543,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 */ /* Back up the original uart configuration to restore it after exit */
if ((termios_state = tcgetattr(_uart_fd, uart_config_original)) < 0) { 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); close(_uart_fd);
return -1; return -1;
} }
@ -552,7 +559,7 @@ int Mavlink::mavlink_open_uart(int baud, const char *uart_name, struct termios *
/* Set baud rate */ /* Set baud rate */
if (cfsetispeed(&uart_config, speed) < 0 || cfsetospeed(&uart_config, speed) < 0) { 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); close(_uart_fd);
return -1; return -1;
} }
@ -560,7 +567,7 @@ int Mavlink::mavlink_open_uart(int baud, const char *uart_name, struct termios *
} }
if ((termios_state = tcsetattr(_uart_fd, TCSANOW, &uart_config)) < 0) { 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); close(_uart_fd);
return -1; return -1;
} }
@ -574,7 +581,9 @@ int Mavlink::mavlink_open_uart(int baud, const char *uart_name, struct termios *
(void)tcsetattr(_uart_fd, TCSANOW, &uart_config); (void)tcsetattr(_uart_fd, TCSANOW, &uart_config);
/* setup output flow control */ /* setup output flow control */
(void)enable_flow_control(true); if (enable_flow_control(true)) {
warnx("ERR FLOW CTRL EN");
}
return _uart_fd; return _uart_fd;
} }
@ -1604,10 +1613,6 @@ Mavlink::configure_stream_threadsafe(const char *stream_name, const float rate)
int int
Mavlink::task_main(int argc, char *argv[]) Mavlink::task_main(int argc, char *argv[])
{ {
/* inform about start */
warnx("start");
fflush(stdout);
int ch; int ch;
_baudrate = 57600; _baudrate = 57600;
_datarate = 0; _datarate = 0;
@ -1736,8 +1741,7 @@ Mavlink::task_main(int argc, char *argv[])
break; break;
} }
warnx("data rate: %d bytes/s", _datarate); warnx("data rate: %d bps, port: %s, baud: %d", _datarate, _device_name, (int)_baudrate);
warnx("port: %s, baudrate: %d", _device_name, _baudrate);
/* flush stdout in case MAVLink is about to take it over */ /* flush stdout in case MAVLink is about to take it over */
fflush(stdout); fflush(stdout);
@ -1782,9 +1786,6 @@ Mavlink::task_main(int argc, char *argv[])
struct vehicle_status_s *status = (struct vehicle_status_s *) status_sub->get_data(); 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 */ /* add default streams depending on mode and intervals depending on datarate */
float rate_mult = _datarate / 1000.0f; float rate_mult = _datarate / 1000.0f;