Experiment with headphones

src/cordic.h

@@ -0,0 +1,50 @@
+#pragma once
+
+/*
+ * Copyright (c) 2020 STMicroelectronics.
+ * All rights reserved.
+ *
+ * This software component is licensed by ST under BSD 3-Clause license,
+ * the "License"; You may not use this file except in compliance with the
+ * License. You may obtain a copy of the License at:
+ *
+ * https://opensource.org/licenses/BSD-3-Clause
+ */
+
+#define CORDIC_MAXITER 9
+#define CORDIC_PI 0x10000000
+
+static int CORDIC_ZTBL[] = { 0x04000000, 0x025C80A4, 0x013F670B, 0x00A2223B, 0x005161A8, 0x0028BAFC,
+			     0x00145EC4, 0x000A2F8B, 0x000517CA, 0x00028BE6, 0x000145F3, 0x0000A2FA,
+			     0x0000517D, 0x000028BE, 0x0000145F, 0x00000A30 };
+
+inline static __attribute__((__unused__)) int fast_atan2(int y, int x)
+{
+	int k, tx, z = 0, fl = 0;
+
+	if (x < 0) {
+		fl = ((y > 0) ? +1 : -1);
+		x = -x;
+		y = -y;
+	}
+
+	for (k = 0; k < CORDIC_MAXITER; k++) {
+		tx = x;
+
+		if (y <= 0) {
+			x -= (y >> k);
+			y += (tx >> k);
+			z -= CORDIC_ZTBL[k];
+		} else {
+			x += (y >> k);
+			y -= (tx >> k);
+			z += CORDIC_ZTBL[k];
+		}
+	}
+
+	if (fl != 0) {
+		z += fl * CORDIC_PI;
+	}
+
+	return z;
+}

src/main.c

@@ -21,6 +21,8 @@
 #include <limits.h>
 #include <stdlib.h>
 
+#include "cordic.h"
+
 #define VREG_VOLTAGE VREG_VOLTAGE_1_20
 #define CLK_SYS_HZ (300 * MHZ)
 #define PSU_PIN 23
@@ -48,6 +50,12 @@ static_assert(RX_STRIDE * 4 < RX_WORDS, "RX_STRIDE * 4 < RX_WORDS");
 static uint32_t rx_cos[RX_WORDS] __attribute__((__aligned__(1 << RX_BITS_DEPTH)));
 static uint32_t rx_sin[RX_WORDS] __attribute__((__aligned__(1 << RX_BITS_DEPTH)));
 
+#define AOUT_PIN 0
+#define AOUT_BITS_DEPTH 9
+#define AOUT_WORDS (1 << (AOUT_BITS_DEPTH - 1))
+
+static uint16_t aout_buf[AOUT_WORDS] __attribute__((__aligned__(1 << AOUT_BITS_DEPTH)));
+
 #define INIT_GAIN 120
 #define INIT_SAMPLE_RATE 100000
 #define INIT_FREQ 94600000
@@ -79,10 +87,16 @@ static int dma_t_samp = -1;
 static int dma_ch_in_cos = -1;
 static int dma_ch_in_sin = -1;
 
+static int dma_ch_aout = -1;
+
+static int dma_t_aout = -1;
+
 static queue_t iq_queue;
-static uint8_t iq_queue_buffer[IQ_QUEUE_LEN][IQ_BLOCK_LEN];
+static int8_t iq_queue_buffer[IQ_QUEUE_LEN][IQ_BLOCK_LEN];
 static size_t iq_queue_pos = 0;
 
