From f14125c160d10e84f3b57c3438bdcc48bba9969d Mon Sep 17 00:00:00 2001 From: garfieldG Date: Wed, 19 Sep 2018 06:29:58 +0300 Subject: [PATCH] Mavlink NuttX network support (#10200) --- src/modules/mavlink/mavlink_main.cpp | 125 ++++++++++++++++++++--- src/modules/mavlink/mavlink_main.h | 25 +++-- src/modules/mavlink/mavlink_params.c | 1 + src/modules/mavlink/mavlink_receiver.cpp | 26 ++++- 4 files changed, 149 insertions(+), 28 deletions(-) diff --git a/src/modules/mavlink/mavlink_main.cpp b/src/modules/mavlink/mavlink_main.cpp index 1bdd517d5f..f90f60e74a 100644 --- a/src/modules/mavlink/mavlink_main.cpp +++ b/src/modules/mavlink/mavlink_main.cpp @@ -57,6 +57,12 @@ #include #include +#ifdef CONFIG_NET +#include +#include +#include +#endif + #include #include #include @@ -96,6 +102,12 @@ #error The non-standard CRTSCTS define is incorrect. Fix this in the OS or replace with (CRTS_IFLOW | CCTS_OFLOW) #endif +#ifdef CONFIG_NET +#define MAVLINK_NET_ADDED_STACK 350 +#else +#define MAVLINK_NET_ADDED_STACK 0 +#endif + #define DEFAULT_REMOTE_PORT_UDP 14550 ///< GCS port per MAVLink spec #define DEFAULT_DEVICE_NAME "/dev/ttyS1" #define MAX_DATA_RATE 10000000 ///< max data rate in bytes/s @@ -236,7 +248,7 @@ Mavlink::Mavlink() : _bytes_txerr(0), _bytes_rx(0), _bytes_timestamp(0), -#ifdef __PX4_POSIX +#if defined(CONFIG_NET) || defined(__PX4_POSIX) _myaddr {}, _src_addr{}, _bcast_addr{}, @@ -887,7 +899,7 @@ Mavlink::send_packet() { int ret = -1; -#ifdef __PX4_POSIX +#if defined(CONFIG_NET) || defined(__PX4_POSIX) /* Only send packets if there is something in the buffer. */ if (_network_buf_len == 0) { @@ -897,8 +909,16 @@ Mavlink::send_packet() if (get_protocol() == UDP) { - ret = sendto(_socket_fd, _network_buf, _network_buf_len, 0, - (struct sockaddr *)&_src_addr, sizeof(_src_addr)); +#ifdef CONFIG_NET + + if (_src_addr_initialized) { +#endif + ret = sendto(_socket_fd, _network_buf, _network_buf_len, 0, + (struct sockaddr *)&_src_addr, sizeof(_src_addr)); +#ifdef CONFIG_NET + } + +#endif /* resend message via broadcast if no valid connection exists */ if ((_mode != MAVLINK_MODE_ONBOARD) && broadcast_enabled() && @@ -971,7 +991,7 @@ Mavlink::send_bytes(const uint8_t *buf, unsigned packet_len) ret = ::write(_uart_fd, buf, packet_len); } -#ifdef __PX4_POSIX +#if defined(CONFIG_NET) || defined(__PX4_POSIX) else { if (_network_buf_len + packet_len < sizeof(_network_buf) / sizeof(_network_buf[0])) { @@ -1107,6 +1127,49 @@ Mavlink::find_broadcast_address() } } +#elif defined (CONFIG_NET) && defined (__PX4_NUTTX) + int ret; + + PX4_INFO("using network interface"); + + struct in_addr eth_addr; + struct in_addr bc_addr; + struct in_addr netmask_addr; + ret = netlib_get_ipv4addr("eth0", ð_addr); + + if (ret != 0) { + PX4_ERR("getting network config failed"); + return; + } + + ret = netlib_get_ipv4netmask("eth0", &netmask_addr); + + if (ret != 0) { + PX4_ERR("getting network config failed"); + return; + } + + PX4_INFO("ipv4addr IP: %s", inet_ntoa(eth_addr)); + PX4_INFO("netmask_addr IP: %s", inet_ntoa(netmask_addr)); + + bc_addr.s_addr = eth_addr.s_addr | ~(netmask_addr.s_addr); + + if (!_broadcast_address_found) { + PX4_INFO("using network interface %s, IP: %s", "eth0", inet_ntoa(eth_addr)); + + //struct in_addr &bc_addr = ((struct sockaddr_in *)&bc_ifreq.ifr_broadaddr)->sin_addr; + PX4_INFO("with broadcast IP: %s", inet_ntoa(bc_addr)); + + _bcast_addr.sin_family = AF_INET; + _bcast_addr.sin_addr = bc_addr; + + _broadcast_address_found = true; + } + +#endif + +#if defined (__PX4_LINUX) || defined (__PX4_DARWIN) || (defined (CONFIG_NET) && defined (__PX4_NUTTX)) + if (_broadcast_address_found) { _bcast_addr.sin_port = htons(_remote_port); @@ -1125,7 +1188,9 @@ Mavlink::find_broadcast_address() } } +#if defined (__PX4_LINUX) || defined (__PX4_DARWIN) delete[] ifconf.ifc_req; +#endif #endif } @@ -1155,7 +1220,7 @@ Mavlink::compute_broadcast_addr(const in_addr &host_addr, const in_addr &netmask void Mavlink::init_udp() { -#if defined (__PX4_LINUX) || defined (__PX4_DARWIN) || defined(__PX4_CYGWIN) +#if defined (__PX4_LINUX) || defined (__PX4_DARWIN) || defined(__PX4_CYGWIN) || defined(CONFIG_NET) PX4_DEBUG("Setting up UDP with port %d", _network_port); @@ -1858,12 +1923,12 @@ Mavlink::task_main(int argc, char *argv[]) bool err_flag = false; int myoptind = 1; const char *myoptarg = nullptr; -#ifdef __PX4_POSIX +#if defined(CONFIG_NET) || defined(__PX4_POSIX) char *eptr; int temp_int_arg; #endif - while ((ch = px4_getopt(argc, argv, "b:r:d:n:u:o:m:t:fwxz", &myoptind, &myoptarg)) != EOF) { + while ((ch = px4_getopt(argc, argv, "b:r:d:n:u:o:m:t:c:fwxz", &myoptind, &myoptarg)) != EOF) { switch (ch) { case 'b': _baudrate = strtoul(myoptarg, nullptr, 10); @@ -1894,7 +1959,7 @@ Mavlink::task_main(int argc, char *argv[]) _interface_name = myoptarg; break; -#ifdef __PX4_POSIX +#if defined(CONFIG_NET) || defined(__PX4_POSIX) case 'u': temp_int_arg = strtoul(myoptarg, &eptr, 10); @@ -1936,6 +2001,29 @@ Mavlink::task_main(int argc, char *argv[]) } break; + +#if defined(CONFIG_NET_IGMP) && defined(CONFIG_NET_ROUTE) + + // multicast + case 'c': + _src_addr.sin_family = AF_INET; + + if (inet_aton(myoptarg, &_src_addr.sin_addr)) { + _src_addr_initialized = true; + + } else { + PX4_ERR("invalid partner ip '%s'", myoptarg); + err_flag = true; + } + + break; +#else + + case 'c': + PX4_ERR("Multicast option is not supported on this platform"); + err_flag = true; + break; +#endif #else case 'u': @@ -2154,6 +2242,14 @@ Mavlink::task_main(int argc, char *argv[]) if (param_sub->update(¶m_time, nullptr)) { mavlink_update_parameters(); + +#if defined(CONFIG_NET) + + if (_param_broadcast_mode.get() != BROADCAST_MODE_MULTICAST) { + _src_addr_initialized = false; + } + +#endif } check_radio_config(); @@ -2615,7 +2711,7 @@ Mavlink::start(int argc, char *argv[]) px4_task_spawn_cmd(buf, SCHED_DEFAULT, SCHED_PRIORITY_DEFAULT, - 2650, + 2650 + MAVLINK_NET_ADDED_STACK, (px4_main_t)&Mavlink::start_helper, (char *const *)argv); @@ -2886,7 +2982,7 @@ Mavlink::set_boot_complete() { _boot_complete = true; -#ifdef __PX4_POSIX +#if defined(CONFIG_NET) || defined(__PX4_POSIX) Mavlink *inst; LL_FOREACH(::_mavlink_instances, inst) { if ((inst->get_mode() != MAVLINK_MODE_ONBOARD) && @@ -2937,7 +3033,7 @@ $ mavlink stream -u 14556 -s HIGHRES_IMU -r 50 PRINT_MODULE_USAGE_PARAM_STRING('d', "/dev/ttyS1", "", "Select Serial Device", true); PRINT_MODULE_USAGE_PARAM_INT('b', 57600, 9600, 3000000, "Baudrate", true); PRINT_MODULE_USAGE_PARAM_INT('r', 0, 10, 10000000, "Maximum sending data rate in B/s (if 0, use baudrate / 20)", true); -#ifdef __PX4_POSIX +#if defined(CONFIG_NET) || defined(__PX4_POSIX) PRINT_MODULE_USAGE_PARAM_INT('u', 14556, 0, 65536, "Select UDP Network Port (local)", true); PRINT_MODULE_USAGE_PARAM_INT('o', 14550, 0, 65536, "Select UDP Network Port (remote)", true); PRINT_MODULE_USAGE_PARAM_STRING('t', "127.0.0.1", nullptr, @@ -2946,6 +3042,9 @@ $ mavlink stream -u 14556 -s HIGHRES_IMU -r 50 PRINT_MODULE_USAGE_PARAM_STRING('m', "normal", "custom|camera|onboard|osd|magic|config|iridium|minimal", "Mode: sets default streams and rates", true); PRINT_MODULE_USAGE_PARAM_STRING('n', nullptr, "", "wifi/ethernet interface name", true); +#if defined(CONFIG_NET_IGMP) && defined(CONFIG_NET_ROUTE) + PRINT_MODULE_USAGE_PARAM_STRING('c', nullptr, "Multicast address in the range [239.0.0.0,239.255.255.255]", "Multicast address (multicasting can be enabled via MAV_BROADCAST param)", true); +#endif PRINT_MODULE_USAGE_PARAM_FLAG('f', "Enable message forwarding to other Mavlink instances", true); PRINT_MODULE_USAGE_PARAM_FLAG('w', "Wait to send, until first message received", true); PRINT_MODULE_USAGE_PARAM_FLAG('x', "Enable FTP", true); @@ -2957,7 +3056,7 @@ $ mavlink stream -u 14556 -s HIGHRES_IMU -r 50 PRINT_MODULE_USAGE_ARG("streams", "Print all enabled streams", true); PRINT_MODULE_USAGE_COMMAND_DESCR("stream", "Configure the sending rate of a stream for a running instance"); -#ifdef __PX4_POSIX +#if defined(CONFIG_NET) || defined(__PX4_POSIX) PRINT_MODULE_USAGE_PARAM_INT('u', 0, 0, 65536, "Select Mavlink instance via local Network Port", true); #endif PRINT_MODULE_USAGE_PARAM_STRING('d', nullptr, "", "Select Mavlink instance via Serial Device", true); diff --git a/src/modules/mavlink/mavlink_main.h b/src/modules/mavlink/mavlink_main.h index 62501a943b..21cee2e67d 100644 --- a/src/modules/mavlink/mavlink_main.h +++ b/src/modules/mavlink/mavlink_main.h @@ -50,20 +50,21 @@ #include #else #include -#include #include #include #endif + +#if defined(CONFIG_NET) || !defined(__PX4_NUTTX) +#include +#include +#endif + #include #include #include #include #include -#ifdef __PX4_POSIX -#include -#endif - #include #include #include @@ -184,7 +185,8 @@ public: enum BROADCAST_MODE { BROADCAST_MODE_OFF = 0, - BROADCAST_MODE_ON + BROADCAST_MODE_ON, + BROADCAST_MODE_MULTICAST }; enum FLOW_CONTROL_MODE { @@ -423,9 +425,9 @@ public: */ telemetry_status_s &get_telemetry_status() { return _tstatus; } - void set_telemetry_status_type(uint8_t type) { _tstatus.type = type; } + void set_telemetry_status_type(uint8_t type) { _tstatus.type = type; } - void update_radio_status(const radio_status_s &radio_status); + void update_radio_status(const radio_status_s &radio_status); ringbuffer::RingBuffer *get_logbuffer() { return &_logbuffer; } @@ -438,11 +440,14 @@ public: unsigned short get_remote_port() { return _remote_port; } int get_socket_fd() { return _socket_fd; }; + #ifdef __PX4_POSIX 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; } @@ -589,7 +594,7 @@ private: unsigned _bytes_rx; uint64_t _bytes_timestamp; -#ifdef __PX4_POSIX +#if defined(CONFIG_NET) || defined(__PX4_POSIX) struct sockaddr_in _myaddr; struct sockaddr_in _src_addr; struct sockaddr_in _bcast_addr; @@ -640,7 +645,7 @@ private: perf_counter_t _loop_perf; /**< loop performance counter */ - void mavlink_update_parameters(); + void mavlink_update_parameters(); int mavlink_open_uart(int baudrate, const char *uart_name, bool force_flow_control); diff --git a/src/modules/mavlink/mavlink_params.c b/src/modules/mavlink/mavlink_params.c index ac85625e5a..310ed4e164 100644 --- a/src/modules/mavlink/mavlink_params.c +++ b/src/modules/mavlink/mavlink_params.c @@ -138,6 +138,7 @@ PARAM_DEFINE_INT32(MAV_FWDEXTSP, 1); * * @value 0 Never broadcast * @value 1 Always broadcast + * @value 2 Only multicast * @group MAVLink */ PARAM_DEFINE_INT32(MAV_BROADCAST, 0); diff --git a/src/modules/mavlink/mavlink_receiver.cpp b/src/modules/mavlink/mavlink_receiver.cpp index ed3307027c..9a7e2f3cb2 100644 --- a/src/modules/mavlink/mavlink_receiver.cpp +++ b/src/modules/mavlink/mavlink_receiver.cpp @@ -67,6 +67,13 @@ #ifndef __PX4_POSIX #include #endif + +#ifdef CONFIG_NET +#include +#include +#include +#endif + #include #include #include @@ -97,6 +104,12 @@ #include "mavlink_main.h" #include "mavlink_command_sender.h" +#ifdef CONFIG_NET +#define MAVLINK_RECEIVER_NET_ADDED_STACK 1360 +#else +#define MAVLINK_RECEIVER_NET_ADDED_STACK 0 +#endif + using matrix::wrap_2pi; MavlinkReceiver::MavlinkReceiver(Mavlink *parent) : @@ -2558,7 +2571,7 @@ void MavlinkReceiver::handle_message_debug_vect(mavlink_message_t *msg) } /** - * Receive data from UART. + * Receive data from UART/UDP */ void * MavlinkReceiver::receive_thread(void *arg) @@ -2574,9 +2587,12 @@ MavlinkReceiver::receive_thread(void *arg) // poll timeout in ms. Also defines the max update frequency of the mission & param manager, etc. const int timeout = 10; -#ifdef __PX4_POSIX +#if defined(__PX4_POSIX) /* 1500 is the Wifi MTU, so we make sure to fit a full packet */ uint8_t buf[1600 * 5]; +#elif defined(CONFIG_NET) + /* 1500 is the Wifi MTU, so we make sure to fit a full packet */ + uint8_t buf[1000]; #else /* the serial port buffers internally as well, we just need to fit a small chunk */ uint8_t buf[64]; @@ -2590,7 +2606,7 @@ MavlinkReceiver::receive_thread(void *arg) fds[0].events = POLLIN; } -#ifdef __PX4_POSIX +#if defined(CONFIG_NET) || defined(__PX4_POSIX) struct sockaddr_in srcaddr = {}; socklen_t addrlen = sizeof(srcaddr); @@ -2625,7 +2641,7 @@ MavlinkReceiver::receive_thread(void *arg) } } -#ifdef __PX4_POSIX +#if defined(CONFIG_NET) || defined(__PX4_POSIX) if (_mavlink->get_protocol() == UDP) { if (fds[0].revents & POLLIN) { @@ -2757,7 +2773,7 @@ MavlinkReceiver::receive_start(pthread_t *thread, Mavlink *parent) param.sched_priority = SCHED_PRIORITY_MAX - 80; (void)pthread_attr_setschedparam(&receiveloop_attr, ¶m); - pthread_attr_setstacksize(&receiveloop_attr, PX4_STACK_ADJUSTED(2840)); + pthread_attr_setstacksize(&receiveloop_attr, PX4_STACK_ADJUSTED(2840 + MAVLINK_RECEIVER_NET_ADDED_STACK)); pthread_create(thread, &receiveloop_attr, MavlinkReceiver::start_helper, (void *)parent); pthread_attr_destroy(&receiveloop_attr);