fly316/PX4-Autopilot
7
0
Fork 0
mirror of https://gitee.com/mirrors_PX4/PX4-Autopilot.git synced 2026-10-15 22:13:33 +08:00
Code Issues Packages Projects Releases Wiki Activity

Mavlink NuttX network support (#10200)

Browse Source
This commit is contained in:
garfieldG
2018-09-18 23:29:58 -04:00
committed by Daniel Agar
parent 31f3bcfc3a
commit f14125c160
4 changed files with 149 additions and 28 deletions
Download Patch File Download Diff File Expand all files Collapse all files
src/modules/mavlink/mavlink_main.cpp
+112 -13
View File
@@ -57,6 +57,12 @@
#include <termios.h>
#include <time.h>
#ifdef CONFIG_NET
#include <arpa/inet.h>
#include <netinet/in.h>
#include <netutils/netlib.h>
#endif
#include <sys/ioctl.h>
#include <sys/types.h>
#include <sys/stat.h>
@@ -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", &eth_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(&param_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", "<file:dev>", "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, "<interface_name>", "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, "<file:dev>", "Select Mavlink instance via Serial Device", true);
src/modules/mavlink/mavlink_main.h
+15 -10
View File
@@ -50,20 +50,21 @@
#include <nuttx/fs/fs.h>
#else
#include <sys/socket.h>
#include <netinet/in.h>
#include <arpa/inet.h>
#include <drivers/device/device.h>
#endif
#if defined(CONFIG_NET) || !defined(__PX4_NUTTX)
#include <netinet/in.h>
#include <net/if.h>
#endif
#include <parameters/param.h>
#include <perf/perf_counter.h>
#include <pthread.h>
#include <systemlib/mavlink_log.h>
#include <drivers/device/ringbuffer.h>
#ifdef __PX4_POSIX
#include <net/if.h>
#endif
#include <uORB/uORB.h>
#include <uORB/topics/mission.h>
#include <uORB/topics/mission_result.h>
@@ -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);
src/modules/mavlink/mavlink_params.c
+1
View File
@@ -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);
src/modules/mavlink/mavlink_receiver.cpp
+21 -5
View File
@@ -67,6 +67,13 @@
#ifndef __PX4_POSIX
#include <termios.h>
#endif
#ifdef CONFIG_NET
#include <net/if.h>
#include <arpa/inet.h>
#include <netinet/in.h>
#endif
#include <errno.h>
#include <stdlib.h>
#include <poll.h>
@@ -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, &param);
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);