+static int slice, chan;
+
 static void bias_set_delay(int delay)
 {
 	delay += 200;
@@ -290,7 +304,10 @@ static void rf_rx_start(int rx_pin, int bias_pin)
 	dma_ch_samp_sin = dma_claim_unused_channel(true);
 	dma_ch_samp_trig = dma_claim_unused_channel(true);
 
+	dma_ch_aout = dma_claim_unused_channel(true);
+
 	dma_t_samp = dma_claim_unused_timer(true);
+	dma_t_aout = dma_claim_unused_timer(true);
 
 	dma_channel_config dma_conf;
 
@@ -385,11 +402,24 @@ static void rf_rx_start(int rx_pin, int bias_pin)
 	channel_config_set_chain_to(&dma_conf, dma_ch_samp_trig);
 	dma_channel_configure(dma_ch_samp_sin, &dma_conf, &pio1->sm[3].instr, samp_insn, 4, false);
 
+	/* Pacing timer for the audio output. */
+	dma_timer_set_fraction(dma_t_aout, 1, CLK_SYS_HZ / 200000);
+
+	dma_conf = dma_channel_get_default_config(dma_ch_aout);
+	channel_config_set_transfer_data_size(&dma_conf, DMA_SIZE_16);
+	channel_config_set_read_increment(&dma_conf, true);
+	channel_config_set_write_increment(&dma_conf, false);
+	channel_config_set_ring(&dma_conf, false, AOUT_BITS_DEPTH);
+	channel_config_set_dreq(&dma_conf, dma_get_timer_dreq(dma_t_aout));
+	dma_channel_configure(dma_ch_aout, &dma_conf, &pwm_hw->slice[slice].cc, aout_buf, UINT_MAX,
+			      false);
+
 	bias_init(rx_pin, bias_pin);
 	adder_init();
 
 	dma_channel_start(dma_ch_rx);
 	dma_channel_start(dma_ch_samp_trig);
+	dma_channel_start(dma_ch_aout);
 	watch_init(rx_pin);
 }
 
@@ -421,6 +451,7 @@ static void rf_rx_stop(void)
 	dma_channel_abort(dma_ch_samp_cos);
 	dma_channel_abort(dma_ch_samp_sin);
 	dma_channel_abort(dma_ch_samp_trig);
+	dma_channel_abort(dma_ch_aout);
 
 	dma_channel_cleanup(dma_ch_rx);
 	dma_channel_cleanup(dma_ch_cp);
@@ -431,6 +462,7 @@ static void rf_rx_stop(void)
 	dma_channel_cleanup(dma_ch_samp_cos);
 	dma_channel_cleanup(dma_ch_samp_sin);
 	dma_channel_cleanup(dma_ch_samp_trig);
+	dma_channel_cleanup(dma_ch_aout);
 
 	dma_channel_unclaim(dma_ch_rx);
 	dma_channel_unclaim(dma_ch_cp);
@@ -441,8 +473,10 @@ static void rf_rx_stop(void)
 	dma_channel_unclaim(dma_ch_samp_cos);
 	dma_channel_unclaim(dma_ch_samp_sin);
 	dma_channel_unclaim(dma_ch_samp_trig);
+	dma_channel_unclaim(dma_ch_aout);
 
 	dma_timer_unclaim(dma_t_samp);
+	dma_timer_unclaim(dma_t_aout);
 
 	dma_ch_rx = -1;
 	dma_ch_cp = -1;
@@ -453,8 +487,10 @@ static void rf_rx_stop(void)
 	dma_ch_samp_cos = -1;
 	dma_ch_samp_sin = -1;
 	dma_ch_samp_trig = -1;
+	dma_ch_aout = -1;
 
 	dma_t_samp = -1;
+	dma_t_aout = -1;
 }
 
 static void rf_rx(void)
@@ -483,8 +519,8 @@ static void rf_rx(void)
 
 		pos = (pos + RX_STRIDE) & (RX_WORDS - 1);
 
-		uint8_t *block = iq_queue_buffer[iq_queue_pos];
+		int8_t *block = iq_queue_buffer[iq_queue_pos];
-		uint8_t *blockptr = block;
+		int8_t *blockptr = block;
 
 		/*
 		 * Since every 2 samples add to either +1 or -1,
@@ -551,7 +587,7 @@ static void rf_rx(void)
 			else if (I < -128)
 				I = -128;
 
-			*blockptr++ = (uint8_t)I + 128;
+			*blockptr++ = I;
 
 			Q *= gain;
 			Q -= (max_amplitude * 181) / 256;
@@ -562,7 +598,7 @@ static void rf_rx(void)
 			else if (Q < -128)
 				Q = -128;
 
-			*blockptr++ = (uint8_t)Q + 128;
+			*blockptr++ = Q;
 		}
 
 		if (queue_try_add(&iq_queue, &block)) {
@@ -649,11 +685,18 @@ static void do_rx(int rx_pin, int bias_pin)
 
 	multicore_launch_core1(rf_rx);
 
-	const uint8_t *block;
+	int8_t *block;
 
 	while (queue_try_remove(&iq_queue, &block))
 		/* Flush the queue */;
 
