Implement native ESP-IDF radar firmware

main/audio_service.c

@@ -0,0 +1,436 @@
+#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;
+}