aepko/Ardupilot2
1
0
Fork 0
Code Issues Pull Requests Actions Packages Projects Releases Wiki Activity

AP_DroneCAN: add msg period measurement to DroneCAN_sniffer

Browse Source
This commit is contained in:
bugobliterator 2023-05-24 16:02:20 +10:00 committed by Andrew Tridgell
parent efe5fb7e69
commit cbef055f6b
1 changed files with 27 additions and 1 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

28
libraries/AP_DroneCAN/examples/DroneCAN_sniffer/DroneCAN_sniffer.cpp
View File

@ -48,6 +48,10 @@ private:
static struct {
const char *msg_name;
uint32_t count;
uint64_t last_time_us;
uint32_t avg_period_us;
uint32_t max_period_us;
uint32_t min_period_us;
} counters[100];
static void count_msg(const char *name)
@ -55,11 +59,26 @@ static void count_msg(const char *name)
for (uint16_t i=0; i<ARRAY_SIZE(counters); i++) {
if (counters[i].msg_name == name) {
counters[i].count++;
uint64_t now = AP_HAL::micros64();
uint32_t period_us = now - counters[i].last_time_us;
counters[i].last_time_us = now;
if (counters[i].avg_period_us == 0) {
counters[i].avg_period_us = period_us;
} else {
counters[i].avg_period_us = (counters[i].avg_period_us * (counters[i].count - 1) + period_us) / counters[i].count;
}
if (counters[i].max_period_us < period_us) {
counters[i].max_period_us = period_us;
}
if (counters[i].min_period_us == 0 || counters[i].min_period_us > period_us) {
counters[i].min_period_us = period_us;
}
break;
}
if (counters[i].msg_name == nullptr) {
counters[i].msg_name = name;
counters[i].count++;
counters[i].last_time_us = AP_HAL::micros64();
break;
}
}
@ -187,8 +206,15 @@ void DroneCAN_sniffer::print_stats(void)
if (counters[i].msg_name == nullptr) {
break;
}
hal.console->printf("%s: %u\n", counters[i].msg_name, unsigned(counters[i].count));
hal.console->printf("%s: %lu AVG_US: %lu MAX_US: %lu MIN_US: %lu\n", counters[i].msg_name,
(long unsigned)counters[i].count,
(long unsigned)counters[i].avg_period_us,
(long unsigned)counters[i].max_period_us,
(long unsigned)counters[i].min_period_us);
counters[i].count = 0;
counters[i].avg_period_us = 0;
counters[i].max_period_us = 0;
counters[i].min_period_us = 0;
}
hal.console->printf("\n");
}