Files
lilygo-t-deck/examples/NoiseSensor/sound_detector.cpp
T
Dejvino b8de153e5a Fix microphone stereo detection on the T-Deck Plus
I2S_CHANNEL_FMT_ALL_LEFT copies a single channel into every slot, so
both microphones read the same signal and the balance stayed at zero.
Use I2S_CHANNEL_FMT_MULTIPLE (the real TDM mode) instead.

The board only fits two of the four ES7210 mic footprints, but the
schematic carries all four. Rather than hardcode MIC1/MIC3, the detector
now measures the TDM slots and locks onto the two that carry signal,
treating the lower slot as the left microphone. Equal maximum gain is
applied to all inputs so the pair amplifies consistently.

Also widen the balance range (claps produce only a few dB of difference
between the closely spaced mics) and recalibrate loudness to the dBFS
range seen with 37.5 dB analog gain.
2026-11-12 14:04:31 +01:00

333 lines
12 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 0
#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: keep a peak-hold score per TDM slot and lock the two loudest
// slots once a sound clearly stands above the noise on both of them.
static constexpr float SLOT_DECAY = 0.995f;
static constexpr float SLOT_LOCK_LEVEL = 0.002f;
static constexpr float SLOT_PAIR_RATIO = 0.20f;
// 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);
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() - suppressUntil_) < 0) {
// Still drain I2S so the DMA doesn't overflow while muted. Drop the
// envelopes too, otherwise the device's own ping would be the first
// thing the detector reports once the mute expires.
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;
}
// Learn which two slots carry the populated microphones: the loudest slots
// on a clear sound are the microphones, the others stay at noise level.
for (int s = 0; s < MIC_SLOTS; s++) {
slotScore_[s] = fmaxf(slotScore_[s] * SLOT_DECAY, rms[s]);
}
if (!pairLocked_) {
int best = 0, second = 1;
if (slotScore_[second] > slotScore_[best]) {
int t = best; best = second; second = t;
}
for (int s = 2; s < MIC_SLOTS; s++) {
if (slotScore_[s] > slotScore_[best]) {
second = best;
best = s;
} else if (slotScore_[s] > slotScore_[second]) {
second = s;
}
}
if (slotScore_[best] > SLOT_LOCK_LEVEL &&
slotScore_[second] > slotScore_[best] * SLOT_PAIR_RATIO) {
leftSlot_ = best < second ? best : second;
rightSlot_ = best < second ? second : best;
pairLocked_ = true;
Serial.printf("Sound detector locked mic slots L=%d R=%d\n",
leftSlot_, rightSlot_);
}
}
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 (active && (uint32_t)(millis() - lastPrintMs) > 250) {
lastPrintMs = millis();
Serial.printf("mic slots %.4f/%.4f/%.4f/%.4f L%d=%.4f R%d=%.4f "
"bal=%+.2f loud=%.2f\n",
rms[0], rms[1], rms[2], rms[3],
leftSlot_, sigL, rightSlot_, sigR,
balance, loudness);
}
#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;
}