File content as of revision 28:fe7747b89fb3:

/**
  ******************************************************************************
  * @file    signal_processing.c
  * @author  Brian Mazzeo
  * @date    2020
  * @brief   This file provides a set of code for signal processing in 487.
  *          Parts are taken from example code from STMIcroelectronics
  ******************************************************************************
  * @attention
  *          This code was specifically developed for BYU ECEn 487 course 
  *          Introduction to Digital Signal Processing.
  *
  *
  ******************************************************************************
  */ 

#include "mbed.h"
#include "stm32746g_discovery_lcd.h"
#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 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                         16000                   // [Hz] Sample rate
#define Omega_C                         1000                    // [Hz] Carrier frequency
#define omega_c                         2*PI*Omega_C/Omega_S    // Hilbert Transform Frequency

/* ---------------------------------------------------------------------- 
** FIR Coefficients buffer generated using fdatool MATLAB function. 
** Fpass = 100
** Fstop = 400
** Order = 255
** Type = equiripple
** Precision = Single-precision floating point
** ------------------------------------------------------------------- */ 

const float32_t LPF_coeffs[256] = {
  -3.166635361e-05,-1.870986307e-05,-2.394712101e-05,-2.990036774e-05,-3.658870264e-05,
  -4.399371028e-05,-5.209316805e-05,-6.081685933e-05,-7.011051639e-05,-7.988819561e-05,
  -9.004087042e-05,-0.000100387304,-0.000110787405,-0.0001210676273,-0.0001309535583,
  -0.0001402696507,-0.0001487127156,-0.000156007547,-0.000161856733,-0.0001659371774,
  -0.0001679307898,-0.0001674937957,-0.0001643116557,-0.0001580463286,-0.0001483879751,
  -0.0001350368111,-0.0001177328377,-9.621375648e-05,-7.031142741e-05,-3.984871728e-05,
  -4.752255791e-06,3.502548134e-05,7.943151286e-05,  0.00012836518,0.0001816200675,
  0.0002389207075,0.0002998831333,0.0003640493087,0.0004308363423,0.0004995908821,
   0.000569541764,0.0006398341502,0.0007095160545,0.0007775573758,0.0008428392466,
  0.0009041857556,0.0009603485814, 0.001010049134, 0.001051956089, 0.001084744697,
   0.001107070362, 0.001117624925, 0.001115125953, 0.001098367968, 0.001066206489,
   0.001017625327,0.0009517096914,0.0008677172009,0.0007650525076,0.0006433351082,
  0.0005023755366,0.0003422336886,0.0001632031344,-3.414340608e-05,-0.0002489731414,
  -0.0004801462928,-0.0007262464496,-0.0009855407989,-0.001256009215,-0.001535319607,
  -0.001820859499, -0.00210972107,-0.002398737939,-0.002684480743, -0.00296330289,
  -0.003231336828,-0.003484555287,-0.003718773369,-0.003929710016,-0.004113005474,
  -0.004264282528,-0.004379170015,-0.004453371279,-0.004482686054,-0.004463084508,
   -0.00439072866,-0.004262049217,-0.004073764663, -0.00382295344,  -0.0035070749,
  -0.003124034265,-0.002672198461,-0.002150455024,-0.001558220945,-0.0008954905206,
  -0.0001628377504,0.0006385485176, 0.001506871195, 0.002439704956,  0.00343400985,
   0.004486127291, 0.005591807421, 0.006746214349, 0.007943967357,  0.00917916093,
    0.01044541784,  0.01173591614,  0.01304345857,  0.01436051074,  0.01567927189,
    0.01699172705,  0.01828972809,  0.01956504397,  0.02080944739,  0.02201477997,
    0.02317302115,  0.02427636459,  0.02531728894,  0.02628861926,   0.0271836035,
     0.0279959701,  0.02871998027,  0.02935049124,  0.02988300286,  0.03031369485,
    0.03063946404,  0.03085796162,  0.03096760809,  0.03096760809,  0.03085796162,
    0.03063946404,  0.03031369485,  0.02988300286,  0.02935049124,  0.02871998027,
     0.0279959701,   0.0271836035,  0.02628861926,  0.02531728894,  0.02427636459,
    0.02317302115,  0.02201477997,  0.02080944739,  0.01956504397,  0.01828972809,
    0.01699172705,  0.01567927189,  0.01436051074,  0.01304345857,  0.01173591614,
    0.01044541784,  0.00917916093, 0.007943967357, 0.006746214349, 0.005591807421,
   0.004486127291,  0.00343400985, 0.002439704956, 0.001506871195,0.0006385485176,
  -0.0001628377504,-0.0008954905206,-0.001558220945,-0.002150455024,-0.002672198461,
  -0.003124034265,  -0.0035070749, -0.00382295344,-0.004073764663,-0.004262049217,
   -0.00439072866,-0.004463084508,-0.004482686054,-0.004453371279,-0.004379170015,
  -0.004264282528,-0.004113005474,-0.003929710016,-0.003718773369,-0.003484555287,
  -0.003231336828, -0.00296330289,-0.002684480743,-0.002398737939, -0.00210972107,
  -0.001820859499,-0.001535319607,-0.001256009215,-0.0009855407989,-0.0007262464496,
  -0.0004801462928,-0.0002489731414,-3.414340608e-05,0.0001632031344,0.0003422336886,
  0.0005023755366,0.0006433351082,0.0007650525076,0.0008677172009,0.0009517096914,
   0.001017625327, 0.001066206489, 0.001098367968, 0.001115125953, 0.001117624925,
   0.001107070362, 0.001084744697, 0.001051956089, 0.001010049134,0.0009603485814,
  0.0009041857556,0.0008428392466,0.0007775573758,0.0007095160545,0.0006398341502,
   0.000569541764,0.0004995908821,0.0004308363423,0.0003640493087,0.0002998831333,
  0.0002389207075,0.0001816200675,  0.00012836518,7.943151286e-05,3.502548134e-05,
  -4.752255791e-06,-3.984871728e-05,-7.031142741e-05,-9.621375648e-05,-0.0001177328377,
  -0.0001350368111,-0.0001483879751,-0.0001580463286,-0.0001643116557,-0.0001674937957,
  -0.0001679307898,-0.0001659371774,-0.000161856733,-0.000156007547,-0.0001487127156,
  -0.0001402696507,-0.0001309535583,-0.0001210676273,-0.000110787405,-0.000100387304,
  -9.004087042e-05,-7.988819561e-05,-7.011051639e-05,-6.081685933e-05,-5.209316805e-05,
  -4.399371028e-05,-3.658870264e-05,-2.990036774e-05,-2.394712101e-05,-1.870986307e-05,
  -3.166635361e-05
};


/* ------------------------------------------------------------------- 
 * 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];


/* 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; 

/**
  * @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);
   }
}

/**
  * @brief  Process audio channel signals
  * @param  L_channel: Pointer to Left channel data (float)
  * @param  R_channel: Pointer to Right channel data (float)
  * @param  Signal_Length: length of data to process
  * @retval None
  */
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 angle */
   float32_t phase_angle = atan2(s0_s1star_sum_imag, s0_s1star_sum_real);
   float32_t direction_angle_radians = asin((c_sound / ((float32_t) mic_distance * (float32_t) Omega_C)) * phase_angle);   
   float32_t direction_angle_degrees = direction_angle_radians * (180 / PI);

    BSP_LCD_SetTextColor(LCD_COLOR_CYAN);
    sprintf(buf, "Angle:%6.2f degrees", direction_angle_degrees);
    BSP_LCD_DisplayStringAt(0, 175, (uint8_t *) buf, LEFT_MODE);

    sprintf(buf, "PAngle:%6.2f rad", phase_angle);
    BSP_LCD_DisplayStringAt(0, 200, (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++;
   }

}