Add more debug output, fix writepos, fix amplitude of 2nd outputampliphase

1 files changed, 63 insertions, 26 deletions

diff --git a/src/fl2k_ampliphase.c b/src/fl2k_ampliphase.c
index 1b92a5c..85a794e 100644
--- a/src/fl2k_ampliphase.c
+++ b/src/fl2k_ampliphase.c
@@ -51,10 +51,8 @@
 
 #include "osmo-fl2k.h"
 
-
 enum inputType_E { INP_REAL, INP_COMPLEX };
 
-
 #define BUFFER_SAMPLES_SHIFT	16
 #define BUFFER_SAMPLES		(1 << BUFFER_SAMPLES_SHIFT)
 #define BUFFER_SAMPLES_MASK	((1 << BUFFER_SAMPLES_SHIFT)-1)
@@ -74,12 +72,15 @@ pthread_t dbg_thread;
 FILE *file;
 int swap_iq = 0;
 
+FILE * fDbg_pd = NULL;
+FILE * fDbg_iq = NULL;
+
 // main thread reads from file and writes into pdbuf and pdslopebuf, if full it waits on iq_cond.
 // iq_worker thread reads from pdbuf and pdslopebuf at readpos (protected by
 // mutex because both threads read its value)
 int32_t * pdbuf;
 int32_t * pdslopebuf;
-int writepos = 0;
+int writepos = 1;
 pthread_mutex_t readpos_mutex;
 int readpos = 0;
 
