diff --git a/src/modules/mavlink/mavlink_main.cpp b/src/modules/mavlink/mavlink_main.cpp index 0c3a7d76f7..8f4e240357 100644 --- a/src/modules/mavlink/mavlink_main.cpp +++ b/src/modules/mavlink/mavlink_main.cpp @@ -333,6 +333,7 @@ Mavlink::get_instance_for_device(const char *device_name) return nullptr; } +#ifdef MAVLINK_UDP Mavlink * Mavlink::get_instance_for_network_port(unsigned long port) { @@ -346,6 +347,7 @@ Mavlink::get_instance_for_network_port(unsigned long port) return nullptr; } +#endif // MAVLINK_UDP int Mavlink::destroy_all_instances() @@ -426,7 +428,7 @@ Mavlink::serial_instance_exists(const char *device_name, Mavlink *self) while (inst != nullptr) { /* don't compare with itself and with non serial instances*/ - if ((inst != self) && (inst->get_protocol() == SERIAL) && !strcmp(device_name, inst->_device_name)) { + if ((inst != self) && (inst->get_protocol() == Protocol::SERIAL) && !strcmp(device_name, inst->_device_name)) { return true; } @@ -749,11 +751,15 @@ Mavlink::get_free_tx_buf() */ int buf_free = 0; +#if defined(MAVLINK_UDP) + // if we are using network sockets, return max length of one packet - if (get_protocol() == UDP || get_protocol() == TCP) { + if (get_protocol() == Protocol::UDP) { return 1500; - } else { + } else +#endif // MAVLINK_UDP + { // No FIONSPACE on Linux todo:use SIOCOUTQ and queue size to emulate FIONSPACE #if defined(__PX4_LINUX) || defined(__PX4_DARWIN) || defined(__PX4_CYGWIN) //Linux cp210x does not support TIOCOUTQ @@ -784,7 +790,7 @@ Mavlink::send_packet() { int ret = -1; -#if defined(CONFIG_NET) || defined(__PX4_POSIX) +#if defined(MAVLINK_UDP) /* Only send packets if there is something in the buffer. */ if (_network_buf_len == 0) { @@ -792,7 +798,7 @@ Mavlink::send_packet() return 0; } - if (get_protocol() == UDP) { + if (get_protocol() == Protocol::UDP) { #ifdef CONFIG_NET @@ -831,13 +837,11 @@ Mavlink::send_packet() } } - } else if (get_protocol() == TCP) { - /* not implemented, but possible to do so */ - PX4_ERR("TCP transport pending implementation"); } _network_buf_len = 0; -#endif + +#endif // MAVLINK_UDP pthread_mutex_unlock(&_send_mutex); return ret; @@ -852,7 +856,7 @@ Mavlink::send_bytes(const uint8_t *buf, unsigned packet_len) _mavlink_start_time = _last_write_try_time; } - if (get_protocol() == SERIAL) { + if (get_protocol() == Protocol::SERIAL) { /* check if there is space in the buffer, let it overflow else */ unsigned buf_free = get_free_tx_buf(); @@ -866,11 +870,11 @@ Mavlink::send_bytes(const uint8_t *buf, unsigned packet_len) size_t ret = -1; /* send message to UART */ - if (get_protocol() == SERIAL) { + if (get_protocol() == Protocol::SERIAL) { ret = ::write(_uart_fd, buf, packet_len); } -#if defined(CONFIG_NET) || defined(__PX4_POSIX) +#if defined(MAVLINK_UDP) else { if (_network_buf_len + packet_len < sizeof(_network_buf) / sizeof(_network_buf[0])) { @@ -881,7 +885,7 @@ Mavlink::send_bytes(const uint8_t *buf, unsigned packet_len) } } -#endif +#endif // MAVLINK_UDP if (ret != (size_t) packet_len) { count_txerrbytes(packet_len); @@ -892,6 +896,7 @@ Mavlink::send_bytes(const uint8_t *buf, unsigned packet_len) } } +#ifdef MAVLINK_UDP void Mavlink::find_broadcast_address() { @@ -1073,6 +1078,7 @@ Mavlink::find_broadcast_address() #endif } +#endif // MAVLINK_UDP #ifdef __PX4_POSIX const in_addr @@ -1096,11 +1102,10 @@ Mavlink::compute_broadcast_addr(const in_addr &host_addr, const in_addr &netmask } #endif +#ifdef MAVLINK_UDP void Mavlink::init_udp() { -#if defined (__PX4_LINUX) || defined (__PX4_DARWIN) || defined(__PX4_CYGWIN) || defined(CONFIG_NET) - PX4_DEBUG("Setting up UDP with port %d", _network_port); _myaddr.sin_family = AF_INET; @@ -1124,9 +1129,8 @@ Mavlink::init_udp() } _src_addr.sin_port = htons(_remote_port); - -#endif } +#endif // MAVLINK_UDP void Mavlink::handle_message(const mavlink_message_t *msg) @@ -1891,21 +1895,21 @@ Mavlink::task_main(int argc, char *argv[]) case 'd': _device_name = myoptarg; - set_protocol(SERIAL); + set_protocol(Protocol::SERIAL); break; case 'n': _interface_name = myoptarg; break; -#if defined(CONFIG_NET) || defined(__PX4_POSIX) +#if defined(MAVLINK_UDP) case 'u': temp_int_arg = strtoul(myoptarg, &eptr, 10); if (*eptr == '\0') { _network_port = temp_int_arg; - set_protocol(UDP); + set_protocol(Protocol::UDP); } else { PX4_ERR("invalid data udp_port '%s'", myoptarg); @@ -1919,7 +1923,7 @@ Mavlink::task_main(int argc, char *argv[]) if (*eptr == '\0') { _remote_port = temp_int_arg; - set_protocol(UDP); + set_protocol(Protocol::UDP); } else { PX4_ERR("invalid remote udp_port '%s'", myoptarg); @@ -2068,7 +2072,7 @@ Mavlink::task_main(int argc, char *argv[]) _datarate = MAX_DATA_RATE; } - if (get_protocol() == SERIAL) { + if (get_protocol() == Protocol::SERIAL) { if (Mavlink::serial_instance_exists(_device_name, this)) { PX4_ERR("%s already running", _device_name); return PX4_ERROR; @@ -2092,7 +2096,11 @@ Mavlink::task_main(int argc, char *argv[]) return OK; } - } else if (get_protocol() == UDP) { + } + +#if defined(MAVLINK_UDP) + + else if (get_protocol() == Protocol::UDP) { if (Mavlink::get_instance_for_network_port(_network_port) != nullptr) { PX4_ERR("port %d already occupied", _network_port); return PX4_ERROR; @@ -2102,6 +2110,8 @@ Mavlink::task_main(int argc, char *argv[]) mavlink_mode_str(_mode), _datarate, _network_port, _remote_port); } +#endif // MAVLINK_UDP + /* initialize send mutex */ pthread_mutex_init(&_send_mutex, nullptr); @@ -2185,13 +2195,17 @@ Mavlink::task_main(int argc, char *argv[]) /* now the instance is fully initialized and we can bump the instance count */ LL_APPEND(_mavlink_instances, this); +#if defined(MAVLINK_UDP) + /* init socket if necessary */ - if (get_protocol() == UDP) { + if (get_protocol() == Protocol::UDP) { init_udp(); } +#endif // MAVLINK_UDP + /* if the protocol is serial, we send the system version blindly */ - if (get_protocol() == SERIAL) { + if (get_protocol() == Protocol::SERIAL) { send_autopilot_capabilites(); } @@ -2360,44 +2374,67 @@ Mavlink::task_main(int argc, char *argv[]) if (_subscribe_to_stream != nullptr) { if (_subscribe_to_stream_rate < -1.5f) { if (configure_streams_to_default(_subscribe_to_stream) == 0) { - if (get_protocol() == SERIAL) { + if (get_protocol() == Protocol::SERIAL) { PX4_DEBUG("stream %s on device %s set to default rate", _subscribe_to_stream, _device_name); + } - } else if (get_protocol() == UDP) { +#if defined(MAVLINK_UDP) + + else if (get_protocol() == Protocol::UDP) { PX4_DEBUG("stream %s on UDP port %d set to default rate", _subscribe_to_stream, _network_port); } +#endif // MAVLINK_UDP + } else { PX4_ERR("setting stream %s to default failed", _subscribe_to_stream); } } else if (configure_stream(_subscribe_to_stream, _subscribe_to_stream_rate) == 0) { if (fabsf(_subscribe_to_stream_rate) > 0.00001f) { - if (get_protocol() == SERIAL) { + if (get_protocol() == Protocol::SERIAL) { PX4_DEBUG("stream %s on device %s enabled with rate %.1f Hz", _subscribe_to_stream, _device_name, (double)_subscribe_to_stream_rate); - } else if (get_protocol() == UDP) { + } + +#if defined(MAVLINK_UDP) + + else if (get_protocol() == Protocol::UDP) { PX4_DEBUG("stream %s on UDP port %d enabled with rate %.1f Hz", _subscribe_to_stream, _network_port, (double)_subscribe_to_stream_rate); } +#endif // MAVLINK_UDP + } else { - if (get_protocol() == SERIAL) { + if (get_protocol() == Protocol::SERIAL) { PX4_DEBUG("stream %s on device %s disabled", _subscribe_to_stream, _device_name); - } else if (get_protocol() == UDP) { + } + +#if defined(MAVLINK_UDP) + + else if (get_protocol() == Protocol::UDP) { PX4_DEBUG("stream %s on UDP port %d disabled", _subscribe_to_stream, _network_port); } + +#endif // MAVLINK_UDP } } else { - if (get_protocol() == SERIAL) { + if (get_protocol() == Protocol::SERIAL) { PX4_ERR("stream %s on device %s not found", _subscribe_to_stream, _device_name); - } else if (get_protocol() == UDP) { + } + +#if defined(MAVLINK_UDP) + + else if (get_protocol() == Protocol::UDP) { PX4_ERR("stream %s on UDP port %d not found", _subscribe_to_stream, _network_port); } + +#endif // MAVLINK_UDP } _subscribe_to_stream = nullptr; @@ -2747,7 +2784,9 @@ Mavlink::display_status() printf("\ttransport protocol: "); switch (_protocol) { - case UDP: +#if defined(MAVLINK_UDP) + + case Protocol::UDP: printf("UDP (%i, remote port: %i)\n", _network_port, _remote_port); #ifdef __PX4_POSIX @@ -2757,12 +2796,9 @@ Mavlink::display_status() #endif break; +#endif // MAVLINK_UDP - case TCP: - printf("TCP\n"); - break; - - case SERIAL: + case Protocol::SERIAL: printf("serial (%s @%i)\n", _device_name, _baudrate); break; } @@ -2817,11 +2853,14 @@ Mavlink::stream_command(int argc, char *argv[]) const char *device_name = DEFAULT_DEVICE_NAME; float rate = -1.0f; const char *stream_name = nullptr; - unsigned short network_port = 0; +#ifdef MAVLINK_UDP char *eptr; int temp_int_arg; + unsigned short network_port = 0; +#endif // MAVLINK_UDP bool provided_device = false; bool provided_network_port = false; + /* * Called via main with original argv * mavlink start @@ -2857,6 +2896,8 @@ Mavlink::stream_command(int argc, char *argv[]) stream_name = argv[i + 1]; i++; +#ifdef MAVLINK_UDP + } else if (0 == strcmp(argv[i], "-u") && i < argc - 1) { provided_network_port = true; temp_int_arg = strtoul(argv[i + 1], &eptr, 10); @@ -2869,6 +2910,7 @@ Mavlink::stream_command(int argc, char *argv[]) } i++; +#endif // MAVLINK_UDP } else { err_flag = true; @@ -2884,8 +2926,11 @@ Mavlink::stream_command(int argc, char *argv[]) if (provided_device && !provided_network_port) { inst = get_instance_for_device(device_name); +#ifdef MAVLINK_UDP + } else if (provided_network_port && !provided_device) { inst = get_instance_for_network_port(network_port); +#endif // MAVLINK_UDP } else if (provided_device && provided_network_port) { PX4_WARN("please provide either a device name or a network port"); @@ -2906,10 +2951,16 @@ Mavlink::stream_command(int argc, char *argv[]) if (provided_device) { PX4_WARN("mavlink for device %s is not running", device_name); - } else { + } + +#ifdef MAVLINK_UDP + + else { PX4_WARN("mavlink for network on port %hu is not running", network_port); } +#endif // MAVLINK_UDP + return 1; } @@ -2926,16 +2977,16 @@ Mavlink::set_boot_complete() { _boot_complete = true; -#if defined(CONFIG_NET) || defined(__PX4_POSIX) +#if defined(MAVLINK_UDP) Mavlink *inst; LL_FOREACH(::_mavlink_instances, inst) { if ((inst->get_mode() != MAVLINK_MODE_ONBOARD) && - (!inst->broadcast_enabled()) && - ((inst->get_protocol() == UDP) || (inst->get_protocol() == TCP))) { + !inst->broadcast_enabled() && inst->get_protocol() == Protocol::UDP) { + PX4_INFO("MAVLink only on localhost (set param MAV_BROADCAST = 1 to enable network)"); } } -#endif +#endif // MAVLINK_UDP } diff --git a/src/modules/mavlink/mavlink_main.h b/src/modules/mavlink/mavlink_main.h index e222846ff3..04ced7f353 100644 --- a/src/modules/mavlink/mavlink_main.h +++ b/src/modules/mavlink/mavlink_main.h @@ -84,14 +84,20 @@ #include "mavlink_ulog.h" #define DEFAULT_BAUD_RATE 57600 -#define DEFAULT_REMOTE_PORT_UDP 14550 ///< GCS port per MAVLink spec #define DEFAULT_DEVICE_NAME "/dev/ttyS1" + #define HASH_PARAM "_HASH_CHECK" -enum Protocol { +#if defined(CONFIG_NET) || defined(__PX4_POSIX) +# define MAVLINK_UDP +# define DEFAULT_REMOTE_PORT_UDP 14550 ///< GCS port per MAVLink spec +#endif // CONFIG_NET || __PX4_POSIX + +enum class Protocol { SERIAL = 0, +#if defined(MAVLINK_UDP) UDP, - TCP, +#endif // MAVLINK_UDP }; using namespace time_literals; @@ -137,8 +143,6 @@ public: static Mavlink *get_instance_for_device(const char *device_name); - static Mavlink *get_instance_for_network_port(unsigned long port); - mavlink_message_t *get_buffer() { return &_mavlink_buffer; } mavlink_status_t *get_status() { return &_mavlink_status; } @@ -259,9 +263,11 @@ public: bool is_connected() { return (hrt_elapsed_time(&_tstatus.heartbeat_time) < 3_s); } -#if defined(CONFIG_NET) || defined(__PX4_POSIX) +#if defined(MAVLINK_UDP) + static Mavlink *get_instance_for_network_port(unsigned long port); + bool broadcast_enabled() { return _param_mav_broadcast.get() == BROADCAST_MODE_ON; } -#endif +#endif // MAVLINK_UDP /** * Set the boot complete flag on all instances @@ -450,30 +456,26 @@ public: unsigned get_system_type() { return _param_mav_type.get(); } - Protocol get_protocol() { return _protocol; } - - unsigned short get_network_port() { return _network_port; } - - unsigned short get_remote_port() { return _remote_port; } + Protocol get_protocol() const { return _protocol; } int get_socket_fd() { return _socket_fd; }; bool _task_should_exit{false}; /**< Mavlink task should exit iff true. */ -#ifdef __PX4_POSIX +#if defined(MAVLINK_UDP) + unsigned short get_network_port() { return _network_port; } + + unsigned short get_remote_port() { return _remote_port; } + const in_addr query_netmask_addr(const int socket_fd, const ifreq &ifreq); const in_addr compute_broadcast_addr(const in_addr &host_addr, const in_addr &netmask_addr); -#endif -#if defined(CONFIG_NET) || defined(__PX4_POSIX) struct sockaddr_in &get_client_source_address() { return _src_addr; } void set_client_source_initialized() { _src_addr_initialized = true; } bool get_client_source_initialized() { return _src_addr_initialized; } -#else - bool get_client_source_initialized() { return true; } #endif uint64_t get_start_time() { return _mavlink_start_time; } @@ -618,7 +620,7 @@ private: unsigned _bytes_rx{0}; uint64_t _bytes_timestamp{0}; -#if defined(CONFIG_NET) || defined(__PX4_POSIX) +#if defined(MAVLINK_UDP) sockaddr_in _myaddr {}; sockaddr_in _src_addr {}; sockaddr_in _bcast_addr {}; @@ -629,14 +631,15 @@ private: bool _broadcast_failed_warned{false}; uint8_t _network_buf[MAVLINK_MAX_PACKET_LEN] {}; unsigned _network_buf_len{0}; -#endif + + unsigned short _network_port{14556}; + unsigned short _remote_port{DEFAULT_REMOTE_PORT_UDP}; +#endif // MAVLINK_UDP const char *_interface_name{nullptr}; int _socket_fd{-1}; - Protocol _protocol{SERIAL}; - unsigned short _network_port{14556}; - unsigned short _remote_port{DEFAULT_REMOTE_PORT_UDP}; + Protocol _protocol{Protocol::SERIAL}; radio_status_s _rstatus {}; telemetry_status_s _tstatus {}; @@ -663,9 +666,9 @@ private: (ParamInt) _param_mav_type, (ParamBool) _param_mav_usehilgps, (ParamBool) _param_mav_fwdextsp, -#if defined(CONFIG_NET) || defined(__PX4_POSIX) +#if defined(MAVLINK_UDP) (ParamInt) _param_mav_broadcast, -#endif +#endif // MAVLINK_UDP (ParamBool) _param_mav_hash_chk_en, (ParamBool) _param_mav_hb_forw_en, (ParamBool) _param_mav_odom_lp, @@ -730,9 +733,12 @@ private: */ void update_rate_mult(); +#if defined(MAVLINK_UDP) void find_broadcast_address(); void init_udp(); +#endif // MAVLINK_UDP + void set_channel(); diff --git a/src/modules/mavlink/mavlink_parameters.cpp b/src/modules/mavlink/mavlink_parameters.cpp index b7a03f4ad6..5e9b55ee46 100644 --- a/src/modules/mavlink/mavlink_parameters.cpp +++ b/src/modules/mavlink/mavlink_parameters.cpp @@ -272,11 +272,11 @@ MavlinkParametersManager::send(const hrt_abstime t) { int max_num_to_send; - if (_mavlink->get_protocol() == SERIAL && !_mavlink->is_usb_uart()) { + if (_mavlink->get_protocol() == Protocol::SERIAL && !_mavlink->is_usb_uart()) { max_num_to_send = 3; } else { - // speed up parameter loading via UDP, TCP or USB: try to send 20 at once + // speed up parameter loading via UDP or USB: try to send 20 at once max_num_to_send = 20; } diff --git a/src/modules/mavlink/mavlink_receiver.cpp b/src/modules/mavlink/mavlink_receiver.cpp index 941072c487..d22333e955 100644 --- a/src/modules/mavlink/mavlink_receiver.cpp +++ b/src/modules/mavlink/mavlink_receiver.cpp @@ -2549,21 +2549,22 @@ MavlinkReceiver::Run() struct pollfd fds[1] = {}; - if (_mavlink->get_protocol() == SERIAL) { + if (_mavlink->get_protocol() == Protocol::SERIAL) { fds[0].fd = _mavlink->get_uart_fd(); fds[0].events = POLLIN; } -#if defined(CONFIG_NET) || defined(__PX4_POSIX) +#if defined(MAVLINK_UDP) struct sockaddr_in srcaddr = {}; socklen_t addrlen = sizeof(srcaddr); - if (_mavlink->get_protocol() == UDP || _mavlink->get_protocol() == TCP) { + if (_mavlink->get_protocol() == Protocol::UDP) { fds[0].fd = _mavlink->get_socket_fd(); fds[0].events = POLLIN; } -#endif +#endif // MAVLINK_UDP + ssize_t nread = 0; hrt_abstime last_send_update = 0; @@ -2574,7 +2575,7 @@ MavlinkReceiver::Run() } if (poll(&fds[0], 1, timeout) > 0) { - if (_mavlink->get_protocol() == SERIAL) { + if (_mavlink->get_protocol() == Protocol::SERIAL) { /* * to avoid reading very small chunks wait for data before reading @@ -2589,44 +2590,41 @@ MavlinkReceiver::Run() } } -#if defined(CONFIG_NET) || defined(__PX4_POSIX) +#if defined(MAVLINK_UDP) - if (_mavlink->get_protocol() == UDP) { + else if (_mavlink->get_protocol() == Protocol::UDP) { if (fds[0].revents & POLLIN) { nread = recvfrom(_mavlink->get_socket_fd(), buf, sizeof(buf), 0, (struct sockaddr *)&srcaddr, &addrlen); } - } else { - // could be TCP or other protocol - } + struct sockaddr_in &srcaddr_last = _mavlink->get_client_source_address(); - struct sockaddr_in &srcaddr_last = _mavlink->get_client_source_address(); + int localhost = (127 << 24) + 1; - int localhost = (127 << 24) + 1; + if (!_mavlink->get_client_source_initialized()) { - if (!_mavlink->get_client_source_initialized()) { + // set the address either if localhost or if 3 seconds have passed + // this ensures that a GCS running on localhost can get a hold of + // the system within the first N seconds + hrt_abstime stime = _mavlink->get_start_time(); - // set the address either if localhost or if 3 seconds have passed - // this ensures that a GCS running on localhost can get a hold of - // the system within the first N seconds - hrt_abstime stime = _mavlink->get_start_time(); + if ((stime != 0 && (hrt_elapsed_time(&stime) > 3_s)) + || (srcaddr_last.sin_addr.s_addr == htonl(localhost))) { - if ((stime != 0 && (hrt_elapsed_time(&stime) > 3_s)) - || (srcaddr_last.sin_addr.s_addr == htonl(localhost))) { + srcaddr_last.sin_addr.s_addr = srcaddr.sin_addr.s_addr; + srcaddr_last.sin_port = srcaddr.sin_port; - srcaddr_last.sin_addr.s_addr = srcaddr.sin_addr.s_addr; - srcaddr_last.sin_port = srcaddr.sin_port; + _mavlink->set_client_source_initialized(); - _mavlink->set_client_source_initialized(); - - PX4_INFO("partner IP: %s", inet_ntoa(srcaddr.sin_addr)); + PX4_INFO("partner IP: %s", inet_ntoa(srcaddr.sin_addr)); + } } } -#endif - // only start accepting messages once we're sure who we talk to if (_mavlink->get_client_source_initialized()) { +#endif // MAVLINK_UDP + /* if read failed, this loop won't execute */ for (ssize_t i = 0; i < nread; i++) { if (mavlink_parse_char(_mavlink->get_channel(), buf[i], &msg, &_status)) { @@ -2667,7 +2665,11 @@ MavlinkReceiver::Run() if (nread > 0) { _mavlink->count_rxbytes(nread); } + +#if defined(MAVLINK_UDP) } + +#endif // MAVLINK_UDP } hrt_abstime t = hrt_absolute_time();