diff --git a/README.md b/README.md
index 629cc9b..03781bb 100644
--- a/README.md
+++ b/README.md
@@ -12,6 +12,6 @@ And the microphone response after equalization should look like:
Theoretically, i.e. with factory calibrated ICS-4343x, this should get you ±1dB(A) measurement within 20Hz-20KHz range.
-The code in this repository is mostly intended as example how you can integrate resonable noise measurement (i.e. *L*Aeq, Equivalent Continuous Sound Level) in your projects.
+The code in this repository is mostly intended as example how you can integrate resonable noise measurement (i.e. *L*Aeq, Equivalent Continuous Sound Level) in your projects.
You can find a bit more information in my [hackday.io](https://hackaday.io/project/166867-esp32-i2s-slm) project.
diff --git a/esp32-i2s-slm.ino b/esp32-i2s-slm.ino
index 4cdd25b..24883a6 100644
--- a/esp32-i2s-slm.ino
+++ b/esp32-i2s-slm.ino
@@ -2,6 +2,7 @@
* Display A-weighted sound level measured by I2S Microphone
*
* (c)2019 Ivan Kostoski
+ * (c)2021 Bim Overbohm (split into files, template)
*
* This program is free software: you can redistribute it and/or modify
* it under the terms of the GNU General Public License as published by
@@ -38,34 +39,39 @@
* response and numeric LAeq(1sec) dB value from the signal RMS.
*/
-#include
-#include "sos-iir-filter.h"
+#include "i2s_mic.h"
+#include "filters.h"
//
// Configuration
//
-#define LEQ_PERIOD 1 // second(s)
-#define WEIGHTING C_weighting // Also avaliable: 'C_weighting' or 'None' (Z_weighting)
-#define LEQ_UNITS "LAeq" // customize based on above weighting used
-#define DB_UNITS "dBA" // customize based on above weighting used
-#define USE_DISPLAY 1
+#define LEQ_PERIOD 1 // second(s)
+#define WEIGHTING C_weighting // Also avaliable: 'C_weighting' or 'None' (Z_weighting)
+#define LEQ_UNITS "LAeq" // customize based on above weighting used
+#define DB_UNITS "dBA" // customize based on above weighting used
+#define USE_DISPLAY 1
// NOTE: Some microphones require at least DC-Blocker filter
-#define MIC_EQUALIZER ICS43434 // See below for defined IIR filters or set to 'None' to disable
-#define MIC_OFFSET_DB 3.0103 // Default offset (sine-wave RMS vs. dBFS). Modify this value for linear calibration
+#define MIC_EQUALIZER ICS43434 // See below for defined IIR filters or set to 'None' to disable
+#define MIC_OFFSET_DB 3.0103 // Default offset (sine-wave RMS vs. dBFS). Modify this value for linear calibration
// Customize these values from microphone datasheet
-#define MIC_SENSITIVITY -26 // dBFS value expected at MIC_REF_DB (Sensitivity value from datasheet)
-#define MIC_REF_DB 94.0 // Value at which point sensitivity is specified in datasheet (dB)
-#define MIC_OVERLOAD_DB 116.0 // dB - Acoustic overload point
-#define MIC_NOISE_DB 29 // dB - Noise floor
-#define MIC_BITS 24 // valid number of bits in I2S data
-#define MIC_CONVERT(s) (s >> (SAMPLE_BITS - MIC_BITS))
-#define MIC_TIMING_SHIFT 0 // Set to one to fix MSB timing for some microphones, i.e. SPH0645LM4H-x
+#define MIC_SENSITIVITY -26 // dBFS value expected at MIC_REF_DB (Sensitivity value from datasheet)
+#define MIC_REF_DB 94.0 // Value at which point sensitivity is specified in datasheet (dB)
+#define MIC_OVERLOAD_DB 116.0 // dB - Acoustic overload point
+#define MIC_NOISE_DB 29 // dB - Noise floor
+#define MIC_BITS 24 // valid number of bits in I2S data
+#define MIC_CONVERT(s) (s >> (SAMPLE_BITS - MIC_BITS))
+#define MIC_TIMING_SHIFT false // Set to one to fix MSB timing for some microphones, i.e. SPH0645LM4H-x
// Calculate reference amplitude value at compile time
-constexpr double MIC_REF_AMPL = pow(10, double(MIC_SENSITIVITY)/20) * ((1<<(MIC_BITS-1))-1);
+constexpr double MIC_REF_AMPL = pow(10, double(MIC_SENSITIVITY) / 20) * ((1 << (MIC_BITS - 1)) - 1);
+
+#define SAMPLE_RATE_HZ 48000 // Hz, fixed to design of IIR filters. Determines maximum frequency that can be analysed by the FFT Fmax=sampleF/2.
+#define SAMPLE_COUNT 2048 // ~40ms sample time, must be power-of-two
+// Static buffer for block of samples
+float samples[SAMPLE_COUNT] __attribute__((aligned(4)));
//
// I2S pins - Can be routed to almost any (unused) ESP32 pin.
@@ -74,341 +80,95 @@ constexpr double MIC_REF_AMPL = pow(10, double(MIC_SENSITIVITY)/20) * ((1<<(MIC_
//
// Below ones are just example for my board layout, put here the pins you will use
//
-#define I2S_WS 18
-#define I2S_SCK 23
-#define I2S_SD 19
+#define I2S_WS 18
+#define I2S_SCK 23
+#define I2S_SD 19
// I2S peripheral to use (0 or 1)
-#define I2S_PORT I2S_NUM_0
+#define I2S_PORT I2S_NUM_0
+
+// Set up microphone
+auto mic = Microphone_I2S(MIC_EQUALIZER);
//
// Setup your display library (and geometry) here
-//
+//
#if (USE_DISPLAY > 0)
- // ThingPulse/esp8266-oled-ssd1306, you may need the latest source and PR#198 for 64x48
- #include
- #define OLED_GEOMETRY GEOMETRY_64_48
- //#define OLED_GEOMETRY GEOMETRY_128_32
- //#define OLED_GEOMETRY GEOMETRY_128_64
- #define OLED_FLIP_V 1
- SSD1306Wire display(0x3c, SDA, SCL, OLED_GEOMETRY);
+// ThingPulse/esp8266-oled-ssd1306, you may need the latest source and PR#198 for 64x48
+#include
+#define OLED_GEOMETRY GEOMETRY_64_48
+//#define OLED_GEOMETRY GEOMETRY_128_32
+//#define OLED_GEOMETRY GEOMETRY_128_64
+#define OLED_FLIP_V 1
+SSD1306Wire display(0x3c, SDA, SCL, OLED_GEOMETRY);
#endif
-
-//
-// IIR Filters
-//
-
-// DC-Blocker filter - removes DC component from I2S data
-// See: https://www.dsprelated.com/freebooks/filters/DC_Blocker.html
-// a1 = -0.9992 should heavily attenuate frequencies below 10Hz
-SOS_IIR_Filter DC_BLOCKER = {
- gain: 1.0,
- sos: {{-1.0, 0.0, +0.9992, 0}}
-};
-
-//
-// Equalizer IIR filters to flatten microphone frequency response
-// See respective .m file for filter design. Fs = 48Khz.
-//
-// Filters are represented as Second-Order Sections cascade with assumption
-// that b0 and a0 are equal to 1.0 and 'gain' is applied at the last step
-// B and A coefficients were transformed with GNU Octave:
-// [sos, gain] = tf2sos(B, A)
-// See: https://www.dsprelated.com/freebooks/filters/Series_Second_Order_Sections.html
-// NOTE: SOS matrix 'a1' and 'a2' coefficients are negatives of tf2sos output
-//
-
-// TDK/InvenSense ICS-43434
-// Datasheet: https://www.invensense.com/wp-content/uploads/2016/02/DS-000069-ICS-43434-v1.1.pdf
-// B = [0.477326418836803, -0.486486982406126, -0.336455844522277, 0.234624646917202, 0.111023257388606];
-// A = [1.0, -1.93073383849136326, 0.86519456089576796, 0.06442838283825100, 0.00111249298800616];
-SOS_IIR_Filter ICS43434 = {
- gain: 0.477326418836803,
- sos: { // Second-Order Sections {b1, b2, -a1, -a2}
- {+0.96986791463971267, 0.23515976355743193, -0.06681948004769928, -0.00111521990688128},
- {-1.98905931743624453, 0.98908924206960169, +1.99755331853906037, -0.99755481510122113}
- }
-};
-
-// TDK/InvenSense ICS-43432
-// Datasheet: https://www.invensense.com/wp-content/uploads/2015/02/ICS-43432-data-sheet-v1.3.pdf
-// B = [-0.45733702338341309 1.12228667105574775 -0.77818278904413563, 0.00968926337978037, 0.10345668405223755]
-// A = [1.0, -3.3420781082912949, 4.4033694320978771, -3.0167072679918010, 1.2265536567647031, -0.2962229189311990, 0.0251085747458112]
-SOS_IIR_Filter ICS43432 = {
- gain: -0.457337023383413,
- sos: { // Second-Order Sections {b1, b2, -a1, -a2}
- {-0.544047931916859, -0.248361759321800, +0.403298891662298, -0.207346186351843},
- {-1.909911869441421, +0.910830292683527, +1.790285722826743, -0.804085812369134},
- {+0.000000000000000, +0.000000000000000, +1.148493493802252, -0.150599527756651}
- }
-};
-
-// TDK/InvenSense INMP441
-// Datasheet: https://www.invensense.com/wp-content/uploads/2015/02/INMP441.pdf
-// B ~= [1.00198, -1.99085, 0.98892]
-// A ~= [1.0, -1.99518, 0.99518]
-SOS_IIR_Filter INMP441 = {
- gain: 1.00197834654696,
- sos: { // Second-Order Sections {b1, b2, -a1, -a2}
- {-1.986920458344451, +0.986963226946616, +1.995178510504166, -0.995184322194091}
- }
-};
-
-// Infineon IM69D130 Shield2Go
-// Datasheet: https://www.infineon.com/dgdl/Infineon-IM69D130-DS-v01_00-EN.pdf?fileId=5546d462602a9dc801607a0e46511a2e
-// B ~= [1.001240684967527, -1.996936108836337, 0.995703101823006]
-// A ~= [1.0, -1.997675693595542, 0.997677044195563]
-// With additional DC blocking component
-SOS_IIR_Filter IM69D130 = {
- gain: 1.00124068496753,
- sos: {
- {-1.0, 0.0, +0.9992, 0}, // DC blocker, a1 = -0.9992
- {-1.994461610298131, 0.994469278738208, +1.997675693595542, -0.997677044195563}
- }
-};
-
-// Knowles SPH0645LM4H-B, rev. B
-// https://cdn-shop.adafruit.com/product-files/3421/i2S+Datasheet.PDF
-// B ~= [1.001234, -1.991352, 0.990149]
-// A ~= [1.0, -1.993853, 0.993863]
-// With additional DC blocking component
-SOS_IIR_Filter SPH0645LM4H_B_RB = {
- gain: 1.00123377961525,
- sos: { // Second-Order Sections {b1, b2, -a1, -a2}
- {-1.0, 0.0, +0.9992, 0}, // DC blocker, a1 = -0.9992
- {-1.988897663539382, +0.988928479008099, +1.993853376183491, -0.993862821429572}
- }
-};
-
-//
-// Weighting filters
-//
-
-//
-// A-weighting IIR Filter, Fs = 48KHz
-// (By Dr. Matt L., Source: https://dsp.stackexchange.com/a/36122)
-// B = [0.169994948147430, 0.280415310498794, -1.120574766348363, 0.131562559965936, 0.974153561246036, -0.282740857326553, -0.152810756202003]
-// A = [1.0, -2.12979364760736134, 0.42996125885751674, 1.62132698199721426, -0.96669962900852902, 0.00121015844426781, 0.04400300696788968]
-SOS_IIR_Filter A_weighting = {
- gain: 0.169994948147430,
- sos: { // Second-Order Sections {b1, b2, -a1, -a2}
- {-2.00026996133106, +1.00027056142719, -1.060868438509278, -0.163987445885926},
- {+4.35912384203144, +3.09120265783884, +1.208419926363593, -0.273166998428332},
- {-0.70930303489759, -0.29071868393580, +1.982242159753048, -0.982298594928989}
- }
-};
-
-//
-// C-weighting IIR Filter, Fs = 48KHz
-// Designed by invfreqz curve-fitting, see respective .m file
-// B = [-0.49164716933714026, 0.14844753846498662, 0.74117815661529129, -0.03281878334039314, -0.29709276192593875, -0.06442545322197900, -0.00364152725482682]
-// A = [1.0, -1.0325358998928318, -0.9524000181023488, 0.8936404694728326 0.2256286147169398 -0.1499917107550188, 0.0156718181681081]
-SOS_IIR_Filter C_weighting = {
- gain: -0.491647169337140,
- sos: {
- {+1.4604385758204708, +0.5275070373815286, +1.9946144559930252, -0.9946217070140883},
- {+0.2376222404939509, +0.0140411206016894, -1.3396585608422749, -0.4421457807694559},
- {-2.0000000000000000, +1.0000000000000000, +0.3775800047420818, -0.0356365756680430}
- }
-};
-
-
-//
-// Sampling
-//
-#define SAMPLE_RATE 48000 // Hz, fixed to design of IIR filters
-#define SAMPLE_BITS 32 // bits
-#define SAMPLE_T int32_t
-#define SAMPLES_SHORT (SAMPLE_RATE / 8) // ~125ms
-#define SAMPLES_LEQ (SAMPLE_RATE * LEQ_PERIOD)
-#define DMA_BANK_SIZE (SAMPLES_SHORT / 16)
-#define DMA_BANKS 32
-
-// Data we push to 'samples_queue'
-struct sum_queue_t {
- // Sum of squares of mic samples, after Equalizer filter
- float sum_sqr_SPL;
- // Sum of squares of weighted mic samples
- float sum_sqr_weighted;
- // Debug only, FreeRTOS ticks we spent processing the I2S data
- uint32_t proc_ticks;
-};
-QueueHandle_t samples_queue;
-
-// Static buffer for block of samples
-float samples[SAMPLES_SHORT] __attribute__((aligned(4)));
-
-//
-// I2S Microphone sampling setup
-//
-void mic_i2s_init() {
- // Setup I2S to sample mono channel for SAMPLE_RATE * SAMPLE_BITS
- // NOTE: Recent update to Arduino_esp32 (1.0.2 -> 1.0.3)
- // seems to have swapped ONLY_LEFT and ONLY_RIGHT channels
- const i2s_config_t i2s_config = {
- mode: i2s_mode_t(I2S_MODE_MASTER | I2S_MODE_RX),
- sample_rate: SAMPLE_RATE,
- bits_per_sample: i2s_bits_per_sample_t(SAMPLE_BITS),
- channel_format: I2S_CHANNEL_FMT_ONLY_LEFT,
- communication_format: i2s_comm_format_t(I2S_COMM_FORMAT_I2S | I2S_COMM_FORMAT_I2S_MSB),
- intr_alloc_flags: ESP_INTR_FLAG_LEVEL1,
- dma_buf_count: DMA_BANKS,
- dma_buf_len: DMA_BANK_SIZE,
- use_apll: true,
- tx_desc_auto_clear: false,
- fixed_mclk: 0
- };
- // I2S pin mapping
- const i2s_pin_config_t pin_config = {
- bck_io_num: I2S_SCK,
- ws_io_num: I2S_WS,
- data_out_num: -1, // not used
- data_in_num: I2S_SD
- };
-
- i2s_driver_install(I2S_PORT, &i2s_config, 0, NULL);
-
- #if (MIC_TIMING_SHIFT > 0)
- // Undocumented (?!) manipulation of I2S peripheral registers
- // to fix MSB timing issues with some I2S microphones
- REG_SET_BIT(I2S_TIMING_REG(I2S_PORT), BIT(9));
- REG_SET_BIT(I2S_CONF_REG(I2S_PORT), I2S_RX_MSB_SHIFT);
- #endif
-
- i2s_set_pin(I2S_PORT, &pin_config);
-
- //FIXME: There is a known issue with esp-idf and sampling rates, see:
- // https://github.com/espressif/esp-idf/issues/2634
- // In the meantime, the below line seems to set sampling rate at ~47999.992Hz
- // fifs_req=24576000, sdm0=149, sdm1=212, sdm2=5, odir=2 -> fifs_reached=24575996
- //NOTE: This seems to be fixed in ESP32 Arduino 1.0.4, esp-idf 3.2
- // Should be safe to remove...
- //#include
- //rtc_clk_apll_enable(1, 149, 212, 5, 2);
-}
-
//
-// I2S Reader Task
-//
-// Rationale for separate task reading I2S is that IIR filter
-// processing cam be scheduled to different core on the ESP32
-// while main task can do something else, like update the
-// display in the example
-//
-// As this is intended to run as separate hihg-priority task,
-// we only do the minimum required work with the I2S data
-// until it is 'compressed' into sum of squares
-//
-// FreeRTOS priority and stack size (in 32-bit words)
-#define I2S_TASK_PRI 4
-#define I2S_TASK_STACK 2048
-//
-void mic_i2s_reader_task(void* parameter) {
- mic_i2s_init();
-
- // Discard first block, microphone may have startup time (i.e. INMP441 up to 83ms)
- size_t bytes_read = 0;
- i2s_read(I2S_PORT, &samples, SAMPLES_SHORT * sizeof(int32_t), &bytes_read, portMAX_DELAY);
-
- while (true) {
- // Block and wait for microphone values from I2S
- //
- // Data is moved from DMA buffers to our 'samples' buffer by the driver ISR
- // and when there is requested ammount of data, task is unblocked
- //
- // Note: i2s_read does not care it is writing in float[] buffer, it will write
- // integer values to the given address, as received from the hardware peripheral.
- i2s_read(I2S_PORT, &samples, SAMPLES_SHORT * sizeof(SAMPLE_T), &bytes_read, portMAX_DELAY);
-
- TickType_t start_tick = xTaskGetTickCount();
-
- // Convert (including shifting) integer microphone values to floats,
- // using the same buffer (assumed sample size is same as size of float),
- // to save a bit of memory
- SAMPLE_T* int_samples = (SAMPLE_T*)&samples;
- for(int i=0; i 0)
- display.init();
- #if (OLED_FLIP_V > 0)
- display.flipScreenVertically();
- #endif
- display.setFont(ArialMT_Plain_16);
- #endif
-
- // Create FreeRTOS queue
- samples_queue = xQueueCreate(8, sizeof(sum_queue_t));
-
- // Create the I2S reader FreeRTOS task
- // NOTE: Current version of ESP-IDF will pin the task
- // automatically to the first core it happens to run on
- // (due to using the hardware FPU instructions).
- // For manual control see: xTaskCreatePinnedToCore
- xTaskCreate(mic_i2s_reader_task, "Mic I2S Reader", I2S_TASK_STACK, NULL, I2S_TASK_PRI, NULL);
-
- sum_queue_t q;
+
+#if (USE_DISPLAY > 0)
+ display.init();
+#if (OLED_FLIP_V > 0)
+ display.flipScreenVertically();
+#endif
+ display.setFont(ArialMT_Plain_16);
+#endif
+
+ mic.begin();
+ Serial.println("Starting sampling from mic");
+ mic.startSampling();
+
uint32_t Leq_samples = 0;
double Leq_sum_sqr = 0;
double Leq_dB = 0;
- // Read sum of samaples, calculated by 'i2s_reader_task'
- while (xQueueReceive(samples_queue, &q, portMAX_DELAY)) {
+ // Read equalized samples read from microphone
+ while (xQueueReceive(mic.sampleQueue(), &samples, portMAX_DELAY))
+ {
+
+ // Sum of squares of mic equalized samples
+ float sum_sqr_SPL = MIC_EQUALIZER.sum_of_squares(samples, samples, SAMPLE_COUNT);
+ // Sum of squares of equalized + weighted mic samples
+ float sum_sqr_weighted = WEIGHTING.sum_of_squares(samples, samples, SAMPLE_COUNT);
// Calculate dB values relative to MIC_REF_AMPL and adjust for microphone reference
- double short_RMS = sqrt(double(q.sum_sqr_SPL) / SAMPLES_SHORT);
+ double short_RMS = sqrt(double(sum_sqr_SPL) / SAMPLE_COUNT);
double short_SPL_dB = MIC_OFFSET_DB + MIC_REF_DB + 20 * log10(short_RMS / MIC_REF_AMPL);
// In case of acoustic overload or below noise floor measurement, report infinty Leq value
- if (short_SPL_dB > MIC_OVERLOAD_DB) {
+ if (short_SPL_dB > MIC_OVERLOAD_DB)
+ {
Leq_sum_sqr = INFINITY;
- } else if (isnan(short_SPL_dB) || (short_SPL_dB < MIC_NOISE_DB)) {
+ }
+ else if (isnan(short_SPL_dB) || (short_SPL_dB < MIC_NOISE_DB))
+ {
Leq_sum_sqr = -INFINITY;
}
// Accumulate Leq sum
- Leq_sum_sqr += q.sum_sqr_weighted;
- Leq_samples += SAMPLES_SHORT;
+ Leq_sum_sqr += sum_sqr_weighted;
+ Leq_samples += SAMPLE_COUNT;
// When we gather enough samples, calculate new Leq value
- if (Leq_samples >= SAMPLE_RATE * LEQ_PERIOD) {
+ if (Leq_samples >= SAMPLE_RATE_HZ * LEQ_PERIOD)
+ {
double Leq_RMS = sqrt(Leq_sum_sqr / Leq_samples);
Leq_dB = MIC_OFFSET_DB + MIC_REF_DB + 20 * log10(Leq_RMS / MIC_REF_AMPL);
Leq_sum_sqr = 0;
Leq_samples = 0;
-
+
// Serial output, customize (or remove) as needed
Serial.printf("%.1f\n", Leq_dB);
@@ -416,42 +176,46 @@ void setup() {
//Serial.printf("%u processing ticks\n", q.proc_ticks);
}
- #if (USE_DISPLAY > 0)
-
- //
- // Example code that displays the measured value.
- // You should customize the below code for your display
- // and display library used.
- //
-
- display.clear();
-
- // It is important to somehow notify when the deivce is out of its range
- // as the calculated values are very likely with big error
- if (Leq_dB > MIC_OVERLOAD_DB) {
- // Display 'Overload' if dB value is over the AOP
- display.drawString(0, 24, "Overload");
- } else if (isnan(Leq_dB) || (Leq_dB < MIC_NOISE_DB)) {
- // Display 'Low' if dB value is below noise floor
- display.drawString(0, 24, "Low");
- }
-
- // The 'short' Leq line
- double short_Leq_dB = MIC_OFFSET_DB + MIC_REF_DB + 20 * log10(sqrt(double(q.sum_sqr_weighted) / SAMPLES_SHORT) / MIC_REF_AMPL);
- uint16_t len = min(max(0, int(((short_Leq_dB - MIC_NOISE_DB) / MIC_OVERLOAD_DB) * (display.getWidth()-1))), display.getWidth()-1);
- display.drawHorizontalLine(0, 0, len);
- display.drawHorizontalLine(0, 1, len);
- display.drawHorizontalLine(0, 2, len);
-
- // The Leq numeric decibels
- display.drawString(0, 4, String(Leq_dB, 1) + " " + DB_UNITS);
-
- display.display();
-
- #endif // USE_DISPLAY
+#if (USE_DISPLAY > 0)
+
+ //
+ // Example code that displays the measured value.
+ // You should customize the below code for your display
+ // and display library used.
+ //
+
+ display.clear();
+
+ // It is important to somehow notify when the deivce is out of its range
+ // as the calculated values are very likely with big error
+ if (Leq_dB > MIC_OVERLOAD_DB)
+ {
+ // Display 'Overload' if dB value is over the AOP
+ display.drawString(0, 24, "Overload");
+ }
+ else if (isnan(Leq_dB) || (Leq_dB < MIC_NOISE_DB))
+ {
+ // Display 'Low' if dB value is below noise floor
+ display.drawString(0, 24, "Low");
+ }
+
+ // The 'short' Leq line
+ double short_Leq_dB = MIC_OFFSET_DB + MIC_REF_DB + 20 * log10(sqrt(double(sum_sqr_weighted) / SAMPLE_COUNT) / MIC_REF_AMPL);
+ uint16_t len = min(max(0, int(((short_Leq_dB - MIC_NOISE_DB) / MIC_OVERLOAD_DB) * (display.getWidth() - 1))), display.getWidth() - 1);
+ display.drawHorizontalLine(0, 0, len);
+ display.drawHorizontalLine(0, 1, len);
+ display.drawHorizontalLine(0, 2, len);
+
+ // The Leq numeric decibels
+ display.drawString(0, 4, String(Leq_dB, 1) + " " + DB_UNITS);
+
+ display.display();
+
+#endif // USE_DISPLAY
}
}
-void loop() {
+void loop()
+{
// Nothing here..
}
diff --git a/filters.h b/filters.h
new file mode 100644
index 0000000..6c3c21e
--- /dev/null
+++ b/filters.h
@@ -0,0 +1,137 @@
+/*
+ * Display A-weighted sound level measured by I2S Microphone
+ *
+ * (c)2019 Ivan Kostoski (original version)
+ * (c)2021 Bim Overbohm (split into files, template)
+ *
+ * This program is free software: you can redistribute it and/or modify
+ * it under the terms of the GNU General Public License as published by
+ * the Free Software Foundation, either version 3 of the License, or
+ * (at your option) any later version.
+ *
+ * This program is distributed in the hope that it will be useful,
+ * but WITHOUT ANY WARRANTY; without even the implied warranty of
+ * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the
+ * GNU General Public License for more details.
+ *
+ * You should have received a copy of the GNU General Public License
+ * along with this program. If not, see .
+ */
+#pragma once
+
+#include "sos-iir-filter.h"
+
+//
+// IIR Filters
+//
+
+// Dummy filter. For testing only
+const SOS_IIR_Filter None(
+ 1.0,
+ {}
+);
+
+// DC-Blocker filter - removes DC component from I2S data
+// See: https://www.dsprelated.com/freebooks/filters/DC_Blocker.html
+// a1 = -0.9992 should heavily attenuate frequencies below 10Hz
+const SOS_IIR_Filter DC_BLOCKER(
+ 1.0,
+ {{-1.0, 0.0, +0.9992, 0.0}}
+);
+
+//
+// Equalizer IIR filters to flatten microphone frequency response
+// See respective .m file for filter design. Fs = 48Khz.
+//
+// Filters are represented as Second-Order Sections cascade with assumption
+// that b0 and a0 are equal to 1.0 and 'gain' is applied at the last step
+// B and A coefficients were transformed with GNU Octave:
+// [sos, gain] = tf2sos(B, A)
+// See: https://www.dsprelated.com/freebooks/filters/Series_Second_Order_Sections.html
+// NOTE: SOS matrix 'a1' and 'a2' coefficients are negatives of tf2sos output
+//
+
+// TDK/InvenSense ICS-43434
+// Datasheet: https://www.invensense.com/wp-content/uploads/2016/02/DS-000069-ICS-43434-v1.1.pdf
+// B = [0.477326418836803, -0.486486982406126, -0.336455844522277, 0.234624646917202, 0.111023257388606];
+// A = [1.0, -1.93073383849136326, 0.86519456089576796, 0.06442838283825100, 0.00111249298800616];
+const SOS_IIR_Filter ICS43434(
+ 0.477326418836803,
+ {// Second-Order Sections {b1, b2, -a1, -a2}
+ {+0.96986791463971267, 0.23515976355743193, -0.06681948004769928, -0.00111521990688128},
+ {-1.98905931743624453, 0.98908924206960169, +1.99755331853906037, -0.99755481510122113}}
+);
+
+// TDK/InvenSense ICS-43432
+// Datasheet: https://www.invensense.com/wp-content/uploads/2015/02/ICS-43432-data-sheet-v1.3.pdf
+// B = [-0.45733702338341309 1.12228667105574775 -0.77818278904413563, 0.00968926337978037, 0.10345668405223755]
+// A = [1.0, -3.3420781082912949, 4.4033694320978771, -3.0167072679918010, 1.2265536567647031, -0.2962229189311990, 0.0251085747458112]
+const SOS_IIR_Filter ICS43432(
+ -0.457337023383413,
+ {// Second-Order Sections {b1, b2, -a1, -a2}
+ {-0.544047931916859, -0.248361759321800, +0.403298891662298, -0.207346186351843},
+ {-1.909911869441421, +0.910830292683527, +1.790285722826743, -0.804085812369134},
+ {+0.000000000000000, +0.000000000000000, +1.148493493802252, -0.150599527756651}}
+);
+
+// TDK/InvenSense INMP441
+// Datasheet: https://www.invensense.com/wp-content/uploads/2015/02/INMP441.pdf
+// B ~= [1.00198, -1.99085, 0.98892]
+// A ~= [1.0, -1.99518, 0.99518]
+const SOS_IIR_Filter INMP441(
+ 1.00197834654696,
+ {// Second-Order Sections {b1, b2, -a1, -a2}
+ {-1.986920458344451, +0.986963226946616, +1.995178510504166, -0.995184322194091}}
+);
+
+// Infineon IM69D130 Shield2Go
+// Datasheet: https://www.infineon.com/dgdl/Infineon-IM69D130-DS-v01_00-EN.pdf?fileId=5546d462602a9dc801607a0e46511a2e
+// B ~= [1.001240684967527, -1.996936108836337, 0.995703101823006]
+// A ~= [1.0, -1.997675693595542, 0.997677044195563]
+// With additional DC blocking component
+const SOS_IIR_Filter IM69D130(
+ 1.00124068496753,
+ {// Second-Order Sections {b1, b2, -a1, -a2}
+ {-1.0, 0.0, +0.9992, 0}, // DC blocker, a1 = -0.9992
+ {-1.994461610298131, 0.994469278738208, +1.997675693595542, -0.997677044195563}}
+);
+
+// Knowles SPH0645LM4H-B, rev. B
+// https://cdn-shop.adafruit.com/product-files/3421/i2S+Datasheet.PDF
+// B ~= [1.001234, -1.991352, 0.990149]
+// A ~= [1.0, -1.993853, 0.993863]
+// With additional DC blocking component
+const SOS_IIR_Filter SPH0645LM4H_B_RB(
+ 1.00123377961525,
+ {// Second-Order Sections {b1, b2, -a1, -a2}
+ {-1.0, 0.0, +0.9992, 0}, // DC blocker, a1 = -0.9992
+ {-1.988897663539382, +0.988928479008099, +1.993853376183491, -0.993862821429572}}
+ );
+
+//
+// Weighting filters
+//
+
+// A-weighting IIR Filter, Fs = 48KHz
+// (By Dr. Matt L., Source: https://dsp.stackexchange.com/a/36122)
+// B = [0.169994948147430, 0.280415310498794, -1.120574766348363, 0.131562559965936, 0.974153561246036, -0.282740857326553, -0.152810756202003]
+// A = [1.0, -2.12979364760736134, 0.42996125885751674, 1.62132698199721426, -0.96669962900852902, 0.00121015844426781, 0.04400300696788968]
+const SOS_IIR_Filter A_weighting(
+ 0.169994948147430,
+ {// Second-Order Sections {b1, b2, -a1, -a2}
+ {-2.00026996133106, +1.00027056142719, -1.060868438509278, -0.163987445885926},
+ {+4.35912384203144, +3.09120265783884, +1.208419926363593, -0.273166998428332},
+ {-0.70930303489759, -0.29071868393580, +1.982242159753048, -0.982298594928989}}
+);
+
+// C-weighting IIR Filter, Fs = 48KHz
+// Designed by invfreqz curve-fitting, see respective .m file
+// B = [-0.49164716933714026, 0.14844753846498662, 0.74117815661529129, -0.03281878334039314, -0.29709276192593875, -0.06442545322197900, -0.00364152725482682]
+// A = [1.0, -1.0325358998928318, -0.9524000181023488, 0.8936404694728326 0.2256286147169398 -0.1499917107550188, 0.0156718181681081]
+const SOS_IIR_Filter C_weighting(
+ -0.491647169337140,
+ {
+ {+1.4604385758204708, +0.5275070373815286, +1.9946144559930252, -0.9946217070140883},
+ {+0.2376222404939509, +0.0140411206016894, -1.3396585608422749, -0.4421457807694559},
+ {-2.0000000000000000, +1.0000000000000000, +0.3775800047420818, -0.0356365756680430}}
+);
diff --git a/i2s_mic.h b/i2s_mic.h
new file mode 100644
index 0000000..e6f3475
--- /dev/null
+++ b/i2s_mic.h
@@ -0,0 +1,238 @@
+/*
+ * Display A-weighted sound level measured by I2S Microphone
+ *
+ * (c)2019 Ivan Kostoski (original version)
+ * (c)2021 Bim Overbohm (split into files, template)
+ *
+ * This program is free software: you can redistribute it and/or modify
+ * it under the terms of the GNU General Public License as published by
+ * the Free Software Foundation, either version 3 of the License, or
+ * (at your option) any later version.
+ *
+ * This program is distributed in the hope that it will be useful,
+ * but WITHOUT ANY WARRANTY; without even the implied warranty of
+ * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the
+ * GNU General Public License for more details.
+ *
+ * You should have received a copy of the GNU General Public License
+ * along with this program. If not, see .
+ */
+
+#pragma once
+
+#include
+#include
+#include
+
+#include "sos-iir-filter.h"
+
+//#define SERIAL_OUTPUT
+
+// I2S microphone connnection
+// NR_OF_SAMPLES = number of microphone samples to take and return in queue
+// I2S pins - Can be routed to almost any (unused) ESP32 pin.
+// SD can be any pin, including input only pins (36-39).
+// SCK (i.e. BCLK) and WS (i.e. L/R CLK) must be output capable pins
+// PIN_WS = I2S word select pin
+// PIN_SCK = I2S clock pin
+// PIN_SD = I2S data pin
+// I2S_PORT = I2S port to use
+// MIC_BITS = number of valid bits in microphone data
+// MSB_SHIFT = set to true to fix MSB timing for some microphones, i.e. SPH0645LM4H-x
+// SAMPLE_RATE = microphone sample rate. must be 48kHz to fit filter design
+template
+class Microphone_I2S
+{
+ static constexpr unsigned TASK_PRIO = 4; // FreeRTOS priority
+ static constexpr unsigned TASK_STACK = 2048; // FreeRTOS stack size (in 32-bit words)
+
+public:
+ using SAMPLE_T = int32_t;
+ using SampleBuffer = float[NR_OF_SAMPLES];
+ static const constexpr uint32_t SAMPLE_BITS = sizeof(SAMPLE_T) * 8;
+
+ /// @brief Create new I2S microphone.
+ /// @param filter Microphone IIR filter function to apply to samples
+ Microphone_I2S(const SOS_IIR_Filter &filter)
+ : m_filter(filter)
+ {
+ }
+
+ void begin()
+ {
+#ifdef SERIAL_OUTPUT
+ Serial.print("Installing microphone I2S driver at ");
+ if (I2S_PORT == I2S_NUM_0)
+ {
+ Serial.println("I2S0");
+ }
+ else if (I2S_PORT == I2S_NUM_1)
+ {
+ Serial.println("I2S1");
+ }
+ else
+ {
+ Serial.println("unknown port");
+ }
+#endif
+ // Setup I2S to sample mono channel for SAMPLE_RATE * SAMPLE_BITS
+ // NOTE: Recent update to Arduino_esp32 (1.0.2 -> 1.0.3)
+ // seems to have swapped ONLY_LEFT and ONLY_RIGHT channels
+ i2s_config_t i2s_config{};
+ i2s_config.mode = i2s_mode_t(I2S_MODE_MASTER | I2S_MODE_RX);
+ i2s_config.sample_rate = SAMPLE_RATE;
+ i2s_config.bits_per_sample = i2s_bits_per_sample_t(SAMPLE_BITS);
+ i2s_config.channel_format = I2S_CHANNEL_FMT_ONLY_LEFT;
+ i2s_config.communication_format = i2s_comm_format_t(I2S_COMM_FORMAT_I2S | I2S_COMM_FORMAT_I2S_MSB);
+ i2s_config.intr_alloc_flags = ESP_INTR_FLAG_LEVEL1;
+ i2s_config.dma_buf_count = 2;
+ i2s_config.dma_buf_len = NR_OF_SAMPLES;
+ i2s_config.use_apll = true;
+ //i2s_config.tx_desc_auto_clear = false;
+ i2s_config.fixed_mclk = 0;
+ i2s_driver_install(I2S_PORT, &i2s_config, 0, nullptr);
+
+ // I2S pin mapping
+#ifdef SERIAL_OUTPUT
+ Serial.println("Installing microphone I2S pin mapping");
+#endif
+ i2s_pin_config_t pin_config{};
+ pin_config.bck_io_num = PIN_SCK;
+ pin_config.ws_io_num = PIN_WS;
+ pin_config.data_out_num = -1; // not used
+ pin_config.data_in_num = PIN_SD;
+ if (MSB_SHIFT)
+ {
+ // Undocumented (?!) manipulation of I2S peripheral registers
+ // to fix MSB timing issues with some I2S microphones
+ REG_SET_BIT(I2S_TIMING_REG(I2S_PORT), BIT(9));
+ REG_SET_BIT(I2S_CONF_REG(I2S_PORT), I2S_RX_MSB_SHIFT);
+ }
+ i2s_set_pin(I2S_PORT, &pin_config);
+
+#ifdef SERIAL_OUTPUT
+ Serial.println("Creating microphone I2S sample queue");
+#endif
+ //FIXME: There is a known issue with esp-idf and sampling rates, see:
+ // https://github.com/espressif/esp-idf/issues/2634
+ // In the meantime, the below line seems to set sampling rate at ~47999.992Hz
+ // fifs_req=24576000, sdm0=149, sdm1=212, sdm2=5, odir=2 -> fifs_reached=24575996
+ //NOTE: This seems to be fixed in ESP32 Arduino 1.0.4, esp-idf 3.2
+ // Should be safe to remove...
+ //#include
+ //rtc_clk_apll_enable(1, 149, 212, 5, 2);
+ // Create FreeRTOS queue
+ m_sampleQueue = xQueueCreate(2, sizeof(SampleBuffer));
+ // Create the I2S reader FreeRTOS task
+ // NOTE: Current version of ESP-IDF will pin the task
+ // automatically to the first core it happens to run on
+ // (due to using the hardware FPU instructions).
+ // For manual control see: xTaskCreatePinnedToCore
+#ifdef SERIAL_OUTPUT
+ Serial.println("Creating microphone I2S reader task");
+#endif
+ xTaskCreate(readerTask, "Microphone_I2S reader", TASK_STACK, this, TASK_PRIO, nullptr);
+ }
+
+ /// @brief Get the queue that stores new sample buffers.
+ QueueHandle_t sampleQueue() const
+ {
+ return m_sampleQueue;
+ }
+
+ /// @brief Start sampling from microphone.
+ void startSampling()
+ {
+ m_isSampling = true;
+ }
+
+ /// @brief Stop sampling from microphone.
+ void stopSampling()
+ {
+ m_isSampling = false;
+ }
+
+private:
+ static void readerTask(void *parameter)
+ {
+#ifdef SERIAL_OUTPUT
+ Serial.println("Mic reader task started");
+#endif
+ auto object = reinterpret_cast(parameter);
+ // Discard first blocks, microphone may have startup time (i.e. INMP441 up to 83ms)
+ size_t bytes_read = 0;
+ for (int i = 0; i < 5; i++)
+ {
+ i2s_read(I2S_PORT, &object->m_sampleBuffer, NR_OF_SAMPLES * sizeof(SAMPLE_T), &bytes_read, portMAX_DELAY);
+ }
+ while (true)
+ {
+ if (object->m_isSampling)
+ {
+ // Block and wait for microphone values from I2S
+ // Data is moved from DMA buffers to our m_sampleBuffer by the driver ISR
+ // and when there is requested amount of data, task is unblocked
+ //
+ // Note: i2s_read does not care it is writing in float[] buffer, it will write
+ // integer values to the given address, as received from the hardware peripheral.
+ i2s_read(I2S_PORT, &object->m_sampleBuffer, NR_OF_SAMPLES * sizeof(SAMPLE_T), &bytes_read, portMAX_DELAY);
+
+ // Debug only. Ticks we spent filtering and summing block of I2S data
+ //TickType_t start_tick = xTaskGetTickCount();
+
+ // Convert (including shifting) integer microphone values to floats,
+ // using the same buffer (assumed sample size is same as size of float),
+ // to save a bit of memory
+ auto int_samples = reinterpret_cast(&object->m_sampleBuffer);
+ for (int i = 0; i < NR_OF_SAMPLES; i++)
+ {
+ object->m_sampleBuffer[i] = int_samples[i] >> (SAMPLE_BITS - MIC_BITS);
+ }
+
+ // filter values and apply gain setting
+ object->m_filter.applyFilters(object->m_sampleBuffer, object->m_sampleBuffer, NR_OF_SAMPLES);
+ object->m_filter.applyGain(object->m_sampleBuffer, object->m_sampleBuffer, NR_OF_SAMPLES);
+
+ // Debug only. Ticks we spent filtering and summing block of I2S data
+ //auto proc_ticks = xTaskGetTickCount() - start_tick;
+
+ // Send the sums to FreeRTOS queue where main task will pick them up
+ // and further calculate decibel values (division, logarithms, etc...)
+ xQueueSend(object->m_sampleQueue, &object->m_sampleBuffer, portMAX_DELAY);
+
+ // Debug only. Print raw microphone sample values
+ /*int vMin = 1000000;
+ int vMax = -vMin;
+ int vAvg = 0;
+ int vNan = 0;
+ for (unsigned int k = 0; k < bytes_read; k++)
+ {
+ if (isnan(object->m_sampleBuffer[k]) || isinf(object->m_sampleBuffer[k]))
+ {
+ object->m_sampleBuffer[k] = 0;
+ vNan++;
+ }
+ if (object->m_sampleBuffer[k] < vMin)
+ {
+ vMin = object->m_sampleBuffer[k];
+ }
+ if (object->m_sampleBuffer[k] > vMax)
+ {
+ vMax = object->m_sampleBuffer[k];
+ }
+ vAvg += object->m_sampleBuffer[k];
+ }
+ vAvg /= bytes_read;
+ Serial.print("Min: "); Serial.print(vMin, 3);
+ Serial.print(", Max: "); Serial.print(vMax, 3);
+ Serial.print(", Avg: "); Serial.print(vAvg, 3);
+ Serial.print(", NAN or INF: "); Serial.println(vNan);*/
+ }
+ }
+ }
+
+ SOS_IIR_Filter m_filter;
+ QueueHandle_t m_sampleQueue;
+ SampleBuffer m_sampleBuffer __attribute__((aligned(4)));
+ bool m_isSampling = false;
+};
diff --git a/sos-iir-filter.h b/sos-iir-filter.h
index c45a2b3..27f81a0 100644
--- a/sos-iir-filter.h
+++ b/sos-iir-filter.h
@@ -2,6 +2,7 @@
* ESP32 Second-Order Sections IIR Filter implementation
*
* (c)2019 Ivan Kostoski
+ * (c)2021 Bim Overbohm (split into files, template)
*
* This program is free software: you can redistribute it and/or modify
* it under the terms of the GNU General Public License as published by
@@ -17,187 +18,121 @@
* along with this program. If not, see .
*/
-#ifndef SOS_IIR_FILTER_H
-#define SOS_IIR_FILTER_H
+#pragma once
-#include
+#include
+#include
+#include
-struct SOS_Coefficients {
- float b1;
- float b2;
- float a1;
- float a2;
+struct SOS_Coefficients
+{
+ float b1;
+ float b2;
+ float a1;
+ float a2;
};
-struct SOS_Delay_State {
- float w0 = 0;
- float w1 = 0;
+struct SOS_Delay_State
+{
+ float w0 = 0.0F;
+ float w1 = 0.0F;
};
-extern "C" {
- int sos_filter_f32(float *input, float *output, int len, const SOS_Coefficients &coeffs, SOS_Delay_State &w);
-}
-__asm__ (
- //
- // ESP32 implementation of IIR Second-Order Section filter
- // Assumes a0 and b0 coefficients are one (1.0)
- //
- // float* a2 = input;
- // float* a3 = output;
- // int a4 = len;
- // float* a5 = coeffs;
- // float* a6 = w;
- // float a7 = gain;
- //
- ".text \n"
- ".align 4 \n"
- ".global sos_filter_f32 \n"
- ".type sos_filter_f32,@function\n"
- "sos_filter_f32: \n"
- " entry a1, 16 \n"
- " lsi f0, a5, 0 \n" // float f0 = coeffs.b1;
- " lsi f1, a5, 4 \n" // float f1 = coeffs.b2;
- " lsi f2, a5, 8 \n" // float f2 = coeffs.a1;
- " lsi f3, a5, 12 \n" // float f3 = coeffs.a2;
- " lsi f4, a6, 0 \n" // float f4 = w[0];
- " lsi f5, a6, 4 \n" // float f5 = w[1];
- " loopnez a4, 1f \n" // for (; len>0; len--) {
- " lsip f6, a2, 4 \n" // float f6 = *input++;
- " madd.s f6, f2, f4 \n" // f6 += f2 * f4; // coeffs.a1 * w0
- " madd.s f6, f3, f5 \n" // f6 += f3 * f5; // coeffs.a2 * w1
- " mov.s f7, f6 \n" // f7 = f6; // b0 assumed 1.0
- " madd.s f7, f0, f4 \n" // f7 += f0 * f4; // coeffs.b1 * w0
- " madd.s f7, f1, f5 \n" // f7 += f1 * f5; // coeffs.b2 * w1 -> result
- " ssip f7, a3, 4 \n" // *output++ = f7;
- " mov.s f5, f4 \n" // f5 = f4; // w1 = w0
- " mov.s f4, f6 \n" // f4 = f6; // w0 = f6
- " 1: \n" // }
- " ssi f4, a6, 0 \n" // w[0] = f4;
- " ssi f5, a6, 4 \n" // w[1] = f5;
- " movi.n a2, 0 \n" // return 0;
- " retw.n \n"
-);
-
-extern "C" {
- float sos_filter_sum_sqr_f32(float *input, float *output, int len, const SOS_Coefficients &coeffs, SOS_Delay_State &w, float gain);
+extern "C"
+{
+ int sos_filter_f32(float *input, float *output, int len, const SOS_Coefficients &coeffs, SOS_Delay_State &w);
}
-__asm__ (
- //
- // ESP32 implementation of IIR Second-Order section filter with applied gain.
- // Assumes a0 and b0 coefficients are one (1.0)
- // Returns sum of squares of filtered samples
- //
- // float* a2 = input;
- // float* a3 = output;
- // int a4 = len;
- // float* a5 = coeffs;
- // float* a6 = w;
- // float a7 = gain;
- //
- ".text \n"
- ".align 4 \n"
- ".global sos_filter_sum_sqr_f32 \n"
- ".type sos_filter_sum_sqr_f32,@function \n"
- "sos_filter_sum_sqr_f32: \n"
- " entry a1, 16 \n"
- " lsi f0, a5, 0 \n" // float f0 = coeffs.b1;
- " lsi f1, a5, 4 \n" // float f1 = coeffs.b2;
- " lsi f2, a5, 8 \n" // float f2 = coeffs.a1;
- " lsi f3, a5, 12 \n" // float f3 = coeffs.a2;
- " lsi f4, a6, 0 \n" // float f4 = w[0];
- " lsi f5, a6, 4 \n" // float f5 = w[1];
- " wfr f6, a7 \n" // float f6 = gain;
- " const.s f10, 0 \n" // float sum_sqr = 0;
- " loopnez a4, 1f \n" // for (; len>0; len--) {
- " lsip f7, a2, 4 \n" // float f7 = *input++;
- " madd.s f7, f2, f4 \n" // f7 += f2 * f4; // coeffs.a1 * w0
- " madd.s f7, f3, f5 \n" // f7 += f3 * f5; // coeffs.a2 * w1;
- " mov.s f8, f7 \n" // f8 = f7; // b0 assumed 1.0
- " madd.s f8, f0, f4 \n" // f8 += f0 * f4; // coeffs.b1 * w0;
- " madd.s f8, f1, f5 \n" // f8 += f1 * f5; // coeffs.b2 * w1;
- " mul.s f9, f8, f6 \n" // f9 = f8 * f6; // f8 * gain -> result
- " ssip f9, a3, 4 \n" // *output++ = f9;
- " mov.s f5, f4 \n" // f5 = f4; // w1 = w0
- " mov.s f4, f7 \n" // f4 = f7; // w0 = f7;
- " madd.s f10, f9, f9 \n" // f10 += f9 * f9; // sum_sqr += f9 * f9;
- " 1: \n" // }
- " ssi f4, a6, 0 \n" // w[0] = f4;
- " ssi f5, a6, 4 \n" // w[1] = f5;
- " rfr a2, f10 \n" // return sum_sqr;
- " retw.n \n" //
-);
-
-
-/**
- * Envelops above asm functions into C++ class
- */
-struct SOS_IIR_Filter {
-
- const int num_sos;
- const float gain;
- SOS_Coefficients* sos = NULL;
- SOS_Delay_State* w = NULL;
-
- // Dynamic constructor
- SOS_IIR_Filter(size_t num_sos, const float gain, const SOS_Coefficients _sos[] = NULL): num_sos(num_sos), gain(gain) {
- if (num_sos > 0) {
- sos = new SOS_Coefficients[num_sos];
- if ((sos != NULL) && (_sos != NULL)) memcpy(sos, _sos, num_sos * sizeof(SOS_Coefficients));
- w = new SOS_Delay_State[num_sos]();
+__asm__(
+ //
+ // ESP32 implementation of IIR Second-Order Section filter
+ // Assumes a0 and b0 coefficients are one (1.0)
+ //
+ // float* a2 = input;
+ // float* a3 = output;
+ // int a4 = len;
+ // float* a5 = coeffs;
+ // float* a6 = w;
+ // float a7 = gain;
+ //
+ ".text \n"
+ ".align 4 \n"
+ ".global sos_filter_f32 \n"
+ ".type sos_filter_f32,@function\n"
+ "sos_filter_f32: \n"
+ " entry a1, 16 \n"
+ " lsi f0, a5, 0 \n" // float f0 = coeffs.b1;
+ " lsi f1, a5, 4 \n" // float f1 = coeffs.b2;
+ " lsi f2, a5, 8 \n" // float f2 = coeffs.a1;
+ " lsi f3, a5, 12 \n" // float f3 = coeffs.a2;
+ " lsi f4, a6, 0 \n" // float f4 = w[0];
+ " lsi f5, a6, 4 \n" // float f5 = w[1];
+ " loopnez a4, 1f \n" // for (; len>0; len--) {
+ " lsip f6, a2, 4 \n" // float f6 = *input++;
+ " madd.s f6, f2, f4 \n" // f6 += f2 * f4; // coeffs.a1 * w0
+ " madd.s f6, f3, f5 \n" // f6 += f3 * f5; // coeffs.a2 * w1
+ " mov.s f7, f6 \n" // f7 = f6; // b0 assumed 1.0
+ " madd.s f7, f0, f4 \n" // f7 += f0 * f4; // coeffs.b1 * w0
+ " madd.s f7, f1, f5 \n" // f7 += f1 * f5; // coeffs.b2 * w1 -> result
+ " ssip f7, a3, 4 \n" // *output++ = f7;
+ " mov.s f5, f4 \n" // f5 = f4; // w1 = w0
+ " mov.s f4, f6 \n" // f4 = f6; // w0 = f6
+ " 1: \n" // }
+ " ssi f4, a6, 0 \n" // w[0] = f4;
+ " ssi f5, a6, 4 \n" // w[1] = f5;
+ " movi.n a2, 0 \n" // return 0;
+ " retw.n \n");
+
+/// @brief Envelops above asm function into C++ class
+class SOS_IIR_Filter
+{
+public:
+ /// @brief Constructor
+ SOS_IIR_Filter(float gain, const std::vector& sos)
+ : m_gain(gain)
+ , m_sos(sos)
+ , m_w(sos.size())
+ {
+ };
+
+ /// @brief Apply defined IIR Filter(s) to input array of floats and write values to output
+ auto applyFilters(float *input, float *output, size_t len) const -> void
+ {
+ float *source = input;
+ for (decltype(m_sos.size()) i = 0; i < m_sos.size(); i++)
+ {
+ sos_filter_f32(source, output, len, m_sos[i], m_w[i]);
+ source = output;
+ }
}
- };
-
- // Template constructor for const filter declaration
- template
- SOS_IIR_Filter(const float gain, const SOS_Coefficients (&sos)[Array_Size]): SOS_IIR_Filter(Array_Size, gain, sos) {};
-
- /**
- * Apply defined IIR Filter to input array of floats, write filtered values to output,
- * and return sum of squares of all filtered values
- */
- inline float filter(float* input, float* output, size_t len) {
- if ((num_sos < 1) || (sos == NULL) || (w == NULL)) return 0;
- float* source = input;
- // Apply all but last Second-Order-Section
- for(int i=0; i<(num_sos-1); i++) {
- sos_filter_f32(source, output, len, sos[i], w[i]);
- source = output;
- }
- // Apply last SOS with gain and return the sum of squares of all samples
- return sos_filter_sum_sqr_f32(source, output, len, sos[num_sos-1], w[num_sos-1], gain);
- }
- ~SOS_IIR_Filter() {
- if (w != NULL) delete[] w;
- if (sos != NULL) delete[] sos;
- }
-
-};
-
-//
-// For testing only
-//
-struct No_IIR_Filter {
- const int num_sos = 0;
- const float gain = 1.0;
-
- No_IIR_Filter() {};
-
- inline float filter(float* input, float* output, size_t len) {
- float sum_sqr = 0;
- float s;
- for(int i=0; i void
+ {
+ if (input != nullptr && output != nullptr)
+ {
+ for (decltype(len) i = 0; i < len; i++)
+ {
+ output[i] = input[i] * m_gain;
+ }
+ }
}
- if (input != output) {
- for(int i=0; i float
+ {
+ float result = 0.0F;
+ if (input != nullptr)
+ {
+ for (decltype(len) i = 0; i < len; i++)
+ {
+ result += input[i] * input[i];
+ }
+ }
+ return result;
}
- return sum_sqr;
- };
-
+
+private:
+ float m_gain = 1.0F;
+ std::vector m_sos{};
+ mutable std::vector m_w{};
};
-
-No_IIR_Filter None;
-
-#endif // SOS_IIR_FILTER_H