+	pwm_set_enabled(slice, true);
+
+	unsigned addr = 0;
+	int prev = 0;
+	int h0 = 0, h1 = 0, h2 = 0, h3 = 0, h4 = 0, h5 = 0, h6 = 0, h7 = 0;
+	int k0 = 0, k1 = 0, k2 = 0, k3 = 0, k4 = 0, k5 = 0, k6 = 0, k7 = 0;
+
 	while (true) {
 		int cmd;
 
@@ -662,12 +705,47 @@ static void do_rx(int rx_pin, int bias_pin)
 				goto done;
 
 		if (queue_try_remove(&iq_queue, &block)) {
-			fwrite(block, IQ_BLOCK_LEN, 1, stdout);
+			for (int i = 0; i < IQ_BLOCK_LEN; i += 2) {
-			fflush(stdout);
+				while (addr == dma_hw->ch[dma_ch_aout].read_addr)
+					sleep_us(1);
+
+				addr += 2;
+
+				if (addr >= (unsigned)aout_buf + AOUT_WORDS * 2)
+					addr = (unsigned)aout_buf;
+
+				unsigned pos = addr - (unsigned)aout_buf;
+				pos /= 2;
+				pos += AOUT_WORDS - 2;
+				pos &= AOUT_WORDS - 1;
+
+				//int sample = (127 / M_PI) * atan2f(block[i + 1], block[i]);
+				int sample = fast_atan2(block[i + 1], block[i]) / (CORDIC_PI / 128);
+				int phase = sample - prev;
+				prev = sample;
+
+				uint8_t audio = 128 + (uint8_t)phase;
+
+				h7 = h6, h6 = h5, h5 = h4, h4 = h3, h3 = h2, h2 = h1, h1 = h0,
+				h0 = audio;
+				audio = (h0 + h1 + h2 + h3 + h4 + h5 + h6 + h7) / 8;
+
+				k7 = k6, k6 = k5, k5 = k4, k4 = k3, k3 = k2, k2 = k1, k1 = k0,
+				k0 = audio;
+				audio = (k0 + k1 + k2 + k3 + k4 + k5 + k6 + k7) / 8;
+
+				aout_buf[pos] = audio;
+				block[i] = audio;
+				block[i + 1] = audio;
+			}
+
+			//fwrite(block, IQ_BLOCK_LEN, 1, stdout);
+			//fflush(stdout);
 		}
 	}
 
 done:
+	pwm_set_enabled(slice, false);
 	multicore_fifo_push_blocking(0);
 	multicore_fifo_pop_blocking();
 	sleep_us(10);
@@ -693,10 +771,24 @@ int main()
 			CLK_SYS_HZ);
 
 	/* Enable PSU PWM mode. */
-	gpio_init(PSU_PIN);
+	gpio_disable_pulls(PSU_PIN);
+	gpio_set_function(PSU_PIN, GPIO_FUNC_SIO);
 	gpio_set_dir(PSU_PIN, GPIO_OUT);
 	gpio_put(PSU_PIN, 1);
 
+	/* Configure audio output. */
+	slice = pwm_gpio_to_slice_num(AOUT_PIN);
+	chan = pwm_gpio_to_channel(AOUT_PIN);
+	gpio_set_drive_strength(AOUT_PIN, GPIO_DRIVE_STRENGTH_2MA);
+	gpio_set_slew_rate(AOUT_PIN, GPIO_SLEW_RATE_SLOW);
+	pwm_set_clkdiv_int_frac(slice, 1, 0);
+	pwm_set_enabled(slice, false);
+	pwm_set_wrap(slice, 2048);
+	pwm_set_chan_level(slice, chan, 0);
+
+	gpio_disable_pulls(AOUT_PIN);
+	gpio_set_function(AOUT_PIN, GPIO_FUNC_PWM);
+
 	bus_ctrl_hw->priority |= BUSCTRL_BUS_PRIORITY_DMA_W_BITS | BUSCTRL_BUS_PRIORITY_DMA_R_BITS;
 
 	stdio_usb_init();
@@ -706,6 +798,10 @@ int main()
 
 	rx_lo_init(INIT_FREQ - INIT_SAMPLE_RATE, true);
 
+	run_command(0x02, 200000);
+	run_command(0x01, 88200000);
+	do_rx(10, 11);
+
 	while (true) {
 		if (check_command() > 0) {
 			static const uint32_t header[3] = { __builtin_bswap32(0x52544c30),