use float
This commit is contained in:
Patrick Moessler
parent
fc4005cc49
commit
6716e396a5
3 changed files with 227 additions and 73 deletions
3
.vscode/settings.json
vendored
3
.vscode/settings.json
vendored
|
|
@ -15,6 +15,7 @@
|
|||
"array": "cpp",
|
||||
"string": "cpp",
|
||||
"string_view": "cpp",
|
||||
"span": "cpp"
|
||||
"span": "cpp",
|
||||
"random": "c"
|
||||
}
|
||||
}
|
||||
|
|
|
|||
|
|
@ -2,12 +2,14 @@
|
|||
** SPDX - License - Identifier : CC0 - 1.0
|
||||
*/
|
||||
|
||||
#include <inttypes.h>
|
||||
#include <math.h>
|
||||
#include <stdio.h>
|
||||
|
||||
#include "esp_system.h"
|
||||
#include "freertos/FreeRTOS.h"
|
||||
#include "freertos/task.h"
|
||||
#include "sdkconfig.h"
|
||||
#include <inttypes.h>
|
||||
#include <stdio.h>
|
||||
|
||||
#include "driver/i2s_std.h"
|
||||
#include "driver/uart.h"
|
||||
|
|
@ -31,16 +33,23 @@ complex format: buf[0]=re[0], buf[1]=im[0]
|
|||
|
||||
#define SAMPLE_RATE 24000
|
||||
#define SAMPLE_COUNT 512
|
||||
#define SAMPLE_CHANNELS 2
|
||||
#define SAMPLE_TYPE int16_t
|
||||
#define SAMPLE_CHANNELS 1
|
||||
#define SAMPLE_TYPE int32_t
|
||||
#define SAMPLE_MAX 2147483648.0f
|
||||
|
||||
#define BUF_COUNT 4
|
||||
#define BUF_COUNT 2
|
||||
|
||||
#define BANDS_COUNT 3
|
||||
|
||||
// ----------------------------------------------------------------
|
||||
/*
|
||||
|
||||
512 fft results
|
||||
-> fft(256) with 512 real values in 256 complex
|
||||
|
||||
mic in 512 samples
|
||||
fft buf 512 samples
|
||||
|
||||
512:
|
||||
bass: 1-4
|
||||
mid: 5-42
|
||||
|
|
@ -48,14 +57,14 @@ treb: 43-255
|
|||
|
||||
*/
|
||||
|
||||
|
||||
#define SAMPLE_BYTES (sizeof(SAMPLE_TYPE))
|
||||
#define BUF_SIZE_SAMPLES (SAMPLE_COUNT * SAMPLE_CHANNELS)
|
||||
#define BUF_SIZE_BYTES (BUF_SIZE_SAMPLES * SAMPLE_BYTES)
|
||||
|
||||
#define FALLOFF(OLD_V, NEW_V) ((OLD_V >> 1) + (OLD_V >> 2) + (NEW_V >> 2))
|
||||
#define ABS(T, V) ((v < 0 ? (((T)0) - v) : v))
|
||||
#define ABS(T, v) ((v < 0 ? (((T)0) - v) : v))
|
||||
#define MAX(a, b) ((b > a) ? (b) : (a))
|
||||
#define MIN(a, b) ((b < a) ? (b) : (a))
|
||||
|
||||
i2s_chan_handle_t rx_handle = NULL;
|
||||
TaskHandle_t proc_task = NULL;
|
||||
|
|
@ -63,12 +72,14 @@ TaskHandle_t output_task = NULL;
|
|||
|
||||
SemaphoreHandle_t output_mutex;
|
||||
|
||||
__attribute__((aligned(16))) static SAMPLE_TYPE i2s_readraw_buff[BUF_SIZE_BYTES * BUF_COUNT];
|
||||
__attribute__((aligned(16))) static SAMPLE_TYPE i2s_readraw_buff[BUF_SIZE_SAMPLES * BUF_COUNT];
|
||||
__attribute__((aligned(16))) static float fft_buffer[SAMPLE_COUNT * BUF_COUNT];
|
||||
static float wind[SAMPLE_COUNT];
|
||||
|
||||
uint32_t current_powers[BANDS_COUNT];
|
||||
uint32_t avg_powers[BANDS_COUNT];
|
||||
|
||||
int16_t floating_max;
|
||||
SAMPLE_TYPE floating_max;
|
||||
|
||||
void app_init_mic(void)
|
||||
{
|
||||
|
|
@ -83,7 +94,7 @@ void app_init_mic(void)
|
|||
.data_bit_width = 8 * SAMPLE_BYTES,
|
||||
.slot_bit_width = I2S_SLOT_BIT_WIDTH_32BIT,
|
||||
.slot_mode = I2S_SLOT_MODE_MONO,
|
||||
.slot_mask = I2S_STD_SLOT_BOTH,
|
||||
.slot_mask = I2S_STD_SLOT_LEFT,
|
||||
.ws_width = 32,
|
||||
.ws_pol = false,
|
||||
.bit_shift = true,
|
||||
|
|
@ -109,48 +120,93 @@ void app_init_mic(void)
|
|||
ESP_ERROR_CHECK(i2s_channel_enable(rx_handle));
|
||||
}
|
||||
|
||||
static void app_process_task(void *args)
|
||||
static void app_output_task(void *args)
|
||||
{
|
||||
uint32_t notify_value;
|
||||
uint32_t in_fft_buf_idx;
|
||||
|
||||
const uint8_t sync_word[] = {0x00, 0x11, 0x80, 0x00, 0x7f, 0xff, 0x11, 0x00};
|
||||
|
||||
while (1)
|
||||
{
|
||||
xTaskNotifyWait(0, 0, ¬ify_value, portMAX_DELAY);
|
||||
if (notify_value >= BUF_COUNT)
|
||||
xTaskNotifyWait(0, 0, &in_fft_buf_idx, portMAX_DELAY);
|
||||
if (in_fft_buf_idx >= BUF_COUNT)
|
||||
{
|
||||
printf("process buf count wrong: %ld", notify_value);
|
||||
printf("output buf count wrong: %ld", in_fft_buf_idx);
|
||||
continue;
|
||||
}
|
||||
|
||||
SAMPLE_TYPE *buf = i2s_readraw_buff + notify_value * BUF_SIZE_SAMPLES;
|
||||
// printf("%08lx - %08lx - %08lx\n", (uint32_t)i2s_readraw_buff, (uint32_t)buf,
|
||||
// (uint32_t)(buf - i2s_readraw_buff));
|
||||
float *fft_buf = fft_buffer + in_fft_buf_idx * SAMPLE_COUNT;
|
||||
|
||||
int16_t max = 0;
|
||||
for (int i = 0; i < SAMPLE_COUNT; i++)
|
||||
// buf[SAMPLE_COUNT - 1] = floating_max;
|
||||
|
||||
uart_write_bytes(UART_NUM_0, sync_word, sizeof(sync_word));
|
||||
|
||||
uart_write_bytes(UART_NUM_0, &floating_max, sizeof(floating_max));
|
||||
uart_write_bytes(UART_NUM_0, fft_buf, sizeof(float) * SAMPLE_COUNT);
|
||||
|
||||
// xSemaphoreTake(output_mutex, portMAX_DELAY);
|
||||
// uart_write_bytes(UART_NUM_0, avg_powers, sizeof(avg_powers));
|
||||
// uart_write_bytes(UART_NUM_0, current_powers, sizeof(current_powers));
|
||||
// xSemaphoreGive(output_mutex);
|
||||
}
|
||||
}
|
||||
|
||||
static void app_process_task(void *args)
|
||||
{
|
||||
uint32_t in_rx_buf_idx;
|
||||
uint32_t next_fft_buf_idx = 0;
|
||||
|
||||
dsps_wind_hann_f32(wind, SAMPLE_COUNT);
|
||||
|
||||
while (1)
|
||||
{
|
||||
/* input check*/
|
||||
xTaskNotifyWait(0, 0, &in_rx_buf_idx, portMAX_DELAY);
|
||||
if (in_rx_buf_idx >= BUF_COUNT)
|
||||
{
|
||||
max = MAX(max, buf[i * 2]);
|
||||
printf("process buf count wrong: %ld", in_rx_buf_idx);
|
||||
continue;
|
||||
}
|
||||
|
||||
floating_max = (max > floating_max) ? max : FALLOFF(floating_max, FALLOFF(floating_max,max));
|
||||
|
||||
// int16_t scale = 0x7fff / floating_max;
|
||||
|
||||
int16_t scale = 8;
|
||||
|
||||
// for (int16_t max_tmp = floating_max; scale < 15 && ((max_tmp & 0x8000) == 0); scale++, max_tmp <<= 1)
|
||||
// ;
|
||||
|
||||
for (int i = 0; i < SAMPLE_COUNT; i++)
|
||||
/* get maximum */
|
||||
{
|
||||
buf[i * 2] <<= scale-1;
|
||||
SAMPLE_TYPE max = 0;
|
||||
SAMPLE_TYPE *in_buf = i2s_readraw_buff + in_rx_buf_idx * BUF_SIZE_SAMPLES;
|
||||
for (int i = 0; i < SAMPLE_COUNT; i++)
|
||||
{
|
||||
max = MAX(max, ABS(SAMPLE_TYPE, in_buf[i]));
|
||||
}
|
||||
floating_max =
|
||||
MAX(10000000, (max > floating_max) ? max : FALLOFF(floating_max, FALLOFF(floating_max, max)));
|
||||
}
|
||||
|
||||
// dsps_mulc_s16_ae32(buf,buf,SAMPLE_COUNT,scale,1,1);
|
||||
/* convert to floats for input to fft */
|
||||
{
|
||||
SAMPLE_TYPE *in_buf = i2s_readraw_buff + in_rx_buf_idx * BUF_SIZE_SAMPLES;
|
||||
float *fft_buf = fft_buffer + next_fft_buf_idx * SAMPLE_COUNT;
|
||||
do
|
||||
{
|
||||
// *fft_buf++ = ((float)*in_buf++) / SAMPLE_MAX;
|
||||
*fft_buf++ = ((float)*in_buf++) / floating_max;
|
||||
} while (in_buf < i2s_readraw_buff + (in_rx_buf_idx + 1) * BUF_SIZE_SAMPLES);
|
||||
}
|
||||
|
||||
// dsps_fft2r_sc16(buf, SAMPLE_COUNT);
|
||||
// dsps_bit_rev_sc16_ansi(buf, SAMPLE_COUNT);
|
||||
// dsps_cplx2real_sc16_ansi(buf, SAMPLE_COUNT);
|
||||
/* do fft */
|
||||
{
|
||||
float *fft_buf = fft_buffer + next_fft_buf_idx * SAMPLE_COUNT;
|
||||
dsps_mul_f32(fft_buf, wind, fft_buf, SAMPLE_COUNT, 1, 1, 1);
|
||||
|
||||
dsps_fft2r_fc32(fft_buf, SAMPLE_COUNT >> 1); // operating on half length but complex
|
||||
dsps_bit_rev2r_fc32(fft_buf, SAMPLE_COUNT >> 1); // operating on half length but complex
|
||||
dsps_cplx2real_fc32(fft_buf, SAMPLE_COUNT >> 1); // operating on half length but complex
|
||||
|
||||
// for (int i = 0; i < SAMPLE_COUNT / 2; i++)
|
||||
// {
|
||||
// fft_buf[i] = 10 * log10f((fft_buf[i * 2 + 0] * fft_buf[i * 2 + 0] +
|
||||
// fft_buf[i * 2 + 1] * fft_buf[i * 2 + 1] + 0.0000001) /
|
||||
// SAMPLE_COUNT);
|
||||
// }
|
||||
}
|
||||
|
||||
xSemaphoreTake(output_mutex, portMAX_DELAY);
|
||||
|
||||
|
|
@ -187,46 +243,18 @@ static void app_process_task(void *args)
|
|||
|
||||
xSemaphoreGive(output_mutex);
|
||||
|
||||
xTaskNotify(output_task, notify_value, eSetValueWithOverwrite);
|
||||
xTaskNotify(output_task, next_fft_buf_idx, eSetValueWithOverwrite);
|
||||
|
||||
next_fft_buf_idx = (next_fft_buf_idx + 1) & (BUF_COUNT - 1);
|
||||
|
||||
// printf("[0] %08x [1] %08x [2] %08x [3]%08x ...\n", (buf[0]), (buf[1]), (buf[2]), (buf[3]));
|
||||
}
|
||||
}
|
||||
|
||||
static void app_output_task(void *args)
|
||||
{
|
||||
uint32_t notify_value;
|
||||
|
||||
const uint8_t sync_word[] = {0x00,0x11,0x80,0x00,0x7f,0xff,0x11,0x00};
|
||||
|
||||
while (1)
|
||||
{
|
||||
xTaskNotifyWait(0, 0, ¬ify_value, portMAX_DELAY);
|
||||
if (notify_value >= BUF_COUNT)
|
||||
{
|
||||
printf("output buf count wrong: %ld", notify_value);
|
||||
continue;
|
||||
}
|
||||
|
||||
SAMPLE_TYPE *buf = i2s_readraw_buff + notify_value * BUF_SIZE_SAMPLES;
|
||||
|
||||
buf[SAMPLE_COUNT - 1] = floating_max;
|
||||
|
||||
uart_write_bytes(UART_NUM_0, sync_word, sizeof(sync_word));
|
||||
|
||||
uart_write_bytes(UART_NUM_0, buf, BUF_SIZE_BYTES);
|
||||
|
||||
// xSemaphoreTake(output_mutex, portMAX_DELAY);
|
||||
// uart_write_bytes(UART_NUM_0, avg_powers, sizeof(avg_powers));
|
||||
// uart_write_bytes(UART_NUM_0, current_powers, sizeof(current_powers));
|
||||
// xSemaphoreGive(output_mutex);
|
||||
}
|
||||
}
|
||||
|
||||
void app_main(void)
|
||||
{
|
||||
size_t bytes_read = 0;
|
||||
uint32_t next_rx = 0;
|
||||
uint32_t next_rx_buf_idx = 0;
|
||||
esp_err_t ret;
|
||||
|
||||
app_init_mic();
|
||||
|
|
@ -243,7 +271,14 @@ void app_main(void)
|
|||
|
||||
uart_driver_install(UART_NUM_0, 256, 256, 0, NULL, 0);
|
||||
|
||||
ret = dsps_fft2r_init_sc16(NULL, SAMPLE_COUNT);
|
||||
ret = dsps_fft2r_init_fc32(NULL, SAMPLE_COUNT >> 1);
|
||||
if (ret != ESP_OK)
|
||||
{
|
||||
ESP_LOGE(TAG, "Not possible to initialize FFT. Error = %i", ret);
|
||||
return;
|
||||
}
|
||||
|
||||
ret = dsps_fft4r_init_fc32(NULL, SAMPLE_COUNT >> 1);
|
||||
if (ret != ESP_OK)
|
||||
{
|
||||
ESP_LOGE(TAG, "Not possible to initialize FFT. Error = %i", ret);
|
||||
|
|
@ -257,11 +292,11 @@ void app_main(void)
|
|||
|
||||
while (1)
|
||||
{
|
||||
if (i2s_channel_read(rx_handle, (char *)i2s_readraw_buff + (BUF_SIZE_BYTES * next_rx), BUF_SIZE_BYTES,
|
||||
if (i2s_channel_read(rx_handle, (char *)i2s_readraw_buff + (BUF_SIZE_BYTES * next_rx_buf_idx), BUF_SIZE_BYTES,
|
||||
&bytes_read, 1000) == ESP_OK)
|
||||
{
|
||||
xTaskNotify(proc_task, next_rx, eSetValueWithOverwrite);
|
||||
next_rx = (next_rx + 1) & (BUF_COUNT - 1);
|
||||
xTaskNotify(proc_task, next_rx_buf_idx, eSetValueWithOverwrite);
|
||||
next_rx_buf_idx = (next_rx_buf_idx + 1) & (BUF_COUNT - 1);
|
||||
// printf("main buf next %ld\n", next_rx);
|
||||
taskYIELD();
|
||||
}
|
||||
|
|
|
|||
118
python/plotserial_fft_float.py
Normal file
118
python/plotserial_fft_float.py
Normal file
|
|
@ -0,0 +1,118 @@
|
|||
# /// script
|
||||
# requires-python = ">=3.13"
|
||||
# dependencies = [
|
||||
# "matplotlib",
|
||||
# "numpy",
|
||||
# "pyserial",
|
||||
# ]
|
||||
# ///
|
||||
|
||||
from struct import unpack
|
||||
from threading import Thread
|
||||
|
||||
import matplotlib.pyplot as plt
|
||||
import numpy as np
|
||||
from matplotlib.animation import FuncAnimation
|
||||
from serial import Serial
|
||||
|
||||
SYNC = bytes.fromhex("001180007fff1100")
|
||||
BLOCK_LENGTH = 512
|
||||
BLOCK_SIZE = 4 * BLOCK_LENGTH
|
||||
|
||||
data: list[bytes] = []
|
||||
|
||||
|
||||
do_recv = True
|
||||
recv_thread = None
|
||||
|
||||
|
||||
def recv_main():
|
||||
ser = Serial("COM5", baudrate=1000000)
|
||||
|
||||
def sync():
|
||||
syncbuf = bytearray(ser.read(len(SYNC)))
|
||||
if syncbuf[: len(SYNC)] == SYNC:
|
||||
return
|
||||
print("not in sync...")
|
||||
synced = False
|
||||
while not synced:
|
||||
if syncbuf[: len(SYNC)] == SYNC:
|
||||
synced = True
|
||||
break
|
||||
syncbuf.append(ser.read(1)[0])
|
||||
syncbuf.pop(0)
|
||||
if not synced:
|
||||
raise ConnectionError
|
||||
print("synced")
|
||||
|
||||
while do_recv:
|
||||
sync()
|
||||
data.append(ser.read(4 + BLOCK_SIZE))
|
||||
|
||||
|
||||
def main() -> None:
|
||||
global recv_thread
|
||||
x1 = np.arange(BLOCK_LENGTH)
|
||||
y1 = np.zeros(BLOCK_LENGTH)
|
||||
x2 = np.arange(BLOCK_LENGTH)
|
||||
y2 = np.zeros(BLOCK_LENGTH)
|
||||
|
||||
fig, ax = plt.subplots()
|
||||
fig.canvas.mpl_connect("close_event", on_close)
|
||||
graph1 = ax.plot(x1, y1)[0]
|
||||
graph2 = ax.plot(x2, y2)[0]
|
||||
plt.ylim(-100, 100)
|
||||
# ax.set_ylim(1 - 2**32, 2**32)
|
||||
|
||||
np.set_printoptions(suppress=True, precision=2)
|
||||
|
||||
recv_thread = Thread(target=recv_main)
|
||||
|
||||
recv_thread.start()
|
||||
|
||||
def update(frame):
|
||||
|
||||
if data:
|
||||
block = data.pop(0)
|
||||
else:
|
||||
return
|
||||
if len(data) > 4:
|
||||
print(f"buffer overflow: {len(data)}")
|
||||
data.clear()
|
||||
|
||||
# print(block.hex())
|
||||
values = unpack(f"I{BLOCK_LENGTH}f", block)
|
||||
|
||||
floating_max = values[0]
|
||||
y1[:] = values[1 : BLOCK_LENGTH + 1]
|
||||
|
||||
print(f"floating_max:{floating_max}")
|
||||
print(f"min:{np.min(y1)}")
|
||||
print(f"max:{np.max(y1)}")
|
||||
print(f"average:{np.average(y1)}")
|
||||
print(f"median:{np.median(y1)}")
|
||||
|
||||
y2[:] = np.roll(y2,-1)
|
||||
y2[-1] = np.log10((60*floating_max / (2**31)))
|
||||
# print(y2)
|
||||
|
||||
graph1.set_ydata(y1)
|
||||
graph2.set_ydata(y2)
|
||||
|
||||
return [graph1]
|
||||
|
||||
anim = FuncAnimation(
|
||||
fig, update, frames=None, cache_frame_data=False, interval=64 / 24000
|
||||
)
|
||||
plt.show()
|
||||
|
||||
|
||||
def on_close(_):
|
||||
global do_recv
|
||||
do_recv = False
|
||||
if recv_thread:
|
||||
recv_thread.join()
|
||||
|
||||
|
||||
if __name__ == "__main__":
|
||||
main()
|
||||
Loading…
Add table
Reference in a new issue