remove DC bias

src/codec2/mod.rs

-use std::collections::VecDeque;
+use std::{
+    collections::VecDeque,
+    ops::{AddAssign, Div, Sub},
+};
 
 const MODE_M_MAX: usize = 4;
 const M: i32 = 2;
-const N_SYM: i32 = 50;
+const N_SYM: i32 = 35;
 
 #[link(name = "fsk", kind = "static")]
 extern "C" {
@@ -88,11 +91,50 @@ struct InternalFsk {
 }
 
 #[repr(C)]
+#[derive(Debug, Copy, Clone)]
 pub struct Complex {
     pub real: libc::c_float,
     pub imag: libc::c_float,
 }
 
+impl Complex {
+    pub const fn zero() -> Self {
+        Self {
+            real: 0.0,
+            imag: 0.0,
+        }
+    }
+}
+
+impl Sub for Complex {
+    type Output = Complex;
+
+    fn sub(self, rhs: Self) -> Complex {
+        Complex {
+            real: self.real - rhs.real,
+            imag: self.imag - rhs.imag,
+        }
+    }
+}
+
+impl AddAssign for Complex {
+    fn add_assign(&mut self, rhs: Self) {
+        self.real += rhs.real;
+        self.imag += rhs.imag;
+    }
+}
+
+impl Div<f32> for Complex {
+    type Output = Complex;
+
+    fn div(self, rhs: f32) -> Complex {
+        Complex {
+            real: self.real / rhs,
+            imag: self.imag / rhs,
+        }
+    }
+}
+
 pub struct Fsk<I: Iterator<Item = Complex>> {
     internal: *mut InternalFsk,
     input_cache: Vec<Complex>,
@@ -102,9 +144,10 @@ pub struct Fsk<I: Iterator<Item = Complex>> {
 
 impl<I: Iterator<Item = Complex>> Fsk<I> {
     pub fn new(iq_iter: I) -> Self {
-        const FS: i32 = 9600 * 5;
+        const P: i32 = 10;
+        const FS: i32 = 9600 * P;
 
-        let internal = unsafe { fsk_create_hbr(FS, 9600, M, 5, N_SYM, -1, 0) };
+        let internal = unsafe { fsk_create_hbr(FS, 9600, M, P, N_SYM, -1, 0) };
 
         // Set upper/lower bound on the FSK peaks, from center
         // (Hz)

src/decoder/dc_bias.rs

+use crate::codec2::Complex;
+
+// 1 seconds @ 48KHz
+const AVERAGE_TIME: usize = 48_000;
+
+pub struct RemoveDcBias<I: Iterator<Item = Complex>> {
+    iter: I,
+    sum: Complex,
+    i: usize,
+    avg: Complex,
+}
+
+impl<I: Iterator<Item = Complex>> RemoveDcBias<I> {
+    pub fn new(iter: I) -> Self {
+        Self {
+            iter,
+            sum: Complex::zero(),
+            i: 0,
+            avg: Complex::zero(),
+        }
+    }
+}
+
+impl<I: Iterator<Item = Complex>> Iterator for RemoveDcBias<I> {
+    type Item = Complex;
+
+    fn next(&mut self) -> Option<Self::Item> {
+        let val = self.iter.next()?;
+
+        self.sum += val;
+        self.i += 1;
+        if self.i == AVERAGE_TIME {
+            self.avg = self.sum / (self.i as f32);
+            self.i = 0;
+            self.sum = Complex::zero();
+        }
+
+        Some(val - self.avg)
+    }
+}

src/decoder/demod.rs

@@ -8,7 +8,7 @@ use crate::MAX_PACKET_LEN;
 use super::packet::{PacketDecoder, Status};
 
 const SYNC_WORD: u32 = 0xABCDEF12;
-const MAX_BIT_FLIPS: u32 = 3;
+const MAX_BIT_FLIPS: u32 = 5;
 
 pub struct Demod<T: DecodeFrom, I: Iterator<Item = T>> {
     rolling_sync: u32,
@@ -49,8 +49,8 @@ impl<T: DecodeFrom, I: Iterator<Item = T>> Demod<T, I> {
                 }
             }
 
-            for i in to_remove {
-                self.decoders.swap_remove(i);
+            for i in to_remove.iter().rev() {
+                self.decoders.swap_remove(*i);
             }
 
             if self.sync_matches() {

src/decoder/mod.rs

@@ -11,8 +11,9 @@ use crate::{
     util::{append_internet_to_packet_route, print_packet},
 };
 
-use self::demod::Demod;
+use self::{dc_bias::RemoveDcBias, demod::Demod};
 
+mod dc_bias;
 mod demod;
 mod packet;
 
@@ -34,7 +35,9 @@ pub fn decode_forever(
             Complex { real, imag }
         });
 
-    let soft_bits = Fsk::new(iq);
+    let iq_no_dc = RemoveDcBias::new(iq);
+
+    let soft_bits = Fsk::new(iq_no_dc);
 
     let demod = Demod::new(soft_bits);