mirror of
https://gitee.com/mirrors_PX4/PX4-Autopilot.git
synced 2026-04-14 10:07:39 +08:00
mavlink: receiver estimate total lost messages
This commit is contained in:
Daniel Agar
Lorenz Meier
parent
fbd64fbdd8
commit
0c58d12216
@ -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"'
|
||||
|
||||
@ -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
|
||||
|
||||
@ -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
|
||||
|
||||
|
||||
@ -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.,
|
||||
|
||||
@ -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();
|
||||
|
||||
|
||||
@ -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)
|
||||
|
||||
@ -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_s> _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_s> _airspeed_pub{ORB_ID(airspeed)};
|
||||
|
||||
Loading…
x
Reference in New Issue
Block a user