From aad24960ec4ab48e224299eb2c85dcd06448a567 Mon Sep 17 00:00:00 2001
From: Sebastien Bourdeauducq <sb@m-labs.hk>
Date: Mon, 26 Aug 2019 16:05:40 +0800
Subject: [PATCH] working position tracking on raspberry pi

---
 config.json    |  5 +++--
 src/main.rs    | 10 +++++++++-
 src/noptica.rs | 49 +++++++++++++++++++++++++++++++++++++++++++++++++
 3 files changed, 61 insertions(+), 3 deletions(-)

diff --git a/config.json b/config.json
index f7c2e13..4921f4c 100644
--- a/config.json
+++ b/config.json
@@ -1,9 +1,10 @@
 {
-    "sample_command": "sudo sigrok-cli -O binary -d fx2lafw --config samplerate=8000000 --continuous",
-    "sample_rate": 8e6,
+    "sample_command": "sudo sigrok-cli -O binary -d fx2lafw --config samplerate=6000000 --continuous",
+    "sample_rate": 6e6,
     "freq_min": 1.9e6,
     "freq_max": 2.1e6,
     "bit_ref": 0,
+    "bit_meas": 1,
 
     "refpll_ki": 4294967,
     "refpll_kl": 85899345
diff --git a/src/main.rs b/src/main.rs
index a4ed169..5d977c8 100644
--- a/src/main.rs
+++ b/src/main.rs
@@ -18,6 +18,7 @@ struct Config {
     freq_min: f64,
     freq_max: f64,
     bit_ref: u8,
+    bit_meas: u8,
     refpll_ki: i64,
     refpll_kl: i64
 }
@@ -36,8 +37,15 @@ fn main() {
         noptica::Dpll::frequency_to_ftw(config.freq_max, config.sample_rate),
         config.refpll_ki,
         config.refpll_kl);
+    let mut tracker = noptica::Tracker::new();
+    let mut decimator = noptica::Decimator::new(200000);
     noptica::sample(&config.sample_command, |rising, _falling| {
         refpll.tick(rising & (1 << config.bit_ref) != 0);
-//        println!("{}", refpll.get_phase_unwrapped());
+        if rising & (1 << config.bit_meas) != 0 {
+            let position = tracker.edge(refpll.get_phase_unwrapped());
+            if let Some(position_avg) = decimator.input(position) {
+                println!("{}", position_avg);
+            }
+        }
     })
 }
diff --git a/src/noptica.rs b/src/noptica.rs
index f7ad262..cfcb8e4 100644
--- a/src/noptica.rs
+++ b/src/noptica.rs
@@ -52,6 +52,55 @@ impl Dpll {
     }
 }
 
+pub struct Tracker {
+    last_phase: i64,
+    current_position: i64
+}
+
+impl Tracker {
+    pub fn new() -> Tracker {
+        Tracker {
+            last_phase: 0,
+            current_position: 0
+        }
+    }
+
+    pub fn edge(&mut self, phase: i64) -> i64 {
+        let phase_diff = phase.wrapping_sub(self.last_phase);
+        self.last_phase = phase;
+        self.current_position += 0x100000000 - phase_diff;
+        self.current_position
+    }
+}
+
+pub struct Decimator {
+    accumulator: i64,
+    current_count: u32,
+    max_count: u32
+}
+
+impl Decimator {
+    pub fn new(max_count: u32) -> Decimator {
+        Decimator {
+            accumulator: 0,
+            current_count: 0,
+            max_count: max_count
+        }
+    }
+
+    pub fn input(&mut self, data: i64) -> Option<i64> {
+        self.accumulator += data;
+        self.current_count += 1;
+        if self.current_count == self.max_count {
+            let average = self.accumulator/(self.current_count as i64);
+            self.accumulator = 0;
+            self.current_count = 0;
+            Some(average)
+        } else {
+            None
+        }
+    }
+}
 
 pub fn sample(command: &str, mut callback: impl FnMut(u8, u8)) {
     let child = std::process::Command::new("sh")