From cbef055f6bb892ecb2dfdfc151ce6df920f65f6e Mon Sep 17 00:00:00 2001
From: bugobliterator <siddharthbharatpurohit@gmail.com>
Date: Wed, 24 May 2023 16:02:20 +1000
Subject: [PATCH] AP_DroneCAN: add msg period measurement to DroneCAN_sniffer

---
 .../DroneCAN_sniffer/DroneCAN_sniffer.cpp     | 28 ++++++++++++++++++-
 1 file changed, 27 insertions(+), 1 deletion(-)

diff --git a/libraries/AP_DroneCAN/examples/DroneCAN_sniffer/DroneCAN_sniffer.cpp b/libraries/AP_DroneCAN/examples/DroneCAN_sniffer/DroneCAN_sniffer.cpp
index 7f09bc2c331..97a7850d41e 100644
--- a/libraries/AP_DroneCAN/examples/DroneCAN_sniffer/DroneCAN_sniffer.cpp
+++ b/libraries/AP_DroneCAN/examples/DroneCAN_sniffer/DroneCAN_sniffer.cpp
@@ -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");
 }