diff --git a/.ci/Jenkinsfile-hardware b/.ci/Jenkinsfile-hardware index eae4080920..1e23b48ef0 100644 --- a/.ci/Jenkinsfile-hardware +++ b/.ci/Jenkinsfile-hardware @@ -857,6 +857,7 @@ void statusFTDI() { sh './Tools/HIL/run_nsh_cmd.py --device `find /dev/serial -name *usb-FTDI_*` --cmd "listener sensors_status_imu"' sh './Tools/HIL/run_nsh_cmd.py --device `find /dev/serial -name *usb-FTDI_*` --cmd "listener px4io_status"' sh './Tools/HIL/run_nsh_cmd.py --device `find /dev/serial -name *usb-FTDI_*` --cmd "listener system_power"' + sh './Tools/HIL/run_nsh_cmd.py --device `find /dev/serial -name *usb-FTDI_*` --cmd "listener telemetry_status"' sh './Tools/HIL/run_nsh_cmd.py --device `find /dev/serial -name *usb-FTDI_*` --cmd "listener vehicle_air_data"' sh './Tools/HIL/run_nsh_cmd.py --device `find /dev/serial -name *usb-FTDI_*` --cmd "listener vehicle_angular_acceleration"' sh './Tools/HIL/run_nsh_cmd.py --device `find /dev/serial -name *usb-FTDI_*` --cmd "listener vehicle_angular_velocity"' @@ -936,6 +937,7 @@ void statusSEGGER() { sh './Tools/HIL/run_nsh_cmd.py --device `find /dev/serial -name *usb-SEGGER_*` --cmd "listener sensors_status_imu"' sh './Tools/HIL/run_nsh_cmd.py --device `find /dev/serial -name *usb-SEGGER_*` --cmd "listener px4io_status"' sh './Tools/HIL/run_nsh_cmd.py --device `find /dev/serial -name *usb-SEGGER_*` --cmd "listener system_power"' + sh './Tools/HIL/run_nsh_cmd.py --device `find /dev/serial -name *usb-SEGGER_*` --cmd "listener telemetry_status"' sh './Tools/HIL/run_nsh_cmd.py --device `find /dev/serial -name *usb-SEGGER_*` --cmd "listener vehicle_air_data"' sh './Tools/HIL/run_nsh_cmd.py --device `find /dev/serial -name *usb-SEGGER_*` --cmd "listener vehicle_angular_acceleration"' sh './Tools/HIL/run_nsh_cmd.py --device `find /dev/serial -name *usb-SEGGER_*` --cmd "listener vehicle_angular_velocity"' diff --git a/msg/telemetry_status.msg b/msg/telemetry_status.msg index de64b94120..e7c3a69509 100644 --- a/msg/telemetry_status.msg +++ b/msg/telemetry_status.msg @@ -17,14 +17,23 @@ bool ftp uint8 streams -float32 data_rate +float32 data_rate # configured maximum data rate (Bytes/s) float32 rate_multiplier -float32 rate_rx +float32 tx_rate_avg # transmit rate average (Bytes/s) +float32 tx_error_rate_avg # transmit error rate average (Bytes/s) +uint32 tx_message_count # total message sent count +uint32 tx_buffer_overruns # number of TX buffer overruns -float32 rate_tx -float32 rate_txerr +float32 rx_rate_avg # transmit rate average (Bytes/s) +uint32 rx_message_count # count of total messages received +uint32 rx_message_count_supported # count of total messages received from supported systems and components (for loss statistics) +uint32 rx_message_lost_count +uint32 rx_buffer_overruns # number of RX buffer overruns +uint32 rx_parse_errors # number of parse errors +uint32 rx_packet_drop_count # number of packet drops +float32 rx_message_lost_rate uint64 HEARTBEAT_TIMEOUT_US = 1500000 # Heartbeat timeout 1.5 seconds diff --git a/platforms/common/include/px4_platform_common/posix.h b/platforms/common/include/px4_platform_common/posix.h index e538885fbc..fec109e86c 100644 --- a/platforms/common/include/px4_platform_common/posix.h +++ b/platforms/common/include/px4_platform_common/posix.h @@ -81,7 +81,7 @@ typedef pollevent_t px4_pollevent_t; #elif defined(__PX4_POSIX) -#define PX4_STACK_OVERHEAD (1024 * 11) +#define PX4_STACK_OVERHEAD (1024 * 24) __BEGIN_DECLS diff --git a/src/modules/mavlink/mavlink_main.cpp b/src/modules/mavlink/mavlink_main.cpp index 6183daef64..75978a6826 100644 --- a/src/modules/mavlink/mavlink_main.cpp +++ b/src/modules/mavlink/mavlink_main.cpp @@ -217,7 +217,6 @@ Mavlink::~Mavlink() perf_free(_loop_perf); perf_free(_loop_interval_perf); perf_free(_send_byte_error_perf); - perf_free(_send_start_tx_buf_low); } void @@ -789,11 +788,11 @@ void Mavlink::send_start(int length) // not enough space in buffer to send count_txerrbytes(length); + _tstatus.tx_buffer_overruns++; + // prevent writes _tx_buffer_low = true; - perf_count(_send_start_tx_buf_low); - } else { _tx_buffer_low = false; } @@ -854,6 +853,7 @@ void Mavlink::send_finish() #endif // MAVLINK_UDP if (ret == (int)_buf_fill) { + _tstatus.tx_message_count++; count_txbytes(_buf_fill); _last_write_success_time = _last_write_try_time; @@ -1494,15 +1494,14 @@ Mavlink::update_rate_mult() float hardware_mult = 1.0f; - /* scale down if we have a TX err rate suggesting link congestion */ - if (_tstatus.rate_txerr > 0.0f && !_radio_status_critical) { - hardware_mult = (_tstatus.rate_tx) / (_tstatus.rate_tx + _tstatus.rate_txerr); + // scale down if we have a TX err rate suggesting link congestion + if ((_tstatus.tx_error_rate_avg > 0.f) && !_radio_status_critical) { + hardware_mult = _tstatus.tx_rate_avg / (_tstatus.tx_rate_avg + _tstatus.tx_error_rate_avg); } else if (_radio_status_available) { // check for RADIO_STATUS timeout and reset - if (hrt_elapsed_time(&_rstatus.timestamp) > (_param_mav_radio_timeout.get() * - 1_s)) { + if (hrt_elapsed_time(&_rstatus.timestamp) > (_param_mav_radio_timeout.get() * 1_s)) { PX4_ERR("instance %d: RADIO_STATUS timeout", _instance_id); _radio_status_available = false; @@ -2233,7 +2232,7 @@ Mavlink::task_main(int argc, char *argv[]) perf_count(_loop_interval_perf); perf_begin(_loop_perf); - hrt_abstime t = hrt_absolute_time(); + const hrt_abstime t = hrt_absolute_time(); update_rate_mult(); @@ -2464,13 +2463,13 @@ Mavlink::task_main(int argc, char *argv[]) } /* update TX/RX rates*/ - if (t > _bytes_timestamp + 1000000) { + if (t > _bytes_timestamp + 1_s) { if (_bytes_timestamp != 0) { - const float dt = (t - _bytes_timestamp) / 1000.0f; + const float dt = (t - _bytes_timestamp) * 1e-6f; - _tstatus.rate_tx = _bytes_tx / dt; - _tstatus.rate_txerr = _bytes_txerr / dt; - _tstatus.rate_rx = _bytes_rx / dt; + _tstatus.tx_rate_avg = _bytes_tx / dt; + _tstatus.tx_error_rate_avg = _bytes_txerr / dt; + _tstatus.rx_rate_avg = _bytes_rx / dt; _bytes_tx = 0; _bytes_txerr = 0; @@ -2785,11 +2784,12 @@ Mavlink::display_status() printf("\tflow control: %s\n", _flow_control_mode ? "ON" : "OFF"); printf("\trates:\n"); - printf("\t tx: %.3f kB/s\n", (double)_tstatus.rate_tx); - printf("\t txerr: %.3f kB/s\n", (double)_tstatus.rate_txerr); + printf("\t tx: %.1f B/s\n", (double)_tstatus.tx_rate_avg); + printf("\t txerr: %.1f B/s\n", (double)_tstatus.tx_error_rate_avg); printf("\t tx rate mult: %.3f\n", (double)_rate_mult); printf("\t tx rate max: %i B/s\n", _datarate); - printf("\t rx: %.3f kB/s\n", (double)_tstatus.rate_rx); + printf("\t rx: %.1f B/s\n", (double)_tstatus.rx_rate_avg); + printf("\t rx loss: %.1f%%\n", (double)_tstatus.rx_message_lost_rate); if (_mavlink_ulog) { printf("\tULog rate: %.1f%% of max %.1f%%\n", (double)_mavlink_ulog->current_data_rate() * 100., diff --git a/src/modules/mavlink/mavlink_main.h b/src/modules/mavlink/mavlink_main.h index e33c5638a8..afa4743eea 100644 --- a/src/modules/mavlink/mavlink_main.h +++ b/src/modules/mavlink/mavlink_main.h @@ -493,9 +493,6 @@ public: if (_mavlink_ulog) { _mavlink_ulog_stop_requested = true; } } - - void set_uorb_main_fd(int fd, unsigned int interval); - bool ftp_enabled() const { return _ftp_on; } bool hash_check_enabled() const { return _param_mav_hash_chk_en.get(); } @@ -611,7 +608,7 @@ private: unsigned _bytes_tx{0}; unsigned _bytes_txerr{0}; unsigned _bytes_rx{0}; - uint64_t _bytes_timestamp{0}; + hrt_abstime _bytes_timestamp{0}; #if defined(MAVLINK_UDP) sockaddr_in _myaddr {}; @@ -677,7 +674,6 @@ private: perf_counter_t _loop_perf{perf_alloc(PC_ELAPSED, MODULE_NAME": tx run elapsed")}; /**< loop performance counter */ perf_counter_t _loop_interval_perf{perf_alloc(PC_INTERVAL, MODULE_NAME": tx run interval")}; /**< loop interval performance counter */ perf_counter_t _send_byte_error_perf{perf_alloc(PC_COUNT, MODULE_NAME": send_bytes error")}; /**< send bytes error count */ - perf_counter_t _send_start_tx_buf_low{perf_alloc(PC_COUNT, MODULE_NAME": send_start tx buffer full")}; /**< available tx buffer smaller than message */ void mavlink_update_parameters(); diff --git a/src/modules/mavlink/mavlink_receiver.cpp b/src/modules/mavlink/mavlink_receiver.cpp index c0be1709a6..f73f8a7b88 100644 --- a/src/modules/mavlink/mavlink_receiver.cpp +++ b/src/modules/mavlink/mavlink_receiver.cpp @@ -3056,6 +3056,7 @@ MavlinkReceiver::Run() /* 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)) { + _total_received_counter++; /* check if we received version 2 and request a switch. */ if (!(_mavlink->get_status()->flags & MAVLINK_STATUS_FLAG_IN_MAVLINK1)) { @@ -3086,12 +3087,126 @@ MavlinkReceiver::Run() /* handle packet with parent object */ _mavlink->handle_message(&msg); + + + // calculate lost messages for this system id + bool px4_sysid_index_found = false; + int px4_sysid_index = 0; + + if (msg.sysid != mavlink_system.sysid) { + for (int sys_id = 1; sys_id < MAX_REMOTE_SYSTEM_IDS; sys_id++) { + if (_system_id_map[sys_id] == msg.sysid) { + // slot found + px4_sysid_index_found = true; + px4_sysid_index = sys_id; + break; + } + } + + // otherwise record newly seen system id in first available slot + if (!px4_sysid_index_found) { + for (int sys_id = 1; sys_id < MAX_REMOTE_SYSTEM_IDS; sys_id++) { + if (_system_id_map[sys_id] == 0) { + // slot available + px4_sysid_index_found = true; + px4_sysid_index = sys_id; + _system_id_map[sys_id] = msg.sysid; + break; + } + } + } + + if (!px4_sysid_index_found) { + PX4_ERR("not enough system id slots (%d)", MAX_REMOTE_SYSTEM_IDS); + } + + } else { + px4_sysid_index_found = true; + } + + // find PX4 component id + uint8_t px4_comp_id = 0; + bool px4_comp_id_found = false; + + for (int id = 0; id < COMP_ID_MAX; id++) { + if (supported_component_map[id] == msg.compid) { + px4_comp_id = id; + px4_comp_id_found = true; + break; + } + } + + if (!px4_comp_id_found) { + PX4_WARN("unsupported component id, msgid: %d, sysid: %d compid: %d", msg.msgid, msg.sysid, msg.compid); + } + + if (px4_comp_id_found && px4_sysid_index_found) { + // Increase receive counter + _total_received_supported_counter++; + + uint8_t last_seq = _last_index[px4_sysid_index][px4_comp_id]; + uint8_t expected_seq = last_seq + 1; + + // Determine what the next expected sequence number is, accounting for + // never having seen a message for this system/component pair. + if (!_sys_comp_present[px4_sysid_index][px4_comp_id]) { + _sys_comp_present[px4_sysid_index][px4_comp_id] = true; + last_seq = msg.seq; + expected_seq = msg.seq; + } + + // And if we didn't encounter that sequence number, record the error + if (msg.seq != expected_seq) { + int lost_messages = 0; + + // Account for overflow during packet loss + if (msg.seq < expected_seq) { + lost_messages = (msg.seq + 255) - expected_seq; + + } else { + lost_messages = msg.seq - expected_seq; + } + + // Log how many were lost + _total_lost_counter += lost_messages; + } + + // And update the last sequence number for this system/component pair + _last_index[px4_sysid_index][px4_comp_id] = msg.seq; + + // Calculate new loss ratio + const float total_sent = _total_received_supported_counter + _total_lost_counter; + float rx_loss_percent = (_total_lost_counter / total_sent) * 100.f; + + _running_loss_percent = (rx_loss_percent * 0.5f) + (_running_loss_percent * 0.5f); + } } } /* count received bytes (nread will be -1 on read error) */ if (nread > 0) { _mavlink->count_rxbytes(nread); + + telemetry_status_s &tstatus = _mavlink->telemetry_status(); + tstatus.rx_message_count = _total_received_counter; + tstatus.rx_message_count_supported = _total_received_supported_counter; + tstatus.rx_message_lost_count = _total_lost_counter; + tstatus.rx_message_lost_rate = _running_loss_percent; + + if (_mavlink_status_last_buffer_overrun != _status.buffer_overrun) { + tstatus.rx_buffer_overruns++; + _mavlink_status_last_buffer_overrun = _status.buffer_overrun; + } + + if (_mavlink_status_last_parse_error != _status.parse_error) { + tstatus.rx_parse_errors++; + _mavlink_status_last_parse_error = _status.parse_error; + } + + if (_mavlink_status_last_packet_rx_drop_count != _status.packet_rx_drop_count) { + tstatus.rx_packet_drop_count++; + _mavlink_status_last_packet_rx_drop_count = _status.packet_rx_drop_count; + } } #if defined(MAVLINK_UDP) diff --git a/src/modules/mavlink/mavlink_receiver.h b/src/modules/mavlink/mavlink_receiver.h index 3104ed95cb..1c8171054c 100644 --- a/src/modules/mavlink/mavlink_receiver.h +++ b/src/modules/mavlink/mavlink_receiver.h @@ -252,6 +252,98 @@ private: mavlink_status_t _status{}; ///< receiver status, used for mavlink_parse_char() + // subset of MAV_COMPONENTs we support + enum SUPPORTED_COMPONENTS : uint8_t { + COMP_ID_ALL, + COMP_ID_AUTOPILOT1, + + COMP_ID_TELEMETRY_RADIO, + + COMP_ID_CAMERA, + COMP_ID_CAMERA2, + + COMP_ID_GIMBAL, + COMP_ID_LOG, + COMP_ID_ADSB, + COMP_ID_OSD, + COMP_ID_PERIPHERAL, + + COMP_ID_FLARM, + + COMP_ID_GIMBAL2, + + COMP_ID_MISSIONPLANNER, + COMP_ID_ONBOARD_COMPUTER, + + COMP_ID_PATHPLANNER, + COMP_ID_OBSTACLE_AVOIDANCE, + COMP_ID_VISUAL_INERTIAL_ODOMETRY, + COMP_ID_PAIRING_MANAGER, + + COMP_ID_IMU, + + COMP_ID_GPS, + COMP_ID_GPS2, + + COMP_ID_UDP_BRIDGE, + COMP_ID_UART_BRIDGE, + COMP_ID_TUNNEL_NODE, + + COMP_ID_MAX + }; + + // map of supported component IDs to MAV_COMP value + const uint8_t supported_component_map[COMP_ID_MAX] { + [COMP_ID_ALL] = MAV_COMP_ID_ALL, + [COMP_ID_AUTOPILOT1] = MAV_COMP_ID_AUTOPILOT1, + + [COMP_ID_TELEMETRY_RADIO] = MAV_COMP_ID_TELEMETRY_RADIO, + + [COMP_ID_CAMERA] = MAV_COMP_ID_CAMERA, + [COMP_ID_CAMERA2] = MAV_COMP_ID_CAMERA2, + + [COMP_ID_GIMBAL] = MAV_COMP_ID_GIMBAL, + [COMP_ID_LOG] = MAV_COMP_ID_LOG, + [COMP_ID_ADSB] = MAV_COMP_ID_ADSB, + [COMP_ID_OSD] = MAV_COMP_ID_OSD, + [COMP_ID_PERIPHERAL] = MAV_COMP_ID_PERIPHERAL, + + [COMP_ID_FLARM] = MAV_COMP_ID_FLARM, + + [COMP_ID_GIMBAL2] = MAV_COMP_ID_GIMBAL2, + + [COMP_ID_MISSIONPLANNER] = MAV_COMP_ID_MISSIONPLANNER, + [COMP_ID_ONBOARD_COMPUTER] = MAV_COMP_ID_ONBOARD_COMPUTER, + + [COMP_ID_PATHPLANNER] = MAV_COMP_ID_PATHPLANNER, + [COMP_ID_OBSTACLE_AVOIDANCE] = MAV_COMP_ID_OBSTACLE_AVOIDANCE, + [COMP_ID_VISUAL_INERTIAL_ODOMETRY] = MAV_COMP_ID_VISUAL_INERTIAL_ODOMETRY, + [COMP_ID_PAIRING_MANAGER] = MAV_COMP_ID_PAIRING_MANAGER, + + [COMP_ID_IMU] = MAV_COMP_ID_IMU, + + [COMP_ID_GPS] = MAV_COMP_ID_GPS, + [COMP_ID_GPS2] = MAV_COMP_ID_GPS2, + + [COMP_ID_UDP_BRIDGE] = MAV_COMP_ID_UDP_BRIDGE, + [COMP_ID_UART_BRIDGE] = MAV_COMP_ID_UART_BRIDGE, + [COMP_ID_TUNNEL_NODE] = MAV_COMP_ID_TUNNEL_NODE, + }; + + static constexpr int MAX_REMOTE_SYSTEM_IDS{8}; + uint8_t _system_id_map[MAX_REMOTE_SYSTEM_IDS] {}; + + uint8_t _last_index[MAX_REMOTE_SYSTEM_IDS][COMP_ID_MAX] {}; ///< Store the last received sequence ID for each system/componenet pair + uint8_t _sys_comp_present[MAX_REMOTE_SYSTEM_IDS][COMP_ID_MAX] {}; ///< First message flag + uint64_t _total_received_counter{0}; ///< The total number of successfully received messages + uint64_t _total_received_supported_counter{0}; ///< The total number of successfully received messages + uint64_t _total_lost_counter{0}; ///< Total messages lost during transmission. + float _running_loss_percent{0}; ///< Loss rate + + uint8_t _mavlink_status_last_buffer_overrun{0}; + uint8_t _mavlink_status_last_parse_error{0}; + uint16_t _mavlink_status_last_packet_rx_drop_count{0}; + // ORB publications uORB::Publication _actuator_controls_pubs[4] {ORB_ID(actuator_controls_0), ORB_ID(actuator_controls_1), ORB_ID(actuator_controls_2), ORB_ID(actuator_controls_3)}; uORB::Publication _airspeed_pub{ORB_ID(airspeed)};