File content as of revision 29:7d3fff4ac41b:

/**
  ******************************************************************************
  * @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*500)              // [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 = 100
** Fstop = 400
** Order = 255
** Type = equiripple
** Precision = Single-precision floating point
** ------------------------------------------------------------------- */ 

const float32_t LPF_coeffs[256] = {
  -0.0007800915628,-0.0001674496889,-0.0001849002729,-0.0002029328898,-0.0002218717564,
  -0.0002413307957,-0.0002615691628,-0.0002821780508,-0.0003034212859,-0.000324951252,
  -0.0003471312812,-0.000369623187,-0.000392670423,-0.0004156443756,-0.0004385936772,
  -0.0004611753393,-0.0004842103226,-0.0005074346554,-0.0005290624686,-0.0005512404023,
  -0.0005724770599,-0.0005929443287,-0.0006127390661,-0.0006314100465,-0.0006491405657,
  -0.0006653870223,-0.000680299243,-0.0006933949771,-0.0007049014093,-0.0007143636467,
  -0.0007218476967,-0.0007267798646,-0.0007293550298,-0.0007292577648,-0.0007265052409,
  -0.0007203418063,-0.0007115086773,-0.0006989029353,-0.0006829997874,-0.0006631698925,
  -0.000639601436,-0.0006118300371,-0.0005799498758,-0.0005434799823,-0.0005025339779,
  -0.0004567068536,-0.0004061603686,-0.0003504161141,-0.0002896062215,-0.0002233447594,
  -0.0001518396166,-7.459068001e-05,8.225716556e-06,9.678629431e-05,0.0001911890868,
  0.0002915640071,0.0003978571913,0.0005103156436,0.0006287195138,0.0007533723838,
    0.00088403275,  0.00102098356, 0.001163926558, 0.001313094515, 0.001468220609,
    0.00162948342, 0.001796556171,  0.00196959055, 0.002148323692, 0.002332823817,
   0.002522682771, 0.002718108008, 0.002918580314, 0.003124272451, 0.003334705485,
   0.003549913643, 0.003769469913, 0.003993340768, 0.004221097566,  0.00445264997,
   0.004687562119, 0.004925743211, 0.005166693125, 0.005410279613,  0.00565600628,
   0.005903759971, 0.006152981427, 0.006403496955, 0.006654826459, 0.006906681694,
   0.007158623077,  0.00741035305, 0.007661380339, 0.007911451161,  0.00816002395,
   0.008406852372, 0.008651374839, 0.008893367834, 0.009132288396, 0.009367863648,
    0.00959957391,  0.00982714165,  0.01005008724,  0.01026809961,  0.01048072334,
    0.01068769302,  0.01088850573,  0.01108295284,  0.01127053425,  0.01145104971,
    0.01162407082,  0.01178936474,  0.01194655988,  0.01209541038,  0.01223561913,
    0.01236695051,  0.01248912979,  0.01260196976,  0.01270521339,  0.01279872097,
    0.01288224943,  0.01295570657,  0.01301891916,  0.01307178196,  0.01311419997,
    0.01314606518,  0.01316735335,  0.01317800116,  0.01317800116,  0.01316735335,
    0.01314606518,  0.01311419997,  0.01307178196,  0.01301891916,  0.01295570657,
    0.01288224943,  0.01279872097,  0.01270521339,  0.01260196976,  0.01248912979,
    0.01236695051,  0.01223561913,  0.01209541038,  0.01194655988,  0.01178936474,
    0.01162407082,  0.01145104971,  0.01127053425,  0.01108295284,  0.01088850573,
    0.01068769302,  0.01048072334,  0.01026809961,  0.01005008724,  0.00982714165,
    0.00959957391, 0.009367863648, 0.009132288396, 0.008893367834, 0.008651374839,
   0.008406852372,  0.00816002395, 0.007911451161, 0.007661380339,  0.00741035305,
   0.007158623077, 0.006906681694, 0.006654826459, 0.006403496955, 0.006152981427,
   0.005903759971,  0.00565600628, 0.005410279613, 0.005166693125, 0.004925743211,
   0.004687562119,  0.00445264997, 0.004221097566, 0.003993340768, 0.003769469913,
   0.003549913643, 0.003334705485, 0.003124272451, 0.002918580314, 0.002718108008,
   0.002522682771, 0.002332823817, 0.002148323692,  0.00196959055, 0.001796556171,
    0.00162948342, 0.001468220609, 0.001313094515, 0.001163926558,  0.00102098356,
    0.00088403275,0.0007533723838,0.0006287195138,0.0005103156436,0.0003978571913,
  0.0002915640071,0.0001911890868,9.678629431e-05,8.225716556e-06,-7.459068001e-05,
  -0.0001518396166,-0.0002233447594,-0.0002896062215,-0.0003504161141,-0.0004061603686,
  -0.0004567068536,-0.0005025339779,-0.0005434799823,-0.0005799498758,-0.0006118300371,
  -0.000639601436,-0.0006631698925,-0.0006829997874,-0.0006989029353,-0.0007115086773,
  -0.0007203418063,-0.0007265052409,-0.0007292577648,-0.0007293550298,-0.0007267798646,
  -0.0007218476967,-0.0007143636467,-0.0007049014093,-0.0006933949771,-0.000680299243,
  -0.0006653870223,-0.0006491405657,-0.0006314100465,-0.0006127390661,-0.0005929443287,
  -0.0005724770599,-0.0005512404023,-0.0005290624686,-0.0005074346554,-0.0004842103226,
  -0.0004611753393,-0.0004385936772,-0.0004156443756,-0.000392670423,-0.000369623187,
  -0.0003471312812,-0.000324951252,-0.0003034212859,-0.0002821780508,-0.0002615691628,
  -0.0002413307957,-0.0002218717564,-0.0002029328898,-0.0001849002729,-0.0001674496889,
  -0.0007800915628
};


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