INMP441_6bandFFT_TEST.ino

#include <driver/i2s.h>
#include <arduinoFFT.h>
#include <math.h>

// =====================================================
// INMP441 6-Band FFT Meter  (A-2b)
// ESP32 DEVKIT V1 / Fs = 16kHz / N = 1024
//
// - DC offset removal
// - Hann window
// - 1024pt FFT
// - 6-band power integration
// - relative dB display
// =====================================================

// ---------- I2S pin assignment ----------
static const int I2S_BCLK_PIN = 14;   // INMP441 SCK / BCLK
static const int I2S_WS_PIN   = 15;   // INMP441 WS / LRCLK
static const int I2S_SD_PIN   = 32;   // INMP441 SD

// ---------- audio settings ----------
static const uint16_t FFT_SIZE    = 1024;
static const double   SAMPLE_RATE = 16000.0;
static const double   BIN_WIDTH   = SAMPLE_RATE / FFT_SIZE;   // 15.625 Hz/bin

// ---------- low-frequency handling ----------
static const bool EXCLUDE_BIN_1 = true;          // ignore 15.625 Hz bin
static const bool INCLUDE_BIN2_IN_63HZ = false;  // include 31.25 Hz in 63Hz band?

// ---------- I2S port ----------
static const i2s_port_t I2S_PORT = I2S_NUM_0;

// ---------- raw I2S buffer ----------
int32_t rawSamples[FFT_SIZE];

// ---------- FFT buffers ----------
double vReal[FFT_SIZE];
double vImag[FFT_SIZE];

ArduinoFFT<double> FFT = ArduinoFFT<double>(vReal, vImag, FFT_SIZE, SAMPLE_RATE);

// =====================================================
// Band definition
// =====================================================
struct BandDef {
  const char* name;
  double fLow;
  double fHigh;
};

// 6-band definition
static const BandDef bands[6] = {
  {"63Hz ",   31.25,   93.75},
  {"160Hz",   93.75,  250.00},
  {"380Hz",  250.00,  625.00},
  {"1kHz ",  625.00, 1625.00},
  {"2.5k ", 1625.00, 4000.00},
  {"6.3k ", 4000.00, 8000.00}
};

// =====================================================
// I2S init
// =====================================================
void initI2SMic()
{
  const i2s_config_t i2s_config = {
    .mode = (i2s_mode_t)(I2S_MODE_MASTER | I2S_MODE_RX),
    .sample_rate = (uint32_t)SAMPLE_RATE,
    .bits_per_sample = I2S_BITS_PER_SAMPLE_32BIT,
    .channel_format = I2S_CHANNEL_FMT_ONLY_LEFT,   // INMP441 L/R = GND想定
    .communication_format = I2S_COMM_FORMAT_STAND_I2S,
    .intr_alloc_flags = ESP_INTR_FLAG_LEVEL1,
    .dma_buf_count = 8,
    .dma_buf_len = 256,
    .use_apll = false,
    .tx_desc_auto_clear = false,
    .fixed_mclk = 0
  };

  const i2s_pin_config_t pin_config = {
    .bck_io_num = I2S_BCLK_PIN,
    .ws_io_num = I2S_WS_PIN,
    .data_out_num = I2S_PIN_NO_CHANGE,
    .data_in_num = I2S_SD_PIN
  };

  i2s_driver_uninstall(I2S_PORT);

  esp_err_t err;
  err = i2s_driver_install(I2S_PORT, &i2s_config, 0, NULL);
  if (err != ESP_OK) {
    Serial.printf("ERROR: i2s_driver_install failed (%d)\n", err);
    while (1) delay(1000);
  }

  err = i2s_set_pin(I2S_PORT, &pin_config);
  if (err != ESP_OK) {
    Serial.printf("ERROR: i2s_set_pin failed (%d)\n", err);
    while (1) delay(1000);
  }

  i2s_zero_dma_buffer(I2S_PORT);
  Serial.println("I2S microphone initialized.");
}

// =====================================================
// Read one block
// =====================================================
bool readMicBlock(int32_t* dst, size_t count)
{
  size_t bytesRead = 0;
  esp_err_t err = i2s_read(
    I2S_PORT,
    (void*)dst,
    count * sizeof(int32_t),
    &bytesRead,
    portMAX_DELAY
  );

  if (err != ESP_OK) {
    Serial.printf("ERROR: i2s_read failed (%d)\n", err);
    return false;
  }

  size_t samplesRead = bytesRead / sizeof(int32_t);
  return (samplesRead == count);
}

// =====================================================
// Prepare FFT input
// 1) convert raw sample
// 2) calculate mean
// 3) subtract mean (DC removal)
// 4) apply Hann window
// =====================================================
double prepareFFTInput(const int32_t* src, uint16_t n)
{
  double mean = 0.0;

  // ---- pass 1 : mean ----
  for (uint16_t i = 0; i < n; i++) {
    double s = (double)(src[i] >> 8);   // 24-bit-ish -> manageable scale
    mean += s;
  }
  mean /= (double)n;

  // ---- pass 2 : DC remove + Hann ----
  for (uint16_t i = 0; i < n; i++) {
    double s = (double)(src[i] >> 8);
    s -= mean;

    double w = 0.5 * (1.0 - cos((2.0 * PI * i) / (n - 1)));
    vReal[i] = s * w;
    vImag[i] = 0.0;
  }

  return mean;
}

// =====================================================
// Helpers
// =====================================================
double powerToDb(double p)
{
  const double eps = 1e-20;
  return 10.0 * log10(p + eps);
}

double binToFreq(uint16_t bin)
{
  return (double)bin * BIN_WIDTH;
}

