The ES7210 power-up transient is loud enough to look like the first sound. It placed one centred blip right after init and then pinned the mic pair to whichever slots were loudest at that instant. If that was the wrong pair, later claps were ignored forever. Ignore the first 800 ms after starting the codec, and re-evaluate the mic pair on every clear sound instead of locking once. The pair only changes after repeating for a few blocks, or immediately when the current pair is silent while another one is loud. Also enable the per-second rms/pair debug line so the slot mapping can be verified on hardware.
365 lines
14 KiB
C++
365 lines
14 KiB
C++
/**
|
|
* @file sound_detector.cpp
|
|
* @brief ES7210 dual-microphone capture + stereo balance/loudness detector.
|
|
*
|
|
* The T-Deck Plus has two populated MEMS microphones on the ES7210. The codec
|
|
* outputs all four ADC channels as a 4-slot TDM stream on SDOUT1:
|
|
* slot 0 = MIC1, slot 1 = MIC2, slot 2 = MIC3, slot 3 = MIC4
|
|
* The board only fits two of the four footprints, and the schematic has all
|
|
* four, so the detector finds the live pair at runtime (the first loud sound
|
|
* identifies the two slots with signal) and treats the lower slot as left.
|
|
*
|
|
* The detector runs in its own FreeRTOS task so the radar loop never blocks on
|
|
* I2S. It publishes smoothed, noise-floor-relative levels that the sketch turns
|
|
* into a blip angle (balance) and distance (loudness).
|
|
*/
|
|
#include "sound_detector.h"
|
|
#include "utilities.h"
|
|
#include "es7210.h"
|
|
#include <Wire.h>
|
|
#include <math.h>
|
|
|
|
#define MIC_I2S_PORT I2S_NUM_1
|
|
#define MIC_BLOCK_FRAMES 256 // 16 ms at 16 kHz
|
|
#define MIC_SLOTS 4 // 4-slot TDM
|
|
#define MIC_GAIN GAIN_37_5DB
|
|
|
|
// Set to 1 to print detected levels while tuning the microphones on hardware.
|
|
#ifndef SOUND_DETECTOR_DEBUG
|
|
#define SOUND_DETECTOR_DEBUG 1
|
|
#endif
|
|
|
|
// Set to 1 if a sound on the left moves the blip to the right (mirrored pair).
|
|
#ifndef MIC_SWAP_LR
|
|
#define MIC_SWAP_LR 0
|
|
#endif
|
|
|
|
// Envelope / detector tuning (per 16 ms block).
|
|
static constexpr float ENV_ATTACK = 0.55f;
|
|
static constexpr float ENV_RELEASE = 0.18f;
|
|
static constexpr float FLOOR_FALL = 0.12f; // fast follow when quieter
|
|
static constexpr float FLOOR_RISE = 0.0008f; // very slow drift upward
|
|
static constexpr float FLOOR_MARGIN = 1.25f;
|
|
static constexpr float ABS_GATE = 0.0008f;
|
|
static constexpr float BALANCE_DEADBAND = 0.02f;
|
|
static constexpr float BALANCE_GAIN = 1.8f; // widen the usable stereo range
|
|
static constexpr float LOUD_MIN_DB = -60.0f;
|
|
static constexpr float LOUD_MAX_DB = -20.0f;
|
|
static constexpr uint32_t HOLD_MS = 260;
|
|
|
|
// Slot discovery: the codec emits four TDM slots but only two microphones are
|
|
// fitted. Whenever a clear sound appears, take the two loudest slots as the mic
|
|
// pair. The current pair is only replaced after the new pair repeats, or
|
|
// immediately if the current pair is silent while the new one is loud.
|
|
static constexpr float SLOT_SELECT_LEVEL = 0.0020f;
|
|
static constexpr float SLOT_PAIR_RATIO = 0.10f;
|
|
static constexpr float SLOT_DEAD_RATIO = 0.25f;
|
|
static constexpr int SLOT_SWITCH_BLOCKS = 3;
|
|
static constexpr uint32_t WARMUP_MS = 800;
|
|
|
|
// ES7210 SDP interface 2 / high-pass registers. The LilyGO driver leaves the
|
|
// codec in 2-channel I2S mode and the HPF disabled; enable both here.
|
|
static const uint8_t ES7210_SDP_INTERFACE2 = 0x12;
|
|
static const uint8_t ES7210_ADC34_HPF2 = 0x20;
|
|
static const uint8_t ES7210_ADC34_HPF1 = 0x21;
|
|
static const uint8_t ES7210_ADC12_HPF1 = 0x22;
|
|
static const uint8_t ES7210_ADC12_HPF2 = 0x23;
|
|
|
|
static bool es7210WriteReg(uint8_t reg, uint8_t value)
|
|
{
|
|
Wire.beginTransmission(ES7210_ADDR);
|
|
Wire.write(reg);
|
|
Wire.write(value);
|
|
return Wire.endTransmission() == 0;
|
|
}
|
|
|
|
bool SoundDetector::begin(uint32_t sampleRate)
|
|
{
|
|
if (running_) return true;
|
|
sampleRate_ = sampleRate;
|
|
|
|
audio_hal_codec_config_t cfg = {};
|
|
cfg.adc_input = AUDIO_HAL_ADC_INPUT_ALL;
|
|
cfg.codec_mode = AUDIO_HAL_CODEC_MODE_ENCODE;
|
|
cfg.i2s_iface.mode = AUDIO_HAL_MODE_SLAVE;
|
|
cfg.i2s_iface.fmt = AUDIO_HAL_I2S_NORMAL;
|
|
cfg.i2s_iface.samples = AUDIO_HAL_16K_SAMPLES;
|
|
cfg.i2s_iface.bits = AUDIO_HAL_BIT_LENGTH_16BITS;
|
|
|
|
esp_err_t err = es7210_adc_init(&Wire, &cfg);
|
|
err |= es7210_adc_config_i2s(cfg.codec_mode, &cfg.i2s_iface);
|
|
|
|
// Put all four ADCs on SDOUT1 as TDM slots. Without 0x02 on this register
|
|
// the codec stays in 2-channel mode and MIC3/MIC4 are only sent on SDOUT2,
|
|
// which is not wired to the ESP32.
|
|
if (!es7210WriteReg(ES7210_SDP_INTERFACE2, 0x02)) {
|
|
Serial.println("ES7210 TDM config failed");
|
|
return false;
|
|
}
|
|
|
|
// High-pass the ADC outputs so the detector sees audio, not DC offset.
|
|
es7210WriteReg(ES7210_ADC12_HPF2, 0x2a);
|
|
es7210WriteReg(ES7210_ADC12_HPF1, 0x0a);
|
|
es7210WriteReg(ES7210_ADC34_HPF2, 0x0a);
|
|
es7210WriteReg(ES7210_ADC34_HPF1, 0x2a);
|
|
|
|
// Use the same analog gain on all four inputs. Only two microphones are
|
|
// fitted, but the populated pair is discovered at runtime, and equal gain
|
|
// is required for a meaningful left/right amplitude comparison.
|
|
err |= es7210_adc_set_gain(ES7210_INPUT_MIC1, MIC_GAIN);
|
|
err |= es7210_adc_set_gain(ES7210_INPUT_MIC2, MIC_GAIN);
|
|
err |= es7210_adc_set_gain(ES7210_INPUT_MIC3, MIC_GAIN);
|
|
err |= es7210_adc_set_gain(ES7210_INPUT_MIC4, MIC_GAIN);
|
|
err |= es7210_adc_ctrl_state(cfg.codec_mode, AUDIO_HAL_CTRL_START);
|
|
if (err != ESP_OK) {
|
|
Serial.println("ES7210 init failed");
|
|
return false;
|
|
}
|
|
|
|
i2s_config_t i2s = {};
|
|
i2s.mode = (i2s_mode_t)(I2S_MODE_MASTER | I2S_MODE_RX);
|
|
i2s.sample_rate = sampleRate_;
|
|
i2s.bits_per_sample = I2S_BITS_PER_SAMPLE_16BIT;
|
|
// MULTIPLE is the TDM mode: ALL_LEFT/ALL_RIGHT copy a single channel into
|
|
// every slot, which made both microphones read the same signal.
|
|
i2s.channel_format = I2S_CHANNEL_FMT_MULTIPLE;
|
|
i2s.communication_format = I2S_COMM_FORMAT_STAND_I2S;
|
|
i2s.intr_alloc_flags = ESP_INTR_FLAG_LEVEL1;
|
|
i2s.dma_buf_count = 8;
|
|
i2s.dma_buf_len = 64;
|
|
i2s.use_apll = false;
|
|
i2s.tx_desc_auto_clear = true;
|
|
i2s.fixed_mclk = 0;
|
|
i2s.mclk_multiple = I2S_MCLK_MULTIPLE_256;
|
|
i2s.bits_per_chan = I2S_BITS_PER_CHAN_16BIT;
|
|
i2s.chan_mask = (i2s_channel_t)(I2S_TDM_ACTIVE_CH0 | I2S_TDM_ACTIVE_CH1 |
|
|
I2S_TDM_ACTIVE_CH2 | I2S_TDM_ACTIVE_CH3);
|
|
i2s.total_chan = MIC_SLOTS;
|
|
|
|
if (i2s_driver_install(MIC_I2S_PORT, &i2s, 0, nullptr) != ESP_OK) {
|
|
Serial.println("Mic I2S install failed");
|
|
return false;
|
|
}
|
|
|
|
i2s_pin_config_t pins = {};
|
|
pins.mck_io_num = BOARD_ES7210_MCLK;
|
|
pins.bck_io_num = BOARD_ES7210_SCK;
|
|
pins.ws_io_num = BOARD_ES7210_LRCK;
|
|
pins.data_in_num = BOARD_ES7210_DIN;
|
|
pins.data_out_num = I2S_PIN_NO_CHANGE;
|
|
i2s_set_pin(MIC_I2S_PORT, &pins);
|
|
i2s_zero_dma_buffer(MIC_I2S_PORT);
|
|
|
|
// The ADC power-up transient is very loud; ignore it so it neither places a
|
|
// blip nor influences which slots are treated as microphones.
|
|
warmupUntil_ = millis() + WARMUP_MS;
|
|
|
|
running_ = true;
|
|
if (xTaskCreatePinnedToCore(taskEntry, "sound-detector", 8192, this, 4,
|
|
&task_, 0) != pdPASS) {
|
|
running_ = false;
|
|
i2s_driver_uninstall(MIC_I2S_PORT);
|
|
Serial.println("Mic task create failed");
|
|
return false;
|
|
}
|
|
return true;
|
|
}
|
|
|
|
void SoundDetector::taskEntry(void *arg)
|
|
{
|
|
static_cast<SoundDetector *>(arg)->taskLoop();
|
|
}
|
|
|
|
void SoundDetector::suppress(uint32_t ms)
|
|
{
|
|
suppressUntil_ = millis() + ms;
|
|
}
|
|
|
|
void SoundDetector::taskLoop()
|
|
{
|
|
static int16_t block[MIC_BLOCK_FRAMES * MIC_SLOTS];
|
|
|
|
for (;;) {
|
|
size_t bytesRead = 0;
|
|
esp_err_t err = i2s_read(MIC_I2S_PORT, block, sizeof(block), &bytesRead,
|
|
portMAX_DELAY);
|
|
if (err != ESP_OK || bytesRead == 0) {
|
|
vTaskDelay(pdMS_TO_TICKS(10));
|
|
continue;
|
|
}
|
|
|
|
size_t frames = bytesRead / (MIC_SLOTS * sizeof(int16_t));
|
|
if (frames == 0) continue;
|
|
|
|
if ((int32_t)(millis() - warmupUntil_) < 0 ||
|
|
(int32_t)(millis() - suppressUntil_) < 0) {
|
|
// Still drain I2S so the DMA doesn't overflow while muted. Drop the
|
|
// envelopes too, otherwise the device's own ping or the ADC power-up
|
|
// transient would be the first thing the detector reports.
|
|
envL_ = 0.0f;
|
|
envR_ = 0.0f;
|
|
publish(0.0f, 0.0f, 0.0f, 0.0f, false);
|
|
continue;
|
|
}
|
|
|
|
processBlock(block, frames);
|
|
}
|
|
}
|
|
|
|
void SoundDetector::processBlock(const int16_t *frames, size_t frameCount)
|
|
{
|
|
// RMS of every TDM slot. Two of the four slots are silent because the
|
|
// board only fits two microphones.
|
|
float rms[MIC_SLOTS] = {};
|
|
for (int s = 0; s < MIC_SLOTS; s++) {
|
|
int64_t sum = 0, sq = 0;
|
|
for (size_t i = 0; i < frameCount; i++) {
|
|
int32_t v = frames[i * MIC_SLOTS + s];
|
|
sum += v;
|
|
sq += (int64_t)v * v;
|
|
}
|
|
float mean = (float)sum / (float)frameCount;
|
|
float var = (float)sq / (float)frameCount - mean * mean;
|
|
if (var < 0.0f) var = 0.0f;
|
|
rms[s] = sqrtf(var) / 32768.0f;
|
|
}
|
|
|
|
// Find the two loudest slots. With only two microphones fitted, a real
|
|
// sound shows up in exactly those slots; the other two stay near the noise
|
|
// floor. Re-evaluate on every clear sound so a wrong initial guess (for
|
|
// example caused by the codec's power-up transient) cannot stick forever.
|
|
int best = 0, second = 1;
|
|
if (rms[second] > rms[best]) {
|
|
int t = best; best = second; second = t;
|
|
}
|
|
for (int s = 2; s < MIC_SLOTS; s++) {
|
|
if (rms[s] > rms[best]) {
|
|
second = best;
|
|
best = s;
|
|
} else if (rms[s] > rms[second]) {
|
|
second = s;
|
|
}
|
|
}
|
|
|
|
if (rms[best] > SLOT_SELECT_LEVEL &&
|
|
rms[second] > rms[best] * SLOT_PAIR_RATIO) {
|
|
int lo = best < second ? best : second;
|
|
int hi = best < second ? second : best;
|
|
if (lo != leftSlot_ || hi != rightSlot_) {
|
|
float currentLevel = fmaxf(rms[leftSlot_], rms[rightSlot_]);
|
|
if (lo == candL_ && hi == candR_) {
|
|
candCount_++;
|
|
} else {
|
|
candL_ = lo;
|
|
candR_ = hi;
|
|
candCount_ = 1;
|
|
}
|
|
// Switch immediately when the current pair is effectively dead,
|
|
// otherwise require the new pair to repeat for a few blocks.
|
|
if (currentLevel < rms[best] * SLOT_DEAD_RATIO ||
|
|
candCount_ >= SLOT_SWITCH_BLOCKS) {
|
|
leftSlot_ = lo;
|
|
rightSlot_ = hi;
|
|
candCount_ = 0;
|
|
envL_ = 0.0f;
|
|
envR_ = 0.0f;
|
|
floorL_ = 0.01f;
|
|
floorR_ = 0.01f;
|
|
Serial.printf("Sound detector using mic slots L=%d R=%d "
|
|
"(rms %.4f %.4f %.4f %.4f)\n",
|
|
leftSlot_, rightSlot_,
|
|
rms[0], rms[1], rms[2], rms[3]);
|
|
}
|
|
} else {
|
|
candCount_ = 0;
|
|
}
|
|
}
|
|
|
|
float rmsL = rms[MIC_SWAP_LR ? rightSlot_ : leftSlot_];
|
|
float rmsR = rms[MIC_SWAP_LR ? leftSlot_ : rightSlot_];
|
|
|
|
// Normalise to full scale, then smooth with a fast attack / slow release.
|
|
envL_ += (rmsL - envL_) * (rmsL > envL_ ? ENV_ATTACK : ENV_RELEASE);
|
|
envR_ += (rmsR - envR_) * (rmsR > envR_ ? ENV_ATTACK : ENV_RELEASE);
|
|
|
|
// Per-channel noise floor: fall quickly when it gets quieter, creep up very
|
|
// slowly so sustained noise does not desensitise the detector.
|
|
floorL_ += (envL_ - floorL_) * (envL_ < floorL_ ? FLOOR_FALL : FLOOR_RISE);
|
|
floorR_ += (envR_ - floorR_) * (envR_ < floorR_ ? FLOOR_FALL : FLOOR_RISE);
|
|
if (floorL_ < 1e-5f) floorL_ = 1e-5f;
|
|
if (floorR_ < 1e-5f) floorR_ = 1e-5f;
|
|
|
|
float sigL = envL_ - floorL_ * FLOOR_MARGIN;
|
|
float sigR = envR_ - floorR_ * FLOOR_MARGIN;
|
|
if (sigL < 0.0f) sigL = 0.0f;
|
|
if (sigR < 0.0f) sigR = 0.0f;
|
|
|
|
// Adaptive gate: a sound must stand clearly above its channel's floor.
|
|
float gateL = fmaxf(floorL_ * 2.0f, ABS_GATE);
|
|
float gateR = fmaxf(floorR_ * 2.0f, ABS_GATE);
|
|
bool present = (sigL > gateL) || (sigR > gateR);
|
|
|
|
uint32_t now = millis();
|
|
if (present) lastActiveMs_ = now;
|
|
bool active = present || (uint32_t)(now - lastActiveMs_) < HOLD_MS;
|
|
|
|
float total = sigL + sigR;
|
|
float balance = 0.0f;
|
|
if (total > 1e-6f) {
|
|
balance = (sigR - sigL) / total;
|
|
if (fabsf(balance) < BALANCE_DEADBAND) {
|
|
balance = 0.0f;
|
|
} else {
|
|
// Remove the deadband, rescale the remaining range to +/-1 and then
|
|
// widen it: the two microphones are close together, so a sound at
|
|
// the side only produces a few dB of level difference.
|
|
float sign = balance < 0.0f ? -1.0f : 1.0f;
|
|
balance = (balance - sign * BALANCE_DEADBAND) / (1.0f - BALANCE_DEADBAND);
|
|
balance *= BALANCE_GAIN;
|
|
if (balance < -1.0f) balance = -1.0f;
|
|
if (balance > 1.0f) balance = 1.0f;
|
|
}
|
|
}
|
|
|
|
float peak = fmaxf(sigL, sigR);
|
|
float db = 20.0f * log10f(peak + 1e-9f);
|
|
float loudness = (db - LOUD_MIN_DB) / (LOUD_MAX_DB - LOUD_MIN_DB);
|
|
if (loudness < 0.0f) loudness = 0.0f;
|
|
if (loudness > 1.0f) loudness = 1.0f;
|
|
|
|
#if SOUND_DETECTOR_DEBUG
|
|
static uint32_t lastPrintMs = 0;
|
|
if ((uint32_t)(millis() - lastPrintMs) > 1000) {
|
|
lastPrintMs = millis();
|
|
Serial.printf("mic rms %.4f %.4f %.4f %.4f | pair L%d R%d | "
|
|
"bal %+.2f loud %.2f%s\n",
|
|
rms[0], rms[1], rms[2], rms[3],
|
|
leftSlot_, rightSlot_, balance, loudness,
|
|
active ? "" : " (quiet)");
|
|
}
|
|
#endif
|
|
|
|
publish(sigL, sigR, balance, loudness, active);
|
|
}
|
|
|
|
void SoundDetector::publish(float left, float right, float balance, float loudness,
|
|
bool active)
|
|
{
|
|
portENTER_CRITICAL(&mux_);
|
|
reading_.left = left;
|
|
reading_.right = right;
|
|
reading_.balance = balance;
|
|
reading_.loudness = loudness;
|
|
reading_.active = active;
|
|
portEXIT_CRITICAL(&mux_);
|
|
}
|
|
|
|
SoundReading SoundDetector::read()
|
|
{
|
|
SoundReading r;
|
|
portENTER_CRITICAL(&mux_);
|
|
r = reading_;
|
|
portEXIT_CRITICAL(&mux_);
|
|
return r;
|
|
}
|