diff --git a/src/main.rs b/src/main.rs
index 8a5130b..c12ca5c 100644
--- a/src/main.rs
+++ b/src/main.rs
@@ -49,12 +49,23 @@ fn main() -> ! {
 
     // Enable external clocking
     tim1.smcr.write(|w| {
-        w.etf().no_filter(); // No filter
+        w.etf().no_filter(); // No filter for to 10Mhz clock
         w.etps().div1(); // No divider
         w.etp().not_inverted(); // on rising edege at ETR pin
         w.ece().enabled() // mode 2 (use ETR pin)
     });
 
+    tim1.ccmr1_input().write(|w| {
+        w.cc1s().ti1();
+        w.ic1f().no_filter()
+        //w.ic1psc().bits(0)
+    });
+
+    tim1.ccer.write(|w| {
+        w.cc1p().set_bit();
+        w.cc1e().set_bit()
+    });
+
     tim1.cr2.write(|w| {
         w.mms().update() // Trigger output on update/overflow
     });
@@ -72,24 +83,61 @@ fn main() -> ! {
         w.sms().ext_clock_mode() // Use trigger as clock
     });
 
+    tim2.ccmr1_input().write(|w| {
+        w.cc1s().ti1();
+        w.ic1f().no_filter()
+        //w.ic1psc().bits(0)
+    });
+
+    tim2.ccer.write(|w| {
+        w.cc1p().set_bit();
+        w.cc1e().set_bit()
+    });
+
     tim1.cr1.write(|w| w.cen().enabled());
     tim2.cr1.write(|w| w.cen().enabled());
 
     let mut delay = Delay::new(cp.SYST, clocks);
 
-    loop {
-        let cnt1 = tim1.cnt.read().bits();
-        let cnt2 = tim2.cnt.read().bits();
-        tim1.cnt.reset();
-        tim2.cnt.reset();
+    let mut last_ic = 0u32;
+    let mut avg = 10f64;
 
-        let sum = cnt2 << 16 | cnt1;
+    // Skip the first measurement, it will be garbage
+    while !tim1.sr.read().cc1if().bit_is_set() || !tim2.sr.read().cc1if().bit_is_set() {
+        delay.delay_ms(10u16);
+    }
+    let ic1 = tim1.ccr1.read().bits();
+    let ic2 = tim2.ccr1.read().bits();
+
+    last_ic = ic2 << 16 | ic1;
+
+    loop {
+        while !tim1.sr.read().cc1if().bit_is_set() || !tim2.sr.read().cc1if().bit_is_set() {
+            delay.delay_ms(10u16);
+        }
+
+        let ic1 = tim1.ccr1.read().bits();
+        let ic2 = tim2.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
+        };
+
+        last_ic = sum_ic;
+
+        let freq = (diff_ic as f64) / 1_000_000f64;
+        avg = avg * 0.99 + freq * 0.01;
 
         rprintln!("Counters:");
-        rprintln!("cnt1: {}", cnt1);
-        rprintln!("cnt1: {}", cnt2);
-        rprintln!("sum: {}", sum);
-
-        delay.delay_ms(1000u16);
+        rprintln!("ic1: {}", ic1);
+        rprintln!("ic2: {}", ic2);
+        rprintln!("sum_ic: {}", sum_ic);
+        rprintln!("diff_ic: {}", diff_ic);
+        rprintln!("freq: {} MHz", freq);
+        rprintln!("avg: {} MHz", avg);
     }
 }