From 954c98f25eabb7ab1d83d9a57364e42b450ec6ec Mon Sep 17 00:00:00 2001
From: Sebastian <sebastian@sebastians-site.de>
Date: Fri, 5 Jan 2024 17:21:24 +0100
Subject: [PATCH] Tweaked control loop

---
 firmware/src/main.rs | 35 +++++++++++++++++++----------------
 1 file changed, 19 insertions(+), 16 deletions(-)

diff --git a/firmware/src/main.rs b/firmware/src/main.rs
index 8e46d6c..7c9ad61 100644
--- a/firmware/src/main.rs
+++ b/firmware/src/main.rs
@@ -66,7 +66,7 @@ mod app {
 
         let clocks = rcc
             .cfgr
-            .use_hse(8.MHz())
+            .use_hse(12.MHz())
             .sysclk(48.MHz())
             .pclk1(24.MHz())
             .freeze(&mut flash.acr);
@@ -90,9 +90,10 @@ mod app {
         let mut pwm =
             cx.device
                 .TIM4
-                .pwm_hz::<Tim4NoRemap, _, _>(pwm_pin, &mut afio.mapr, 32.kHz(), &clocks);
+                .pwm_hz::<Tim4NoRemap, _, _>(pwm_pin, &mut afio.mapr, 16.kHz(), &clocks);
         pwm.enable(Channel::C1);
 
+        defmt::info!("Max PWM is {}", pwm.get_max_duty());
         defmt::info!("PWM Setup done");
 
         let tim2 = cx.device.TIM2;
@@ -238,8 +239,10 @@ mod app {
 
         let mut last_ic = ic2 << 16 | ic1;
 
+        let mut samples: [f64; WINDOW_LEN] = [10.0; WINDOW_LEN];
+        let mut short_avg: f64 = 10.0;
+
         loop {
-            let mut short_avg = 0.0f64;
             let mut last_freq = 10.0f64;
 
             let mut count = 0;
@@ -275,37 +278,37 @@ mod app {
                     continue;
                 }
 
+                samples[count] = freq;
+                short_avg = 0.0;
+                for i in 0..WINDOW_LEN {
+                    short_avg += samples[i] / WINDOW_LEN as f64;
+                }
+                defmt::info!("short_avg:\t{} MHz", short_avg);
+
                 cx.shared.device_status.lock(|device_status| {
                     device_status.measured_frequency = freq;
+                    device_status.average_frequency = short_avg;
+                    device_status.pwm = cur_pwm as u16;
                 });
 
-                short_avg += freq / WINDOW_LEN as f64;
-
                 count += 1;
                 board_led.toggle();
             }
 
             defmt::info!("short_avg:\t{} MHz", short_avg);
-            cx.shared.device_status.lock(|device_status| {
-                device_status.average_frequency = short_avg;
-            });
 
             let diff = (TARGET_FREQ - short_avg) * 1_000_000.0;
-            if diff > 1.0 || diff < -1.0 {
-                cur_pwm += diff as i32 * 15;
-            } else if diff < -0.001 {
+            if diff > 0.1 || diff < -0.1 {
+                cur_pwm += (diff * 30.0) as i32;
+            } else if diff < -0.01 {
                 cur_pwm -= 1;
-            } else if diff > 0.001 {
+            } else if diff > 0.01 {
                 cur_pwm += 1;
             }
 
             cur_pwm = if cur_pwm < 0 { 0 } else { cur_pwm };
             cur_pwm = if cur_pwm > max_pwm { max_pwm } else { cur_pwm };
 
-            cx.shared.device_status.lock(|device_status| {
-                device_status.pwm = cur_pwm as u16;
-            });
-
             pwm.set_duty(Channel::C1, cur_pwm as u16);
             defmt::info!("pwm:\t{}", cur_pwm);