R&J
lab 1 code
Dependencies: CMSIS-DSP_for_STM32F746G BSP_DISCO_F746NG
Diff: signal_processing.cpp
- Revision:
- 31:5a0235b66851
- Parent:
- 30:debea332cdfe
- Child:
- 34:5bf89ab5e247
--- a/signal_processing.cpp Fri Jan 03 20:15:03 2020 +0000
+++ b/signal_processing.cpp Fri Jan 03 21:00:56 2020 +0000
@@ -19,151 +19,11 @@
#include "arm_math.h"
-/* ----------------------------------------------------------------------
-** Defines for signal processing
-** ------------------------------------------------------------------- */
-
-#define AUDIO_BLOCK_SAMPLES ((uint32_t)128) // Number of samples (L and R) in audio block (each samples is 16 bits)
-#define NUM_TAPS 64
-#define CYCLE_AVERAGE_NUM 50
-
-
-#define mic_distance 0.0211 // [m] Distance between microphones on ST32F746G
-#define c_sound 344 // [m/s] Speed of sound at 71 deg Fahrenheit
-#define Omega_S (2*PI*16000) // [rad Hz] Sample rate
-#define Omega_C (2*PI*2000) // [rad Hz] Carrier frequency
-#define omega_c 2*PI*Omega_C/Omega_S // Hilbert Transform Frequency
-
-/* ----------------------------------------------------------------------
-** FIR Coefficients buffer generated using fdatool MATLAB function.
-** Fpass = 10
-** Fstop = 100
-** Order = 255
-** Type = equiripple
-** Precision = Single-precision floating point
-** ------------------------------------------------------------------- */
-
-const float32_t LPF_coeffs[256] = {
- 0.007104042452,0.0006973274867,0.0007323734462,0.0007667868049,0.0008037738735,
- 0.0008398960927,0.0008782913792,0.0009155147709,0.0009549160604,0.0009931699606,
- 0.001033760724, 0.001073651598, 0.001116432832, 0.001159087871, 0.001204611151,
- 0.001248990768, 0.001294256072, 0.001336314948, 0.001380768954, 0.00143639138,
- 0.001479929546, 0.001529163332, 0.001579547534, 0.001628990169, 0.001680716057,
- 0.001731572906, 0.001784725231, 0.001837128773, 0.001891752472, 0.001945362659,
- 0.002000829903, 0.002055054298, 0.002111451235, 0.002167258877, 0.002225656994,
- 0.00228298991, 0.002341142856, 0.002397627104, 0.002459470183, 0.002516909502,
- 0.002577944659, 0.002637568163, 0.002699105535, 0.002759578172, 0.002821756992,
- 0.002882712055, 0.002945428248, 0.003007095773, 0.003070619656, 0.00313297566,
- 0.003196678124, 0.003258993849, 0.003322744276, 0.003385637654, 0.003450219054,
- 0.003513017204, 0.003576596035, 0.003640421201, 0.003704034258, 0.003767020069,
- 0.003830909962, 0.00389353768, 0.00395733444, 0.004019823391, 0.004083196633,
- 0.004145130981, 0.004207855556, 0.004269362427, 0.004331791773, 0.004392801784,
- 0.004454457201, 0.004514472093, 0.004575279541, 0.004634896293, 0.004694812,
- 0.004752826411, 0.004812198691, 0.004869329743, 0.004927231465, 0.004983349238,
- 0.005039713811, 0.00509452261, 0.005149625242, 0.005203116219, 0.005256726872,
- 0.005308494903, 0.005360371433, 0.005410598125, 0.005460814573, 0.00550937932,
- 0.0055576046, 0.005604189355, 0.005650800653, 0.005695586558, 0.005739701446,
- 0.005782630295, 0.005824764259, 0.00586552918, 0.005905705504, 0.005944130477,
- 0.005981951021, 0.00601811707, 0.006053663325, 0.00608762214, 0.006120662671,
- 0.006152128801, 0.006182731595, 0.006211698055, 0.00623987196, 0.006266170647,
- 0.006291479338, 0.006315360311, 0.006338228006, 0.006359170191, 0.006379307248,
- 0.00639748713, 0.006414768752, 0.006430387963, 0.00644480763, 0.006457572803,
- 0.006469102111, 0.006479041651, 0.006487841718, 0.006494853646, 0.006500702817,
- 0.00650490867, 0.00650779251, 0.006509374361, 0.006509374361, 0.00650779251,
- 0.00650490867, 0.006500702817, 0.006494853646, 0.006487841718, 0.006479041651,
- 0.006469102111, 0.006457572803, 0.00644480763, 0.006430387963, 0.006414768752,
- 0.00639748713, 0.006379307248, 0.006359170191, 0.006338228006, 0.006315360311,
- 0.006291479338, 0.006266170647, 0.00623987196, 0.006211698055, 0.006182731595,
- 0.006152128801, 0.006120662671, 0.00608762214, 0.006053663325, 0.00601811707,
- 0.005981951021, 0.005944130477, 0.005905705504, 0.00586552918, 0.005824764259,
- 0.005782630295, 0.005739701446, 0.005695586558, 0.005650800653, 0.005604189355,
- 0.0055576046, 0.00550937932, 0.005460814573, 0.005410598125, 0.005360371433,
- 0.005308494903, 0.005256726872, 0.005203116219, 0.005149625242, 0.00509452261,
- 0.005039713811, 0.004983349238, 0.004927231465, 0.004869329743, 0.004812198691,
- 0.004752826411, 0.004694812, 0.004634896293, 0.004575279541, 0.004514472093,
- 0.004454457201, 0.004392801784, 0.004331791773, 0.004269362427, 0.004207855556,
- 0.004145130981, 0.004083196633, 0.004019823391, 0.00395733444, 0.00389353768,
- 0.003830909962, 0.003767020069, 0.003704034258, 0.003640421201, 0.003576596035,
- 0.003513017204, 0.003450219054, 0.003385637654, 0.003322744276, 0.003258993849,
- 0.003196678124, 0.00313297566, 0.003070619656, 0.003007095773, 0.002945428248,
- 0.002882712055, 0.002821756992, 0.002759578172, 0.002699105535, 0.002637568163,
- 0.002577944659, 0.002516909502, 0.002459470183, 0.002397627104, 0.002341142856,
- 0.00228298991, 0.002225656994, 0.002167258877, 0.002111451235, 0.002055054298,
- 0.002000829903, 0.001945362659, 0.001891752472, 0.001837128773, 0.001784725231,
- 0.001731572906, 0.001680716057, 0.001628990169, 0.001579547534, 0.001529163332,
- 0.001479929546, 0.00143639138, 0.001380768954, 0.001336314948, 0.001294256072,
- 0.001248990768, 0.001204611151, 0.001159087871, 0.001116432832, 0.001073651598,
- 0.001033760724,0.0009931699606,0.0009549160604,0.0009155147709,0.0008782913792,
- 0.0008398960927,0.0008037738735,0.0007667868049,0.0007323734462,0.0006973274867,
- 0.007104042452
-};
-
-
-
-/* -------------------------------------------------------------------
- * Declare State buffer of size (numTaps + blockSize - 1)
- * ------------------------------------------------------------------- */
-
-static float32_t S_0i_FIRstate[AUDIO_BLOCK_SAMPLES + NUM_TAPS - 1];
-static float32_t S_0q_FIRstate[AUDIO_BLOCK_SAMPLES + NUM_TAPS - 1];
-static float32_t S_1i_FIRstate[AUDIO_BLOCK_SAMPLES + NUM_TAPS - 1];
-static float32_t S_1q_FIRstate[AUDIO_BLOCK_SAMPLES + NUM_TAPS - 1];
-
-/* Need to have multiplies pre-computed */
-static float32_t i_phase_mult_array[AUDIO_BLOCK_SAMPLES];
-static float32_t q_phase_mult_array[AUDIO_BLOCK_SAMPLES];
-
-static float32_t x_0i[AUDIO_BLOCK_SAMPLES];
-static float32_t x_0q[AUDIO_BLOCK_SAMPLES];
-static float32_t x_1i[AUDIO_BLOCK_SAMPLES];
-static float32_t x_1q[AUDIO_BLOCK_SAMPLES];
-
-static float32_t s_0i[AUDIO_BLOCK_SAMPLES];
-static float32_t s_0q[AUDIO_BLOCK_SAMPLES];
-static float32_t s_1i[AUDIO_BLOCK_SAMPLES];
-static float32_t s_1q[AUDIO_BLOCK_SAMPLES];
-
-static float32_t s0_s1star_real[AUDIO_BLOCK_SAMPLES];
-static float32_t s0_s1star_imag[AUDIO_BLOCK_SAMPLES];
-
-static float32_t cycle_results[CYCLE_AVERAGE_NUM];
-uint32_t cycle_count;
-
-/* Important to have the structure outside of the execution so it can be initialized */
-arm_fir_instance_f32 S_0i;
-arm_fir_instance_f32 S_0q;
-arm_fir_instance_f32 S_1i;
-arm_fir_instance_f32 S_1q;
-
-
-/* FUNCTION DEFINITIONS BELOW */
-
/**
* @brief Initialize filter structures to be used in loops later
* @retval None
*/
void initalize_signal_processing(void) {
-
- /* Call FIR init function to initialize the instance structure. */
- //arm_fir_init_f32(&S, NUM_TAPS, (float32_t *)&firCoeffs32[0], &firStateF32[0], AUDIO_BLOCK_SAMPLES);
- arm_fir_init_f32(&S_0i, NUM_TAPS, (float32_t *) &LPF_coeffs[0], &S_0i_FIRstate[0], AUDIO_BLOCK_SAMPLES);
- arm_fir_init_f32(&S_0q, NUM_TAPS, (float32_t *) &LPF_coeffs[0], &S_0q_FIRstate[0], AUDIO_BLOCK_SAMPLES);
- arm_fir_init_f32(&S_1i, NUM_TAPS, (float32_t *) &LPF_coeffs[0], &S_1i_FIRstate[0], AUDIO_BLOCK_SAMPLES);
- arm_fir_init_f32(&S_1q, NUM_TAPS, (float32_t *) &LPF_coeffs[0], &S_1q_FIRstate[0], AUDIO_BLOCK_SAMPLES);
-
- uint32_t i;
- for (i=0; i<AUDIO_BLOCK_SAMPLES; i++)
- {
- i_phase_mult_array[i] = cos(omega_c * i);
- q_phase_mult_array[i] = -sin(omega_c * i);
- }
-
- for (i=0; i<CYCLE_AVERAGE_NUM; i++)
- {
- cycle_results[i] = 0;
- }
- cycle_count = 0;
-
}
/**
@@ -175,100 +35,4 @@
*/
void process_audio_channel_signals(float* L_channel, float* R_channel, uint16_t Signal_Length)
{
- char buf[40];
- /*
- BSP_LCD_SetTextColor(LCD_COLOR_CYAN);
- sprintf(buf, "Processing Signals" );
- BSP_LCD_DisplayStringAt(0, 150, (uint8_t *) buf, LEFT_MODE);
- */
-
- uint32_t i;
-
- /* Need these addresses */
- float32_t* L_chan_mem_address = L_channel;
- float32_t* R_chan_mem_address = R_channel;
- float32_t L_audio_value;
- float32_t R_audio_value;
-
- for (i=0; i<Signal_Length; i++)
- {
- L_audio_value = *L_chan_mem_address;
- x_0i[i] = L_audio_value * i_phase_mult_array[i];
- x_0q[i] = L_audio_value * q_phase_mult_array[i];
-
- R_audio_value = *R_chan_mem_address;
- x_1i[i] = R_audio_value * i_phase_mult_array[i];
- x_1q[i] = R_audio_value * q_phase_mult_array[i];
-
- L_chan_mem_address++;
- R_chan_mem_address++;
- }
-
- /* Calls the FIR filters and proceeses the audio block */
- arm_fir_f32(&S_0i, &x_0i[0], &s_0i[0], AUDIO_BLOCK_SAMPLES);
- arm_fir_f32(&S_0q, &x_0q[0], &s_0q[0], AUDIO_BLOCK_SAMPLES);
- arm_fir_f32(&S_1i, &x_1i[0], &s_1i[0], AUDIO_BLOCK_SAMPLES);
- arm_fir_f32(&S_1q, &x_1q[0], &s_1q[0], AUDIO_BLOCK_SAMPLES);
-
-
- float32_t s0_s1star_sum_real = 0;
- float32_t s0_s1star_sum_imag = 0;
-
- /* Complex Multiply and Summation*/
- for (i=0; i<Signal_Length; i++)
- {
- s0_s1star_real[i] = s_0i[i] * s_1i[i] + s_0q[i] * s_1q[i];
- s0_s1star_imag[i] = s_0q[i] * s_1i[i] - s_0i[i] * s_1q[i];
-
- s0_s1star_sum_real += s0_s1star_real[i];
- s0_s1star_sum_imag += s0_s1star_imag[i];
-
- }
-
- /* Compute phase angle */
- float32_t phase_angle = atan2(s0_s1star_sum_imag, s0_s1star_sum_real);
-
- /* The following computes a running average - to reduce noise in the phase */
- cycle_results[cycle_count] = phase_angle;
- float32_t current_sum = 0;
- for (i=0; i<CYCLE_AVERAGE_NUM; i++)
- {
- current_sum += cycle_results[i];
- }
- phase_angle = current_sum / CYCLE_AVERAGE_NUM;
-
- /* Computes the actual DOA angle */
- float32_t delta_t = phase_angle / Omega_C;
- float32_t direction_angle_radians = asin(c_sound * delta_t / mic_distance);
- float32_t direction_angle_degrees = direction_angle_radians * (180 / PI);
-
- BSP_LCD_SetTextColor(LCD_COLOR_CYAN);
- sprintf(buf, "DOA[deg]:%6.2f", direction_angle_degrees);
- BSP_LCD_DisplayStringAt(0, 150, (uint8_t *) buf, LEFT_MODE);
-
- /*
- sprintf(buf, "PAngle:%6.2f rad", phase_angle);
- BSP_LCD_DisplayStringAt(0, 200, (uint8_t *) buf, LEFT_MODE);
-
- BSP_LCD_SetTextColor(LCD_COLOR_CYAN);
- sprintf(buf, "Delta_t:%9.6f seconds", delta_t);
- BSP_LCD_DisplayStringAt(0, 225, (uint8_t *) buf, LEFT_MODE);
- */
-
- L_chan_mem_address = L_channel;
- R_chan_mem_address = R_channel;
- for (i=0; i<Signal_Length; i++)
- {
- //L_audio_value = *L_chan_mem_address;
- L_audio_value = *L_chan_mem_address;
- *L_chan_mem_address = L_audio_value;
- L_chan_mem_address++;
-
- R_audio_value = *R_chan_mem_address;
- *R_chan_mem_address = R_audio_value;
- R_chan_mem_address++;
- }
-
- cycle_count++;
- if (cycle_count == CYCLE_AVERAGE_NUM) {cycle_count = 0;}
}