From 1e22ca7b2e1d8762f50b6a0f8f80a506b367fdc9 Mon Sep 17 00:00:00 2001
From: Sean Cross <sean@xobs.io>
Date: Wed, 10 Jun 2020 17:14:44 +0800
Subject: [PATCH] demod: make nco generation self-contained

Signed-off-by: Sean Cross <sean@xobs.io>
---
 src/bpsk/demod.c | 41 ++++++++++++++++++++++-------------------
 1 file changed, 22 insertions(+), 19 deletions(-)

diff --git a/src/bpsk/demod.c b/src/bpsk/demod.c
index 7ab2067..1c4b060 100644
--- a/src/bpsk/demod.c
+++ b/src/bpsk/demod.c
@@ -24,6 +24,8 @@ struct nco_state
     float32_t freq;       // Hz
 
     float32_t phase; // rad
+    float32_t offset;
+    float32_t error;
 };
 
 struct bpsk_state
@@ -42,7 +44,6 @@ struct bpsk_state
     float32_t fir_state[SAMPLES_PER_PERIOD + FIR_STAGES - 1];
 
     struct nco_state nco;
-    float32_t nco_error;
 
     arm_fir_instance_f32 i_lpf;
     float32_t i_lpf_state[SAMPLES_PER_PERIOD + LPF_STAGES - 1];
@@ -72,22 +73,28 @@ static void print_char(uint32_t c)
     printf("%c", varcode_to_char(c >> 2));
 }
 
-static void nco(float32_t control, uint32_t timestep, float32_t *i,
-                float32_t *q)
+static void make_nco(float32_t *i, float32_t *q)
 {
     // control is a number from +1 to -1, which translates to -pi to +pi
     // additional phase per time step timestep is the current time step,
     // expressed in terms of samples since t=0 i is a pointer to the in-phase
     // return value q is a pointer to the quadrature return value
 
-    bpsk_state.nco.phase += (control / PI);
+    float32_t timestep = bpsk_state.nco.offset;
+    while (timestep < SAMPLES_PER_PERIOD + bpsk_state.nco.offset)
+    {
+        bpsk_state.nco.phase += (bpsk_state.nco.error / PI);
+        *i++ = arm_cos_f32((timestep * bpsk_state.nco.freq * 2 * PI) /
+                               bpsk_state.nco.samplerate +
+                           bpsk_state.nco.phase);
+        *q++ = arm_sin_f32((timestep * bpsk_state.nco.freq * 2 * PI) /
+                               bpsk_state.nco.samplerate +
+                           bpsk_state.nco.phase);
+        timestep += 1;
+    }
 
-    *i = arm_cos_f32((((float32_t)timestep) * bpsk_state.nco.freq * 2 * PI) /
-                         bpsk_state.nco.samplerate +
-                     bpsk_state.nco.phase);
-    *q = arm_sin_f32((((float32_t)timestep) * bpsk_state.nco.freq * 2 * PI) /
-                         bpsk_state.nco.samplerate +
-                     bpsk_state.nco.phase);
+    // XXX MAKE SURE TO DEAL WITH TIMESTEP WRPAPING
+    bpsk_state.nco.offset = timestep;
 }
 
 void bpsk_demod_init(void)
@@ -98,7 +105,8 @@ void bpsk_demod_init(void)
     bpsk_state.nco.samplerate = SAMPLE_RATE;
     bpsk_state.nco.freq = CARRIER_TONE;
     bpsk_state.nco.phase = 0.0;
-    bpsk_state.nco_error = 0.0;
+    bpsk_state.nco.error = 0.0;
+    bpsk_state.nco.offset = 0;
 
     bpsk_state.agc = 1.0;
     bpsk_state.agc_step = 0.05;
@@ -257,12 +265,7 @@ static void bpsk_core(void)
     // current timestamp.
     float32_t i_samps[SAMPLES_PER_PERIOD];
     float32_t q_samps[SAMPLES_PER_PERIOD];
-    static int nco_offset = 0;
-    for (unsigned int i = 0; i < SAMPLES_PER_PERIOD; i++)
-    {
-        nco(bpsk_state.nco_error, i + nco_offset, &(i_samps[i]), &(q_samps[i]));
-    }
-    nco_offset += SAMPLES_PER_PERIOD;
+    make_nco(i_samps, q_samps);
 
     static float32_t i_mult_samps[SAMPLES_PER_PERIOD];
     static float32_t q_mult_samps[SAMPLES_PER_PERIOD];
@@ -289,8 +292,8 @@ static void bpsk_core(void)
         avg += errorwindow[i];
     }
     avg /= ((float32_t)SAMPLES_PER_PERIOD);
-    bpsk_state.nco_error = -(avg);
-    // printf("err: %0.04f\n", bpsk_state.nco_error);
+    bpsk_state.nco.error = -(avg);
+    // printf("err: %0.04f\n", bpsk_state.nco.error);
 }
 
 int bpsk_demod(int *bit, demod_sample_t *samples, uint32_t nb, uint32_t *processed_samples)