From 87f84d5254d58daee0a94dfa13315d22c8aa49bf Mon Sep 17 00:00:00 2001
From: Sebastian <sebastian@sebastians-site.de>
Date: Mon, 18 Dec 2023 11:23:26 +0100
Subject: [PATCH] Switched to a fixed window for averaging

---
 src/main.rs | 100 ++++++++++++++++++++++++++++++----------------------
 1 file changed, 58 insertions(+), 42 deletions(-)

diff --git a/src/main.rs b/src/main.rs
index f02ebe0..9278e9b 100644
--- a/src/main.rs
+++ b/src/main.rs
@@ -79,7 +79,6 @@ mod app {
 
         defmt::info!("PWM Setup done");
 
-
         let tim2 = cx.device.TIM2;
         unsafe {
             let rcc = &*RCC::ptr();
@@ -162,6 +161,8 @@ mod app {
         )
     }
 
+    const WINDOW_LEN : usize = 60;
+
     #[task(local=[tim2, tim3, pwm, board_led])]
     async fn update_pwm(cx: update_pwm::Context) {
         defmt::info!("Update Task started");
@@ -171,10 +172,9 @@ mod app {
         let pwm = cx.local.pwm;
         let board_led = cx.local.board_led;
 
-        let mut last_ic = 0u32;
         let mut avg = 10f64;
-        let max_pwm = pwm.get_max_duty() as u32;
-        let mut cur_pwm = 3000u32; //max_pwm / 2;
+        let max_pwm = pwm.get_max_duty() as i32;
+        let mut cur_pwm = max_pwm / 2;
 
         // Inialize last_ic
         while !tim2.sr.read().cc1if().bit_is_set() || !tim3.sr.read().cc1if().bit_is_set() {
@@ -183,55 +183,71 @@ mod app {
         let ic1 = tim2.ccr1().read().bits();
         let ic2 = tim3.ccr1().read().bits();
 
-        last_ic = ic2 << 16 | ic1;
+        let mut last_ic = ic2 << 16 | ic1;
 
-       loop {
-            while !tim3.sr.read().cc1if().bit_is_set() || !tim3.sr.read().cc1if().bit_is_set() {
-                Systick::delay(10.millis()).await;
+        loop {
+            let mut short_avg = 0.0f64;
+
+            let mut count = 0;
+            while count < WINDOW_LEN {
+                while !tim3.sr.read().cc1if().bit_is_set() || !tim3.sr.read().cc1if().bit_is_set() {
+                    Systick::delay(10.millis()).await;
+                }
+                let ic1 = tim2.ccr1().read().bits();
+                let ic2 = tim3.ccr1().read().bits();
+
+                let sum_ic = ic2 << 16 | ic1;
+
+                let diff_ic = if sum_ic > last_ic {
+                    sum_ic - last_ic
+                } else {
+                    u32::MAX - last_ic + sum_ic
+                };
+
+                defmt::info!("ic1:\t{}", ic1);
+                defmt::info!("ic2:\t{}", ic2);
+                defmt::info!("sum_ic:\t{}", sum_ic);
+                defmt::info!("diff_ic:\t{}", diff_ic);
+
+                last_ic = sum_ic;
+
+                let freq = (diff_ic as f64) / 1_000_000f64;
+                defmt::info!("freq:\t{} MHz", freq);
+
+                let diff = freq - avg;
+                if diff > 0.000_100 || diff < -0.000_100 {
+                    defmt::info!("Out of range, dropping sample.");
+                    continue;
+                }
+
+                short_avg += freq / WINDOW_LEN as f64;
+                avg = avg * 0.999 + freq * 0.001;
+
+
+                count += 1;
+                board_led.toggle();
             }
 
-            let ic1 = tim2.ccr1().read().bits();
-            let ic2 = tim3.ccr1().read().bits();
+            defmt::info!("short_avg:\t{} MHz", short_avg);
 
-            let sum_ic = ic2 << 16 | ic1;
-
-            let diff_ic = if sum_ic > last_ic {
-                sum_ic - last_ic
-            } else {
-                u32::MAX - last_ic + sum_ic
-            };
-
-            last_ic = sum_ic;
-
-            let freq = (diff_ic as f64) / 1_000_000f64;
-            let diff = freq - avg;
-
-            board_led.toggle();
-
-            if diff > 0.000_100 || diff < -0.000_100 {
-                continue;
+            let diff = (10.0 - short_avg) * 1_000_000.0;
+            if diff > 1.0 || diff < -1.0 {
+                cur_pwm += diff as i32 * 10;
+            } else if short_avg > 10.0 {
+                cur_pwm -= 1;
+            } else if short_avg < 10.0 {
+                cur_pwm += 1;
             }
 
-            avg = avg * 0.999 + freq * 0.001;
-
-            cur_pwm = if 10_000_000 >= diff_ic {
-                cur_pwm + (10_000_000 - diff_ic)
-            } else {
-                cur_pwm - (diff_ic - 10_000_000)
-            };
+            cur_pwm = if cur_pwm < 0 { 0 } else { cur_pwm };
             cur_pwm = if cur_pwm > max_pwm { max_pwm } else { cur_pwm };
 
             pwm.set_duty(Channel::C1, cur_pwm as u16);
 
-            defmt::info!("ic1:\t{}", ic1);
-            defmt::info!("ic2:\t{}", ic2);
-            defmt::info!("sum_ic:\t{}", sum_ic);
-            defmt::info!("diff_ic:\t{}", diff_ic);
-            defmt::info!("freq:\t{} MHz", freq);
             defmt::info!("avg:\t{} MHz", avg);
             defmt::info!("pwm:\t{}", cur_pwm);
 
-             Systick::delay(500.millis()).await;
-       }
+            Systick::delay(500.millis()).await;
+        }
     }
-}
\ No newline at end of file
+}