From 3a7a77d49e620420ee33996eec0a70a7c186f6bb Mon Sep 17 00:00:00 2001
From: Kevin O'Connor <kevin@koconnor.net>
Date: Mon, 9 Jan 2017 23:08:23 -0500
Subject: [PATCH] basecmd: Improve accuracy of stats "sumsq" variable

Use a base of 256 instead of 65536 when calculating the sum of the
square of the clock differences in the stats.  This makes the
calculation more accurate.  Export the new base via DECL_CONSTANT for
the host to access.  Use DIV_ROUND_UP() when adjusting for the base to
ensure no lost ticks.  Do the division after multiplication in the
common case where the time between stats_task() invocations is less
than 64K ticks.

Signed-off-by: Kevin O'Connor <kevin@koconnor.net>
---
 klippy/mcu.py |  9 +++++----
 src/basecmd.c | 15 +++++++++++++--
 2 files changed, 18 insertions(+), 6 deletions(-)

diff --git a/klippy/mcu.py b/klippy/mcu.py
index 33a70076..5d8fb31f 100644
--- a/klippy/mcu.py
+++ b/klippy/mcu.py
@@ -333,6 +333,7 @@ class MCU:
         self._print_start_time = 0.
         self._mcu_freq = 0.
         # Stats
+        self._stats_sumsq_base = 0.
         self._mcu_tick_avg = 0.
         self._mcu_tick_stddev = 0.
     def handle_mcu_stats(self, params):
@@ -341,10 +342,8 @@ class MCU:
         tick_sum = params['sum']
         c = 1.0 / (count * self._mcu_freq)
         self._mcu_tick_avg = tick_sum * c
-        tick_sumsq = params['sumsq']
-        tick_sumavgsq = ((tick_sum // (256*count)) * count)**2
-        self._mcu_tick_stddev = c * 256. * math.sqrt(
-            count * tick_sumsq - tick_sumavgsq)
+        tick_sumsq = params['sumsq'] * self._stats_sumsq_base
+        self._mcu_tick_stddev = c * math.sqrt(count*tick_sumsq - tick_sum**2)
     def handle_shutdown(self, params):
         if self.is_shutdown:
             return
@@ -359,6 +358,8 @@ class MCU:
             self._printer.reactor.update_timer(
                 self._timeout_timer, time.time() + self.COMM_TIMEOUT)
         self._mcu_freq = float(self.serial.msgparser.config['CLOCK_FREQ'])
+        self._stats_sumsq_base = float(
+            self.serial.msgparser.config['STATS_SUMSQ_BASE'])
         self._emergency_stop_cmd = self.lookup_command("emergency_stop")
         self._clear_shutdown_cmd = self.lookup_command("clear_shutdown")
         self.register_msg(self.handle_shutdown, 'shutdown')
diff --git a/src/basecmd.c b/src/basecmd.c
index 2c9562b6..4902bd9d 100644
--- a/src/basecmd.c
+++ b/src/basecmd.c
@@ -191,15 +191,26 @@ command_get_status(uint32_t *args)
 }
 DECL_COMMAND_FLAGS(command_get_status, HF_IN_SHUTDOWN, "get_status");
 
+#define SUMSQ_BASE 256
+DECL_CONSTANT(STATS_SUMSQ_BASE, SUMSQ_BASE);
+
 static void
 stats_task(void)
 {
     static uint32_t last, count, sumsq;
     uint32_t cur = sched_read_time();
-    uint32_t diff = (cur - last) >> 8;
+    uint32_t diff = cur - last;
     last = cur;
     count++;
-    uint32_t nextsumsq = sumsq + diff*diff;
+    // Calculate sum of diff^2 - be careful of integer overflow
+    uint32_t nextsumsq;
+    if (diff <= 0xffff) {
+        nextsumsq = sumsq + DIV_ROUND_UP(diff * diff, SUMSQ_BASE);
+    } else if (diff <= 0xfffff) {
+        nextsumsq = sumsq + DIV_ROUND_UP(diff, SUMSQ_BASE) * diff;
+    } else {
+        nextsumsq = 0xffffffff;
+    }
     if (nextsumsq < sumsq)
         nextsumsq = 0xffffffff;
     sumsq = nextsumsq;