46 #include <arpa/inet.h> 47 #include <netinet/in.h> 48 #include <netutils/netlib.h> 60 #ifndef MAVLINK_CRC_EXTRA 61 #error MAVLINK_CRC_EXTRA has to be defined on PX4 systems 65 #if defined (CRTSCTS) && defined (__PX4_NUTTX) && (CRTSCTS != (CRTS_IFLOW | CCTS_OFLOW)) 66 #error The non-standard CRTSCTS define is incorrect. Fix this in the OS or replace with (CRTS_IFLOW | CCTS_OFLOW) 70 #define MAVLINK_NET_ADDED_STACK 350 72 #define MAVLINK_NET_ADDED_STACK 0 75 #define FLOW_CONTROL_DISABLE_THRESHOLD 40 76 #define MAX_DATA_RATE 10000000
77 #define MAIN_LOOP_DELAY 10000
94 #ifdef MAVLINK_PRINT_PACKETS 96 for (
unsigned i = 0; i < length; i++) {
97 printf(
"%02x", (
unsigned char)ch[i]);
110 #ifdef MAVLINK_PRINT_PACKETS 111 printf(
"START PACKET (%u): ", (
unsigned)chan);
122 #ifdef MAVLINK_PRINT_PACKETS 165 ModuleParams(nullptr)
171 int sys_id = _param_mav_sys_id.get();
173 if (sys_id > 0 && sys_id < 255) {
177 int comp_id = _param_mav_comp_id.get();
179 if (comp_id > 0 && comp_id < 255) {
219 int32_t proto = _param_mav_proto_ver.get();
226 if (_param_mav_type.get() < 0 || _param_mav_type.get() >= MAV_TYPE_ENUM_END) {
227 _param_mav_type.set(0);
228 _param_mav_type.commit_no_notification();
229 PX4_ERR(
"MAV_TYPE parameter invalid, resetting to 0.");
253 #ifdef MAVLINK_COMM_4 259 #ifdef MAVLINK_COMM_5 265 #ifdef MAVLINK_COMM_6 273 PX4_WARN(
"instance ID is out of range");
288 if ((version == 1 || version == 0) &&
290 get_status()->flags |= MAVLINK_STATUS_FLAG_OUT_MAVLINK1;
293 }
else if (version == 2 &&
295 get_status()->flags &= ~(MAVLINK_STATUS_FLAG_OUT_MAVLINK1);
303 size_t inst_index = 0;
342 Mavlink::get_instance_for_network_port(
unsigned long port)
347 if (inst->_network_port == port) {
354 #endif // MAVLINK_UDP 360 Mavlink *inst_to_del =
nullptr;
363 unsigned iterations = 0;
365 PX4_INFO(
"waiting for instances to stop");
367 while (next_inst !=
nullptr) {
368 inst_to_del = next_inst;
369 next_inst = inst_to_del->
next;
380 if (iterations > 1000) {
381 PX4_ERR(
"Couldn't stop all mavlink instances.");
389 while (_mavlink_instances) {
391 LL_DELETE(_mavlink_instances, inst_to_del);
396 PX4_INFO(
"all instances stopped");
405 unsigned iterations = 0;
407 while (inst !=
nullptr) {
409 printf(
"\ninstance #%u:\n", iterations);
411 if (show_streams_status) {
424 return (iterations == 0);
432 while (inst !=
nullptr) {
451 const mavlink_msg_entry_t *meta = mavlink_get_msg_entry(msg->msgid);
453 int target_system_id = 0;
454 int target_component_id = 0;
459 if (meta->flags & MAV_MSG_ENTRY_FLAG_HAVE_TARGET_SYSTEM) {
460 target_system_id = (_MAV_PAYLOAD(msg))[meta->target_system_ofs];
463 if (meta->flags & MAV_MSG_ENTRY_FLAG_HAVE_TARGET_COMPONENT) {
464 target_component_id = (_MAV_PAYLOAD(msg))[meta->target_component_ofs];
470 const bool target_system_id_ok =
471 (target_system_id == 0 || target_system_id ==
self->get_system_id());
475 const bool target_component_id_ok =
476 (target_component_id == 0 || target_component_id !=
self->get_component_id());
479 const bool heartbeat_check_ok =
480 (msg->msgid != MAVLINK_MSG_ID_HEARTBEAT ||
self->forward_heartbeats_enabled());
482 if (target_system_id_ok && target_component_id_ok && heartbeat_check_ok) {
506 #define B460800 460800 510 #define B500000 500000 514 #define B921600 921600 518 #define B1000000 1000000 525 case 0: speed = B0;
break;
527 case 50: speed = B50;
break;
529 case 75: speed = B75;
break;
531 case 110: speed = B110;
break;
533 case 134: speed = B134;
break;
535 case 150: speed = B150;
break;
537 case 200: speed = B200;
break;
539 case 300: speed = B300;
break;
541 case 600: speed = B600;
break;
543 case 1200: speed = B1200;
break;
545 case 1800: speed = B1800;
break;
547 case 2400: speed = B2400;
break;
549 case 4800: speed = B4800;
break;
551 case 9600: speed = B9600;
break;
553 case 19200: speed = B19200;
break;
555 case 38400: speed = B38400;
break;
557 case 57600: speed = B57600;
break;
559 case 115200: speed = B115200;
break;
561 case 230400: speed = B230400;
break;
563 case 460800: speed =
B460800;
break;
565 case 500000: speed =
B500000;
break;
567 case 921600: speed =
B921600;
break;
569 case 1000000: speed =
B1000000;
break;
573 case 1500000: speed = B1500000;
break;
578 case 2000000: speed = B2000000;
break;
583 case 3000000: speed = B3000000;
break;
587 PX4_ERR(
"Unsupported baudrate: %d\n\tsupported examples:\n\t9600, 19200, 38400, 57600\t\n115200\n230400\n460800\n500000\n921600\n1000000\n",
599 _uart_fd = ::open(uart_name, O_RDWR | O_NOCTTY);
616 if (armed_sub.get().armed) {
623 px4_usleep(errcode == ENOTCONN ? 1000000 : 100000);
624 _uart_fd = ::open(uart_name, O_RDWR | O_NOCTTY);
633 struct termios uart_config;
637 if ((termios_state = tcgetattr(
_uart_fd, &uart_config)) < 0) {
638 PX4_ERR(
"ERR GET CONF %s: %d\n", uart_name, termios_state);
644 uart_config.c_oflag &= ~ONLCR;
649 if (cfsetispeed(&uart_config, speed) < 0 || cfsetospeed(&uart_config, speed) < 0) {
650 PX4_ERR(
"ERR SET BAUD %s: %d\n", uart_name, termios_state);
663 #if defined(__PX4_LINUX) || defined(__PX4_DARWIN) || defined(__PX4_CYGWIN) 665 cfmakeraw(&uart_config);
668 if ((termios_state = tcsetattr(
_uart_fd, TCSANOW, &uart_config)) < 0) {
669 PX4_WARN(
"ERR SET CONF %s\n", uart_name);
681 PX4_WARN(
"hardware flow control not supported");
696 struct termios uart_config;
698 int ret = tcgetattr(
_uart_fd, &uart_config);
701 uart_config.c_cflag |= CRTSCTS;
704 uart_config.c_cflag &= ~CRTSCTS;
708 ret = tcsetattr(
_uart_fd, TCSANOW, &uart_config);
727 if (_param_sys_hitl.get() == 2) {
755 #if defined(MAVLINK_UDP) 762 #endif // MAVLINK_UDP 765 #if defined(__PX4_LINUX) || defined(__PX4_DARWIN) || defined(__PX4_CYGWIN) 769 (void) ioctl(
_uart_fd, FIONSPACE, (
unsigned long)&buf_free);
794 #if defined(MAVLINK_UDP) 797 if (_network_buf_len == 0) {
806 if (_src_addr_initialized) {
808 ret = sendto(
_socket_fd, _network_buf, _network_buf_len, 0,
809 (
struct sockaddr *)&_src_addr,
sizeof(_src_addr));
817 (!get_client_source_initialized()
820 if (!_broadcast_address_found) {
821 find_broadcast_address();
824 if (_broadcast_address_found && _network_buf_len > 0) {
826 int bret = sendto(
_socket_fd, _network_buf, _network_buf_len, 0,
827 (
struct sockaddr *)&_bcast_addr,
sizeof(_bcast_addr));
830 if (!_broadcast_failed_warned) {
831 PX4_ERR(
"sending broadcast failed, errno: %d: %s", errno, strerror(errno));
832 _broadcast_failed_warned =
true;
836 _broadcast_failed_warned =
false;
843 _network_buf_len = 0;
845 #endif // MAVLINK_UDP 864 if (buf_free < packet_len) {
878 #if defined(MAVLINK_UDP) 881 if (_network_buf_len + packet_len <
sizeof(_network_buf) /
sizeof(_network_buf[0])) {
882 memcpy(&_network_buf[_network_buf_len], buf, packet_len);
883 _network_buf_len += packet_len;
889 #endif // MAVLINK_UDP 891 if (ret != (
size_t) packet_len) {
902 Mavlink::find_broadcast_address()
904 #if defined(__PX4_LINUX) || defined(__PX4_DARWIN) || defined(__PX4_CYGWIN) 905 struct ifconf ifconf;
908 #if defined(__APPLE__) && defined(__MACH__) || defined(__CYGWIN__) 911 ifconf.ifc_len = 1024;
915 ifconf.ifc_req =
nullptr;
918 ret = ioctl(
_socket_fd, SIOCGIFCONF, &ifconf);
921 PX4_WARN(
"getting required buffer size failed");
927 PX4_DEBUG(
"need to allocate %d bytes", ifconf.ifc_len);
930 ifconf.ifc_req = (
struct ifreq *)(
new uint8_t[ifconf.ifc_len]);
932 if (ifconf.ifc_req ==
nullptr) {
933 PX4_ERR(
"Could not allocate ifconf buffer");
937 memset(ifconf.ifc_req, 0, ifconf.ifc_len);
939 ret = ioctl(
_socket_fd, SIOCGIFCONF, &ifconf);
942 PX4_ERR(
"getting network config failed");
943 delete[] ifconf.ifc_req;
949 struct ifreq *cur_ifreq = (
struct ifreq *) & (((uint8_t *)ifconf.ifc_req)[offset]);
954 offset < ifconf.ifc_len;
955 #if defined(__APPLE__) && defined(__MACH__) 959 offset += IF_NAMESIZE
960 + (
sizeof(
struct sockaddr) > cur_ifreq->ifr_addr.sa_len ?
961 sizeof(struct sockaddr) : cur_ifreq->ifr_addr.sa_len)
965 offset += sizeof(struct ifreq)
969 cur_ifreq = (
struct ifreq *) & (((uint8_t *)ifconf.ifc_req)[offset]);
971 PX4_DEBUG(
"looking at %s", cur_ifreq->ifr_name);
974 if (strcmp(cur_ifreq->ifr_name,
"lo") == 0 ||
975 strcmp(cur_ifreq->ifr_name,
"lo0") == 0 ||
976 strcmp(cur_ifreq->ifr_name,
"lo1") == 0 ||
977 strcmp(cur_ifreq->ifr_name,
"lo2") == 0) {
978 PX4_DEBUG(
"skipping loopback");
982 struct in_addr &sin_addr = ((
struct sockaddr_in *)&cur_ifreq->ifr_addr)->sin_addr;
988 uint8_t first_byte = sin_addr.s_addr & 0xFF;
990 if (first_byte != 192 && first_byte != 172 && first_byte != 10) {
994 if (!_broadcast_address_found) {
995 const struct in_addr netmask_addr = query_netmask_addr(
_socket_fd, *cur_ifreq);
996 const struct in_addr broadcast_addr = compute_broadcast_addr(sin_addr, netmask_addr);
1000 PX4_INFO(
"using network interface %s, IP: %s", cur_ifreq->ifr_name, inet_ntoa(sin_addr));
1001 PX4_INFO(
"with netmask: %s", inet_ntoa(netmask_addr));
1002 PX4_INFO(
"and broadcast IP: %s", inet_ntoa(broadcast_addr));
1004 _bcast_addr.sin_family = AF_INET;
1005 _bcast_addr.sin_addr = broadcast_addr;
1007 _broadcast_address_found =
true;
1010 PX4_DEBUG(
"ignoring additional network interface %s, IP: %s",
1011 cur_ifreq->ifr_name, inet_ntoa(sin_addr));
1015 #elif defined (CONFIG_NET) && defined (__PX4_NUTTX) 1018 PX4_INFO(
"using network interface");
1020 struct in_addr eth_addr;
1021 struct in_addr bc_addr;
1022 struct in_addr netmask_addr;
1023 ret = netlib_get_ipv4addr(
"eth0", ð_addr);
1026 PX4_ERR(
"getting network config failed");
1030 ret = netlib_get_ipv4netmask(
"eth0", &netmask_addr);
1033 PX4_ERR(
"getting network config failed");
1037 PX4_INFO(
"ipv4addr IP: %s", inet_ntoa(eth_addr));
1038 PX4_INFO(
"netmask_addr IP: %s", inet_ntoa(netmask_addr));
1040 bc_addr.s_addr = eth_addr.s_addr | ~(netmask_addr.s_addr);
1042 if (!_broadcast_address_found) {
1043 PX4_INFO(
"using network interface %s, IP: %s",
"eth0", inet_ntoa(eth_addr));
1046 PX4_INFO(
"with broadcast IP: %s", inet_ntoa(bc_addr));
1048 _bcast_addr.sin_family = AF_INET;
1049 _bcast_addr.sin_addr = bc_addr;
1051 _broadcast_address_found =
true;
1056 #if defined (__PX4_LINUX) || defined (__PX4_DARWIN) || (defined (CONFIG_NET) && defined (__PX4_NUTTX)) 1058 if (_broadcast_address_found) {
1059 _bcast_addr.sin_port = htons(_remote_port);
1061 int broadcast_opt = 1;
1063 if (setsockopt(
_socket_fd, SOL_SOCKET, SO_BROADCAST, &broadcast_opt,
sizeof(broadcast_opt)) < 0) {
1064 PX4_WARN(
"setting broadcast permission failed");
1067 _broadcast_address_not_found_warned =
false;
1070 if (!_broadcast_address_not_found_warned) {
1071 PX4_WARN(
"no broadcasting address found");
1072 _broadcast_address_not_found_warned =
true;
1076 #if defined (__PX4_LINUX) || defined (__PX4_DARWIN) 1077 delete[] ifconf.ifc_req;
1082 #endif // MAVLINK_UDP 1086 Mavlink::query_netmask_addr(
const int socket_fd,
const ifreq &ifreq)
1088 struct ifreq netmask_ifreq;
1089 memset(&netmask_ifreq, 0,
sizeof(netmask_ifreq));
1090 strncpy(netmask_ifreq.ifr_name, ifreq.ifr_name, IF_NAMESIZE);
1091 ioctl(socket_fd, SIOCGIFNETMASK, &netmask_ifreq);
1093 return ((
struct sockaddr_in *)&netmask_ifreq.ifr_addr)->sin_addr;
1097 Mavlink::compute_broadcast_addr(
const in_addr &host_addr,
const in_addr &netmask_addr)
1099 struct in_addr broadcast_addr;
1100 broadcast_addr.s_addr = ~netmask_addr.s_addr | host_addr.s_addr;
1102 return broadcast_addr;
1110 PX4_DEBUG(
"Setting up UDP with port %d", _network_port);
1112 _myaddr.sin_family = AF_INET;
1113 _myaddr.sin_addr.s_addr = htonl(INADDR_ANY);
1114 _myaddr.sin_port = htons(_network_port);
1116 if ((
_socket_fd = socket(AF_INET, SOCK_DGRAM, 0)) < 0) {
1117 PX4_WARN(
"create socket failed: %s", strerror(errno));
1121 if (bind(
_socket_fd, (
struct sockaddr *)&_myaddr,
sizeof(_myaddr)) < 0) {
1122 PX4_WARN(
"bind failed: %s", strerror(errno));
1127 if (!_src_addr_initialized) {
1128 _src_addr.sin_family = AF_INET;
1129 inet_aton(
"127.0.0.1", &_src_addr.sin_addr);
1132 _src_addr.sin_port = htons(_remote_port);
1134 #endif // MAVLINK_UDP 1174 if (status_sub->update(&status)) {
1175 mavlink_autopilot_version_t
msg = {};
1177 msg.capabilities = MAV_PROTOCOL_CAPABILITY_MISSION_FLOAT;
1178 msg.capabilities |= MAV_PROTOCOL_CAPABILITY_MISSION_INT;
1179 msg.capabilities |= MAV_PROTOCOL_CAPABILITY_PARAM_FLOAT;
1180 msg.capabilities |= MAV_PROTOCOL_CAPABILITY_COMMAND_INT;
1181 msg.capabilities |= MAV_PROTOCOL_CAPABILITY_FTP;
1182 msg.capabilities |= MAV_PROTOCOL_CAPABILITY_SET_ATTITUDE_TARGET;
1183 msg.capabilities |= MAV_PROTOCOL_CAPABILITY_SET_POSITION_TARGET_LOCAL_NED;
1184 msg.capabilities |= MAV_PROTOCOL_CAPABILITY_SET_ACTUATOR_TARGET;
1185 msg.capabilities |= MAV_PROTOCOL_CAPABILITY_MAVLINK2;
1186 msg.capabilities |= MAV_PROTOCOL_CAPABILITY_MISSION_FENCE;
1187 msg.capabilities |= MAV_PROTOCOL_CAPABILITY_MISSION_RALLY;
1195 constexpr
size_t fw_vendor_version_length = 3;
1196 memcpy(&msg.flight_custom_version, &fw_git_version_binary,
sizeof(msg.flight_custom_version));
1197 memcpy(&msg.flight_custom_version, &fw_vendor_version, fw_vendor_version_length);
1198 memcpy(&msg.middleware_custom_version, &fw_git_version_binary,
sizeof(msg.middleware_custom_version));
1200 memcpy(&msg.os_custom_version, &os_git_version_binary,
sizeof(msg.os_custom_version));
1201 #ifdef CONFIG_CDCACM_VENDORID 1202 msg.vendor_id = CONFIG_CDCACM_VENDORID;
1206 #ifdef CONFIG_CDCACM_PRODUCTID 1212 board_get_uuid32(uid);
1213 msg.uid = (((uint64_t)uid[PX4_CPU_UUID_WORD32_UNIQUE_M]) << 32) | uid[PX4_CPU_UUID_WORD32_UNIQUE_H];
1215 #ifndef BOARD_HAS_NO_UUID 1216 px4_guid_t px4_guid;
1217 board_get_px4_guid(px4_guid);
1218 static_assert(
sizeof(px4_guid_t) ==
sizeof(msg.uid2),
"GUID byte length mismatch");
1219 memcpy(&msg.uid2, &px4_guid,
sizeof(msg.uid2));
1222 #ifdef CONFIG_ARCH_BOARD_PX4_SITL 1231 mavlink_msg_autopilot_version_send_struct(
get_channel(), &msg);
1238 mavlink_protocol_version_t
msg = {};
1241 msg.min_version = 100;
1242 msg.max_version = 200;
1246 memcpy(&msg.library_version_hash, &mavlink_lib_git_version_binary,
sizeof(msg.library_version_hash));
1252 mavlink_msg_protocol_version_send_struct(
get_channel(), &msg);
1260 if (!disable_sharing) {
1263 if (sub->get_topic() == topic && sub->get_instance() ==
instance) {
1281 PX4_DEBUG(
"configure_stream(%s, %.3f)", stream_name, (
double)rate);
1286 if (rate > 0.000001
f) {
1287 interval = (1000000.0f / rate);
1289 }
else if (rate < 0.0
f) {
1293 for (
const auto &stream :
_streams) {
1294 if (strcmp(stream_name, stream->get_name()) == 0) {
1295 if (interval != 0) {
1297 stream->set_interval(interval);
1301 _streams.deleteNode(stream);
1309 if (interval == 0) {
1318 if (stream !=
nullptr) {
1320 _streams.add(stream);
1326 PX4_WARN(
"stream %s not found", stream_name);
1344 unsigned n = strlen(stream_name) + 1;
1345 char *s =
new char[n];
1346 strcpy(s, stream_name);
1409 if (available < 0) {
1413 if (size > available) {
1418 char *c = (
char *) ptr;
1443 if (available == 0) {
1449 if (available > 0) {
1469 int size = MAVLINK_NUM_NON_PAYLOAD_BYTES + msg->len;
1483 PX4_ERR(
"Failed to allocate a shell");
1489 PX4_ERR(
"Failed to start shell (%i)", ret);
1511 float const_rate = 0.0f;
1515 for (
const auto &stream :
_streams) {
1516 if (stream->const_rate()) {
1517 const_rate += (stream->get_interval() > 0) ? stream->get_size_avg() * 1000000.0f / stream->get_interval() : 0;
1520 rate += (stream->get_interval() > 0) ? stream->get_size_avg() * 1000000.0f / stream->get_interval() : 0;
1524 float mavlink_ulog_streaming_rate_inv = 1.0f;
1531 float bandwidth_mult = (float)(
_datarate * mavlink_ulog_streaming_rate_inv - const_rate) / rate;
1535 bandwidth_mult = fminf(1.0
f, bandwidth_mult);
1538 float hardware_mult = 1.0f;
1548 PX4_ERR(
"instance %d: RADIO_STATUS timeout",
_instance_id);
1560 _rate_mult = fminf(bandwidth_mult, hardware_mult);
1594 bool stream_configured =
false;
1596 auto configure_stream_local =
1597 [&stream_configured, configure_single_stream, &ret,
this](
const char *stream_name,
float rate) {
1598 if (!configure_single_stream || strcmp(configure_single_stream, stream_name) == 0) {
1601 if (ret_local != 0) {
1605 stream_configured =
true;
1609 const float unlimited_rate = -1.0f;
1613 configure_stream_local(
"ADSB_VEHICLE", unlimited_rate);
1614 configure_stream_local(
"ALTITUDE", 1.0
f);
1615 configure_stream_local(
"ATTITUDE", 15.0
f);
1616 configure_stream_local(
"ATTITUDE_TARGET", 2.0
f);
1617 configure_stream_local(
"BATTERY_STATUS", 0.5
f);
1618 configure_stream_local(
"CAMERA_IMAGE_CAPTURED", unlimited_rate);
1619 configure_stream_local(
"COLLISION", unlimited_rate);
1620 configure_stream_local(
"DEBUG", 1.0
f);
1621 configure_stream_local(
"DEBUG_FLOAT_ARRAY", 1.0
f);
1622 configure_stream_local(
"DEBUG_VECT", 1.0
f);
1623 configure_stream_local(
"DISTANCE_SENSOR", 0.5
f);
1624 configure_stream_local(
"ESTIMATOR_STATUS", 0.5
f);
1625 configure_stream_local(
"EXTENDED_SYS_STATE", 1.0
f);
1626 configure_stream_local(
"GLOBAL_POSITION_INT", 5.0
f);
1627 configure_stream_local(
"GPS2_RAW", 1.0
f);
1628 configure_stream_local(
"GPS_RAW_INT", 1.0
f);
1629 configure_stream_local(
"HOME_POSITION", 0.5
f);
1630 configure_stream_local(
"LOCAL_POSITION_NED", 1.0
f);
1631 configure_stream_local(
"NAMED_VALUE_FLOAT", 1.0
f);
1632 configure_stream_local(
"NAV_CONTROLLER_OUTPUT", 1.0
f);
1633 configure_stream_local(
"OBSTACLE_DISTANCE", 1.0
f);
1634 configure_stream_local(
"ORBIT_EXECUTION_STATUS", 2.0
f);
1635 configure_stream_local(
"PING", 0.1
f);
1636 configure_stream_local(
"POSITION_TARGET_GLOBAL_INT", 1.0
f);
1637 configure_stream_local(
"POSITION_TARGET_LOCAL_NED", 1.5
f);
1638 configure_stream_local(
"RC_CHANNELS", 5.0
f);
1639 configure_stream_local(
"SERVO_OUTPUT_RAW_0", 1.0
f);
1640 configure_stream_local(
"SYS_STATUS", 1.0
f);
1641 configure_stream_local(
"UTM_GLOBAL_POSITION", 0.5
f);
1642 configure_stream_local(
"VFR_HUD", 4.0
f);
1643 configure_stream_local(
"WIND_COV", 0.5
f);
1647 configure_stream_local(
"ACTUATOR_CONTROL_TARGET0", 10.0
f);
1648 configure_stream_local(
"ADSB_VEHICLE", unlimited_rate);
1649 configure_stream_local(
"ALTITUDE", 10.0
f);
1650 configure_stream_local(
"ATTITUDE", 100.0
f);
1651 configure_stream_local(
"ATTITUDE_QUATERNION", 50.0
f);
1652 configure_stream_local(
"ATTITUDE_TARGET", 10.0
f);
1653 configure_stream_local(
"BATTERY_STATUS", 0.5
f);
1654 configure_stream_local(
"CAMERA_CAPTURE", 2.0
f);
1655 configure_stream_local(
"CAMERA_IMAGE_CAPTURED", unlimited_rate);
1656 configure_stream_local(
"CAMERA_TRIGGER", unlimited_rate);
1657 configure_stream_local(
"COLLISION", unlimited_rate);
1658 configure_stream_local(
"DEBUG", 10.0
f);
1659 configure_stream_local(
"DEBUG_FLOAT_ARRAY", 10.0
f);
1660 configure_stream_local(
"DEBUG_VECT", 10.0
f);
1661 configure_stream_local(
"DISTANCE_SENSOR", 10.0
f);
1662 configure_stream_local(
"ESTIMATOR_STATUS", 1.0
f);
1663 configure_stream_local(
"EXTENDED_SYS_STATE", 5.0
f);
1664 configure_stream_local(
"GLOBAL_POSITION_INT", 50.0
f);
1665 configure_stream_local(
"GPS2_RAW", unlimited_rate);
1666 configure_stream_local(
"GPS_RAW_INT", unlimited_rate);
1667 configure_stream_local(
"HIGHRES_IMU", 50.0
f);
1668 configure_stream_local(
"HOME_POSITION", 0.5
f);
1669 configure_stream_local(
"LOCAL_POSITION_NED", 30.0
f);
1670 configure_stream_local(
"NAMED_VALUE_FLOAT", 10.0
f);
1671 configure_stream_local(
"NAV_CONTROLLER_OUTPUT", 10.0
f);
1672 configure_stream_local(
"ODOMETRY", 30.0
f);
1673 configure_stream_local(
"OPTICAL_FLOW_RAD", 10.0
f);
1674 configure_stream_local(
"ORBIT_EXECUTION_STATUS", 5.0
f);
1675 configure_stream_local(
"PING", 1.0
f);
1676 configure_stream_local(
"POSITION_TARGET_GLOBAL_INT", 10.0
f);
1677 configure_stream_local(
"POSITION_TARGET_LOCAL_NED", 10.0
f);
1678 configure_stream_local(
"RC_CHANNELS", 20.0
f);
1679 configure_stream_local(
"SERVO_OUTPUT_RAW_0", 10.0
f);
1680 configure_stream_local(
"SYS_STATUS", 5.0
f);
1681 configure_stream_local(
"SYSTEM_TIME", 1.0
f);
1682 configure_stream_local(
"TIMESYNC", 10.0
f);
1683 configure_stream_local(
"TRAJECTORY_REPRESENTATION_WAYPOINTS", 5.0
f);
1684 configure_stream_local(
"UTM_GLOBAL_POSITION", 1.0
f);
1685 configure_stream_local(
"VFR_HUD", 10.0
f);
1686 configure_stream_local(
"WIND_COV", 10.0
f);
1690 configure_stream_local(
"HIGHRES_IMU", unlimited_rate);
1691 configure_stream_local(
"TIMESYNC", 10.0
f);
1695 configure_stream_local(
"ADSB_VEHICLE", unlimited_rate);
1696 configure_stream_local(
"ALTITUDE", 10.0
f);
1697 configure_stream_local(
"ATTITUDE", 20.0
f);
1698 configure_stream_local(
"ATTITUDE_TARGET", 2.0
f);
1699 configure_stream_local(
"BATTERY_STATUS", 0.5
f);
1700 configure_stream_local(
"CAMERA_IMAGE_CAPTURED", unlimited_rate);
1701 configure_stream_local(
"CAMERA_TRIGGER", unlimited_rate);
1702 configure_stream_local(
"COLLISION", unlimited_rate);
1703 configure_stream_local(
"DEBUG", 1.0
f);
1704 configure_stream_local(
"DEBUG_FLOAT_ARRAY", 1.0
f);
1705 configure_stream_local(
"DEBUG_VECT", 1.0
f);
1706 configure_stream_local(
"DISTANCE_SENSOR", 10.0
f);
1707 configure_stream_local(
"ESTIMATOR_STATUS", 1.0
f);
1708 configure_stream_local(
"EXTENDED_SYS_STATE", 1.0
f);
1709 configure_stream_local(
"GLOBAL_POSITION_INT", 5.0
f);
1710 configure_stream_local(
"GPS2_RAW", 1.0
f);
1711 configure_stream_local(
"GPS_RAW_INT", 1.0
f);
1712 configure_stream_local(
"HOME_POSITION", 0.5
f);
1713 configure_stream_local(
"LOCAL_POSITION_NED", 30.0
f);
1714 configure_stream_local(
"NAMED_VALUE_FLOAT", 1.0
f);
1715 configure_stream_local(
"NAV_CONTROLLER_OUTPUT", 1.5
f);
1716 configure_stream_local(
"ODOMETRY", 30.0
f);
1717 configure_stream_local(
"OPTICAL_FLOW_RAD", 1.0
f);
1718 configure_stream_local(
"ORBIT_EXECUTION_STATUS", 5.0
f);
1719 configure_stream_local(
"PING", 0.1
f);
1720 configure_stream_local(
"POSITION_TARGET_GLOBAL_INT", 1.5
f);
1721 configure_stream_local(
"POSITION_TARGET_LOCAL_NED", 1.5
f);
1722 configure_stream_local(
"RC_CHANNELS", 5.0
f);
1723 configure_stream_local(
"SERVO_OUTPUT_RAW_0", 1.0
f);
1724 configure_stream_local(
"SYS_STATUS", 5.0
f);
1725 configure_stream_local(
"TRAJECTORY_REPRESENTATION_WAYPOINTS", 5.0
f);
1726 configure_stream_local(
"UTM_GLOBAL_POSITION", 1.0
f);
1727 configure_stream_local(
"VFR_HUD", 4.0
f);
1728 configure_stream_local(
"WIND_COV", 1.0
f);
1733 configure_stream_local(
"ALTITUDE", 10.0
f);
1734 configure_stream_local(
"ATTITUDE", 25.0
f);
1735 configure_stream_local(
"ATTITUDE_TARGET", 10.0
f);
1736 configure_stream_local(
"BATTERY_STATUS", 0.5
f);
1737 configure_stream_local(
"ESTIMATOR_STATUS", 1.0
f);
1738 configure_stream_local(
"EXTENDED_SYS_STATE", 1.0
f);
1739 configure_stream_local(
"GLOBAL_POSITION_INT", 10.0
f);
1740 configure_stream_local(
"GPS_RAW_INT", 1.0
f);
1741 configure_stream_local(
"HOME_POSITION", 0.5
f);
1742 configure_stream_local(
"RC_CHANNELS", 5.0
f);
1743 configure_stream_local(
"SERVO_OUTPUT_RAW_0", 1.0
f);
1744 configure_stream_local(
"SYS_STATUS", 5.0
f);
1745 configure_stream_local(
"SYSTEM_TIME", 1.0
f);
1746 configure_stream_local(
"VFR_HUD", 25.0
f);
1747 configure_stream_local(
"WIND_COV", 2.0
f);
1759 configure_stream_local(
"ACTUATOR_CONTROL_TARGET0", 30.0
f);
1760 configure_stream_local(
"ADSB_VEHICLE", unlimited_rate);
1761 configure_stream_local(
"ALTITUDE", 10.0
f);
1762 configure_stream_local(
"ATTITUDE", 50.0
f);
1763 configure_stream_local(
"ATTITUDE_QUATERNION", 50.0
f);
1764 configure_stream_local(
"ATTITUDE_TARGET", 8.0
f);
1765 configure_stream_local(
"BATTERY_STATUS", 0.5
f);
1766 configure_stream_local(
"CAMERA_IMAGE_CAPTURED", unlimited_rate);
1767 configure_stream_local(
"CAMERA_TRIGGER", unlimited_rate);
1768 configure_stream_local(
"COLLISION", unlimited_rate);
1769 configure_stream_local(
"DEBUG", 50.0
f);
1770 configure_stream_local(
"DEBUG_FLOAT_ARRAY", 50.0
f);
1771 configure_stream_local(
"DEBUG_VECT", 50.0
f);
1772 configure_stream_local(
"DISTANCE_SENSOR", 10.0
f);
1773 configure_stream_local(
"ESTIMATOR_STATUS", 5.0
f);
1774 configure_stream_local(
"EXTENDED_SYS_STATE", 2.0
f);
1775 configure_stream_local(
"GLOBAL_POSITION_INT", 10.0
f);
1776 configure_stream_local(
"GPS2_RAW", unlimited_rate);
1777 configure_stream_local(
"GPS_RAW_INT", unlimited_rate);
1778 configure_stream_local(
"HIGHRES_IMU", 50.0
f);
1779 configure_stream_local(
"HOME_POSITION", 0.5
f);
1780 configure_stream_local(
"LOCAL_POSITION_NED", 30.0
f);
1781 configure_stream_local(
"MANUAL_CONTROL", 5.0
f);
1782 configure_stream_local(
"NAMED_VALUE_FLOAT", 50.0
f);
1783 configure_stream_local(
"NAV_CONTROLLER_OUTPUT", 10.0
f);
1784 configure_stream_local(
"ODOMETRY", 30.0
f);
1785 configure_stream_local(
"OPTICAL_FLOW_RAD", 10.0
f);
1786 configure_stream_local(
"ORBIT_EXECUTION_STATUS", 5.0
f);
1787 configure_stream_local(
"PING", 1.0
f);
1788 configure_stream_local(
"POSITION_TARGET_GLOBAL_INT", 10.0
f);
1789 configure_stream_local(
"RC_CHANNELS", 10.0
f);
1790 configure_stream_local(
"SCALED_IMU", 25.0
f);
1791 configure_stream_local(
"SCALED_IMU2", 25.0
f);
1792 configure_stream_local(
"SCALED_IMU3", 25.0
f);
1793 configure_stream_local(
"SERVO_OUTPUT_RAW_0", 20.0
f);
1794 configure_stream_local(
"SERVO_OUTPUT_RAW_1", 20.0
f);
1795 configure_stream_local(
"SYS_STATUS", 1.0
f);
1796 configure_stream_local(
"SYSTEM_TIME", 1.0
f);
1797 configure_stream_local(
"TIMESYNC", 10.0
f);
1798 configure_stream_local(
"UTM_GLOBAL_POSITION", 1.0
f);
1799 configure_stream_local(
"VFR_HUD", 20.0
f);
1800 configure_stream_local(
"WIND_COV", 10.0
f);
1804 configure_stream_local(
"HIGH_LATENCY2", 0.015
f);
1808 configure_stream_local(
"ALTITUDE", 0.5
f);
1809 configure_stream_local(
"ATTITUDE", 10.0
f);
1810 configure_stream_local(
"EXTENDED_SYS_STATE", 0.1
f);
1811 configure_stream_local(
"GLOBAL_POSITION_INT", 5.0
f);
1812 configure_stream_local(
"GPS_RAW_INT", 0.5
f);
1813 configure_stream_local(
"HOME_POSITION", 0.1
f);
1814 configure_stream_local(
"NAMED_VALUE_FLOAT", 1.0
f);
1815 configure_stream_local(
"RC_CHANNELS", 0.5
f);
1816 configure_stream_local(
"SYS_STATUS", 0.1
f);
1817 configure_stream_local(
"VFR_HUD", 1.0
f);
1825 if (configure_single_stream && !stream_configured && strcmp(configure_single_stream,
"HEARTBEAT") != 0) {
1840 bool _force_flow_control =
false;
1857 bool err_flag =
false;
1859 const char *myoptarg =
nullptr;
1860 #if defined(CONFIG_NET) || defined(__PX4_POSIX) 1865 while ((ch = px4_getopt(argc, argv,
"b:r:d:n:u:o:m:t:c:fwxz", &myoptind, &myoptarg)) != EOF) {
1868 if (px4_get_parameter_value(myoptarg,
_baudrate) != 0) {
1869 PX4_ERR(
"baudrate parsing failed");
1873 if (_baudrate < 9600 || _baudrate > 3000000) {
1874 PX4_ERR(
"invalid baud rate '%s'", myoptarg);
1881 if (px4_get_parameter_value(myoptarg,
_datarate) != 0) {
1882 PX4_ERR(
"datarate parsing failed");
1887 PX4_ERR(
"invalid data rate '%s'", myoptarg);
1902 #if defined(MAVLINK_UDP) 1905 temp_int_arg = strtoul(myoptarg, &eptr, 10);
1907 if (*eptr ==
'\0') {
1908 _network_port = temp_int_arg;
1912 PX4_ERR(
"invalid data udp_port '%s'", myoptarg);
1919 temp_int_arg = strtoul(myoptarg, &eptr, 10);
1921 if (*eptr ==
'\0') {
1922 _remote_port = temp_int_arg;
1926 PX4_ERR(
"invalid remote udp_port '%s'", myoptarg);
1933 _src_addr.sin_family = AF_INET;
1935 if (inet_aton(myoptarg, &_src_addr.sin_addr)) {
1936 _src_addr_initialized =
true;
1939 PX4_ERR(
"invalid partner ip '%s'", myoptarg);
1945 #if defined(CONFIG_NET_IGMP) && defined(CONFIG_NET_ROUTE) 1949 _src_addr.sin_family = AF_INET;
1951 if (inet_aton(myoptarg, &_src_addr.sin_addr)) {
1952 _src_addr_initialized =
true;
1955 PX4_ERR(
"invalid partner ip '%s'", myoptarg);
1963 PX4_ERR(
"Multicast option is not supported on this platform");
1972 PX4_ERR(
"UDP options not supported on this platform");
1985 if (px4_get_parameter_value(myoptarg, mode) == 0) {
1990 PX4_ERR(
"invalid mode");
1995 if (strcmp(myoptarg,
"custom") == 0) {
1998 }
else if (strcmp(myoptarg,
"camera") == 0) {
2002 }
else if (strcmp(myoptarg,
"onboard") == 0) {
2005 }
else if (strcmp(myoptarg,
"osd") == 0) {
2008 }
else if (strcmp(myoptarg,
"magic") == 0) {
2011 }
else if (strcmp(myoptarg,
"config") == 0) {
2014 }
else if (strcmp(myoptarg,
"iridium") == 0) {
2018 }
else if (strcmp(myoptarg,
"minimal") == 0) {
2021 }
else if (strcmp(myoptarg,
"extvision") == 0) {
2024 }
else if (strcmp(myoptarg,
"extvisionmin") == 0) {
2028 PX4_ERR(
"invalid mode");
2049 _force_flow_control =
true;
2096 PX4_INFO(
"mode: %s, data rate: %d B/s on %s @ %dB",
2116 #if defined(MAVLINK_UDP) 2119 if (Mavlink::get_instance_for_network_port(_network_port) !=
nullptr) {
2120 PX4_ERR(
"port %d already occupied", _network_port);
2124 PX4_INFO(
"mode: %s, data rate: %d B/s on udp port %hu remote port %hu",
2128 #endif // MAVLINK_UDP 2140 PX4_ERR(
"msg buf alloc fail");
2152 uint64_t status_time = 0;
2190 PX4_ERR(
"configure_streams_to_default() failed");
2213 #if defined(MAVLINK_UDP) 2220 #endif // MAVLINK_UDP 2247 if (parameter_update_sub.updated()) {
2250 parameter_update_sub.copy(&pupdate);
2255 #if defined(CONFIG_NET) 2258 _src_addr_initialized =
false;
2261 #endif // CONFIG_NET 2292 if ((vehicle_cmd.command == vehicle_command_s::VEHICLE_CMD_CONTROL_HIGH_LATENCY) &&
2294 if (vehicle_cmd.param1 > 0.5f) {
2315 command_ack.
timestamp = vehicle_cmd.timestamp;
2316 command_ack.command = vehicle_cmd.command;
2317 command_ack.result = vehicle_command_ack_s::VEHICLE_RESULT_ACCEPTED;
2318 command_ack.from_external = !vehicle_cmd.from_external;
2319 command_ack.target_system = vehicle_cmd.source_system;
2320 command_ack.target_component = vehicle_cmd.source_component;
2322 command_ack_pub.publish(command_ack);
2327 uint16_t current_command_ack = 0;
2331 if (!command_ack.from_external) {
2332 mavlink_command_ack_t
msg;
2333 msg.result = command_ack.result;
2334 msg.command = command_ack.command;
2335 msg.progress = command_ack.result_param1;
2336 msg.result_param2 = command_ack.result_param2;
2337 msg.target_system = command_ack.target_system;
2338 msg.target_component = command_ack.target_component;
2339 current_command_ack = command_ack.command;
2344 mavlink_msg_command_ack_send_struct(
get_channel(), &msg);
2357 if (
get_free_tx_buf() >= MAVLINK_MSG_ID_SERIAL_CONTROL_LEN + MAVLINK_NUM_NON_PAYLOAD_BYTES) {
2358 mavlink_serial_control_t
msg;
2360 msg.flags = SERIAL_CONTROL_FLAG_REPLY;
2362 msg.device = SERIAL_CONTROL_DEV_SHELL;
2364 mavlink_msg_serial_control_send_struct(
get_channel(), &msg);
2376 if (current_command_ack == vehicle_command_s::VEHICLE_CMD_LOGGING_START) {
2384 PX4_WARN(
"mavlink ulog stream update failed, stopping (%i)", ret);
2396 for (
const auto &stream :
_streams) {
2401 if (stream->get_id() == MAVLINK_MSG_ID_HIGH_LATENCY2) {
2406 if (stream->get_id() == MAVLINK_MSG_ID_HEARTBEAT) {
2424 if (available > 0) {
2427 mavlink_message_t
msg;
2428 write_ptr = (uint8_t *)&msg;
2431 size_t read_count = available;
2433 if (read_count >
sizeof(mavlink_message_t)) {
2434 read_count =
sizeof(mavlink_message_t);
2437 memcpy(write_ptr, read_ptr, read_count);
2447 if (is_part && read_count <
sizeof(mavlink_message_t)) {
2448 write_ptr += read_count;
2450 read_count =
sizeof(mavlink_message_t) - read_count;
2451 memcpy(write_ptr, read_ptr, read_count);
2518 PX4_INFO(
"exiting channel %i", (
int)
_channel);
2532 #if defined(MAVLINK_UDP) 2535 PX4_DEBUG(
"stream %s on UDP port %d set to default rate",
_subscribe_to_stream, _network_port);
2538 #endif // MAVLINK_UDP 2552 #if defined(MAVLINK_UDP) 2555 PX4_DEBUG(
"stream %s on UDP port %d enabled with rate %.1f Hz",
_subscribe_to_stream, _network_port,
2559 #endif // MAVLINK_UDP 2567 #if defined(MAVLINK_UDP) 2573 #endif // MAVLINK_UDP 2582 #if defined(MAVLINK_UDP) 2588 #endif // MAVLINK_UDP 2617 if (
_uart_fd >= 0 && _param_mav_radio_id.get() != 0
2618 &&
_tstatus.
type == telemetry_status_s::LINK_TYPE_3DR_RADIO) {
2624 px4_usleep(1200000);
2625 fprintf(fs,
"+++\n");
2626 px4_usleep(1200000);
2628 if (_param_mav_radio_id.get() > 0) {
2630 fprintf(fs,
"ATS3=%u\n", _param_mav_radio_id.get());
2635 fprintf(fs,
"AT&F\n");
2640 fprintf(fs,
"AT&W");
2657 PX4_WARN(
"open fd %d failed",
_uart_fd);
2661 _param_mav_radio_id.set(0);
2662 _param_mav_radio_id.commit_no_notification();
2677 PX4_ERR(
"OUT OF MEM");
2700 PX4_ERR(
"Maximum MAVLink instance count of %d reached.",
2707 sprintf(buf,
"mavlink_if%d", ic);
2713 px4_task_spawn_cmd(buf,
2715 SCHED_PRIORITY_DEFAULT,
2718 (
char *
const *)argv);
2728 const unsigned sleeptime = 500;
2731 const unsigned limit = 100 * 1000 / sleeptime;
2736 px4_usleep(sleeptime);
2755 printf(
"\tmavlink chan: #%u\n",
_channel);
2759 printf(
"\ttype:\t\t");
2762 case telemetry_status_s::LINK_TYPE_3DR_RADIO:
2763 printf(
"3DR RADIO\n");
2773 case telemetry_status_s::LINK_TYPE_USB:
2774 printf(
"USB CDC\n");
2778 printf(
"GENERIC LINK OR RADIO\n");
2783 printf(
"\tno radio status.\n");
2787 printf(
"\trates:\n");
2790 printf(
"\t tx rate mult: %.3f\n", (
double)
_rate_mult);
2791 printf(
"\t tx rate max: %i B/s\n",
_datarate);
2799 printf(
"\tFTP enabled: %s, TX enabled: %s\n",
2805 printf(
"\ttransport protocol: ");
2808 #if defined(MAVLINK_UDP) 2811 printf(
"UDP (%i, remote port: %i)\n", _network_port, _remote_port);
2814 if (get_client_source_initialized()) {
2815 printf(
"\tpartner IP: %s\n", inet_ntoa(get_client_source_address().sin_addr));
2820 #endif // MAVLINK_UDP 2828 printf(
"\tping statistics:\n");
2840 printf(
"\t%-20s%-16s %s\n",
"Name",
"Rate Config (current) [Hz]",
"Message Size (if active) [B]");
2844 for (
const auto &stream :
_streams) {
2845 const int interval = stream->get_interval();
2846 const unsigned size = stream->get_size();
2850 strcpy(rate_str,
"unlimited");
2853 float rate = 1000000.0f / (float)interval;
2856 float rate_current = stream->const_rate() ? rate : rate * rate_mult;
2857 snprintf(rate_str,
sizeof(rate_str),
"%6.2f (%.3f)", (
double)rate, (
double)rate_current);
2860 printf(
"\t%-30s%-16s", stream->get_name(), rate_str);
2863 printf(
" %3i\n", size);
2876 const char *stream_name =
nullptr;
2880 unsigned short network_port = 0;
2881 #endif // MAVLINK_UDP 2882 bool provided_device =
false;
2883 bool provided_network_port =
false;
2896 bool err_flag =
false;
2902 if (0 == strcmp(argv[i],
"-r") && i < argc - 1) {
2903 rate = strtod(argv[i + 1],
nullptr);
2911 }
else if (0 == strcmp(argv[i],
"-d") && i < argc - 1) {
2912 provided_device =
true;
2913 device_name = argv[i + 1];
2916 }
else if (0 == strcmp(argv[i],
"-s") && i < argc - 1) {
2917 stream_name = argv[i + 1];
2922 }
else if (0 == strcmp(argv[i],
"-u") && i < argc - 1) {
2923 provided_network_port =
true;
2924 temp_int_arg = strtoul(argv[i + 1], &eptr, 10);
2926 if (*eptr ==
'\0') {
2927 network_port = temp_int_arg;
2934 #endif // MAVLINK_UDP 2943 if (!err_flag && stream_name !=
nullptr) {
2947 if (provided_device && !provided_network_port) {
2952 }
else if (provided_network_port && !provided_device) {
2953 inst = get_instance_for_network_port(network_port);
2954 #endif // MAVLINK_UDP 2956 }
else if (provided_device && provided_network_port) {
2957 PX4_WARN(
"please provide either a device name or a network port");
2965 if (inst !=
nullptr) {
2972 if (provided_device) {
2973 PX4_WARN(
"mavlink for device %s is not running", device_name);
2980 PX4_WARN(
"mavlink for network on port %hu is not running", network_port);
2983 #endif // MAVLINK_UDP 3001 #if defined(MAVLINK_UDP) 3005 !inst->broadcast_enabled() && inst->
get_protocol() == Protocol::UDP) {
3007 PX4_INFO(
"MAVLink only on localhost (set param MAV_BROADCAST = 1 to enable network)");
3010 #endif // MAVLINK_UDP 3017 PRINT_MODULE_DESCRIPTION(
3020 This module implements the MAVLink protocol, which can be used on a Serial link or UDP network connection. 3021 It communicates with the system via uORB: some messages are directly handled in the module (eg. mission 3022 protocol), others are published via uORB (eg. vehicle_command). 3024 Streams are used to send periodic messages with a specific rate, such as the vehicle attitude. 3025 When starting the mavlink instance, a mode can be specified, which defines the set of enabled streams with their rates. 3026 For a running instance, streams can be configured via `mavlink stream` command. 3028 There can be multiple independent instances of the module, each connected to one serial device or network port. 3031 The implementation uses 2 threads, a sending and a receiving thread. The sender runs at a fixed rate and dynamically 3032 reduces the rates of the streams if the combined bandwidth is higher than the configured rate (`-r`) or the 3033 physical link becomes saturated. This can be checked with `mavlink status`, see if `rate mult` is less than 1. 3035 **Careful**: some of the data is accessed and modified from both threads, so when changing code or extend the 3036 functionality, this needs to be take into account, in order to avoid race conditions and corrupt data. 3039 Start mavlink on ttyS1 serial with baudrate 921600 and maximum sending rate of 80kB/s: 3040 $ mavlink start -d /dev/ttyS1 -b 921600 -m onboard -r 80000 3042 Start mavlink on UDP port 14556 and enable the HIGHRES_IMU message with 50Hz: 3043 $ mavlink start -u 14556 -r 1000000 3044 $ mavlink stream -u 14556 -s HIGHRES_IMU -r 50 3047 PRINT_MODULE_USAGE_NAME("mavlink",
"communication");
3048 PRINT_MODULE_USAGE_COMMAND_DESCR(
"start",
"Start a new instance");
3049 PRINT_MODULE_USAGE_PARAM_STRING(
'd',
"/dev/ttyS1",
"<file:dev>",
"Select Serial Device",
true);
3050 PRINT_MODULE_USAGE_PARAM_INT(
'b', 57600, 9600, 3000000,
"Baudrate (can also be p:<param_name>)",
true);
3051 PRINT_MODULE_USAGE_PARAM_INT(
'r', 0, 10, 10000000,
"Maximum sending data rate in B/s (if 0, use baudrate / 20)",
true);
3052 #if defined(CONFIG_NET) || defined(__PX4_POSIX) 3053 PRINT_MODULE_USAGE_PARAM_INT(
'u', 14556, 0, 65536,
"Select UDP Network Port (local)",
true);
3054 PRINT_MODULE_USAGE_PARAM_INT(
'o', 14550, 0, 65536,
"Select UDP Network Port (remote)",
true);
3055 PRINT_MODULE_USAGE_PARAM_STRING(
't',
"127.0.0.1",
nullptr,
3056 "Partner IP (broadcasting can be enabled via MAV_BROADCAST param)",
true);
3058 PRINT_MODULE_USAGE_PARAM_STRING(
'm',
"normal",
"custom|camera|onboard|osd|magic|config|iridium|minimal|extvsision",
3059 "Mode: sets default streams and rates",
true);
3060 PRINT_MODULE_USAGE_PARAM_STRING(
'n',
nullptr,
"<interface_name>",
"wifi/ethernet interface name",
true);
3061 #if defined(CONFIG_NET_IGMP) && defined(CONFIG_NET_ROUTE) 3062 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);
3064 PRINT_MODULE_USAGE_PARAM_FLAG(
'f',
"Enable message forwarding to other Mavlink instances",
true);
3065 PRINT_MODULE_USAGE_PARAM_FLAG(
'w',
"Wait to send, until first message received",
true);
3066 PRINT_MODULE_USAGE_PARAM_FLAG(
'x',
"Enable FTP",
true);
3067 PRINT_MODULE_USAGE_PARAM_FLAG(
'z',
"Force flow control always on",
true);
3069 PRINT_MODULE_USAGE_COMMAND_DESCR(
"stop-all",
"Stop all instances");
3071 PRINT_MODULE_USAGE_COMMAND_DESCR(
"status",
"Print status for all instances");
3072 PRINT_MODULE_USAGE_ARG(
"streams",
"Print all enabled streams",
true);
3074 PRINT_MODULE_USAGE_COMMAND_DESCR(
"stream",
"Configure the sending rate of a stream for a running instance");
3075 #if defined(CONFIG_NET) || defined(__PX4_POSIX) 3076 PRINT_MODULE_USAGE_PARAM_INT(
'u', -1, 0, 65536,
"Select Mavlink instance via local Network Port",
true);
3078 PRINT_MODULE_USAGE_PARAM_STRING(
'd',
nullptr,
"<file:dev>",
"Select Mavlink instance via Serial Device",
true);
3079 PRINT_MODULE_USAGE_PARAM_STRING(
's',
nullptr,
nullptr,
"Mavlink stream to configure",
false);
3080 PRINT_MODULE_USAGE_PARAM_FLOAT(
'r', -1.0
f, 0.0
f, 2000.0
f,
"Rate in Hz (0 = turn off, -1 = set to default)",
false);
3082 PRINT_MODULE_USAGE_COMMAND_DESCR(
"boot_complete",
3083 "Enable sending of messages. (Must be) called as last step in startup script.");
3094 if (!strcmp(argv[1],
"start")) {
3097 }
else if (!strcmp(argv[1],
"stop")) {
3098 PX4_WARN(
"mavlink stop is deprecated, use stop-all instead");
3102 }
else if (!strcmp(argv[1],
"stop-all")) {
3105 }
else if (!strcmp(argv[1],
"status")) {
3106 bool show_streams_status = argc > 2 && strcmp(argv[2],
"streams") == 0;
3109 }
else if (!strcmp(argv[1],
"stream")) {
3112 }
else if (!strcmp(argv[1],
"boot_complete")) {
#define mavlink_log_info(_pub, _text,...)
Send a mavlink info message (not printed to console).
void check_radio_config()
Check the configuration of a connected radio.
constexpr _Tp constrain(_Tp val, _Tp min_val, _Tp max_val)
#define LL_FOREACH(head, el)
__EXPORT uint64_t px4_mavlink_lib_version_binary(void)
MAVLink lib version in binary form (first part of the git tag)
#define MAX_DATA_RATE
max data rate in bytes/s
static Mavlink * get_instance(int instance)
void handle_message(const mavlink_message_t *msg)
void begin_send()
This is the beginning of a MAVLINK_START_UART_SEND/MAVLINK_END_UART_SEND transaction.
#define mavlink_log_critical(_pub, _text,...)
Send a mavlink critical message and print to console.
static constexpr unsigned RADIO_BUFFER_LOW_PERCENTAGE
ringbuffer::RingBuffer _logbuffer
static struct vehicle_status_s status
void stop()
stop the stream.
uint32_t px4_firmware_version(void)
get the PX4 Firmware version
float current_data_rate() const
void set_manual_input_mode_generation(bool generation_enabled)
Set manual input generation mode.
Definition of geo / math functions to perform geodesic calculations.
bool _radio_status_available
uint64_t _mavlink_start_time
int _datarate
data rate for normal streams (attitude, position, etc.)
static int destroy_all_instances()
List< MavlinkOrbSubscription * > _subscriptions
bool get_flow_control_enabled()
void mavlink_end_uart_send(mavlink_channel_t chan, int length)
void send_statustext_info(const char *string)
Send a status text with loglevel INFO.
static const char * mavlink_mode_str(enum MAVLINK_MODE mode)
void message_buffer_mark_read(int n)
bool _is_usb_uart
Port is USB.
static constexpr unsigned RADIO_BUFFER_HALF_PERCENTAGE
int set_hil_enabled(bool hil_enabled)
Enable / disable Hardware in the Loop simulation mode.
bool _wait_to_transmit
Wait to transmit until received messages.
bool publish(const T &data)
Publish the struct.
List< MavlinkStream * > _streams
MAVLink 2.0 protocol interface definition.
mavlink_status_t * mavlink_get_channel_status(uint8_t channel)
uint64_t px4_os_version_binary(void)
Operating system version in binary form (first part of the git tag)
void send_statustext_emergency(const char *string)
Send a status text with loglevel EMERGENCY.
void check_requested_subscriptions()
void start_ack_received()
this is called when we got an vehicle_command_ack from the logger
int message_buffer_get_ptr(void **ptr, bool *is_part)
~Mavlink()
Destructor, also kills the mavlinks task.
mavlink_status_t * get_status()
uint64_t _last_write_try_time
enum MAVLINK_MODE get_mode()
int message_buffer_count()
static bool serial_instance_exists(const char *device_name, Mavlink *self)
unsigned get_free_tx_buf()
Get the free space in the transmit buffer.
void mavlink_update_parameters()
MavlinkULog * _mavlink_ulog
pthread_mutex_t _send_mutex
#define mavlink_and_console_log_info(_pub, _text,...)
Send a mavlink emergency message and print to console.
void update_rate_mult()
Update rate mult so total bitrate will be equal to _datarate.
void publish_telemetry_status()
bool _task_should_exit
Mavlink task should exit iff true.
int configure_stream(const char *stream_name, const float rate=-1.0f)
Configure a single stream.
#define DEFAULT_DEVICE_NAME
Protocol get_protocol() const
static char msg[NUM_MSG][CONFIG_USART1_TXBUFSIZE]
int handle_update(mavlink_channel_t channel)
periodic update method: check for ulog stream messages and handle retransmission. ...
int start()
Start the mavlink shell.
bool _transmitting_enabled_commanded
static constexpr int MAVLINK_MAX_INSTANCES
mavlink_system_t mavlink_system
#define ORB_ID(_name)
Generates a pointer to the uORB metadata structure for a given topic.
uint32_t px4_os_version(void)
operating system version
int mavlink_open_uart(const int baudrate=DEFAULT_BAUD_RATE, const char *uart_name=DEFAULT_DEVICE_NAME, const bool force_flow_control=false)
void set_interval(const int interval)
Get the interval.
void subscribe_from_beginning(bool from_beginning)
unsigned _main_loop_delay
mainloop delay, depends on data rate
void perf_count(perf_counter_t handle)
Count a performance event.
int enable_flow_control(enum FLOW_CONTROL_MODE enabled)
Enable / disable hardware flow control.
static int get_uart_fd(unsigned index)
static int get_status_all_instances(bool show_streams_status)
void perf_free(perf_counter_t handle)
Free a counter.
static hrt_abstime _first_start_time
void send_protocol_version()
Send the protocol version of MAVLink.
static constexpr int MAVLINK_MIN_INTERVAL
uint64_t _bytes_timestamp
#define MAIN_LOOP_DELAY
100 Hz @ 1000 bytes/s data rate
void display_status_streams()
Display the status of all enabled streams.
static bool _boot_complete
static int start_helper(int argc, char *argv[])
pthread_mutex_t _message_buffer_mutex
Vector< float, 6 > f(float t, const Matrix< float, 6, 1 > &, const Matrix< float, 3, 1 > &)
int get_instance_id() const
static hrt_abstime hrt_elapsed_time(const hrt_abstime *then)
Compute the delta between a timestamp taken in the past and now.
bool _transmitting_enabled
#define LL_APPEND(head, add)
int configure_streams_to_default(const char *configure_single_stream=nullptr)
Configure default streams according to _mode for either all streams or only a single stream...
int send_packet()
Flush the transmit buffer and send one MAVLink packet.
volatile bool _mavlink_ulog_stop_requested
bool update(uint64_t *time, void *data)
Check if subscription updated based on timestamp.
bool high_latency_data_link_lost
mavlink_message_buffer _message_buffer
void display_status()
Display the mavlink status.
uint64_t px4_firmware_version_binary(void)
Firmware version in binary form (first part of the git tag)
pthread_t _receive_thread
static int stream_command(int argc, char *argv[])
void perf_end(perf_counter_t handle)
End a performance event.
static constexpr int MAVLINK_MAX_INTERVAL
static void forward_message(const mavlink_message_t *msg, Mavlink *self)
static Mavlink * _mavlink_instances
mavlink_channel_t get_channel() const
void send_bytes(const uint8_t *buf, unsigned packet_len)
Send bytes out on the link.
MavlinkShell * get_shell()
get the Mavlink shell.
__BEGIN_DECLS typedef uint64_t hrt_abstime
Absolute time, in microsecond units.
static constexpr unsigned RADIO_BUFFER_CRITICAL_LOW_PERCENTAGE
bool _first_heartbeat_sent
mavlink_message_t * get_buffer()
void message_buffer_destroy()
Tools for system version detection.
void mavlink_start_uart_send(mavlink_channel_t chan, int length)
bool _hil_enabled
Hardware In the Loop mode.
static int instance_count()
void send_autopilot_capabilites()
Send the capabilities of this autopilot in terms of the MAVLink spec.
void set_protocol(Protocol p)
Set communication protocol for this mavlink instance.
#define FLOW_CONTROL_DISABLE_THRESHOLD
picked so that some messages still would fit it.
perf_counter_t _loop_interval_perf
loop performance counter
void pass_message(const mavlink_message_t *msg)
MavlinkStream * create_mavlink_stream(const char *stream_name, Mavlink *mavlink)
static void write(bootloader_app_shared_t *pshared)
void mavlink_send_uart_bytes(mavlink_channel_t chan, const uint8_t *ch, int length)
Send multiple chars (uint8_t) over a comm channel.
void resend_message(mavlink_message_t *msg)
Resend message as is, don't change sequence number and CRC.
uint32_t px4_firmware_vendor_version(void)
get the PX4 Firmware vendor version
bool update_if_changed(void *data)
Check if the subscription has been updated.
char * _subscribe_to_stream
uORB::PublicationQueued< telemetry_status_s > _telem_status_pub
MavlinkShell * _mavlink_shell
#define LL_DELETE(head, del)
static void receive_start(pthread_t *thread, Mavlink *parent)
Start the receiver thread.
float _subscribe_to_stream_rate
rate of stream to subscribe to (0=disable, -1=unlimited, -2=default)
orb_advert_t _mavlink_log_pub
int32_t _protocol_version_switch
#define mavlink_log_emergency(_pub, _text,...)
Send a mavlink emergency message and print to console.
MavlinkOrbSubscription * add_orb_subscription(const orb_id_t topic, int instance=0, bool disable_sharing=false)
Add a mavlink orb topic subscription while ensuring that only a single object exists for a given topi...
float maximum_data_rate() const
size_t read(uint8_t *buffer, size_t len)
Read from the shell.
void configure_stream_threadsafe(const char *stream_name, float rate=-1.0f)
#define CONFIG_CDCACM_PRODUCTID
void send_statustext_critical(const char *string)
Send a status text with loglevel CRITICAL.
void set_proto_version(unsigned version)
Set the MAVLink version.
static void initialize()
initialize: call this once on startup (this function is not thread-safe!)
void close_shell()
close the Mavlink shell if it is open
int message_buffer_init(int size)
telemetry_status_s _tstatus
static void initialize()
initialize: call this once on startup (this function is not thread-safe!)
const char * _device_name
FLOW_CONTROL_MODE _flow_control_mode
size_t available()
Get the number of bytes that can be read.
bool message_buffer_write(const void *ptr, int size)
mavlink_channel_t _channel
int task_main(int argc, char *argv[])
Main mavlink task.
bool _radio_status_critical
#define MAVLINK_NET_ADDED_STACK
uint32_t px4_board_version(void)
get the board version (last 8 bytes should be silicon ID, if any)
const char * _interface_name
void count_txbytes(unsigned n)
Count transmitted bytes.
void perf_begin(perf_counter_t handle)
Begin a performance event.
mavlink_message_t * mavlink_get_channel_buffer(uint8_t channel)
ping_statistics_s _ping_stats
int32_t _protocol_version
__EXPORT hrt_abstime hrt_absolute_time(void)
Get absolute time in [us] (does not wrap).
void set_telemetry_status_type(uint8_t type)
void count_txerrbytes(unsigned n)
Count bytes not transmitted because of errors.
static int start(int argc, char *argv[])
Start the mavlink task.
uint64_t _last_write_success_time
__EXPORT int mavlink_main(int argc, char *argv[])
Mavlink app start / stop handling function.
static void set_boot_complete()
Set the boot complete flag on all instances.
void update_radio_status(const radio_status_s &radio_status)
static Mavlink * get_instance_for_device(const char *device_name)