From 9afa0ae91f5eedeee6da628de58967eeaaf30f64 Mon Sep 17 00:00:00 2001
From: LongHairedHacker <sebastian@sebastians-site.de>
Date: Mon, 31 Aug 2020 21:45:51 +0200
Subject: [PATCH] First working version with slow efc

---
 src/main.rs | 54 ++++++++++++++++++++++++++++++++++++++++++++++-------
 1 file changed, 47 insertions(+), 7 deletions(-)

diff --git a/src/main.rs b/src/main.rs
index c12ca5c..5a8c53a 100644
--- a/src/main.rs
+++ b/src/main.rs
@@ -7,7 +7,18 @@ extern crate panic_semihosting;
 use cortex_m_rt::entry;
 use embedded_hal::digital::v2::OutputPin;
 use rtt_target::{rprintln, rtt_init_print};
-use stm32f1xx_hal::{delay::Delay, pac, pac::TIM1, pac::TIM2, prelude::*, rcc::Enable, rcc::Reset};
+use stm32f1xx_hal::{
+    delay::Delay,
+    pac,
+    pac::TIM1,
+    pac::TIM2,
+    prelude::*,
+    rcc::Enable,
+    rcc::Reset,
+    timer::{Tim3NoRemap, Timer},
+};
+
+const target_freq: f64 = 10.0f64;
 
 #[entry]
 fn main() -> ! {
@@ -41,6 +52,15 @@ fn main() -> ! {
     // in order to configure the port. For pins 0-7, crl should be passed instead.
     let mut led = gpioc.pc13.into_push_pull_output(&mut gpioc.crh);
 
+    let mut afio = dp.AFIO.constrain(&mut rcc.apb2);
+    let mut gpioa = dp.GPIOA.split(&mut rcc.apb2);
+    let pwm_pin = gpioa.pa6.into_alternate_push_pull(&mut gpioa.crl);
+    let mut pwm = Timer::tim3(dp.TIM3, &clocks, &mut rcc.apb1)
+        .pwm::<Tim3NoRemap, _, _, _>(pwm_pin, &mut afio.mapr, 10.khz())
+        .split();
+
+    pwm.enable();
+
     // Setup timers
     let tim1 = dp.TIM1;
 
@@ -101,6 +121,8 @@ fn main() -> ! {
 
     let mut last_ic = 0u32;
     let mut avg = 10f64;
+    let max_pwm = pwm.get_max_duty() as f64;
+    let mut cur_pwm = max_pwm / 2.0;
 
     // Skip the first measurement, it will be garbage
     while !tim1.sr.read().cc1if().bit_is_set() || !tim2.sr.read().cc1if().bit_is_set() {
@@ -130,14 +152,32 @@ fn main() -> ! {
         last_ic = sum_ic;
 
         let freq = (diff_ic as f64) / 1_000_000f64;
+        let diff = freq - avg;
+        if diff > 0.000_030 || diff < -0.000_030 {
+            continue;
+        }
+
         avg = avg * 0.99 + freq * 0.01;
 
+        let error = target_freq - avg;
+        cur_pwm += error * 100000.0;
+        cur_pwm = if cur_pwm < 0.0 {
+            0.0
+        } else if cur_pwm > max_pwm {
+            max_pwm
+        } else {
+            cur_pwm
+        };
+
+        pwm.set_duty(cur_pwm as u16);
+
         rprintln!("Counters:");
-        rprintln!("ic1: {}", ic1);
-        rprintln!("ic2: {}", ic2);
-        rprintln!("sum_ic: {}", sum_ic);
-        rprintln!("diff_ic: {}", diff_ic);
-        rprintln!("freq: {} MHz", freq);
-        rprintln!("avg: {} MHz", avg);
+        rprintln!("ic1:\t{}", ic1);
+        rprintln!("ic2:\t{}", ic2);
+        rprintln!("sum_ic:\t{}", sum_ic);
+        rprintln!("diff_ic:\t{}", diff_ic);
+        rprintln!("freq:\t{} MHz", freq);
+        rprintln!("avg:\t{} MHz", avg);
+        rprintln!("pwm:\t{}", cur_pwm);
     }
 }