Mirror/ardupilot
5
0
Fork 0
mirror of https://github.com/ArduPilot/ardupilot synced 2025-01-03 14:38:30 -04:00
Code Issues Packages Projects Releases Wiki Activity

GCS_MAVLink: add simple stats to dataflash

Browse Source
This commit is contained in:
Peter Barker 2019-02-07 13:29:55 +11:00 committed by Peter Barker
parent 4ae64416c3
commit 9f27e91f33
2 changed files with 44 additions and 0 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

6
libraries/GCS_MAVLink/GCS.h
View File

@ -446,6 +446,8 @@ protected:
private:
void log_mavlink_stats();
MAV_RESULT _set_mode_common(const MAV_MODE base_mode, const uint32_t custom_mode);
virtual void handleMessage(mavlink_message_t * msg) = 0;
@ -687,6 +689,9 @@ private:
void zero_rc_outputs();
uint8_t last_tx_seq;
uint16_t send_packet_count;
#if GCS_DEBUG_SEND_MESSAGE_TIMINGS
struct {
uint32_t longest_time_us;
@ -701,6 +706,7 @@ private:
uint16_t max_slowdown_ms;
#endif
uint32_t last_mavlink_stats_logged;
};
/// @class GCS

38
libraries/GCS_MAVLink/GCS_Common.cpp
View File

@ -1266,6 +1266,15 @@ void GCS_MAVLINK::update_send()
try_send_message_stats.max_retry_deferred_body_type = 4;
}
#endif
// update the number of packets transmitted base on seqno, making
// the assumption that we don't send more than 256 messages
// between the last pass through here
mavlink_status_t *status = mavlink_get_channel_status(chan);
if (status != nullptr) {
send_packet_count += (status->current_tx_seq - last_tx_seq);
last_tx_seq = status->current_tx_seq;
}
}
void GCS_MAVLINK::remove_message_from_bucket(int8_t bucket, ap_message id)
@ -1510,6 +1519,14 @@ GCS_MAVLINK::update_receive(uint32_t max_time_us)
}
}
// consider logging mavlink stats to dataflash:
if (is_active() || is_streaming()) {
if (tnow - last_mavlink_stats_logged > 1000) {
log_mavlink_stats();
last_mavlink_stats_logged = tnow;
}
}
#if GCS_DEBUG_SEND_MESSAGE_TIMINGS
const uint16_t now16_ms{AP_HAL::millis16()};
@ -1593,6 +1610,27 @@ GCS_MAVLINK::update_receive(uint32_t max_time_us)
hal.util->perf_end(_perf_update);
}
/*
record stats about this link to dataflash
*/
void GCS_MAVLINK::log_mavlink_stats()
{
mavlink_status_t *status = mavlink_get_channel_status(chan);
if (status == nullptr) {
return;
}
const struct log_MAV pkt = {
LOG_PACKET_HEADER_INIT(LOG_MAV_MSG),
time_us : AP_HAL::micros64(),
chan : (uint8_t)chan,
packet_tx_count : send_packet_count,
packet_rx_success_count: status->packet_rx_success_count,
packet_rx_drop_count : status->packet_rx_drop_count
};
AP::logger().WriteBlock(&pkt, sizeof(pkt));
}
/*
send the SYSTEM_TIME message