Files
lilygo-t-deck/examples/NoiseSensor/sound_detector.cpp
T

478 lines
17 KiB
C++

/**
* @file sound_detector.cpp
* @brief ES7210 dual-microphone capture + stereo balance/loudness detector.
*
* The T-Deck Plus carries an ES7210 ADC with two populated MEMS microphones.
* The codec emits a fixed 4-slot TDM frame ([MIC1, MIC2, MIC3, MIC4]); only two
* of those slots are wired to a real microphone. Which two depends on the board
* revision, so the detector starts from the documented MIC1/MIC3 pair (slots 0
* and 2) and re-learns the pair from the slots that actually respond to sound.
*
* The ESP32 DMA may pack each 16-bit TDM slot as a tight 16-bit word (4 int16
* per frame) or pad it into one half of a 32-bit word (8 int16 per frame).
* Reading a padded stream with the packed stride mixes the microphones
* together, so the detector probes the stream at start-up and adapts.
*
* 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_30DB
// 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.0f;
static constexpr float ABS_GATE = 0.0008f;
static constexpr float BALANCE_DEADBAND = 0.005f;
static constexpr float BALANCE_GAIN = 4.0f; // widen the usable stereo range
static constexpr float LOUD_MIN_DB = -55.0f;
static constexpr float LOUD_MAX_DB = -6.0f;
static constexpr uint32_t HOLD_MS = 260;
// Populated-slot discovery. A fitted microphone swings far above its own noise
// floor; a floating or railed input stays flat (the high-pass removes DC).
static constexpr float SLOT_MIN_SWING = 0.010f; // 1% of full scale
static constexpr float SLOT_SECOND_RATIO = 0.15f;
static constexpr float SLOT_THIRD_RATIO = 0.30f;
static constexpr float SLOT_DEAD_RATIO = 0.50f;
static constexpr int SLOT_SWITCH_BLOCKS = 4;
static constexpr float SLOT_MAX_DECAY = 0.997f; // peak hold, ~3.7 s half-life
static constexpr float SLOT_MIN_RISE = 0.002f; // slow, so gaps keep the floor
// A channel parked at the rails (overdriven analogue gain) carries no usable
// audio; ignore it rather than reporting a permanent maximum.
static constexpr float RAIL_LEVEL = 0.95f;
// The codec always delivers four TDM slots, but only two are fitted. The
// power-up transient is very loud and can be mistaken for signal.
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. All four slots are emitted even though
// only two are populated, so the frame boundary never shifts.
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 sampleCount = bytesRead / sizeof(int16_t);
if (sampleCount == 0) continue;
if (!layoutReady_) {
probeLayout(block, sampleCount);
}
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 we report.
for (int s = 0; s < MIC_SLOTS; s++) { env_[s] = 0.0f; floor_[s] = 0.01f; }
float z[4] = {0,0,0,0};
publish(0.0f, 0.0f, z, 0.0f, 0.0f, false);
continue;
}
if (!layoutReady_) {
finaliseLayout();
}
processBlock(block, sampleCount);
}
}
size_t SoundDetector::sampleOffset(int slot) const
{
if (frameWords_ == 8) {
return (size_t)slot * 2 + sampleHalf_;
}
return (size_t)slot;
}
void SoundDetector::probeLayout(const int16_t *samples, size_t sampleCount)
{
if (probeStartMs_ == 0) probeStartMs_ = millis();
probeSamples_ += sampleCount;
// Energy per position inside an 8-word (two-frame) window. Packed frames
// light up the low four positions; padded frames light up one half only.
for (size_t i = 0; i < sampleCount; i++) {
uint32_t pos = probeIndex_ & 7;
double v = samples[i];
posSq_[pos] += v * v;
posCount_[pos]++;
probeIndex_++;
}
}
void SoundDetector::finaliseLayout()
{
uint32_t elapsed = millis() - probeStartMs_;
double bytesPerSec = elapsed ? (double)probeSamples_ * 2.0 * 1000.0 / elapsed : 0.0;
double packedRate = (double)sampleRate_ * MIC_SLOTS * 2.0;
double paddedRate = packedRate * 2.0;
// The sample rate is fixed, so the byte rate directly reveals whether each
// 16-bit slot occupies one word or half of a 32-bit word.
frameWords_ = (bytesPerSec > (packedRate + paddedRate) * 0.5) ? 8 : 4;
sampleHalf_ = 0;
if (frameWords_ == 8) {
double evenEnergy = posSq_[0] + posSq_[2] + posSq_[4] + posSq_[6];
double oddEnergy = posSq_[1] + posSq_[3] + posSq_[5] + posSq_[7];
sampleHalf_ = (oddEnergy >= evenEnergy) ? 1 : 0;
}
layoutReady_ = true;
Serial.printf("Sound detector DMA layout: %s (%s half), "
"%.0f bytes/s (packed %.0f, padded %.0f)\n",
frameWords_ == 8 ? "32-bit words" : "packed 16-bit",
sampleHalf_ ? "high" : "low",
bytesPerSec, packedRate, paddedRate);
Serial.print("Sound detector slot energy:");
for (int i = 0; i < 8; i++) {
double rms = posCount_[i] ? sqrt(posSq_[i] / posCount_[i]) : 0.0;
Serial.printf(" %d:%.0f", i, rms);
}
Serial.println();
}
void SoundDetector::updateSlots(const float *rms)
{
float swing[MIC_SLOTS];
for (int s = 0; s < MIC_SLOTS; s++) {
// Track a decaying maximum and a slowly rising minimum. A real
// microphone has quiet gaps between sounds, so its minimum stays low
// while its maximum follows the peaks. A floating or railed slot has
// both pinned at the same level and therefore no range.
if (rms[s] > slotMax_[s]) {
slotMax_[s] = rms[s];
} else {
slotMax_[s] *= SLOT_MAX_DECAY;
}
if (rms[s] < slotMin_[s]) {
slotMin_[s] = rms[s];
} else {
slotMin_[s] += (rms[s] - slotMin_[s]) * SLOT_MIN_RISE;
}
swing[s] = slotMax_[s] - slotMin_[s];
if (swing[s] < 0.0f) swing[s] = 0.0f;
}
// Rank the slots by how much they respond to sound.
int order[MIC_SLOTS] = { 0, 1, 2, 3 };
for (int i = 0; i < MIC_SLOTS; i++) {
for (int j = i + 1; j < MIC_SLOTS; j++) {
if (swing[order[j]] > swing[order[i]]) {
int t = order[i]; order[i] = order[j]; order[j] = t;
}
}
}
float s0 = swing[order[0]];
float s1 = swing[order[1]];
float s2 = swing[order[2]];
bool valid = s0 > SLOT_MIN_SWING &&
s1 > s0 * SLOT_SECOND_RATIO &&
s2 < s1 * SLOT_THIRD_RATIO;
if (valid) {
int lo = order[0] < order[1] ? order[0] : order[1];
int hi = order[0] < order[1] ? order[1] : order[0];
if (lo != leftSlot_ || hi != rightSlot_) {
if (lo == candL_ && hi == candR_) {
candCount_++;
} else {
candL_ = lo;
candR_ = hi;
candCount_ = 1;
}
float currentSwing = fmaxf(swing[leftSlot_], swing[rightSlot_]);
if (currentSwing < s0 * SLOT_DEAD_RATIO ||
candCount_ >= SLOT_SWITCH_BLOCKS) {
leftSlot_ = lo;
rightSlot_ = hi;
candCount_ = 0;
for (int s = 0; s < MIC_SLOTS; s++) { env_[s] = 0.0f; floor_[s] = 0.01f; }
Serial.printf("Sound detector mic slots L=%d R=%d "
"(swing %.4f %.4f %.4f %.4f)\n",
leftSlot_, rightSlot_,
swing[0], swing[1], swing[2], swing[3]);
}
} else {
candCount_ = 0;
}
} else {
candCount_ = 0;
}
}
void SoundDetector::processBlock(const int16_t *samples, size_t sampleCount)
{
size_t frames = sampleCount / frameWords_;
if (frames == 0) return;
// RMS of every TDM slot, using the word packing discovered at start-up.
float rms[MIC_SLOTS] = {};
for (int s = 0; s < MIC_SLOTS; s++) {
size_t off = sampleOffset(s);
int64_t sum = 0, sq = 0;
for (size_t i = 0; i < frames; i++) {
int32_t v = samples[i * frameWords_ + off];
sum += v;
sq += (int64_t)v * v;
}
float mean = (float)sum / (float)frames;
float var = (float)sq / (float)frames - mean * mean;
if (var < 0.0f) var = 0.0f;
rms[s] = sqrtf(var) / 32768.0f;
}
updateSlots(rms);
#if SOUND_DETECTOR_DEBUG
if (!rawDumped_) {
rawDumped_ = true;
Serial.print("Sound detector raw:");
for (int i = 0; i < 24 && i < (int)sampleCount; i++) {
Serial.printf(" %04x", (uint16_t)samples[i]);
}
Serial.println();
}
#endif
// Monitor all four TDM slots independently. A railed slot is zeroed.
float sig[MIC_SLOTS];
float peak = 0.0f;
bool present = false;
for (int s = 0; s < MIC_SLOTS; s++) {
float r = rms[s];
if (r > RAIL_LEVEL) r = 0.0f;
env_[s] += (r - env_[s]) * (r > env_[s] ? ENV_ATTACK : ENV_RELEASE);
if (env_[s] < floor_[s]) {
floor_[s] += (env_[s] - floor_[s]) * FLOOR_FALL;
} else if (env_[s] < floor_[s] * 1.5f) {
floor_[s] += (env_[s] - floor_[s]) * FLOOR_RISE;
} else {
floor_[s] += (env_[s] - floor_[s]) * 0.05f;
}
if (floor_[s] < 1e-5f) floor_[s] = 1e-5f;
float v = env_[s] - floor_[s] * FLOOR_MARGIN;
if (v < 0.0f) v = 0.0f;
sig[s] = v;
if (v > peak) peak = v;
if (v > ABS_GATE) present = true;
}
uint32_t now = millis();
if (present) lastActiveMs_ = now;
bool active = present || (uint32_t)(now - lastActiveMs_) < HOLD_MS;
// Keep a left/right summary for compatibility (max of the two left-ish /
// right-ish slots).
float sigL = fmaxf(sig[0], sig[2]);
float sigR = fmaxf(sig[1], sig[3]);
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 {
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 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) > 300) {
lastPrintMs = millis();
Serial.printf("mic rms %.4f %.4f %.4f %.4f | sig %.3f %.3f %.3f %.3f%s\n",
rms[0], rms[1], rms[2], rms[3],
sig[0], sig[1], sig[2], sig[3],
active ? "" : " (quiet)");
}
#endif
publish(sigL, sigR, sig, balance, loudness, active);
}
void SoundDetector::publish(float left, float right, const float *slots, float balance,
float loudness, bool active)
{
portENTER_CRITICAL(&mux_);
reading_.left = left;
reading_.right = right;
if (slots) {
for (int s = 0; s < 4; s++) reading_.slots[s] = slots[s];
}
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;
}