From 10b2cb7702b9357620a50ad70b185e5fd81f6e75 Mon Sep 17 00:00:00 2001 From: TT Date: Sun, 18 Sep 2016 10:11:18 +0900 Subject: [PATCH] add dsp.c (hilbert, iir) --- Makefile | 2 +- dsp.c | 89 +++++++++++++++++++++++++++++++++++++++++++++++++++++++ main.c | 41 +++++++++++++------------ nanovna.h | 12 +++++--- 4 files changed, 120 insertions(+), 24 deletions(-) create mode 100644 dsp.c diff --git a/Makefile b/Makefile index 3a60ee1..30d26ac 100644 --- a/Makefile +++ b/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) \ diff --git a/dsp.c b/dsp.c new file mode 100644 index 0000000..0e3ca1a --- /dev/null +++ b/dsp.c @@ -0,0 +1,89 @@ +#include +#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(); +} diff --git a/main.c b/main.c index 8905ea6..2663714 100644 --- a/main.c +++ b/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[]) diff --git a/nanovna.h b/nanovna.h index a7308ba..3c91b83 100644 --- a/nanovna.h +++ b/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);