478 lines
17 KiB
C++
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;
|
|
}
|