diff --git a/msg/telemetry_status.msg b/msg/telemetry_status.msg index e7c3a69509..af767d4dcc 100644 --- a/msg/telemetry_status.msg +++ b/msg/telemetry_status.msg @@ -28,7 +28,6 @@ uint32 tx_buffer_overruns # number of TX buffer overruns 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 diff --git a/src/modules/mavlink/mavlink_receiver.cpp b/src/modules/mavlink/mavlink_receiver.cpp index 318a092a2e..9c57817834 100644 --- a/src/modules/mavlink/mavlink_receiver.cpp +++ b/src/modules/mavlink/mavlink_receiver.cpp @@ -1,6 +1,6 @@ /**************************************************************************** * - * Copyright (c) 2012-2019 PX4 Development Team. All rights reserved. + * Copyright (c) 2012-2021 PX4 Development Team. All rights reserved. * * Redistribution and use in source and binary forms, with or without * modification, are permitted provided that the following conditions @@ -71,43 +71,6 @@ #define MAVLINK_RECEIVER_NET_ADDED_STACK 0 #endif -const uint8_t MavlinkReceiver::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, -}; - MavlinkReceiver::~MavlinkReceiver() { delete _tune_publisher; @@ -3045,7 +3008,6 @@ 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)) { @@ -3077,99 +3039,7 @@ 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 && !_reported_unsupported_comp_id) { - PX4_WARN("unsupported component id, msgid: %d, sysid: %d compid: %d", msg.msgid, msg.sysid, msg.compid); - _reported_unsupported_comp_id = true; - } - - 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); - } + update_rx_stats(msg); } } @@ -3179,9 +3049,8 @@ MavlinkReceiver::Run() 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; + tstatus.rx_message_lost_rate = static_cast(_total_lost_counter) / static_cast(_total_received_counter); if (_mavlink_status_last_buffer_overrun != _status.buffer_overrun) { tstatus.rx_buffer_overruns++; @@ -3232,6 +3101,59 @@ MavlinkReceiver::Run() } } +void MavlinkReceiver::update_rx_stats(const mavlink_message_t &message) +{ + const bool component_states_has_still_space = [this, &message]() { + for (unsigned i = 0; i < MAX_REMOTE_COMPONENTS; ++i) { + if (_component_states[i].system_id == message.sysid && _component_states[i].component_id == message.compid) { + + int lost_messages = 0; + const uint8_t expected_seq = _component_states[i].last_sequence + 1; + + // Account for overflow during packet loss + if (message.seq < expected_seq) { + lost_messages = (message.seq + 255) - expected_seq; + + } else { + lost_messages = message.seq - expected_seq; + } + + _component_states[i].missed_messages += lost_messages; + + ++_component_states[i].received_messages; + _component_states[i].last_time_received_ms = hrt_absolute_time() / 1000; + _component_states[i].last_sequence = message.seq; + + // Also update overall stats + ++_total_received_counter; + _total_lost_counter += lost_messages; + + return true; + + } else if (_component_states[i].system_id == 0 && _component_states[i].component_id == 0) { + _component_states[i].system_id = message.sysid; + _component_states[i].component_id = message.compid; + + ++_component_states[i].received_messages; + _component_states[i].last_time_received_ms = hrt_absolute_time() / 1000; + _component_states[i].last_sequence = message.seq; + + // Also update overall stats + ++_total_received_counter; + + return true; + } + } + + return false; + }(); + + if (!component_states_has_still_space && !_warned_component_states_full_once) { + PX4_WARN("Max remote components of %u used up", MAX_REMOTE_COMPONENTS); + _warned_component_states_full_once = true; + } +} + void * MavlinkReceiver::start_helper(void *context) { diff --git a/src/modules/mavlink/mavlink_receiver.h b/src/modules/mavlink/mavlink_receiver.h index 352358d798..020d36c882 100644 --- a/src/modules/mavlink/mavlink_receiver.h +++ b/src/modules/mavlink/mavlink_receiver.h @@ -1,6 +1,6 @@ /**************************************************************************** * - * Copyright (c) 2012-2020 PX4 Development Team. All rights reserved. + * Copyright (c) 2012-2021 PX4 Development Team. All rights reserved. * * Redistribution and use in source and binary forms, with or without * modification, are permitted provided that the following conditions @@ -225,6 +225,7 @@ private: void schedule_tune(const char *tune); + void update_rx_stats(const mavlink_message_t &message); Mavlink *_mavlink; @@ -238,60 +239,20 @@ private: orb_advert_t _mavlink_log_pub{nullptr}; - // 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 + static constexpr int MAX_REMOTE_COMPONENTS{8}; + struct ComponentState { + uint32_t last_time_received_ms{0}; + uint32_t received_messages{0}; + uint32_t missed_messages{0}; + uint8_t system_id{0}; + uint8_t component_id{0}; + uint8_t last_sequence{0}; }; + ComponentState _component_states[MAX_REMOTE_COMPONENTS] {}; + bool _warned_component_states_full_once{false}; - // map of supported component IDs to MAV_COMP value - static const uint8_t supported_component_map[COMP_ID_MAX]; - - bool _reported_unsupported_comp_id{false}; - - 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};