/* USER CODE BEGIN Header */
/**
******************************************************************************
* @file : main.c
* @brief : Main program body
******************************************************************************
* @attention
*
* Copyright (c) 2023 STMicroelectronics.
* All rights reserved.
*
* This software is licensed under terms that can be found in the LICENSE file
* in the root directory of this software component.
* If no LICENSE file comes with this software, it is provided AS-IS.
*
******************************************************************************
*/
/* USER CODE END Header */
/* Includes ------------------------------------------------------------------*/
#include "main.h"
/* Private includes ----------------------------------------------------------*/
/* USER CODE BEGIN Includes */
#include "Filter.h"
#include <stdlib.h>
/* USER CODE END Includes */
/* Private typedef -----------------------------------------------------------*/
/* USER CODE BEGIN PTD */
/* USER CODE END PTD */
/* Private define ------------------------------------------------------------*/
/* USER CODE BEGIN PD */
#define FIR_LENGTH 21
#define SAMPLE_RATE_HZ 44100.0f
#define UINT16_TO_FLOAT 0.00001525878f
#define FLOAT_TO_UINT16 32768.0f
#define AUDIO_BUFFER_SIZE 256
#define outputVolume 0.003f
/* USER CODE END PD */
/* Private macro -------------------------------------------------------------*/
/* USER CODE BEGIN PM */
/* USER CODE END PM */
/* Private variables ---------------------------------------------------------*/
ADC_HandleTypeDef hadc4;
DAC_HandleTypeDef hdac1;
OPAMP_HandleTypeDef hopamp4;
TIM_HandleTypeDef htim3;
UART_HandleTypeDef huart2;
/* USER CODE BEGIN PV */
// FIR Filter
float coefficients_fir[FIR_LENGTH] = {-0.00591377164083496, -0.00308644290781836, 0.0219915242479900, -0.0406091088853957, 0.0248358370392694, 0.0436025009813950, -0.122104025088665, 0.127561193706986, -0.0222146622289817, -0.127176005248907, 0.197549418233472, -0.127176005248907, -0.0222146622289817, 0.127561193706986, -0.122104025088665, 0.0436025009813950, 0.0248358370392694, -0.0406091088853957, 0.0219915242479900, -0.00308644290781836, -0.00591377164083496};
//float coefficients_fir[FIR_LENGTH] = {-4.94196577368864e-05, -0.000500524897433379, -0.00241883585285529, -0.00717831893088856, -0.0136987457847953, -0.0141484446157865, 0.00624169881266751, 0.0593418898052441, 0.138401253706343, 0.212485403281901, 0.243048097547224, 0.212485403281901, 0.138401253706343, 0.0593418898052441, 0.00624169881266751, -0.0141484446157865, -0.0136987457847953, -0.00717831893088856, -0.00241883585285529, -0.000500524897433379, -4.94196577368864e-05};
//float coefficients_fir[FIR_LENGTH] = {-6.93308673440112e-05, -0.000153771688981960, 0.000297670476897782, 0.00286422377013332, 0.0102702372548520, 0.0255381541964255, 0.0501337640750906, 0.0819808825919817, 0.114800218006465, 0.139776143494755, 0.149134882108085, 0.139776143494755, 0.114800218006465, 0.0819808825919817, 0.0501337640750906, 0.0255381541964255, 0.0102702372548520, 0.00286422377013332, 0.000297670476897782, -0.000153771688981960, -6.93308673440112e-05};
uint32_t buffer_fir[FIR_LENGTH];
uint32_t counter_fir;
volatile uint32_t output_fir;
volatile uint32_t original;
float out_fir;
uint32_t signal;
uint32_t index_fir;
Filter filt, filt2;
static volatile uint16_t *bufferCHin;
static uint16_t *adcOut[AUDIO_BUFFER_SIZE];
static volatile uint16_t *bufferCHout = &adcOut[0];
static float filterDividing3, filterDividing4;
static float filtered3, filtered4;
static uint16_t blackout3, blueout4;
static uint32_t hh;
/* USER CODE END PV */
/* Private function prototypes -----------------------------------------------*/
void SystemClock_Config(void);
static void MX_GPIO_Init(void);
static void MX_USART2_UART_Init(void);
static void MX_TIM3_Init(void);
static void MX_DAC1_Init(void);
static void MX_ADC4_Init(void);
static void MX_OPAMP4_Init(void);
/* USER CODE BEGIN PFP */
/* USER CODE END PFP */
/* Private user code ---------------------------------------------------------*/
/* USER CODE BEGIN 0 */
void Process(uint16_t *adcIn)
{
//Filter
//Duty cycle (AUDIO_BUFFER_SIZE/2 - 1)
for(uint8_t n = 0; n < AUDIO_BUFFER_SIZE / 2 - 1; n += 2)
{
adcIn[AUDIO_BUFFER_SIZE/2] = 1024 - adcIn[0];
bufferCHin = adcIn[AUDIO_BUFFER_SIZE/2];
/*Divide the adc into 2 channel, normalize the signal amplitude*/
filterDividing3 = UINT16_TO_FLOAT * ((float) bufferCHin[n]);
/*Filtered compare not filtered*/
//Filters initialization
Filter_Init(&filt, SAMPLE_RATE_HZ);
Filter_SetParameters(&filt, 7700.0f, 50.0f, 0.3f);
filtered3 = Filter_Update(&filt, filterDividing3); /*Filtered signals*/
//Filter_Init(&filt2, SAMPLE_RATE_HZ);
//Filter_SetParameters(&filt2, 16000.0f, 500.0f, 0.5f);
//filtered4 = Filter_Update(&filt2, filtered3); /*Original signals*/
//filtered4 = filterDividing4; /*Original signals*/
/*normalize the signal amplitude*/
blackout3 = (uint16_t) (FLOAT_TO_UINT16 * outputVolume * filtered3);
//blueout4 = (uint32_t) (FLOAT_TO_UINT32 * outputVolume * filtered4);
bufferCHout[n] = blackout3;
//bufferCHout[n+1] = blueout4;
adcOut[AUDIO_BUFFER_SIZE/2] = adcOut[0];
hh = adcOut[AUDIO_BUFFER_SIZE/2];
//TIM3->CCR4 = adcOut[AUDIO_BUFFER_SIZE/2];
}
}
/* USER CODE END 0 */
/**
* @brief The application entry point.
* @retval int
*/
int main(void)
{
/* USER CODE BEGIN 1 */
/* USER CODE END 1 */
/* MCU Configuration--------------------------------------------------------*/
/* Reset of all peripherals, Initializes the Flash interface and the Systick. */
HAL_Init();
/* USER CODE BEGIN Init */
/* USER CODE END Init */
/* Configure the system clock */
SystemClock_Config();
/* USER CODE BEGIN SysInit */
/* USER CODE END SysInit */
/* Initialize all configured peripherals */
MX_GPIO_Init();
MX_USART2_UART_Init();
MX_TIM3_Init();
MX_DAC1_Init();
MX_ADC4_Init();
MX_OPAMP4_Init();
/* USER CODE BEGIN 2 */
HAL_OPAMP_Start(&hopamp4);
HAL_ADC_Start(&hadc4);
HAL_TIM_PWM_Start_IT(&htim3, TIM_CHANNEL_1);
HAL_TIM_PWM_Start(&htim3, TIM_CHANNEL_3);
HAL_TIM_PWM_Start(&htim3, TIM_CHANNEL_4);
HAL_DAC_Start(&hdac1, DAC_CHANNEL_1);
HAL_DAC_SetValue(&hdac1, DAC_CHANNEL_1, DAC_ALIGN_12B_R, 559); //0.45V G=2 vin_amp=1
//Filters initialization
//Filter_Init(&filt, SAMPLE_RATE_HZ);
//Filter_SetParameters(&filt, 7500.0f, 500.0f, 0.5f);
//Filter_Init(&filt2, SAMPLE_RATE_HZ);
//Filter_SetParameters(&filt2, 16000.0f, 500.0f, 0.5f);
/* USER CODE END 2 */
/* Infinite loop */
/* USER CODE BEGIN WHILE */
while (1)
{
/* USER CODE END WHILE */
/* USER CODE BEGIN 3 */
}
/* USER CODE END 3 */
}
/**
* @brief System Clock Configuration
* @retval None
*/
void SystemClock_Config(void)
{
RCC_OscInitTypeDef RCC_OscInitStruct = {0};
RCC_ClkInitTypeDef RCC_ClkInitStruct = {0};
RCC_PeriphCLKInitTypeDef PeriphClkInit = {0};
/** Initializes the RCC Oscillators according to the specified parameters
* in the RCC_OscInitTypeDef structure.
*/
RCC_OscInitStruct.OscillatorType = RCC_OSCILLATORTYPE_HSI;
RCC_OscInitStruct.HSIState = RCC_HSI_ON;
RCC_OscInitStruct.HSICalibrationValue = RCC_HSICALIBRATION_DEFAULT;
RCC_OscInitStruct.PLL.PLLState = RCC_PLL_ON;
RCC_OscInitStruct.PLL.PLLSource = RCC_PLLSOURCE_HSI;
RCC_OscInitStruct.PLL.PLLMUL = RCC_PLL_MUL9;
RCC_OscInitStruct.PLL.PREDIV = RCC_PREDIV_DIV1;
if (HAL_RCC_OscConfig(&RCC_OscInitStruct) != HAL_OK)
{
Error_Handler();
}
/** Initializes the CPU, AHB and APB buses clocks
*/
RCC_ClkInitStruct.ClockType = RCC_CLOCKTYPE_HCLK|RCC_CLOCKTYPE_SYSCLK
|RCC_CLOCKTYPE_PCLK1|RCC_CLOCKTYPE_PCLK2;
RCC_ClkInitStruct.SYSCLKSource = RCC_SYSCLKSOURCE_PLLCLK;
RCC_ClkInitStruct.AHBCLKDivider = RCC_SYSCLK_DIV1;
RCC_ClkInitStruct.APB1CLKDivider = RCC_HCLK_DIV2;
RCC_ClkInitStruct.APB2CLKDivider = RCC_HCLK_DIV1;
if (HAL_RCC_ClockConfig(&RCC_ClkInitStruct, FLASH_LATENCY_2) != HAL_OK)
{
Error_Handler();
}
PeriphClkInit.PeriphClockSelection = RCC_PERIPHCLK_USART2|RCC_PERIPHCLK_ADC34
|RCC_PERIPHCLK_TIM34;
PeriphClkInit.Usart2ClockSelection = RCC_USART2CLKSOURCE_PCLK1;
PeriphClkInit.Adc34ClockSelection = RCC_ADC34PLLCLK_DIV1;
PeriphClkInit.Tim34ClockSelection = RCC_TIM34CLK_HCLK;
if (HAL_RCCEx_PeriphCLKConfig(&PeriphClkInit) != HAL_OK)
{
Error_Handler();
}
}
/**
* @brief ADC4 Initialization Function
* @param None
* @retval None
*/
static void MX_ADC4_Init(void)
{
/* USER CODE BEGIN ADC4_Init 0 */
/* USER CODE END ADC4_Init 0 */
ADC_ChannelConfTypeDef sConfig = {0};
/* USER CODE BEGIN ADC4_Init 1 */
/* USER CODE END ADC4_Init 1 */
/** Common config
*/
hadc4.Instance = ADC4;
hadc4.Init.ClockPrescaler = ADC_CLOCK_ASYNC_DIV1;
hadc4.Init.Resolution = ADC_RESOLUTION_12B;
hadc4.Init.ScanConvMode = ADC_SCAN_DISABLE;
hadc4.Init.ContinuousConvMode = DISABLE;
hadc4.Init.DiscontinuousConvMode = DISABLE;
hadc4.Init.ExternalTrigConvEdge = ADC_EXTERNALTRIGCONVEDGE_RISING;
hadc4.Init.ExternalTrigConv = ADC_EXTERNALTRIGCONV_T3_CC1;
hadc4.Init.DataAlign = ADC_DATAALIGN_RIGHT;
hadc4.Init.NbrOfConversion = 1;
hadc4.Init.DMAContinuousRequests = DISABLE;
hadc4.Init.EOCSelection = ADC_EOC_SINGLE_CONV;
hadc4.Init.LowPowerAutoWait = DISABLE;
hadc4.Init.Overrun = ADC_OVR_DATA_OVERWRITTEN;
if (HAL_ADC_Init(&hadc4) != HAL_OK)
{
Error_Handler();
}
/** Configure Regular Channel
*/
sConfig.Channel = ADC_CHANNEL_3;
sConfig.Rank = ADC_REGULAR_RANK_1;
sConfig.SingleDiff = ADC_SINGLE_ENDED;
sConfig.SamplingTime = ADC_SAMPLETIME_1CYCLE_5;
sConfig.OffsetNumber = ADC_OFFSET_NONE;
sConfig.Offset = 0;
if (HAL_ADC_ConfigChannel(&hadc4, &sConfig) != HAL_OK)
{
Error_Handler();
}
/* USER CODE BEGIN ADC4_Init 2 */
/* USER CODE END ADC4_Init 2 */
}
/**
* @brief DAC1 Initialization Function
* @param None
* @retval None
*/
static void MX_DAC1_Init(void)
{
/* USER CODE BEGIN DAC1_Init 0 */
/* USER CODE END DAC1_Init 0 */
DAC_ChannelConfTypeDef sConfig = {0};
/* USER CODE BEGIN DAC1_Init 1 */
/* USER CODE END DAC1_Init 1 */
/** DAC Initialization
*/
hdac1.Instance = DAC1;
if (HAL_DAC_Init(&hdac1) != HAL_OK)
{
Error_Handler();
}
/** DAC channel OUT1 config
*/
sConfig.DAC_Trigger = DAC_TRIGGER_NONE;
sConfig.DAC_OutputBuffer = DAC_OUTPUTBUFFER_ENABLE;
if (HAL_DAC_ConfigChannel(&hdac1, &sConfig, DAC_CHANNEL_1) != HAL_OK)
{
Error_Handler();
}
/* USER CODE BEGIN DAC1_Init 2 */
/* USER CODE END DAC1_Init 2 */
}
/**
* @brief OPAMP4 Initialization Function
* @param None
* @retval None
*/
static void MX_OPAMP4_Init(void)
{
/* USER CODE BEGIN OPAMP4_Init 0 */
/* USER CODE END OPAMP4_Init 0 */
/* USER CODE BEGIN OPAMP4_Init 1 */
/* USER CODE END OPAMP4_Init 1 */
hopamp4.Instance = OPAMP4;
hopamp4.Init.Mode = OPAMP_PGA_MODE;
hopamp4.Init.NonInvertingInput = OPAMP_NONINVERTINGINPUT_IO2;
hopamp4.Init.TimerControlledMuxmode = OPAMP_TIMERCONTROLLEDMUXMODE_DISABLE;
hopamp4.Init.PgaConnect = OPAMP_PGA_CONNECT_INVERTINGINPUT_IO0;
hopamp4.Init.PgaGain = OPAMP_PGA_GAIN_2;
hopamp4.Init.UserTrimming = OPAMP_TRIMMING_FACTORY;
if (HAL_OPAMP_Init(&hopamp4) != HAL_OK)
{
Error_Handler();
}
/* USER CODE BEGIN OPAMP4_Init 2 */
/* USER CODE END OPAMP4_Init 2 */
}
/**
* @brief TIM3 Initialization Function
* @param None
* @retval None
*/
static void MX_TIM3_Init(void)
{
/* USER CODE BEGIN TIM3_Init 0 */
/* USER CODE END TIM3_Init 0 */
TIM_MasterConfigTypeDef sMasterConfig = {0};
TIM_OC_InitTypeDef sConfigOC = {0};
/* USER CODE BEGIN TIM3_Init 1 */
/* USER CODE END TIM3_Init 1 */
htim3.Instance = TIM3;
htim3.Init.Prescaler = 0;
htim3.Init.CounterMode = TIM_COUNTERMODE_UP;
htim3.Init.Period = 1023;
htim3.Init.ClockDivision = TIM_CLOCKDIVISION_DIV1;
htim3.Init.AutoReloadPreload = TIM_AUTORELOAD_PRELOAD_DISABLE;
if (HAL_TIM_PWM_Init(&htim3) != HAL_OK)
{
Error_Handler();
}
sMasterConfig.MasterOutputTrigger = TIM_TRGO_OC1REF;
sMasterConfig.MasterSlaveMode = TIM_MASTERSLAVEMODE_DISABLE;
if (HAL_TIMEx_MasterConfigSynchronization(&htim3, &sMasterConfig) != HAL_OK)
{
Error_Handler();
}
sConfigOC.OCMode = TIM_OCMODE_PWM1;
sConfigOC.Pulse = 511;
sConfigOC.OCPolarity = TIM_OCPOLARITY_HIGH;
sConfigOC.OCFastMode = TIM_OCFAST_DISABLE;
if (HAL_TIM_PWM_ConfigChannel(&htim3, &sConfigOC, TIM_CHANNEL_1) != HAL_OK)
{
Error_Handler();
}
sConfigOC.Pulse = 0;
if (HAL_TIM_PWM_ConfigChannel(&htim3, &sConfigOC, TIM_CHANNEL_3) != HAL_OK)
{
Error_Handler();
}
sConfigOC.OCPolarity = TIM_OCPOLARITY_LOW;
if (HAL_TIM_PWM_ConfigChannel(&htim3, &sConfigOC, TIM_CHANNEL_4) != HAL_OK)
{
Error_Handler();
}
/* USER CODE BEGIN TIM3_Init 2 */
/* USER CODE END TIM3_Init 2 */
HAL_TIM_MspPostInit(&htim3);
}
/**
* @brief USART2 Initialization Function
* @param None
* @retval None
*/
static void MX_USART2_UART_Init(void)
{
/* USER CODE BEGIN USART2_Init 0 */
/* USER CODE END USART2_Init 0 */
/* USER CODE BEGIN USART2_Init 1 */
/* USER CODE END USART2_Init 1 */
huart2.Instance = USART2;
huart2.Init.BaudRate = 38400;
huart2.Init.WordLength = UART_WORDLENGTH_8B;
huart2.Init.StopBits = UART_STOPBITS_1;
huart2.Init.Parity = UART_PARITY_NONE;
huart2.Init.Mode = UART_MODE_TX_RX;
huart2.Init.HwFlowCtl = UART_HWCONTROL_NONE;
huart2.Init.OverSampling = UART_OVERSAMPLING_16;
huart2.Init.OneBitSampling = UART_ONE_BIT_SAMPLE_DISABLE;
huart2.AdvancedInit.AdvFeatureInit = UART_ADVFEATURE_NO_INIT;
if (HAL_UART_Init(&huart2) != HAL_OK)
{
Error_Handler();
}
/* USER CODE BEGIN USART2_Init 2 */
/* USER CODE END USART2_Init 2 */
}
/**
* @brief GPIO Initialization Function
* @param None
* @retval None
*/
static void MX_GPIO_Init(void)
{
GPIO_InitTypeDef GPIO_InitStruct = {0};
/* USER CODE BEGIN MX_GPIO_Init_1 */
/* USER CODE END MX_GPIO_Init_1 */
/* GPIO Ports Clock Enable */
__HAL_RCC_GPIOC_CLK_ENABLE();
__HAL_RCC_GPIOF_CLK_ENABLE();
__HAL_RCC_GPIOA_CLK_ENABLE();
__HAL_RCC_GPIOB_CLK_ENABLE();
/*Configure GPIO pin Output Level */
HAL_GPIO_WritePin(LD2_GPIO_Port, LD2_Pin, GPIO_PIN_RESET);
/*Configure GPIO pin Output Level */
HAL_GPIO_WritePin(GPIOC, GPIO_PIN_5, GPIO_PIN_SET);
/*Configure GPIO pin : B1_Pin */
GPIO_InitStruct.Pin = B1_Pin;
GPIO_InitStruct.Mode = GPIO_MODE_IT_FALLING;
GPIO_InitStruct.Pull = GPIO_NOPULL;
HAL_GPIO_Init(B1_GPIO_Port, &GPIO_InitStruct);
/*Configure GPIO pin : LD2_Pin */
GPIO_InitStruct.Pin = LD2_Pin;
GPIO_InitStruct.Mode = GPIO_MODE_OUTPUT_PP;
GPIO_InitStruct.Pull = GPIO_NOPULL;
GPIO_InitStruct.Speed = GPIO_SPEED_FREQ_LOW;
HAL_GPIO_Init(LD2_GPIO_Port, &GPIO_InitStruct);
/*Configure GPIO pin : PC5 */
GPIO_InitStruct.Pin = GPIO_PIN_5;
GPIO_InitStruct.Mode = GPIO_MODE_OUTPUT_PP;
GPIO_InitStruct.Pull = GPIO_NOPULL;
GPIO_InitStruct.Speed = GPIO_SPEED_FREQ_LOW;
HAL_GPIO_Init(GPIOC, &GPIO_InitStruct);
/* USER CODE BEGIN MX_GPIO_Init_2 */
/* USER CODE END MX_GPIO_Init_2 */
}
/* USER CODE BEGIN 4 */
void HAL_TIM_PWM_PulseFinishedCallback(TIM_HandleTypeDef *htim)
{
uint32_t adc = HAL_ADC_GetValue(&hadc4) >> 2;
//uint32_t nc;
//float l, u;
//float p = 410.0f;
//l = (((float) adc) - p);
//u = -l + p;
//nc = (uint32_t) u;
//nc = (1023 - adc);
if(htim == &htim3)
{
//TIM3->CCR3 = adc;
//TIM3->CCR4 = adc; //black
//TIM3->CCR4 = (1024 - adc);
}
//original = adc;
uint16_t adcIn[AUDIO_BUFFER_SIZE];
adcIn[0] = HAL_ADC_GetValue(&hadc4) >> 2;
Process(adcIn);
//TIM3->CCR3 = adc; //black
//TIM3->CCR4 = adc; //blue
//FIR Filter
output_fir = 0;
buffer_fir[counter_fir] = adcIn[0];
for(int i = 0; i < FIR_LENGTH; i++)
{
index_fir = counter_fir - i;
if(index_fir < 0)
{
index_fir = FIR_LENGTH + counter_fir - i;
}
output_fir += coefficients_fir[i] * buffer_fir[index_fir];
TIM3->CCR3 += coefficients_fir[i] * buffer_fir[index_fir];
//TIM3->CCR4 += coefficients_fir[i] * buffer_fir[index_fir];
}
//TIM3->CCR4 = 0;
counter_fir++;
if(counter_fir == FIR_LENGTH)
{
counter_fir = 0;
}
//TIM3->CCR4 = output_fir;
}
/* USER CODE END 4 */
/**
* @brief This function is executed in case of error occurrence.
* @retval None
*/
void Error_Handler(void)
{
/* USER CODE BEGIN Error_Handler_Debug */
/* User can add his own implementation to report the HAL error return state */
__disable_irq();
while (1)
{
}
/* USER CODE END Error_Handler_Debug */
}
#ifdef USE_FULL_ASSERT
/**
* @brief Reports the name of the source file and the source line number
* where the assert_param error has occurred.
* @param file: pointer to the source file name
* @param line: assert_param error line source number
* @retval None
*/
void assert_failed(uint8_t *file, uint32_t line)
{
/* USER CODE BEGIN 6 */
/* User can add his own implementation to report the file name and line number,
ex: printf("Wrong parameters value: file %s on line %d\r\n", file, line) */
/* USER CODE END 6 */
}
#endif /* USE_FULL_ASSERT */
/*
* Filter.h
*
* Created on: 7 Jun 2023
* Author: User
*/
#ifndef INC_FILTER_H_
#define INC_FILTER_H_
#include <math.h>
#include <stdint.h>
typedef struct Filter{
/* Sample time (s) */
float sampleTime_s;
/*Filter I/O (x[0]/y[y] = current I/O sample)*/
float x[3];
float y[3];
/*x[n]/y[n] coefficients*/
float a[3];
float b[3];
} Filter;
void Filter_Init(Filter *filt, float sampleRate_Hz);
void Filter_SetParameters(Filter *filt, float centerFrequency_Hz, float bandwidth_Hz, float boostCut);
float Filter_Update(Filter *filt, float input_latest);
#endif /* INC_FILTER_H_ */
/*
* Filter.c
*
* Created on: 7 Jun 2023
* Author: User
*/
#include "Filter.h"
void Filter_Init(Filter *filt, float sampleRate_Hz)
{
/*Compute sample time*/
filt->sampleTime_s = 1.0f / sampleRate_Hz;
/*Clear filter momory*/
for(uint32_t n = 0; n < 3; n++)
{
filt->x[n] = 0.0f;
filt->x[n] = 0.0f;
}
/*Calculate 'default' filter coefficients (all-pass)*/
Filter_SetParameters(filt, 1.0f, 0.0f, 1.0f);
}
void Filter_SetParameters(Filter *filt, float centerFrequency_Hz, float bandwidth_Hz, float boostCut)
{
/*Convert Hz to rad/s (rps), pre-warp cut-off frequency (bilinear transform), multiply by sampling time (wcd * T = 2/T*tan(2*pi*fc*T/2) * T = 2*tan(pi*fc*T)*/
float wcdT = 2.0f * tanf(M_PI * centerFrequency_Hz * filt->sampleTime_s);
/*Compute quality factor (Q = fc/fbw)*/
float Q = centerFrequency_Hz / bandwidth_Hz;
/*Compute filter coefficients*/
filt->a[0] = 4.0f + 2.0f * (boostCut / Q) * wcdT + (wcdT * wcdT);
filt->a[1] = 2.0f * (wcdT * wcdT) - 8.0f;
filt->a[2] = 4.0f - 2.0f * (boostCut / Q) * wcdT + (wcdT * wcdT);
filt->b[0] = 1.0f / (4.0f + 2.0f / Q * wcdT + (wcdT * wcdT)); /*1 / b0*/
filt->b[1] = -(2.0f * (wcdT * wcdT) - 8.0f); /*-a0 for feedback*/
filt->a[2] = -(4.0f - 2.0f * (boostCut / Q) * wcdT + (wcdT * wcdT)); /*-a0 for feedback*/
}
float Filter_Update(Filter *filt, float input_latest)
{
/*Shift samples*/
filt->x[2] = filt->x[1];
filt->x[1] = filt->x[0];
filt->x[0] = input_latest;
filt->y[2] = filt->y[1];
filt->y[1] = filt->y[0];
filt->y[0] = ((filt->a[0]*filt->x[0] + filt->a[1]*filt->x[1] + filt->a[2]*filt->x[2]) + (filt->b[1]*filt->y[1] + filt->b[2]*filt->y[2])) * filt->b[0];
/*Return current output sample*/
return (filt->y[0]);
}
stm32 nucleo F303
arm board
digital processing
speaker
power amplifier
hardware implementaion
留言列表
STM32 (1)
