improve estimator speed / accuracy and bench

firmware/litex-fw/src/main.rs

@@ -218,6 +218,10 @@ impl KarlsenLpf {
 const ESTIMATOR_SAMPLES: usize = 1024;
 const ESTIMATOR_WORDS: usize = ESTIMATOR_SAMPLES / 32;
 
+/// Pitch estimator based on 1-bit autocorrellator.
+/// Essentially this is storing the sign of each sample in 1 bit
+/// and then performing a linear (not circular!) correllation
+/// of 1.5T window across a 1T length window at 0 to 0.5T shift.
 struct PitchEstimator {
     bitstream: [u32; ESTIMATOR_WORDS],
     bit: usize,
@@ -239,12 +243,23 @@ impl PitchEstimator {
     }
 
     fn measure_and_reset(&mut self) -> usize {
-        let mut correllation: [u32; ESTIMATOR_SAMPLES] = [0u32; ESTIMATOR_SAMPLES];
+        let n_shift_start_corr: usize = 128;
+        let n_samples_corr = (ESTIMATOR_SAMPLES * 2) / 3;
+        let n_words_corr = n_samples_corr / 32;
         let mut bitstream_shifted = self.bitstream.clone();
-        for n_shift in 0..ESTIMATOR_SAMPLES {
+        let mut notch_min: u32 = u32::MAX;
+        let mut notch_min_index: usize = 0;
+        for n_shift in 0..ESTIMATOR_SAMPLES / 2 {
             // Compute total mismatch in current shift by counting ones after XOR.
-            for n_word in 0..ESTIMATOR_WORDS {
-                correllation[n_shift] += (self.bitstream[n_word] ^ bitstream_shifted[n_word]).count_ones();
+            let mut this_corr: u32 = 0;
+            if n_shift >= n_shift_start_corr {
+                for n_word in 0..n_words_corr {
+                    this_corr += (self.bitstream[n_word] ^ bitstream_shifted[n_word]).count_ones();
+                }
+                if this_corr < notch_min {
+                    notch_min = this_corr;
+                    notch_min_index = n_shift;
+                }
             }
             // Shift entire `bitstream_shifted` array left by 1 bit
             let first_word = bitstream_shifted[0];
@@ -257,15 +272,8 @@ impl PitchEstimator {
             }
             // The first and last words are special cases.
             bitstream_shifted[0] = (first_word << 1) | (bitstream_shifted[0] & 1);
-            bitstream_shifted[ESTIMATOR_WORDS-1] |= first_word >> 31
-        }
-        let mut notch_min: u32 = u32::MAX;
-        let mut notch_min_index: usize = 0;
-        for n in 128..(ESTIMATOR_SAMPLES-128) {
-            if correllation[n] < notch_min {
-                notch_min = correllation[n];
-                notch_min_index = n;
-            }
+            // Circular correllation is actually worse
+            // bitstream_shifted[ESTIMATOR_WORDS-1] |= first_word >> 31
         }
         info!("ESTIMATE idx:{} val:{}", notch_min_index, notch_min);
         // Reset the estimator so we can feed it again.
@@ -409,16 +417,21 @@ fn main() -> ! {
             if LAST_IRQ_PERIOD != 0 {
                 log::info!("irq_load_percent: {}", (LAST_IRQ_LEN * 100) / LAST_IRQ_PERIOD);
             }
+
+    let peripherals = pac::Peripherals::steal();
+
+            peripherals.TIMER0.uptime_latch.write(|w| w.bits(1));
+            let trace_pitch = peripherals.TIMER0.uptime_cycles0.read().bits();
             if let Some(ref mut est) = ESTIMATOR {
                 let mut n = est.measure_and_reset();
-                while n * 2 < 1024 {
-                    n *= 2;
-                }
                 if let Some(ref mut p) = PITCH {
                     p.window = n;
                 }
             }
+            peripherals.TIMER0.uptime_latch.write(|w| w.bits(1));
+            let trace_pitch2 = peripherals.TIMER0.uptime_cycles0.read().bits();
+            log::info!("est_len: {}", trace_pitch2 - trace_pitch);
         }
-        timer.delay_ms(100u32);
+        timer.delay_ms(5000u32);
     }
 }