fly316/PX4-Autopilot
7
0
Fork 0
mirror of https://gitee.com/mirrors_PX4/PX4-Autopilot.git synced 2026-07-16 12:50:34 +08:00
Code Issues Packages Projects Releases Wiki Activity

Added support in Mavlink Ethernet channel parameters

Browse Source 
Mavlink Ethernet channel settings such as udp port, remote port and broadcast mode now can be changed dynamically via parameters.
This commit is contained in:
garfieldG
2021-01-31 05:36:27 -08:00
committed by David Sidrane
parent b66a9629e0
commit 3cd9b3c2cf
36 changed files with 242 additions and 85 deletions
Download Patch File Download Diff File Expand all files Collapse all files
src/modules/mavlink/mavlink_main.cpp
+31 -25
View File
@@ -1809,11 +1809,10 @@ Mavlink::task_main(int argc, char *argv[])
int myoptind = 1;
const char *myoptarg = nullptr;
#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:c:fswxzZ", &myoptind, &myoptarg)) != EOF) {
while ((ch = px4_getopt(argc, argv, "b:r:d:n:u:o:m:t:c:fswxzZp", &myoptind, &myoptarg)) != EOF) {
switch (ch) {
case 'b':
if (px4_get_parameter_value(myoptarg, _baudrate) != 0) {
@@ -1859,29 +1858,25 @@ Mavlink::task_main(int argc, char *argv[])
#if defined(MAVLINK_UDP)
case 'u':
temp_int_arg = strtoul(myoptarg, &eptr, 10);
if (*eptr == '\0') {
_network_port = temp_int_arg;
set_protocol(Protocol::UDP);
if (px4_get_parameter_value(myoptarg, temp_int_arg) != 0) {
PX4_ERR("invalid data udp_port");
err_flag = true;
} else {
PX4_ERR("invalid data udp_port '%s'", myoptarg);
err_flag = true;
_network_port = temp_int_arg;
set_protocol(Protocol::UDP);
}
break;
case 'o':
temp_int_arg = strtoul(myoptarg, &eptr, 10);
if (*eptr == '\0') {
_remote_port = temp_int_arg;
set_protocol(Protocol::UDP);
if (px4_get_parameter_value(myoptarg, temp_int_arg) != 0) {
PX4_ERR("invalid remote udp_port");
err_flag = true;
} else {
PX4_ERR("invalid remote udp_port '%s'", myoptarg);
err_flag = true;
_remote_port = temp_int_arg;
set_protocol(Protocol::UDP);
}
break;
@@ -1899,6 +1894,10 @@ Mavlink::task_main(int argc, char *argv[])
break;
case 'p':
_mav_broadcast = BROADCAST_MODE_ON;
break;
#if defined(CONFIG_NET_IGMP) && defined(CONFIG_NET_ROUTE)
// multicast
@@ -1907,6 +1906,7 @@ Mavlink::task_main(int argc, char *argv[])
if (inet_aton(myoptarg, &_src_addr.sin_addr)) {
_src_addr_initialized = true;
_mav_broadcast = BROADCAST_MODE_MULTICAST;
} else {
PX4_ERR("invalid partner ip '%s'", myoptarg);
@@ -1923,6 +1923,7 @@ Mavlink::task_main(int argc, char *argv[])
#endif
#else
case 'p':
case 'u':
case 'o':
case 't':
@@ -2206,7 +2207,7 @@ Mavlink::task_main(int argc, char *argv[])
#if defined(CONFIG_NET)
if (_param_mav_broadcast.get() != BROADCAST_MODE_MULTICAST) {
if (!multicast_enabled()) {
_src_addr_initialized = false;
}
@@ -2767,6 +2768,12 @@ Mavlink::display_status()
case Protocol::UDP:
printf("UDP (%i, remote port: %i)\n", _network_port, _remote_port);
printf("\tBroadcast enabled: %s\n",
broadcast_enabled() ? "YES" : "NO");
#if defined(CONFIG_NET_IGMP) && defined(CONFIG_NET_ROUTE)
printf("\tMulticast enabled: %s\n",
multicast_enabled() ? "YES" : "NO");
#endif
#ifdef __PX4_POSIX
if (get_client_source_initialized()) {
@@ -2833,7 +2840,6 @@ Mavlink::stream_command(int argc, char *argv[])
float rate = -1.0f;
const char *stream_name = nullptr;
#ifdef MAVLINK_UDP
char *eptr;
int temp_int_arg;
unsigned short network_port = 0;
#endif // MAVLINK_UDP
@@ -2879,13 +2885,12 @@ Mavlink::stream_command(int argc, char *argv[])
} else if (0 == strcmp(argv[i], "-u") && i < argc - 1) {
provided_network_port = true;
temp_int_arg = strtoul(argv[i + 1], &eptr, 10);
if (*eptr == '\0') {
network_port = temp_int_arg;
if (px4_get_parameter_value(argv[i + 1], temp_int_arg) != 0) {
err_flag = true;
} else {
err_flag = true;
network_port = temp_int_arg;
}
i++;
@@ -2963,7 +2968,7 @@ Mavlink::set_boot_complete()
if (inst && (inst->get_mode() != MAVLINK_MODE_ONBOARD) &&
!inst->broadcast_enabled() && inst->get_protocol() == Protocol::UDP) {
PX4_INFO("MAVLink only on localhost (set param MAV_BROADCAST = 1 to enable network)");
PX4_INFO("MAVLink only on localhost (set param MAV_{i}_BROADCAST = 1 to enable network)");
}
}
@@ -3010,16 +3015,17 @@ $ mavlink stream -u 14556 -s HIGHRES_IMU -r 50
PRINT_MODULE_USAGE_PARAM_INT('b', 57600, 9600, 3000000, "Baudrate (can also be p:<param_name>)", true);
PRINT_MODULE_USAGE_PARAM_INT('r', 0, 10, 10000000, "Maximum sending data rate in B/s (if 0, use baudrate / 20)", true);
#if defined(CONFIG_NET) || defined(__PX4_POSIX)
PRINT_MODULE_USAGE_PARAM_FLAG('p', "Enable Broadcast", true);
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,
"Partner IP (broadcasting can be enabled via MAV_BROADCAST param)", true);
"Partner IP (broadcasting can be enabled via MAV_{i}_BROADCAST param)", true);
#endif
PRINT_MODULE_USAGE_PARAM_STRING('m', "normal", "custom|camera|onboard|osd|magic|config|iridium|minimal|extvision|extvisionmin|gimbal",
"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);
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_{i}_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);
src/modules/mavlink/mavlink_main.h
+5 -4
View File
@@ -265,7 +265,9 @@ public:
#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; }
bool broadcast_enabled() { return _mav_broadcast == BROADCAST_MODE_ON; }
bool multicast_enabled() { return _mav_broadcast == BROADCAST_MODE_MULTICAST; }
#endif // MAVLINK_UDP
/**
@@ -610,6 +612,8 @@ private:
hrt_abstime _bytes_timestamp{0};
#if defined(MAVLINK_UDP)
BROADCAST_MODE _mav_broadcast {BROADCAST_MODE_OFF};
sockaddr_in _myaddr {};
sockaddr_in _src_addr {};
sockaddr_in _bcast_addr {};
@@ -659,9 +663,6 @@ private:
(ParamInt<px4::params::MAV_TYPE>) _param_mav_type,
(ParamBool<px4::params::MAV_USEHILGPS>) _param_mav_usehilgps,
(ParamBool<px4::params::MAV_FWDEXTSP>) _param_mav_fwdextsp,
#if defined(MAVLINK_UDP)
(ParamInt<px4::params::MAV_BROADCAST>) _param_mav_broadcast,
#endif // MAVLINK_UDP
(ParamBool<px4::params::MAV_HASH_CHK_EN>) _param_mav_hash_chk_en,
(ParamBool<px4::params::MAV_HB_FORW_EN>) _param_mav_hb_forw_en,
(ParamBool<px4::params::MAV_ODOM_LP>) _param_mav_odom_lp,
src/modules/mavlink/mavlink_params.c
-13
View File
@@ -130,19 +130,6 @@ PARAM_DEFINE_INT32(MAV_USEHILGPS, 0);
*/
PARAM_DEFINE_INT32(MAV_FWDEXTSP, 1);
/**
* Broadcast heartbeats on local network
*
* This allows a ground control station to automatically find the drone
* on the local network.
*
* @value 0 Never broadcast
* @value 1 Always broadcast
* @value 2 Only multicast
* @group MAVLink
*/
PARAM_DEFINE_INT32(MAV_BROADCAST, 0);
/**
* Parameter hash check.
*
src/modules/mavlink/module.yaml
+53 -2
View File
@@ -3,7 +3,15 @@ __max_num_config_instances: &max_num_config_instances 3
module_name: MAVLink
serial_config:
- command: |
set MAV_ARGS "-b p:${BAUD_PARAM} -m p:MAV_${i}_MODE -r p:MAV_${i}_RATE"
if [ $SERIAL_DEV != ethernet ]; then
set MAV_ARGS "-d ${SERIAL_DEV} -b p:${BAUD_PARAM} -m p:MAV_${i}_MODE -r p:MAV_${i}_RATE"
else
set MAV_ARGS "-u p:MAV_${i}_UDP_PRT -o p:MAV_${i}_REMOTE_PRT -m p:MAV_${i}_MODE -r p:MAV_${i}_RATE"
if param compare MAV_${i}_BROADCAST 1
then
set MAV_ARGS "${MAV_ARGS} -p"
fi
fi
if param compare MAV_${i}_FORWARD 1
then
set MAV_ARGS "${MAV_ARGS} -f"
@@ -12,7 +20,7 @@ serial_config:
then
set MAV_ARGS "${MAV_ARGS} -s"
fi
mavlink start -d ${SERIAL_DEV} ${MAV_ARGS} -x
mavlink start ${MAV_ARGS} -x
port_config_param:
name: MAV_${i}_CONFIG
group: MAVLink
@@ -22,6 +30,7 @@ serial_config:
# 2: Board-specific / no fixed function or port
default: [TEL1, "", ""]
num_instances: *max_num_config_instances
ethernet:
parameters:
- group: MAVLink
@@ -97,3 +106,45 @@ parameters:
reboot_required: true
num_instances: *max_num_config_instances
default: [true, true, true]
MAV_${i}_UDP_PRT:
description:
short: MAVLink Network Port for instance ${i}
long: |
If ethernet enabled and selected as configuration for MAVLink instance ${i},
selected udp port will be set and used in MAVLink instance ${i}.
type: int32
reboot_required: true
num_instances: *max_num_config_instances
default: [14556, 0, 0]
ethernet:
MAV_${i}_REMOTE_PRT:
description:
short: MAVLink Remote Port for instance ${i}
long: |
If ethernet enabled and selected as configuration for MAVLink instance ${i},
selected remote port will be set and used in MAVLink instance ${i}.
type: int32
reboot_required: true
num_instances: *max_num_config_instances
default: [14550, 0, 0]
ethernet:
MAV_${i}_BROADCAST:
description:
short: Broadcast heartbeats on local network for MAVLink instance ${i}
long: |
This allows a ground control station to automatically find the drone
on the local network.
type: enum
values:
0: Never broadcast
1: Always broadcast
2: Only multicast
num_instances: *max_num_config_instances
default: [1, 0, 0]
ethernet: