437 lines
13 KiB
C
437 lines
13 KiB
C
#include "audio_service.h"
|
|
|
|
#include <math.h>
|
|
#include <stddef.h>
|
|
#include <stdint.h>
|
|
#include <string.h>
|
|
|
|
#include "driver/i2s_std.h"
|
|
#include "esp_log.h"
|
|
#include "freertos/FreeRTOS.h"
|
|
#include "freertos/task.h"
|
|
|
|
/* CrowPanel 7.0-inch V3 onboard NS4168 connections. */
|
|
#define AUDIO_I2S_PORT I2S_NUM_0
|
|
#define AUDIO_PIN_BCLK 42
|
|
#define AUDIO_PIN_LRCLK 18
|
|
#define AUDIO_PIN_DOUT 17
|
|
|
|
#define AUDIO_SAMPLE_RATE_HZ 16000U
|
|
#define AUDIO_DMA_BUFFER_COUNT 4
|
|
#define AUDIO_DMA_FRAMES 128U
|
|
#define AUDIO_CHANNEL_COUNT 2U
|
|
|
|
#define PING_DURATION_MS 70U
|
|
#define PING_TOTAL_FRAMES ((AUDIO_SAMPLE_RATE_HZ * PING_DURATION_MS) / 1000U)
|
|
#define PING_ATTACK_FRAMES ((AUDIO_SAMPLE_RATE_HZ * 4U) / 1000U)
|
|
#define PING_RELEASE_FRAMES ((AUDIO_SAMPLE_RATE_HZ * 12U) / 1000U)
|
|
#define PING_PEAK_AMPLITUDE 7000
|
|
|
|
#define DISTANCE_MIN_M 0.3f
|
|
#define DISTANCE_MAX_M 8.0f
|
|
#define PING_FREQUENCY_NEAR_HZ 1800U
|
|
#define PING_FREQUENCY_FAR_HZ 700U
|
|
#define PING_INTERVAL_NEAR_MS 120U
|
|
#define PING_INTERVAL_FAR_MS 900U
|
|
|
|
#define SINE_TABLE_BITS 8U
|
|
#define SINE_TABLE_SIZE (1U << SINE_TABLE_BITS)
|
|
#define AUDIO_TASK_STACK_BYTES 3072U
|
|
#define AUDIO_WRITE_TIMEOUT_MS 25U
|
|
|
|
typedef struct {
|
|
bool enabled;
|
|
bool has_target;
|
|
float nearest_distance_m;
|
|
} audio_state_snapshot_t;
|
|
|
|
static const char *TAG = "audio_service";
|
|
static portMUX_TYPE s_state_lock = portMUX_INITIALIZER_UNLOCKED;
|
|
static bool s_initialized;
|
|
static bool s_initializing;
|
|
static bool s_enabled = true;
|
|
static bool s_has_target;
|
|
static float s_nearest_distance_m;
|
|
static TaskHandle_t s_audio_task;
|
|
static i2s_chan_handle_t s_tx_channel;
|
|
|
|
/* Kept out of the worker stack: 512 B each for the LUT and stereo DMA chunk. */
|
|
static int16_t s_sine_table[SINE_TABLE_SIZE];
|
|
static int16_t s_pcm_chunk[AUDIO_DMA_FRAMES * AUDIO_CHANNEL_COUNT];
|
|
|
|
static void audio_task(void *context);
|
|
|
|
static void init_sine_table(void)
|
|
{
|
|
const float phase_scale = (2.0f * 3.14159265358979323846f) / (float)SINE_TABLE_SIZE;
|
|
|
|
for (uint32_t i = 0; i < SINE_TABLE_SIZE; ++i) {
|
|
s_sine_table[i] = (int16_t)(sinf((float)i * phase_scale) * 32767.0f);
|
|
}
|
|
}
|
|
|
|
static audio_state_snapshot_t state_snapshot(void)
|
|
{
|
|
audio_state_snapshot_t snapshot;
|
|
|
|
taskENTER_CRITICAL(&s_state_lock);
|
|
snapshot.enabled = s_enabled;
|
|
snapshot.has_target = s_has_target;
|
|
snapshot.nearest_distance_m = s_nearest_distance_m;
|
|
taskEXIT_CRITICAL(&s_state_lock);
|
|
|
|
return snapshot;
|
|
}
|
|
|
|
static void notify_audio_task(TaskHandle_t task)
|
|
{
|
|
if (task != NULL) {
|
|
xTaskNotifyGive(task);
|
|
}
|
|
}
|
|
|
|
static uint32_t map_frequency_hz(float distance_m)
|
|
{
|
|
const float position = (distance_m - DISTANCE_MIN_M) /
|
|
(DISTANCE_MAX_M - DISTANCE_MIN_M);
|
|
const float span = (float)(PING_FREQUENCY_NEAR_HZ - PING_FREQUENCY_FAR_HZ);
|
|
|
|
return (uint32_t)((float)PING_FREQUENCY_NEAR_HZ - (position * span) + 0.5f);
|
|
}
|
|
|
|
static uint32_t map_interval_ms(float distance_m)
|
|
{
|
|
const float position = (distance_m - DISTANCE_MIN_M) /
|
|
(DISTANCE_MAX_M - DISTANCE_MIN_M);
|
|
const float span = (float)(PING_INTERVAL_FAR_MS - PING_INTERVAL_NEAR_MS);
|
|
|
|
return (uint32_t)((float)PING_INTERVAL_NEAR_MS + (position * span) + 0.5f);
|
|
}
|
|
|
|
static uint32_t envelope_q15(uint32_t frame)
|
|
{
|
|
if (frame < PING_ATTACK_FRAMES) {
|
|
return (frame * 32767U) / PING_ATTACK_FRAMES;
|
|
}
|
|
|
|
const uint32_t frames_after = PING_TOTAL_FRAMES - frame - 1U;
|
|
if (frames_after < PING_RELEASE_FRAMES) {
|
|
return (frames_after * 32767U) / PING_RELEASE_FRAMES;
|
|
}
|
|
|
|
return 32767U;
|
|
}
|
|
|
|
static bool write_pcm(const int16_t *samples, size_t frame_count)
|
|
{
|
|
const uint8_t *source = (const uint8_t *)samples;
|
|
const size_t total_bytes = frame_count * AUDIO_CHANNEL_COUNT * sizeof(int16_t);
|
|
size_t offset = 0;
|
|
|
|
while (offset < total_bytes) {
|
|
size_t bytes_written = 0;
|
|
const esp_err_t err = i2s_channel_write(s_tx_channel,
|
|
source + offset,
|
|
total_bytes - offset,
|
|
&bytes_written,
|
|
AUDIO_WRITE_TIMEOUT_MS);
|
|
if (err != ESP_OK || bytes_written == 0U) {
|
|
ESP_LOGW(TAG, "I2S write failed: %s (%u/%u bytes)",
|
|
esp_err_to_name(err),
|
|
(unsigned)offset,
|
|
(unsigned)total_bytes);
|
|
return false;
|
|
}
|
|
offset += bytes_written;
|
|
}
|
|
|
|
return true;
|
|
}
|
|
|
|
static bool target_still_audible(void)
|
|
{
|
|
const audio_state_snapshot_t snapshot = state_snapshot();
|
|
return snapshot.enabled && snapshot.has_target;
|
|
}
|
|
|
|
static bool play_ping(uint32_t frequency_hz)
|
|
{
|
|
uint32_t phase = 0;
|
|
const uint32_t phase_step = (uint32_t)(((uint64_t)frequency_hz << 32) /
|
|
AUDIO_SAMPLE_RATE_HZ);
|
|
|
|
for (uint32_t first_frame = 0; first_frame < PING_TOTAL_FRAMES;
|
|
first_frame += AUDIO_DMA_FRAMES) {
|
|
if (!target_still_audible()) {
|
|
return false;
|
|
}
|
|
|
|
uint32_t frame_count = PING_TOTAL_FRAMES - first_frame;
|
|
if (frame_count > AUDIO_DMA_FRAMES) {
|
|
frame_count = AUDIO_DMA_FRAMES;
|
|
}
|
|
|
|
for (uint32_t i = 0; i < frame_count; ++i) {
|
|
const uint32_t frame = first_frame + i;
|
|
const uint32_t envelope = envelope_q15(frame);
|
|
const int32_t wave = s_sine_table[phase >> (32U - SINE_TABLE_BITS)];
|
|
int32_t sample = (wave * PING_PEAK_AMPLITUDE) >> 15;
|
|
sample = (sample * (int32_t)envelope) >> 15;
|
|
phase += phase_step;
|
|
|
|
s_pcm_chunk[i * AUDIO_CHANNEL_COUNT] = (int16_t)sample;
|
|
s_pcm_chunk[(i * AUDIO_CHANNEL_COUNT) + 1U] = (int16_t)sample;
|
|
}
|
|
|
|
if (!write_pcm(s_pcm_chunk, frame_count)) {
|
|
return false;
|
|
}
|
|
}
|
|
|
|
return true;
|
|
}
|
|
|
|
/*
|
|
* Queue one DMA ring of zero samples. Once these writes complete, every tone
|
|
* buffer has reached the peripheral, so it is safe to clear and stop the DMA
|
|
* without clipping the 70 ms envelope tail.
|
|
*/
|
|
static void drain_with_silence(void)
|
|
{
|
|
memset(s_pcm_chunk, 0, sizeof(s_pcm_chunk));
|
|
for (uint32_t i = 0; i < AUDIO_DMA_BUFFER_COUNT; ++i) {
|
|
if (!write_pcm(s_pcm_chunk, AUDIO_DMA_FRAMES)) {
|
|
break;
|
|
}
|
|
}
|
|
}
|
|
|
|
static void stop_output(bool *i2s_running, bool drain)
|
|
{
|
|
if (!*i2s_running) {
|
|
return;
|
|
}
|
|
|
|
if (drain) {
|
|
drain_with_silence();
|
|
}
|
|
(void)i2s_channel_disable(s_tx_channel);
|
|
*i2s_running = false;
|
|
}
|
|
|
|
static void audio_task(void *context)
|
|
{
|
|
(void)context;
|
|
bool i2s_running = false;
|
|
bool active_session = false;
|
|
TickType_t last_ping_tick = 0;
|
|
|
|
taskENTER_CRITICAL(&s_state_lock);
|
|
s_audio_task = xTaskGetCurrentTaskHandle();
|
|
taskEXIT_CRITICAL(&s_state_lock);
|
|
|
|
for (;;) {
|
|
const audio_state_snapshot_t snapshot = state_snapshot();
|
|
|
|
if (!snapshot.enabled || !snapshot.has_target) {
|
|
stop_output(&i2s_running, false);
|
|
active_session = false;
|
|
(void)ulTaskNotifyTake(pdTRUE, portMAX_DELAY);
|
|
continue;
|
|
}
|
|
|
|
const TickType_t now = xTaskGetTickCount();
|
|
if (active_session) {
|
|
const TickType_t interval_ticks =
|
|
pdMS_TO_TICKS(map_interval_ms(snapshot.nearest_distance_m));
|
|
const TickType_t elapsed_ticks = now - last_ping_tick;
|
|
if (elapsed_ticks < interval_ticks) {
|
|
(void)ulTaskNotifyTake(pdTRUE, interval_ticks - elapsed_ticks);
|
|
continue;
|
|
}
|
|
}
|
|
|
|
if (!i2s_running) {
|
|
if (i2s_channel_enable(s_tx_channel) != ESP_OK) {
|
|
ESP_LOGE(TAG, "Could not start I2S output");
|
|
(void)ulTaskNotifyTake(pdTRUE, pdMS_TO_TICKS(250));
|
|
continue;
|
|
}
|
|
i2s_running = true;
|
|
}
|
|
|
|
last_ping_tick = xTaskGetTickCount();
|
|
active_session = true;
|
|
const bool completed = play_ping(map_frequency_hz(snapshot.nearest_distance_m));
|
|
stop_output(&i2s_running, completed);
|
|
}
|
|
}
|
|
|
|
esp_err_t audio_service_init(void)
|
|
{
|
|
taskENTER_CRITICAL(&s_state_lock);
|
|
if (s_initialized) {
|
|
taskEXIT_CRITICAL(&s_state_lock);
|
|
return ESP_OK;
|
|
}
|
|
if (s_initializing) {
|
|
taskEXIT_CRITICAL(&s_state_lock);
|
|
return ESP_ERR_INVALID_STATE;
|
|
}
|
|
s_initializing = true;
|
|
taskEXIT_CRITICAL(&s_state_lock);
|
|
|
|
init_sine_table();
|
|
|
|
i2s_chan_config_t channel_config =
|
|
I2S_CHANNEL_DEFAULT_CONFIG(AUDIO_I2S_PORT, I2S_ROLE_MASTER);
|
|
channel_config.dma_desc_num = AUDIO_DMA_BUFFER_COUNT;
|
|
channel_config.dma_frame_num = AUDIO_DMA_FRAMES;
|
|
channel_config.auto_clear = true;
|
|
|
|
const i2s_std_config_t standard_config = {
|
|
.clk_cfg = I2S_STD_CLK_DEFAULT_CONFIG(AUDIO_SAMPLE_RATE_HZ),
|
|
.slot_cfg = I2S_STD_PHILIPS_SLOT_DEFAULT_CONFIG(
|
|
I2S_DATA_BIT_WIDTH_16BIT, I2S_SLOT_MODE_STEREO),
|
|
.gpio_cfg = {
|
|
.mclk = I2S_GPIO_UNUSED,
|
|
.bclk = AUDIO_PIN_BCLK,
|
|
.ws = AUDIO_PIN_LRCLK,
|
|
.dout = AUDIO_PIN_DOUT,
|
|
.din = I2S_GPIO_UNUSED,
|
|
.invert_flags = {
|
|
.mclk_inv = false,
|
|
.bclk_inv = false,
|
|
.ws_inv = false,
|
|
},
|
|
},
|
|
};
|
|
|
|
esp_err_t err = i2s_new_channel(&channel_config, &s_tx_channel, NULL);
|
|
if (err == ESP_OK) {
|
|
err = i2s_channel_init_std_mode(s_tx_channel, &standard_config);
|
|
}
|
|
|
|
if (err != ESP_OK) {
|
|
if (s_tx_channel != NULL) {
|
|
(void)i2s_del_channel(s_tx_channel);
|
|
s_tx_channel = NULL;
|
|
}
|
|
taskENTER_CRITICAL(&s_state_lock);
|
|
s_initializing = false;
|
|
taskEXIT_CRITICAL(&s_state_lock);
|
|
ESP_LOGE(TAG, "NS4168 I2S initialisation failed: %s", esp_err_to_name(err));
|
|
return err;
|
|
}
|
|
|
|
const BaseType_t task_created = xTaskCreate(audio_task,
|
|
"proximity_audio",
|
|
AUDIO_TASK_STACK_BYTES,
|
|
NULL,
|
|
tskIDLE_PRIORITY + 4,
|
|
NULL);
|
|
if (task_created != pdPASS) {
|
|
(void)i2s_del_channel(s_tx_channel);
|
|
s_tx_channel = NULL;
|
|
taskENTER_CRITICAL(&s_state_lock);
|
|
s_initializing = false;
|
|
taskEXIT_CRITICAL(&s_state_lock);
|
|
return ESP_ERR_NO_MEM;
|
|
}
|
|
|
|
taskENTER_CRITICAL(&s_state_lock);
|
|
s_initialized = true;
|
|
s_initializing = false;
|
|
taskEXIT_CRITICAL(&s_state_lock);
|
|
|
|
ESP_LOGI(TAG, "NS4168 ready: 16 kHz, stereo, BCLK=%d LRCLK=%d DOUT=%d",
|
|
AUDIO_PIN_BCLK, AUDIO_PIN_LRCLK, AUDIO_PIN_DOUT);
|
|
return ESP_OK;
|
|
}
|
|
|
|
void audio_service_set_enabled(bool enabled)
|
|
{
|
|
TaskHandle_t task;
|
|
|
|
taskENTER_CRITICAL(&s_state_lock);
|
|
s_enabled = enabled;
|
|
task = s_audio_task;
|
|
taskEXIT_CRITICAL(&s_state_lock);
|
|
|
|
notify_audio_task(task);
|
|
}
|
|
|
|
bool audio_service_is_enabled(void)
|
|
{
|
|
bool enabled;
|
|
|
|
taskENTER_CRITICAL(&s_state_lock);
|
|
enabled = s_enabled;
|
|
taskEXIT_CRITICAL(&s_state_lock);
|
|
|
|
return enabled;
|
|
}
|
|
|
|
void audio_service_set_nearest_distance(float distance_m)
|
|
{
|
|
if (!isfinite(distance_m) || distance_m <= 0.0f) {
|
|
audio_service_clear_target();
|
|
return;
|
|
}
|
|
|
|
if (distance_m < DISTANCE_MIN_M) {
|
|
distance_m = DISTANCE_MIN_M;
|
|
} else if (distance_m > DISTANCE_MAX_M) {
|
|
distance_m = DISTANCE_MAX_M;
|
|
}
|
|
|
|
TaskHandle_t task;
|
|
taskENTER_CRITICAL(&s_state_lock);
|
|
s_nearest_distance_m = distance_m;
|
|
s_has_target = true;
|
|
task = s_audio_task;
|
|
taskEXIT_CRITICAL(&s_state_lock);
|
|
|
|
notify_audio_task(task);
|
|
}
|
|
|
|
void audio_service_clear_target(void)
|
|
{
|
|
TaskHandle_t task;
|
|
|
|
taskENTER_CRITICAL(&s_state_lock);
|
|
s_has_target = false;
|
|
s_nearest_distance_m = 0.0f;
|
|
task = s_audio_task;
|
|
taskEXIT_CRITICAL(&s_state_lock);
|
|
|
|
notify_audio_task(task);
|
|
}
|
|
|
|
bool audio_service_has_target(void)
|
|
{
|
|
bool has_target;
|
|
|
|
taskENTER_CRITICAL(&s_state_lock);
|
|
has_target = s_has_target;
|
|
taskEXIT_CRITICAL(&s_state_lock);
|
|
|
|
return has_target;
|
|
}
|
|
|
|
bool audio_service_get_nearest_distance(float *distance_m)
|
|
{
|
|
bool has_target;
|
|
float distance;
|
|
|
|
taskENTER_CRITICAL(&s_state_lock);
|
|
has_target = s_has_target;
|
|
distance = s_nearest_distance_m;
|
|
taskEXIT_CRITICAL(&s_state_lock);
|
|
|
|
if (has_target && distance_m != NULL) {
|
|
*distance_m = distance;
|
|
}
|
|
return has_target;
|
|
}
|