add dsp.c (hilbert, iir)

Makefile

@@ -119,7 +119,7 @@ CSRC = $(STARTUPSRC) \
        $(SHELLSRC) \
        $(DSPLIBSRC) \
        usbcfg.c \
-       main.c si5351.c si5351_low.c tlv320aic3204.c 
+       main.c si5351.c si5351_low.c tlv320aic3204.c dsp.c
 
 #       $(TESTSRC) \
 

dsp.c

@@ -0,0 +1,89 @@
+#include <arm_math.h>
+#include "nanovna.h"
+
+int16_t ref_state[STATE_LEN];
+int16_t ref_buf[SAMPLE_LEN];
+int16_t refq_buf[SAMPLE_LEN];
+
+int16_t samp_buf[SAMPLE_LEN];
+
+
+// Bi-Quad IIR Filter state
+q15_t bq_state1[4 * 4];
+q15_t bq_state2[4 * 4];
+
+q15_t bq_coeffs[] = {
+         189, 0,    -72,   189, 26371, -15931,
+        1008, 0,  -1952,  1008, 25915, -15917,
+        1761, 0,  -2113,  1761, 26887, -16201,
+        3075, 0,  -5627,  3075, 25801, -16186,
+};
+
+arm_biquad_casd_df1_inst_q15 bq1 = { 3, bq_state1, bq_coeffs, 1};
+arm_biquad_casd_df1_inst_q15 bq2 = { 3, bq_state2, bq_coeffs, 1};
+
+const q15_t hilbert31_coeffs[] = {
+  20570, 6125, 2918, 1456, 682, 279, 91, 19 
+};
+
+static void
+hilbert_transform(void)
+{ 
+  __SIMD32_TYPE *src = __SIMD32_CONST(ref_state);
+  __SIMD32_TYPE *dst = __SIMD32_CONST(refq_buf);
+  int j;
+
+  for (j = 0; j < SAMPLE_LEN / 2; j++) {
+    int i;
+    int32_t acc0 = 0;
+    int32_t accn0 = 0;
+    int32_t acc1 = 0;
+    int32_t accn1 = 0;
+
+    for (i = 0; i < 8; i += 2) {
+      uint32_t c = *(uint32_t*)&hilbert31_coeffs[i];
+#define OFFSET (STATE_LEN / 2 / 2)
+      __SIMD32_TYPE a0 = src[OFFSET - i-1];
+      __SIMD32_TYPE a1 = src[OFFSET - i-2];
+      __SIMD32_TYPE b0 = src[OFFSET + i];
+      __SIMD32_TYPE b1 = src[OFFSET + i+1];
+
+      __SIMD32_TYPE a = __PKHTB(a1, a0, 16);
+      __SIMD32_TYPE b = __PKHTB(b1, b0, 16);
+      acc0 = __SMLAD(c, b, acc0);
+      accn0 = __SMLAD(c, a, accn0);
+      a = __PKHBT(a0, a1, 16);
+      b = __PKHBT(b0, b1, 16);
+      acc1 = __SMLAD(c, b, acc1);
+      accn1 = __SMLAD(c, a, accn1);
+    }
+    acc0 -= accn0;
+    acc1 -= accn1;
+    *dst++ = __PKHTB(acc0, acc1, 16);
+    src++;
+  }
+
+  dst = __SIMD32_CONST(ref_state);
+  for (j = 0; j < STATE_LEN / 2; j++) {
+    *dst++ = *src++;
+  }
+}
+
+void
+dsp_process(int16_t *capture, size_t length)
+{
+  uint32_t *p = (uint32_t*)capture;
+  uint32_t len = length / 2;
+  uint32_t i;
+  for (i = 0; i < len; i++) {
+    uint32_t sr = *p++;
+    ref_buf[i] = sr & 0xffff;
+    samp_buf[i] = (sr>>16) & 0xffff;
+  }
+
+  // apply low pass filter
+  //arm_biquad_cascade_df1_q15(&bq1, ref_buf, ref_buf, len);
+  //arm_biquad_cascade_df1_q15(&bq2, samp_buf, samp_buf, len);
+
+  hilbert_transform();
+}

