From 68d70cc8c0ed4c215d8861b5a5fd83f6baaff793 Mon Sep 17 00:00:00 2001 From: Lorenz Meier Date: Sat, 29 Jul 2017 10:41:19 +0200 Subject: [PATCH] MAVLink: Use modern PX4 output format to enable system logging for errors. --- src/modules/mavlink/mavlink_main.cpp | 44 ++++++++++++++-------------- 1 file changed, 22 insertions(+), 22 deletions(-) diff --git a/src/modules/mavlink/mavlink_main.cpp b/src/modules/mavlink/mavlink_main.cpp index 01949ab7e9..a6c701b178 100644 --- a/src/modules/mavlink/mavlink_main.cpp +++ b/src/modules/mavlink/mavlink_main.cpp @@ -319,7 +319,7 @@ Mavlink::Mavlink() : #endif default: - warnx("instance ID is out of range"); + PX4_WARN("instance ID is out of range"); px4_task_exit(1); break; } @@ -443,7 +443,7 @@ Mavlink::destroy_all_instances() unsigned iterations = 0; - warnx("waiting for instances to stop"); + PX4_INFO("waiting for instances to stop"); while (next_inst != nullptr) { inst_to_del = next_inst; @@ -474,7 +474,7 @@ Mavlink::destroy_all_instances() } printf("\n"); - warnx("all instances stopped"); + PX4_INFO("all instances stopped"); return OK; } @@ -785,7 +785,7 @@ int Mavlink::mavlink_open_uart(int baud, const char *uart_name) /* Initialize the uart config */ if ((termios_state = tcgetattr(_uart_fd, &uart_config)) < 0) { - warnx("ERR GET CONF %s: %d\n", uart_name, termios_state); + PX4_ERR("ERR GET CONF %s: %d\n", uart_name, termios_state); ::close(_uart_fd); return -1; } @@ -798,7 +798,7 @@ int Mavlink::mavlink_open_uart(int baud, const char *uart_name) /* Set baud rate */ if (cfsetispeed(&uart_config, speed) < 0 || cfsetospeed(&uart_config, speed) < 0) { - warnx("ERR SET BAUD %s: %d\n", uart_name, termios_state); + PX4_ERR("ERR SET BAUD %s: %d\n", uart_name, termios_state); ::close(_uart_fd); return -1; } @@ -1765,7 +1765,7 @@ Mavlink::task_main(int argc, char *argv[]) _baudrate = strtoul(myoptarg, nullptr, 10); if (_baudrate < 9600 || _baudrate > 3000000) { - warnx("invalid baud rate '%s'", myoptarg); + PX4_ERR("invalid baud rate '%s'", myoptarg); err_flag = true; } @@ -1775,7 +1775,7 @@ Mavlink::task_main(int argc, char *argv[]) _datarate = strtoul(myoptarg, nullptr, 10); if (_datarate < 10 || _datarate > MAX_DATA_RATE) { - warnx("invalid data rate '%s'", myoptarg); + PX4_ERR("invalid data rate '%s'", myoptarg); err_flag = true; } @@ -1796,7 +1796,7 @@ Mavlink::task_main(int argc, char *argv[]) set_protocol(UDP); } else { - warnx("invalid data udp_port '%s'", myoptarg); + PX4_ERR("invalid data udp_port '%s'", myoptarg); err_flag = true; } @@ -1810,7 +1810,7 @@ Mavlink::task_main(int argc, char *argv[]) set_protocol(UDP); } else { - warnx("invalid remote udp_port '%s'", myoptarg); + PX4_ERR("invalid remote udp_port '%s'", myoptarg); err_flag = true; } @@ -1823,7 +1823,7 @@ Mavlink::task_main(int argc, char *argv[]) _src_addr_initialized = true; } else { - warnx("invalid partner ip '%s'", myoptarg); + PX4_ERR("invalid partner ip '%s'", myoptarg); err_flag = true; } @@ -1833,7 +1833,7 @@ Mavlink::task_main(int argc, char *argv[]) case 'u': case 'o': case 't': - warnx("UDP options not supported on this platform"); + PX4_ERR("UDP options not supported on this platform"); err_flag = true; break; #endif @@ -1907,7 +1907,7 @@ Mavlink::task_main(int argc, char *argv[]) if (get_protocol() == SERIAL) { if (Mavlink::instance_exists(_device_name, this)) { - warnx("%s already running", _device_name); + PX4_ERR("%s already running", _device_name); return PX4_ERROR; } @@ -1921,7 +1921,7 @@ Mavlink::task_main(int argc, char *argv[]) _uart_fd = mavlink_open_uart(_baudrate, _device_name); if (_uart_fd < 0 && _mode != MAVLINK_MODE_CONFIG) { - warn("could not open %s", _device_name); + PX4_ERR("could not open %s", _device_name); return PX4_ERROR; } else if (_uart_fd < 0 && _mode == MAVLINK_MODE_CONFIG) { @@ -1931,7 +1931,7 @@ Mavlink::task_main(int argc, char *argv[]) } else if (get_protocol() == UDP) { if (Mavlink::get_instance_for_network_port(_network_port) != nullptr) { - warnx("port %d already occupied", _network_port); + PX4_ERR("port %d already occupied", _network_port); return PX4_ERROR; } @@ -1949,7 +1949,7 @@ Mavlink::task_main(int argc, char *argv[]) * marker ring buffer approach. */ if (OK != message_buffer_init(2 * sizeof(mavlink_message_t) + 1)) { - warnx("msg buf:"); + PX4_ERR("msg buf alloc fail"); return 1; } @@ -2387,7 +2387,7 @@ Mavlink::task_main(int argc, char *argv[]) _mavlink_ulog = nullptr; } - warnx("exiting channel %i", (int)_channel); + PX4_INFO("exiting channel %i", (int)_channel); return OK; } @@ -2454,7 +2454,7 @@ int Mavlink::start_helper(int argc, char *argv[]) /* out of memory */ res = -ENOMEM; - warnx("OUT OF MEM"); + PX4_ERR("OUT OF MEM"); } else { /* this will actually only return once MAVLink exits */ @@ -2477,8 +2477,8 @@ Mavlink::start(int argc, char *argv[]) int ic = Mavlink::instance_count(); if (ic == Mavlink::MAVLINK_MAX_INSTANCES) { - warnx("Maximum MAVLink instance count of %d reached.", - (int)Mavlink::MAVLINK_MAX_INSTANCES); + PX4_ERR("Maximum MAVLink instance count of %d reached.", + (int)Mavlink::MAVLINK_MAX_INSTANCES); return 1; } @@ -2669,7 +2669,7 @@ Mavlink::stream_command(int argc, char *argv[]) inst = get_instance_for_network_port(network_port); } else if (provided_device && provided_network_port) { - warnx("please provide either a device name or a network port"); + PX4_WARN("please provide either a device name or a network port"); return 1; } @@ -2681,10 +2681,10 @@ Mavlink::stream_command(int argc, char *argv[]) // If the link is not running we should complain, but not fall over // because this is so easy to get wrong and not fatal. Warning is sufficient. if (provided_device) { - warnx("mavlink for device %s is not running", device_name); + PX4_WARN("mavlink for device %s is not running", device_name); } else { - warnx("mavlink for network on port %hu is not running", network_port); + PX4_WARN("mavlink for network on port %hu is not running", network_port); } return 1;