// inclusive lower, inclusive upper
uint16_t freqToBinFloor(double freq)
{
  int b = (int)floor(freq / BIN_WIDTH + 1e-9);
  if (b < 0) b = 0;
  if (b > (FFT_SIZE / 2 - 1)) b = FFT_SIZE / 2 - 1;
  return (uint16_t)b;
}

uint16_t freqToBinCeil(double freq)
{
  int b = (int)ceil(freq / BIN_WIDTH - 1e-9);
  if (b < 0) b = 0;
  if (b > (FFT_SIZE / 2 - 1)) b = FFT_SIZE / 2 - 1;
  return (uint16_t)b;
}

// =====================================================
// Find peak bin in valid range
// (ignore DC and optionally ignore bin1)
// =====================================================
void findPeakBin(uint16_t& peakBin, double& peakMag)
{
  uint16_t startBin = EXCLUDE_BIN_1 ? 2 : 1;

  peakBin = startBin;
  peakMag = vReal[startBin];

  for (uint16_t i = startBin + 1; i < FFT_SIZE / 2; i++) {
    if (vReal[i] > peakMag) {
      peakMag = vReal[i];
      peakBin = i;
    }
  }
}

// =====================================================
// Integrate one band by power sum
//
// vReal[k] already contains magnitude after complexToMagnitude()
// bandPower = sum( magnitude^2 )
//
// Special handling:
// - bin0 is always ignored
// - bin1 can be ignored
// - 63Hz band can optionally exclude bin2 (31.25Hz)
// =====================================================
double integrateBandPower(double fLow, double fHigh, bool is63Band)
{
  uint16_t binStart = freqToBinCeil(fLow);
  uint16_t binEnd   = freqToBinFloor(fHigh);

  if (binEnd < binStart) return 0.0;

  double sumPower = 0.0;

  for (uint16_t bin = binStart; bin <= binEnd; bin++) {

    // ignore DC
    if (bin == 0) continue;

    // optionally ignore bin1
    if (EXCLUDE_BIN_1 && bin == 1) continue;

    // optionally ignore bin2 from 63Hz band
    if (is63Band && !INCLUDE_BIN2_IN_63HZ && bin == 2) continue;

    double mag = vReal[bin];
    sumPower += mag * mag;
  }

  return sumPower;
}

// =====================================================
// Print band info with included bins
// =====================================================
void printBandInfo(const BandDef& b, uint8_t index, double power, double levelDb)
{
  uint16_t binStart = freqToBinCeil(b.fLow);
  uint16_t binEnd   = freqToBinFloor(b.fHigh);

  Serial.printf("%-5s : %8.2f dB   [%.2f - %.2f Hz]  bins ",
                b.name, levelDb, b.fLow, b.fHigh);

  bool first = true;
  for (uint16_t bin = binStart; bin <= binEnd; bin++) {
    if (bin == 0) continue;
    if (EXCLUDE_BIN_1 && bin == 1) continue;
    if (index == 0 && !INCLUDE_BIN2_IN_63HZ && bin == 2) continue;

    if (!first) Serial.print(",");
    Serial.print(bin);
    first = false;
  }
  Serial.println();
}

// =====================================================
// setup
// =====================================================
void setup()
{
  Serial.begin(115200);
  delay(1500);

  Serial.println();
  Serial.println("============================================================");
  Serial.println("INMP441 6-Band FFT Meter  (A-2b)");
  Serial.println("Fs = 16kHz, FFT = 1024, Hann window, DC removal");
  Serial.println("Band levels = power-sum relative dB");
  Serial.println("============================================================");
  Serial.printf("BIN_WIDTH = %.3f Hz\n", BIN_WIDTH);
  Serial.printf("EXCLUDE_BIN_1          = %s\n", EXCLUDE_BIN_1 ? "true" : "false");
  Serial.printf("INCLUDE_BIN2_IN_63HZ   = %s\n", INCLUDE_BIN2_IN_63HZ ? "true" : "false");
  Serial.println("------------------------------------------------------------");

  initI2SMic();
}

// =====================================================
// loop
// =====================================================
void loop()
{
  // 1) read one block
  if (!readMicBlock(rawSamples, FFT_SIZE)) {
    Serial.println("Block read incomplete.");
    delay(500);
    return;
  }

  // 2) prepare FFT input
  double blockMean = prepareFFTInput(rawSamples, FFT_SIZE);

  // 3) FFT
  FFT.compute(FFT_FORWARD);
  FFT.complexToMagnitude();

  // 4) find peak
  uint16_t peakBin;
  double peakMag;
  findPeakBin(peakBin, peakMag);

  double peakFreq = binToFreq(peakBin);
  double peakDb   = 20.0 * log10(peakMag + 1e-12);

  // 5) integrate bands
  double bandPower[6];
  double bandDb[6];

  for (uint8_t i = 0; i < 6; i++) {
    bool is63Band = (i == 0);
    bandPower[i] = integrateBandPower(bands[i].fLow, bands[i].fHigh, is63Band);
    bandDb[i]    = powerToDb(bandPower[i]);
  }

  // 6) print
  Serial.println();
  Serial.println("------------------------------------------------------------");
  Serial.printf("Block mean (before DC removal): %.2f\n", blockMean);
  Serial.printf("Peak bin   : %u\n", peakBin);
  Serial.printf("Peak freq  : %.2f Hz\n", peakFreq);
  Serial.printf("Peak mag   : %.2f\n", peakMag);
  Serial.printf("Peak level : %.2f dB (relative, single-bin)\n", peakDb);
  Serial.println();

  for (uint8_t i = 0; i < 6; i++) {
    printBandInfo(bands[i], i, bandPower[i], bandDb[i]);
  }

  delay(250);
}