File content as of revision 30:debea332cdfe:

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

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