diff --git a/ESP32_TTGO/INMP441_MeterBarForSoundLevel/INMP441_MeterBarForSoundLevel.ino b/ESP32_TTGO/INMP441_MeterBarForSoundLevel/INMP441_MeterBarForSoundLevel.ino
new file mode 100644
index 0000000..2093ffb
--- /dev/null
+++ b/ESP32_TTGO/INMP441_MeterBarForSoundLevel/INMP441_MeterBarForSoundLevel.ino
@@ -0,0 +1,468 @@
+// Based on SLM Project: https://github.com/ikostoski/esp32-i2s-slm
+/*
+ * Display A-weighted sound level measured by I2S Microphone
+ *
+ * (c)2019 Ivan Kostoski
+ *
+ * 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 .
+ */
+
+/*
+ * Sketch samples audio data from I2S microphone, processes the data
+ * with digital IIR filters and calculates A or C weighted Equivalent
+ * Continuous Sound Level (Leq)
+ *
+ * I2S is setup to sample data at Fs=48000KHz (fixed value due to
+ * design of digital IIR filters). Data is read from I2S queue
+ * in 'sample blocks' (default 125ms block, equal to 6000 samples)
+ * by 'i2s_reader_task', filtered trough two IIR filters (equalizer
+ * and weighting), summed up and pushed into 'samples_queue' as
+ * sum of squares of filtered samples. The main task then pulls data
+ * from the queue and calculates decibel value relative to microphone
+ * reference amplitude, derived from datasheet sensitivity dBFS
+ * value, number of bits in I2S data, and the reference value for
+ * which the sensitivity is specified (typically 94dB, pure sine
+ * wave at 1KHz).
+ *
+ * Displays line on the small OLED screen with 'short' LAeq(125ms)
+ * response and numeric LAeq(1sec) dB value from the signal RMS.
+ */
+
+#include
+#include "sos-iir-filter.h"
+#include // Hardware-specific library
+TFT_eSPI tft = TFT_eSPI(); // Invoke custom library
+
+// https://github.com/Bodmer/TFT_eFEX
+#include // Include the extension graphics functions library
+TFT_eFEX fex = TFT_eFEX(&tft); // Create TFT_eFX object "efx" with poin
+//
+// 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
+
+// NOTE: Some microphones require at least DC-Blocker filter
+#define MIC_EQUALIZER INMP441 // 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 33 // 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
+
+// Calculate reference amplitude value at compile time
+constexpr double MIC_REF_AMPL = pow(10, double(MIC_SENSITIVITY)/20) * ((1<<(MIC_BITS-1))-1);
+
+//
+// I2S pins - Can be routed to almost any (unused) ESP32 pin.
+// SD can be any pin, inlcuding input only pins (36-39).
+// SCK (i.e. BCLK) and WS (i.e. L/R CLK) must be output capable pins
+//
+// Below ones are just example for my board layout, put here the pins you will use
+//
+#define I2S_WS 15
+#define I2S_SD 13
+#define I2S_SCK 2
+
+// I2S peripheral to use (0 or 1)
+#define I2S_PORT I2S_NUM_0
+
+// Drawing Meter Bar
+#define X_POS 10
+#define BAR_GAP 10
+#define BAR_HEIGHT 8
+#define BAR_WIDTH (TFT_WIDTH - 20)
+#define MIN_LEVEL 0
+#define MAX_LEVEL 23
+
+int preLevel = 0;
+
+// Drawing Meter Bar Color
+uint16_t redInactiveColor = fex.luminance(255, 0, 0, 16);
+uint16_t yellowInactiveColor = fex.luminance(255, 255, 0, 16);
+uint16_t greenInactiveColor = fex.luminance(0, 255, 0, 16);
+uint16_t redActiveColor = fex.luminance(255, 0, 0, 196);
+uint16_t yellowActiveColor = fex.luminance(255, 255, 0, 196);
+uint16_t greenActiveColor = fex.luminance(0, 255, 0, 196);
+
+//
+// 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 MIC_OVERLOAD_DB) {
+ Leq_sum_sqr = INFINITY;
+ } 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;
+
+ // When we gather enough samples, calculate new Leq value
+ if (Leq_samples >= SAMPLE_RATE * 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);
+ // Debug only
+ //Serial.printf("%u processing ticks\n", q.proc_ticks);
+ }
+ findDecibelLevel((int)Leq_dB);
+ }
+}
+
+// Display Initialization
+void displayInit(){
+ tft.init();
+ tft.setRotation(2);
+ tft.fillScreen(TFT_BLACK);
+ drawDecibelMeter(MAX_LEVEL, MIN_LEVEL, false);
+}
+
+// Find Decibel Level (0 - 23)
+void findDecibelLevel(int Leq_dB){
+
+ int level = map(Leq_dB, MIC_NOISE_DB, MIC_OVERLOAD_DB, MIN_LEVEL, MAX_LEVEL);
+
+ if(level >= MAX_LEVEL){
+ level = MAX_LEVEL;
+ }else if(level <=MIN_LEVEL){
+ level = MIN_LEVEL;
+ }
+
+ if(preLevel > level){
+ drawDecibelMeter(preLevel, level, false);
+ }else if(preLevel < level){
+ drawDecibelMeter(level, preLevel, true);
+ }
+
+ preLevel = level;
+}
+
+// Draw Decibel Meter
+void drawDecibelMeter(int pre, int cur, bool isActive){
+ for(int idx=cur, yPos = BAR_GAP * cur; idx <=pre; idx++){
+ if(idx > 20){
+ tft.fillRect(X_POS, yPos, BAR_WIDTH, BAR_HEIGHT, isActive? redActiveColor : redInactiveColor);
+ }else if(idx >=18 && idx <=20){
+ tft.fillRect(X_POS, yPos, BAR_WIDTH, BAR_HEIGHT, isActive? yellowActiveColor: yellowInactiveColor);
+ }else{
+ tft.fillRect(X_POS, yPos, BAR_WIDTH, BAR_HEIGHT, isActive? greenActiveColor: greenInactiveColor);
+ }
+ yPos+= BAR_GAP;
+ }
+}
+
+void loop() {
+ // Nothing here..
+}
diff --git a/ESP32_TTGO/INMP441_MeterBarForSoundLevel/sos-iir-filter.h b/ESP32_TTGO/INMP441_MeterBarForSoundLevel/sos-iir-filter.h
new file mode 100644
index 0000000..c45a2b3
--- /dev/null
+++ b/ESP32_TTGO/INMP441_MeterBarForSoundLevel/sos-iir-filter.h
@@ -0,0 +1,203 @@
+/*
+ * ESP32 Second-Order Sections IIR Filter implementation
+ *
+ * (c)2019 Ivan Kostoski
+ *
+ * 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 .
+ */
+
+#ifndef SOS_IIR_FILTER_H
+#define SOS_IIR_FILTER_H
+
+#include
+
+struct SOS_Coefficients {
+ float b1;
+ float b2;
+ float a1;
+ float a2;
+};
+
+struct SOS_Delay_State {
+ float w0 = 0;
+ float w1 = 0;
+};
+
+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);
+}
+__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]();
+ }
+ };
+
+ // 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