diff --git a/src/modules/mavlink/mavlink_main.cpp b/src/modules/mavlink/mavlink_main.cpp index 97a1468ec7..47d7554a4a 100644 --- a/src/modules/mavlink/mavlink_main.cpp +++ b/src/modules/mavlink/mavlink_main.cpp @@ -308,6 +308,10 @@ Mavlink::instance_count() Mavlink * Mavlink::get_instance_for_device(const char *device_name) { + if (device_name == nullptr) { + return nullptr; + } + LockGuard lg{mavlink_module_mutex}; for (Mavlink *inst : mavlink_module_instances) { @@ -417,6 +421,10 @@ Mavlink::get_status_all_instances(bool show_streams_status) bool Mavlink::serial_instance_exists(const char *device_name, Mavlink *self) { + if (device_name == nullptr) { + return false; + } + LockGuard lg{mavlink_module_mutex}; for (Mavlink *inst : mavlink_module_instances) { @@ -979,7 +987,7 @@ const in_addr Mavlink::compute_broadcast_addr(const in_addr &host_addr, const in return broadcast_addr; } -void Mavlink::init_udp() +bool Mavlink::init_udp() { PX4_DEBUG("Setting up UDP with port %hu", _network_port); @@ -988,13 +996,13 @@ void Mavlink::init_udp() _myaddr.sin_port = htons(_network_port); if ((_socket_fd = socket(AF_INET, SOCK_DGRAM, 0)) < 0) { - PX4_WARN("create socket failed: %s", strerror(errno)); - return; + PX4_ERR("create socket failed: %s", strerror(errno)); + return false; } if (bind(_socket_fd, (struct sockaddr *)&_myaddr, sizeof(_myaddr)) < 0) { - PX4_WARN("bind failed: %s", strerror(errno)); - return; + PX4_ERR("bind failed: %s", strerror(errno)); + return false; } /* set default target address, but not for onboard mode (will be set on first received packet) */ @@ -1004,6 +1012,8 @@ void Mavlink::init_udp() } _src_addr.sin_port = htons(_remote_port); + + return true; } #endif // MAVLINK_UDP @@ -1856,12 +1866,6 @@ int Mavlink::task_main(int argc, char *argv[]) { int ch; - _baudrate = 57600; - _datarate = 0; - _mode = MAVLINK_MODE_COUNT; - FLOW_CONTROL_MODE _flow_control = FLOW_CONTROL_AUTO; - - _interface_name = nullptr; // We don't care about the name and verb at this point. argc -= 2; @@ -1876,6 +1880,8 @@ Mavlink::task_main(int argc, char *argv[]) int temp_int_arg; #endif + FLOW_CONTROL_MODE flow_control = FLOW_CONTROL_AUTO; + while ((ch = px4_getopt(argc, argv, "b:r:d:n:u:o:m:t:c:fswxzZp", &myoptind, &myoptarg)) != EOF) { switch (ch) { case 'b': @@ -2002,7 +2008,7 @@ Mavlink::task_main(int argc, char *argv[]) case 'm': { - int mode; + int mode = -1; if (px4_get_parameter_value(myoptarg, mode) == 0) { if (mode >= 0 && mode < (int)MAVLINK_MODE_COUNT) { @@ -2081,11 +2087,11 @@ Mavlink::task_main(int argc, char *argv[]) break; case 'z': - _flow_control = FLOW_CONTROL_ON; + flow_control = FLOW_CONTROL_ON; break; case 'Z': - _flow_control = FLOW_CONTROL_OFF; + flow_control = FLOW_CONTROL_OFF; break; default: @@ -2099,29 +2105,6 @@ Mavlink::task_main(int argc, char *argv[]) return PX4_ERROR; } - /* USB serial is indicated by /dev/ttyACMx */ - if (strncmp(_device_name, "/dev/ttyACM", 11) == 0) { - if (_datarate == 0) { - _datarate = 100000; - } - - /* USB has no baudrate, but use a magic number for 'fast' */ - _baudrate = 2000000; - - if (_mode == MAVLINK_MODE_COUNT) { - _mode = MAVLINK_MODE_CONFIG; - } - - _ftp_on = true; - _is_usb_uart = true; - - set_telemetry_status_type(telemetry_status_s::LINK_TYPE_USB); - } - - if (_mode == MAVLINK_MODE_COUNT) { - _mode = MAVLINK_MODE_NORMAL; - } - if (_datarate == 0) { /* convert bits to bytes and use 1/2 of bandwidth by default */ _datarate = _baudrate / 20; @@ -2132,16 +2115,56 @@ Mavlink::task_main(int argc, char *argv[]) } if (get_protocol() == Protocol::SERIAL) { - if (Mavlink::serial_instance_exists(_device_name, this)) { - PX4_ERR("%s already running", _device_name); + if (_device_name) { + + if (Mavlink::serial_instance_exists(_device_name, this)) { + PX4_ERR("%s already running", _device_name); + return PX4_ERROR; + } + + if (access(_device_name, F_OK) != 0) { + PX4_ERR("invalid device (-d) %s", _device_name); + return PX4_ERROR; + + } else if (access(_device_name, R_OK | W_OK) != 0) { + PX4_ERR("unable to access device %s", _device_name); + return PX4_ERROR; + } + + // USB serial is indicated by /dev/ttyACMx + if (strncmp(_device_name, "/dev/ttyACM", 11) == 0) { + if (_datarate == 0) { + _datarate = 100000; + } + + /* USB has no baudrate, but use a magic number for 'fast' */ + _baudrate = 2000000; + + if (_mode == MAVLINK_MODE_COUNT) { + _mode = MAVLINK_MODE_CONFIG; + } + + _is_usb_uart = true; + + set_telemetry_status_type(telemetry_status_s::LINK_TYPE_USB); + } + + } else { + PX4_ERR("serial device not set"); + usage(); + return PX4_ERROR; + } + + // open the UART device after setting the instance, as it might block */ + _uart_fd = mavlink_open_uart(_baudrate, _device_name, flow_control); + + if (_uart_fd < 0) { + PX4_ERR("could not open %s", _device_name); return PX4_ERROR; } PX4_INFO("mode: %s, data rate: %d B/s on %s @ %dB", mavlink_mode_str(_mode), _datarate, _device_name, _baudrate); - - /* flush stdout in case MAVLink is about to take it over */ - fflush(stdout); } #if defined(MAVLINK_UDP) @@ -2152,6 +2175,11 @@ Mavlink::task_main(int argc, char *argv[]) return PX4_ERROR; } + // init socket + if (!init_udp()) { + return PX4_ERROR; + } + PX4_INFO("mode: %s, data rate: %d B/s on udp port %hu remote port %hu", mavlink_mode_str(_mode), _datarate, _network_port, _remote_port); } @@ -2195,7 +2223,6 @@ Mavlink::task_main(int argc, char *argv[]) /* Activate sending the data by default (for the IRIDIUM mode it will be disabled after the first round of packages is sent)*/ _transmitting_enabled = true; - _transmitting_enabled_commanded = true; if (_mode == MAVLINK_MODE_IRIDIUM) { _transmitting_enabled_commanded = false; @@ -2220,36 +2247,7 @@ Mavlink::task_main(int argc, char *argv[]) } /* set main loop delay depending on data rate to minimize CPU overhead */ - _main_loop_delay = (MAIN_LOOP_DELAY * 1000) / _datarate; - - /* hard limit to 1000 Hz at max */ - if (_main_loop_delay < MAVLINK_MIN_INTERVAL) { - _main_loop_delay = MAVLINK_MIN_INTERVAL; - } - - /* hard limit to 100 Hz at least */ - if (_main_loop_delay > MAVLINK_MAX_INTERVAL) { - _main_loop_delay = MAVLINK_MAX_INTERVAL; - } - - /* open the UART device after setting the instance, as it might block */ - if (get_protocol() == Protocol::SERIAL) { - _uart_fd = mavlink_open_uart(_baudrate, _device_name, _flow_control); - - if (_uart_fd < 0) { - PX4_ERR("could not open %s", _device_name); - return PX4_ERROR; - } - } - -#if defined(MAVLINK_UDP) - - /* init socket if necessary */ - if (get_protocol() == Protocol::UDP) { - init_udp(); - } - -#endif // MAVLINK_UDP + _main_loop_delay = math::constrain((MAIN_LOOP_DELAY * 1000) / _datarate, MAVLINK_MIN_INTERVAL, MAVLINK_MAX_INTERVAL); _task_id = px4_getpid(); @@ -3132,7 +3130,7 @@ Mavlink::stop_command(int argc, char *argv[]) int Mavlink::stream_command(int argc, char *argv[]) { - const char *device_name = DEFAULT_DEVICE_NAME; + const char *device_name = nullptr; float rate = -1.0f; const char *stream_name = nullptr; #ifdef MAVLINK_UDP diff --git a/src/modules/mavlink/mavlink_main.h b/src/modules/mavlink/mavlink_main.h index a3938f3408..5a9054e35f 100644 --- a/src/modules/mavlink/mavlink_main.h +++ b/src/modules/mavlink/mavlink_main.h @@ -86,7 +86,6 @@ #include "mavlink_ulog.h" #define DEFAULT_BAUD_RATE 57600 -#define DEFAULT_DEVICE_NAME "/dev/ttyS1" #define HASH_PARAM "_HASH_CHECK" @@ -195,7 +194,7 @@ public: */ int get_component_id() const { return mavlink_system.compid; } - const char *_device_name{DEFAULT_DEVICE_NAME}; + const char *_device_name{nullptr}; enum MAVLINK_MODE { MAVLINK_MODE_NORMAL = 0, @@ -535,7 +534,7 @@ private: px4::atomic_bool _task_running{false}; bool _transmitting_enabled{true}; - bool _transmitting_enabled_commanded{false}; + bool _transmitting_enabled_commanded{true}; bool _first_heartbeat_sent{false}; orb_advert_t _mavlink_log_pub{nullptr}; @@ -687,8 +686,7 @@ private: void mavlink_update_parameters(); - int mavlink_open_uart(const int baudrate = DEFAULT_BAUD_RATE, - const char *uart_name = DEFAULT_DEVICE_NAME, + int mavlink_open_uart(const int baudrate, const char *uart_name, const FLOW_CONTROL_MODE flow_control = FLOW_CONTROL_AUTO); static constexpr unsigned RADIO_BUFFER_CRITICAL_LOW_PERCENTAGE = 25; @@ -747,7 +745,7 @@ private: #if defined(MAVLINK_UDP) void find_broadcast_address(); - void init_udp(); + bool init_udp(); #endif // MAVLINK_UDP