FSK

src/main.rs

@@ -5,6 +5,10 @@ use std::{
     ops::{Add, Div, Mul, Sub},
 };
 
+mod complex;
+use complex::Complex;
+
+// Utilities
 fn map<T>(input: T, in_min: T, in_max: T, out_min: T, out_max: T) -> T
 where
     T: Clone + Add<Output = T> + Mul<Output = T> + Sub<Output = T> + Div<Output = T>,
@@ -12,6 +16,11 @@ where
     ((input - in_min.clone()) / (in_max - in_min)) * (out_max - out_min.clone()) + out_min
 }
 
+fn euclid_mod(a: f32, m: f32) -> f32 {
+    let r = a % m;
+    if r < 0.0 { r + m } else { r }
+}
+
 struct Nco {
     // Phase of NCO
     theta: u32,  // 0 <=> 0, 0xFFFFFFFF <=> 2pi
@@ -30,11 +39,7 @@ impl Nco {
 
     // Sets freq, freq in radian per sample
     pub fn set_frequency(&mut self, freq: f32) {
-        if freq < 0.0 {
-            self.dtheta = map(2. * PI - freq, 0., 2. * PI, 0., 0xFFFFFFFFu32 as f32).floor() as u32;
-        } else {
-            self.dtheta = map(freq, 0., 2. * PI, 0., 0xFFFFFFFFu32 as f32).floor() as u32;
-        }
+        self.dtheta = map(euclid_mod(freq, 2. * PI), 0., 2. * PI, 0., 0xFFFFFFFFu32 as f32).floor() as u32;
     }
 
     // Adjusts freq, freq in radian per sample
@@ -47,9 +52,8 @@ impl Nco {
     }
 
     pub fn step(&mut self) {
-        let bef = self.theta;
+        let bef = self.theta as i64;
         self.theta = self.theta.overflowing_add(self.dtheta).0;
-        println!("{}", bef.overflowing_sub(self.theta).0);
     }
 
     pub fn step_n(&mut self, n: u32) {
@@ -66,6 +70,11 @@ impl Nco {
     pub fn cos(&self) -> f32 {
         map(self.theta as f32, 0., 0xFFFFFFFFu32 as f32, 0., 2. * PI).cos()
     }
+
+    pub fn cexp(&self) -> Complex<f32>
+    {
+        Complex::new(self.cos(), self.sin())
+    }
 }
 
 struct BFSKMod<'a, T: Iterator<Item = bool>> {
@@ -92,26 +101,62 @@ where
         }
     }
 
-    pub fn step_modulate(&mut self) -> Option<f32> {
+    pub fn step_modulate(&mut self) -> Option<Complex<f32>> {
         if self.sample_index == self.samples_per_bit {
             self.sample_index = 0;
             let bit = self.bit_stream.next()?;
 
-            let frequency = if bit { self.bandwidth } else { -self.bandwidth };
+            let frequency = if bit { self.bandwidth / 2.0 } else { -self.bandwidth / 2.0 };
             self.oscillator.set_frequency(frequency);
         }
 
         self.sample_index += 1;
         self.oscillator.step();
-        Some(self.oscillator.sin())
+        Some(self.oscillator.cexp())
     }
 }
 
-fn main() {
-    let sample_rate = 44100;
-    let mut frequency = 500.0; //HZ
+fn main()
+{
+    modulate();
+}
+
+fn test()
+{
+    let sample_rate = 44100;
+    let f1 = -100.0; //HZ
+    let f2 = 500.0; //HZ
+
+    let spec = hound::WavSpec {
+        channels: 1,
+        sample_rate,
+        bits_per_sample: 16,
+        sample_format: hound::SampleFormat::Int,
+    };
+    let mut writer = hound::WavWriter::create("sine.wav", spec).unwrap();
+
+    let mut o1 = Nco::new(2. * PI * (f1 / sample_rate as f32));
+    let mut o2 = Nco::new(2. * PI * (f2 / sample_rate as f32));
+
+    for i in 0..sample_rate
+    {
+        let amplitude = i16::MAX as f32;
+        let sample = o1.cexp() * o2.cexp();
+        writer.write_sample((amplitude * sample.re) as i16).unwrap();
+
+        o1.step();
+        o2.step();
+    }
+
+    writer.finalize().unwrap();
+}
+
+fn modulate() {
+    let sample_rate = 44100;
+    let mut frequency = 2000.0; //HZ
+    let mut bandwidth = 500.0; //HZ
+                               
 
-    /*
     let path = "a.jpg";
     let file = File::open(path).unwrap();
     let mut bit_stream = file.bytes().flat_map(|byte| {
@@ -127,13 +172,14 @@ fn main() {
             (byte >> 7) & 1 == 1,
         ]
     });
-    */
-
-    let mut bit_stream = (0..22000).map(|_| false).chain((0..22000).map(|_| false));
+    //let mut bit_stream = (0..22000).map(|_| false).chain((0..22000).map(|_| true));
+    //let mut bit_stream = (0..22000).flat_map(|_| [true, false]);
 
+    let baud_rate = 400;
+    println!("{} samples/bit", sample_rate/baud_rate);
     let mut bfsk = BFSKMod::new(
-        20,
-        2. * PI * (frequency / sample_rate as f32),
+        sample_rate / baud_rate,
+        2. * PI * (bandwidth / sample_rate as f32),
         &mut bit_stream,
     );
 
@@ -145,8 +191,17 @@ fn main() {
     };
     let mut writer = hound::WavWriter::create("sine.wav", spec).unwrap();
 
+    let mut lo = Nco::new(2. * PI * (frequency / sample_rate as f32));
+
+    let prev = Complex::new(0., 0.);
+    let alpha = 1.0 - (-2.0 * PI * ((1.5 * 0.5 * bandwidth) / sample_rate as f32));
     while let Some(sample) = bfsk.step_modulate() {
         let amplitude = i16::MAX as f32;
-        writer.write_sample((amplitude * sample) as i16).unwrap();
+        let c_sample = lo.cexp() * sample;
+
+        let filtered = prev + (c_sample - prev) * alpha;
+        writer.write_sample((amplitude * c_sample.re) as i16).unwrap();
+        lo.step();
     }
+    writer.finalize().unwrap();
 }