Compare commits
1 commit
master
...
headphones
| Author | SHA1 | Date | |
|---|---|---|---|
| 83bb953842 |
2 changed files with 155 additions and 9 deletions
50
src/cordic.h
Normal file
50
src/cordic.h
Normal file
|
|
@ -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;
|
||||
}
|
||||
114
src/main.c
114
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];
|
||||
uint8_t *blockptr = block;
|
||||
int8_t *block = iq_queue_buffer[iq_queue_pos];
|
||||
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);
|
||||
fflush(stdout);
|
||||
for (int i = 0; i < IQ_BLOCK_LEN; i += 2) {
|
||||
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),
|
||||
|
|
|
|||
Loading…
Reference in a new issue