main.c

@@ -170,7 +170,7 @@ int16_t rx_buffer[AUDIO_BUFFER_LEN * 2];
 
 int16_t dump_buffer[AUDIO_BUFFER_LEN];
 volatile int16_t request_dump = 0;
-
+int16_t dump_selection = 0;
 
 void i2s_end_callback(I2SDriver *i2sp, size_t offset, size_t n)
 {
@@ -181,14 +181,21 @@ void i2s_end_callback(I2SDriver *i2sp, size_t offset, size_t n)
   int16_t *p = &rx_buffer[offset];
   (void)i2sp;
   (void)n;
-  //palSetPad(GPIOC, GPIOC_LED);
+  palClearPad(GPIOC, GPIOC_LED);
+
+  dsp_process(p, n);
 
   if (request_dump > 0) {
+    if (dump_selection == 1)
+      p = samp_buf;
+    else if (dump_selection == 2)
+      p = ref_buf;
+    else if (dump_selection == 3)
+      p = refq_buf;
     if (request_dump == 1)
       memcpy(dump_buffer, p, sizeof dump_buffer);
     --request_dump;
   }
-  //dsp_process(p, n);
 
 #if PORT_SUPPORTS_RT
   cnt_e = port_rt_get_counter_value();
@@ -197,7 +204,7 @@ void i2s_end_callback(I2SDriver *i2sp, size_t offset, size_t n)
   stat.last_counter_value = cnt_s;
 #endif
   stat.callback_count++;
-  //palClearPad(GPIOC, GPIOC_LED);
+  palSetPad(GPIOC, GPIOC_LED);
 }
 
 static const I2SConfig i2sconfig = {
@@ -213,33 +220,29 @@ static const I2SConfig i2sconfig = {
 static void cmd_data(BaseSequentialStream *chp, int argc, char *argv[])
 {
   int i, j;
+  int len;
   (void)argc;
   (void)argv;
 
-#if 0  
-  int16_t *buf = rx_buffer;
-  i2sStopExchange(&I2SD2);
-  for (i = 0; i < AUDIO_BUFFER_LEN; ) {
-    for (j = 0; j < 16; j++, i++) {
-      chprintf(chp, "%04x ", 0xffff & (int)buf[i]);
-    }
-    chprintf(chp, "\r\n");
-  }
-  i2sStartExchange(&I2SD2);
-#else
   int16_t *buf = dump_buffer;
+
+  if (argc == 1)
+    dump_selection = atoi(argv[0]);
+
   request_dump = 3;
-  palClearPad(GPIOC, GPIOC_LED);
+  //palClearPad(GPIOC, GPIOC_LED);
   while (request_dump)
     ;
-  for (i = 0; i < AUDIO_BUFFER_LEN; ) {
+  len = AUDIO_BUFFER_LEN;
+  if (dump_selection != 0)
+    len /= 2;
+  for (i = 0; i < len; ) {
     for (j = 0; j < 16; j++, i++) {
       chprintf(chp, "%04x ", 0xffff & (int)buf[i]);
     }
     chprintf(chp, "\r\n");
   }
-  palSetPad(GPIOC, GPIOC_LED);
-#endif
+  //palSetPad(GPIOC, GPIOC_LED);
 }
 
 static void cmd_gain(BaseSequentialStream *chp, int argc, char *argv[])

nanovna.h

@@ -27,10 +27,14 @@ extern void ui_process(void);
 extern int16_t rx_buffer[];
 extern int16_t tx_buffer[];
 
-extern int16_t buffer_i[];
-extern int16_t buffer_q[];
+#define STATE_LEN 32
+#define SAMPLE_LEN 240
 
-void dsp_process(int16_t *src, int16_t *dst, size_t len);
-void set_agc_mode(int agcmode);
+extern int16_t ref_state[];
+extern int16_t ref_buf[];
+extern int16_t refq_buf[];
+extern int16_t samp_buf[];
+
+void dsp_process(int16_t *src, size_t len);
 
 void si5351_set_frequency_with_offset(int freq, int offset, uint8_t drive_strength);