diff --git a/libraries/AP_HAL_Linux/Perf.cpp b/libraries/AP_HAL_Linux/Perf.cpp
index 89b310b060..134b7bd494 100644
--- a/libraries/AP_HAL_Linux/Perf.cpp
+++ b/libraries/AP_HAL_Linux/Perf.cpp
@@ -15,6 +15,7 @@
* You should have received a copy of the GNU General Public License along
* with this program. If not, see .
*/
+#include
#include
#include
@@ -48,6 +49,45 @@ Perf *Perf::get_instance()
return _instance;
}
+void Perf::_debug_counters()
+{
+ uint64_t now = AP_HAL::millis64();
+
+ if (now - _last_debug_msec < 5000) {
+ return;
+ }
+
+ pthread_rwlock_rdlock(&_perf_counters_lock);
+ unsigned int uc = _update_count;
+ auto v = _perf_counters;
+ pthread_rwlock_unlock(&_perf_counters_lock);
+
+ if (uc != _update_count) {
+ fprintf(stderr, "WARNING!! potentially wrong counters!!!");
+ }
+
+ for (auto &c : v) {
+ if (!c.count) {
+ fprintf(stderr, "%-30s\t"
+ "(no events)\n", c.name);
+ } else if (c.type == Util::PC_ELAPSED) {
+ fprintf(stderr, "%-30s\t"
+ "count: %llu\t"
+ "min: %llu\t"
+ "max: %llu\t"
+ "avg: %.4f\t"
+ "stddev: %.4f\n",
+ c.name, c.count, c.least, c.most, c.mean, sqrt(c.m2));
+ } else {
+ fprintf(stderr, "%-30s\t"
+ "count: %llu\n",
+ c.name, c.count);
+ }
+ }
+
+ _last_debug_msec = now;
+}
+
Perf::Perf()
{
if (pthread_rwlock_init(&_perf_counters_lock, nullptr) != 0) {
@@ -58,6 +98,10 @@ Perf::Perf()
* number of perf counters for now; if we grow more, it will just
* reallocate the memory pool */
_perf_counters.reserve(50);
+
+#ifdef DEBUG_PERF
+ hal.scheduler->register_timer_process(FUNCTOR_BIND_MEMBER(&Perf::_debug_counters, void));
+#endif
}
void Perf::begin(Util::perf_counter_t pc)
diff --git a/libraries/AP_HAL_Linux/Perf.h b/libraries/AP_HAL_Linux/Perf.h
index eb16051ace..7ad8474176 100644
--- a/libraries/AP_HAL_Linux/Perf.h
+++ b/libraries/AP_HAL_Linux/Perf.h
@@ -80,6 +80,10 @@ private:
Perf();
+ void _debug_counters();
+
+ uint64_t _last_debug_msec;
+
std::vector _perf_counters;
/* synchronize addition of new perf counters */