mirror of
https://github.com/0015/ThatProject.git
synced 2026-01-12 09:17:42 +03:00
ESP32 | INMP441 | Tutorial - [Part.2] Emitting Sound Level Meter through BLE (ft. FreeRTOS)
This commit is contained in:
Eric
@@ -0,0 +1,23 @@
|
||||
#include "slm.h"
|
||||
#include "ble.h"
|
||||
|
||||
void setup() {
|
||||
Serial.begin(115200);
|
||||
task_slm();
|
||||
delay(500);
|
||||
task_ble();
|
||||
}
|
||||
|
||||
void task_slm(){
|
||||
samples_queue = xQueueCreate(8, sizeof(sum_queue_t));
|
||||
xTaskCreate(mic_i2s_reader_task, "Mic I2S Reader", I2S_TASK_STACK, NULL, I2S_TASK_PRI, NULL);
|
||||
}
|
||||
|
||||
void task_ble(){
|
||||
setupBLE();
|
||||
xTaskCreate(ble_Notifier, "BLE Notifier", BLE_TASK_STACK, NULL, BLE_TASK_PRI, NULL);
|
||||
}
|
||||
|
||||
void loop() {
|
||||
// put your main code here, to run repeatedly:
|
||||
}
|
||||
@@ -0,0 +1,82 @@
|
||||
#define BLE_TASK_PRI 4
|
||||
#define BLE_TASK_STACK 10000
|
||||
#include <BLEDevice.h>
|
||||
#include <BLEServer.h>
|
||||
#include <BLEUtils.h>
|
||||
#include <BLE2902.h>
|
||||
|
||||
BLEServer* pServer = NULL;
|
||||
BLECharacteristic* pCharacteristic = NULL;
|
||||
uint8_t clientNum;
|
||||
|
||||
// See the following for generating UUIDs:
|
||||
// https://www.uuidgenerator.net/
|
||||
|
||||
#define SERVICE_UUID "4fafc201-1fb5-459e-8fcc-c5c9c331914b"
|
||||
#define CHARACTERISTIC_UUID "beb5483e-36e1-4688-b7f5-ea07361b26a8"
|
||||
|
||||
class MyServerCallbacks: public BLEServerCallbacks {
|
||||
void onConnect(BLEServer* pServer) {
|
||||
clientNum++;
|
||||
BLEDevice::startAdvertising();
|
||||
};
|
||||
|
||||
void onDisconnect(BLEServer* pServer) {
|
||||
clientNum--;
|
||||
}
|
||||
};
|
||||
|
||||
void setupBLE() {
|
||||
clientNum = 0;
|
||||
// Create the BLE Device
|
||||
BLEDevice::init("ESP32-THAT-PROJECT");
|
||||
|
||||
// Create the BLE Server
|
||||
pServer = BLEDevice::createServer();
|
||||
pServer->setCallbacks(new MyServerCallbacks());
|
||||
|
||||
// Create the BLE Service
|
||||
BLEService *pService = pServer->createService(SERVICE_UUID);
|
||||
|
||||
// Create a BLE Characteristic
|
||||
pCharacteristic = pService->createCharacteristic(
|
||||
CHARACTERISTIC_UUID,
|
||||
BLECharacteristic::PROPERTY_NOTIFY
|
||||
);
|
||||
|
||||
// https://www.bluetooth.com/specifications/gatt/viewer?attributeXmlFile=org.bluetooth.descriptor.gatt.client_characteristic_configuration.xml
|
||||
// Create a BLE Descriptor
|
||||
pCharacteristic->addDescriptor(new BLE2902());
|
||||
|
||||
// Start the service
|
||||
pService->start();
|
||||
|
||||
// Start advertising
|
||||
BLEAdvertising *pAdvertising = BLEDevice::getAdvertising();
|
||||
pAdvertising->addServiceUUID(SERVICE_UUID);
|
||||
pAdvertising->setScanResponse(false);
|
||||
pAdvertising->setMinPreferred(0x0); // set value to 0x00 to not advertise this parameter
|
||||
BLEDevice::startAdvertising();
|
||||
Serial.println("Waiting a client connection to notify...");
|
||||
}
|
||||
|
||||
void ble_Notifier(void *pvParameter) {
|
||||
|
||||
double _Leq_dB;
|
||||
while(xQueueReceive(samples_queue, &_Leq_dB, portMAX_DELAY)){
|
||||
// notify changed value
|
||||
if (clientNum > 0) {
|
||||
String tempString = String(_Leq_dB);
|
||||
Serial.println(tempString);
|
||||
pCharacteristic->setValue((char*)tempString.c_str());
|
||||
pCharacteristic->notify();
|
||||
vTaskDelay(100);
|
||||
}
|
||||
// disconnecting
|
||||
if (clientNum <= 0) {
|
||||
vTaskDelay(500);
|
||||
pServer->startAdvertising(); // restart advertising
|
||||
Serial.println("start advertising");
|
||||
}
|
||||
}
|
||||
}
|
||||
@@ -0,0 +1,367 @@
|
||||
/*
|
||||
* 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 <https://www.gnu.org/licenses/>.
|
||||
*/
|
||||
|
||||
/*
|
||||
* 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)DB_UNITS
|
||||
* response and numeric LAeq(1sec) dB value from the signal RMS.
|
||||
*/
|
||||
|
||||
#include <driver/i2s.h>
|
||||
#include "sos-iir-filter.h"
|
||||
|
||||
//
|
||||
// Configuration
|
||||
//
|
||||
|
||||
#define LEQ_PERIOD 0.5 // 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_SCK 2
|
||||
#define I2S_SD 13
|
||||
|
||||
// I2S peripheral to use (0 or 1)
|
||||
#define I2S_PORT I2S_NUM_0
|
||||
|
||||
//
|
||||
// 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 <soc/rtc.h>
|
||||
//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);
|
||||
|
||||
|
||||
uint32_t Leq_samples = 0;
|
||||
double Leq_sum_sqr = 0;
|
||||
double Leq_dB = 0;
|
||||
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<SAMPLES_SHORT; i++) samples[i] = MIC_CONVERT(int_samples[i]);
|
||||
|
||||
sum_queue_t q;
|
||||
// Apply equalization and calculate Z-weighted sum of squares,
|
||||
// writes filtered samples back to the same buffer.
|
||||
q.sum_sqr_SPL = MIC_EQUALIZER.filter(samples, samples, SAMPLES_SHORT);
|
||||
|
||||
// Apply weighting and calucate weigthed sum of squares
|
||||
q.sum_sqr_weighted = WEIGHTING.filter(samples, samples, SAMPLES_SHORT);
|
||||
|
||||
// Debug only. Ticks we spent filtering and summing block of I2S data
|
||||
q.proc_ticks = xTaskGetTickCount() - start_tick;
|
||||
|
||||
// Send the sums to FreeRTOS queue where main task will pick them up
|
||||
// and further calcualte decibel values (division, logarithms, etc...)
|
||||
|
||||
|
||||
double short_RMS = sqrt(double(q.sum_sqr_SPL) / SAMPLES_SHORT);
|
||||
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) {
|
||||
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);
|
||||
}
|
||||
|
||||
xQueueSend(samples_queue, &Leq_dB, portMAX_DELAY);
|
||||
}
|
||||
}
|
||||
@@ -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 <https://www.gnu.org/licenses/>.
|
||||
*/
|
||||
|
||||
#ifndef SOS_IIR_FILTER_H
|
||||
#define SOS_IIR_FILTER_H
|
||||
|
||||
#include <stdint.h>
|
||||
|
||||
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 <size_t Array_Size>
|
||||
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<len; i++) {
|
||||
s = input[i];
|
||||
sum_sqr += s * s;
|
||||
}
|
||||
if (input != output) {
|
||||
for(int i=0; i<len; i++) output[i] = input[i];
|
||||
}
|
||||
return sum_sqr;
|
||||
};
|
||||
|
||||
};
|
||||
|
||||
No_IIR_Filter None;
|
||||
|
||||
#endif // SOS_IIR_FILTER_H
|
||||
@@ -0,0 +1,39 @@
|
||||
void setup() {
|
||||
Serial.begin(115200);
|
||||
xTaskCreate(task_1, "task_1", 2048, NULL, 1, NULL);
|
||||
delay(500);
|
||||
xTaskCreate(task_2, "task_2", configMINIMAL_STACK_SIZE, NULL, 1, NULL);
|
||||
}
|
||||
|
||||
void task_1(void *pvParameter){
|
||||
int i=0;
|
||||
while(true){
|
||||
if(i>10) i=0;
|
||||
|
||||
//Serial.print("[1] TASK value: ");
|
||||
//Serial.println(i);
|
||||
printf("[1] TASK value: %d\n", i);
|
||||
|
||||
i++;
|
||||
|
||||
vTaskDelay(125);
|
||||
}
|
||||
}
|
||||
|
||||
void task_2(void *pvParameter){
|
||||
int i=0;
|
||||
while(true){
|
||||
if(i>10) i=0;
|
||||
|
||||
Serial.print("\t\t\t\t\t[2] TASK value: ");
|
||||
Serial.println(i);
|
||||
|
||||
i++;
|
||||
|
||||
vTaskDelay(333);
|
||||
}
|
||||
}
|
||||
|
||||
void loop() {
|
||||
// put your main code here, to run repeatedly:
|
||||
}
|
||||
Reference in New Issue
Block a user