@@ -254,11 +255,17 @@ static inline void dds_complex(dds_t *dds, int8_t * i, int8_t * q)
 {
     uint32_t phase_idx_i, phase_idx_q;
     int32_t amp_i, amp_q;
+    int32_t amp;
 
     // get current carrier phase, add phase mod, calculate table index
     phase_idx_i = (dds->phase - dds->phase_delta) >> TRIG_TABLE_SHIFT;
     phase_idx_q = (dds->phase + dds->phase_delta) >> TRIG_TABLE_SHIFT;
 
+    if (fDbg_iq) {
+        fprintf(fDbg_iq, "p=%12u d=%8d ix_i=%8d ix_q=%8d ",
+                dds->phase, dds->phase_delta, phase_idx_i, phase_idx_q);
+    }
+
     if (phase_idx_q > 255 || phase_idx_i > 255) {
         fprintf(stderr, "phase_idx_q       %d\n", phase_idx_q);
         fprintf(stderr, "phase_idx_i       %d\n", phase_idx_i);
@@ -272,17 +279,17 @@ static inline void dds_complex(dds_t *dds, int8_t * i, int8_t * q)
     // advance dds generator, wraparound on overflow
     dds->phase += dds->phase_step;
 
-    //amp = 255;
-    amp_i = (int32_t) (creal(dds->amplitude) * 32767.0);  // 0..15
-    amp_q = (int32_t) (cimag(dds->amplitude) * 32767.0);
-
-    amp_i = amp_i * trig_table.inphase[phase_idx_i];      // 0..31
-                                                          //
-    amp_q = amp_q * trig_table.quadrature[phase_idx_q];   // 0..31
+    amp = (int32_t)(creal(dds->amplitude) * 32767.0);  // 0..15
+    amp_i = amp * trig_table.inphase[phase_idx_i];      // 0..31
+    amp_q = amp * trig_table.quadrature[phase_idx_q];   // 0..31
 
     *i = (int8_t) (amp_i >> 24);        // 0..31 >> 24 => 0..8
     *q = (int8_t) (amp_q >> 24);        // 0..31 >> 24 => 0..8
 
+    if (fDbg_iq) {
+        fprintf(fDbg_iq, "i=%8d q=%8d ps=%8d\n", *i, *q, dds->phase_slope);
+    }
+
     /* advance modulation signal by interpolated input from baseband */
     dds->amplitude      += dds->ampslope;
     dds->phase_delta    += dds->phase_slope;
@@ -317,6 +324,9 @@ static void *iq_worker(void *arg)
         /* set phase modulation value from audio input */
         dds_set_phase(&base_signal, pdbuf[readpos], pdslopebuf[readpos]);
         pthread_mutex_lock(&readpos_mutex);
+        if (fDbg_iq) {
+            fprintf(fDbg_iq, "rp=%4d pd=%8d ps=%8d\n", readpos, pdbuf[readpos], pdslopebuf[readpos]);
+        }
         readpos++;
         readpos &= BUFFER_SAMPLES_MASK;
         pthread_mutex_unlock(&readpos_mutex);
@@ -338,7 +348,9 @@ static void *iq_worker(void *arg)
                 qambuf  = qtxbuf;
                 qtxbuf  = tmp_ptr;
 
-                fprintf(stderr, ".");
+                if (fDbg_iq) {
+                    fprintf(fDbg_iq, "swap\n");
+                }
                 pthread_mutex_lock(&cb_mutex);
                 pthread_cond_wait(&cb_cond, &cb_mutex);
                 pthread_mutex_unlock(&cb_mutex);
@@ -385,13 +397,12 @@ static inline int writelen(int maxlen)
 
 
 
-static inline float modulate_sample_ampliphase(const int lastwritepos, const float lastamp, const float sample, float modulationIndex)
+static inline float modulate_sample_ampliphase(const float lastamp, const float sample, float modulationIndex)
 {
-    float amp, slope, pd, pdslope;
+    float slope, pd, pdslope;
 
     /* Calculate modulator amplitudes at this point to lessen
      * the calculations needed in the signal generator */
-    amp = sample;
 
     /* What we do here is calculate a linear "slope" from
     the previous sample to this one. This is then used by
@@ -399,7 +410,7 @@ static inline float modulate_sample_ampliphase(const int lastwritepos, const flo
     with each sample without the need to recalculate
     the dds parameters. In fact this gives us a very
     efficient and pretty good interpolation filter. */
-    slope = amp - lastamp;
+    slope = sample - lastamp;
     slope = slope * 1.0/ (float) rf_to_baseband_sample_ratio;
     /* set phase-delta buf*/
     /* soll:                             -45° .. +45°                               */
@@ -413,7 +424,7 @@ static inline float modulate_sample_ampliphase(const int lastwritepos, const flo
         abort();
     }
 
-    pdslope = slope   * modulationIndex * INT32_MAX_AS_FLOAT;
+    pdslope = slope * modulationIndex * INT32_MAX_AS_FLOAT;
     if (pdslope < (float)INT32_MIN || pdslope > (float)INT32_MAX) {
         fprintf(stderr, "pdslope out of bounds %f\n", pdslope);
         abort();
@@ -422,7 +433,11 @@ static inline float modulate_sample_ampliphase(const int lastwritepos, const flo
     pdbuf[writepos]      = pd;
     pdslopebuf[writepos] = pdslope;
 
-    return amp;
+    if (fDbg_pd) {
+        fprintf(fDbg_pd, "%d,%f,%d,%d\n", writepos, sample, pdbuf[writepos], pdslopebuf[writepos]);
+    }
+
+    return sample;
 }
 
 
@@ -446,11 +461,9 @@ void ampliphase_modulator(enum inputType_E input_type, const float modulation_in
     float complex lastamp = 0;
     int16_t baseband_buf_real[BASEBAND_BUF_SIZE];
     int16_t baseband_buf_cplx[BASEBAND_BUF_SIZE][2];
-    uint32_t lastwritepos = writepos;
     float sample;
 
     while (!do_exit) {
-        int swap = swap_iq;
         len = writelen(BASEBAND_BUF_SIZE);
         if (len > /* for efficiency, do not read 1 sample at a time */ BASEBAND_BUF_SIZE/4) {
             if (input_type == INP_REAL) {
@@ -474,23 +487,32 @@ void ampliphase_modulator(enum inputType_E input_type, const float modulation_in
 
             if (len == 0) {
                 if (ferror(file)) {
+                    fprintf(stderr, "Exit because file error\n");
                     do_exit = 1;
+                    pthread_cond_signal(&iq_cond);
                 }
-                if (!ignore_eof && feof(file)) {
-                    do_exit = 1;
+                if (feof(file)) {
+                    if (ignore_eof) {
+                        fprintf(stderr, "Rewinding input file\n");
+                        fseek(file, 0, SEEK_SET);
+                    }
+                    else {
+                        fprintf(stderr, "Exit because eof\n");
+                        do_exit = 1;
+                        pthread_cond_signal(&iq_cond);
+                    }
                 }
             }
 
             for (i = 0; i < len; i++) {
-                //sample = (float) baseband_buf_cplx[i][0+swap] / 32768.0 + I * (float) baseband_buf_cplx[i][1-swap] / 32768.0;
+                //sample = (float) baseband_buf_cplx[i][swap_iq % 2] / 32768.0 + I * (float) baseband_buf_cplx[i][(swap_iq+1)%2] / 32768.0;
                 /* keep sample real for the moment ... complex bb to be implemented
                  * this is amplitude only */
-                sample = (float) baseband_buf_cplx[i][0+swap] / 32768.0;
+                sample = (float) baseband_buf_cplx[i][swap_iq % 2] / 32768.0;
 
                 /* Modulate and buffer the sample */
-                lastamp = modulate_sample_ampliphase(lastwritepos, lastamp, sample, modulation_index);
-                lastwritepos = writepos++;
-                writepos %= BUFFER_SAMPLES;
+                lastamp = modulate_sample_ampliphase(lastamp, sample, modulation_index);
+                writepos = (writepos + 1) % BUFFER_SAMPLES;
             }
         } else {
             pthread_mutex_lock(&iq_mutex);
@@ -698,11 +720,26 @@ int main(int argc, char **argv)
             perror("");
             goto out;
         }
+
         r = pthread_create(&dbg_thread, &attr, file_worker, fDbg);
         if (r < 0) {
             fprintf(stderr, "Error spawning debug-file worker thread.\n");
             goto out;
         }
+
+        fDbg_pd = fopen("debug-pd.txt","w");
+        if (fDbg_pd == NULL) {
+            fprintf(stderr, "Failed to open 'debug-pd.out' file.\n");
+            perror("");
+            goto out;
+        }
+
+        fDbg_iq = fopen("debug-iq.txt","w");
+        if (fDbg_iq == NULL) {
+            fprintf(stderr, "Failed to open 'debug-pd.out' file.\n");
+            perror("");
+            goto out;
+        }
     }
     else {
         fl2k_open(&dev, (uint32_t)dev_index);