DSPRelated.com
   Code Snippets

Fast SIMD sine and cosine

● January 31, 2011 ● Coded in Mixed C and ASM for the SHARC
// Fast sine and cosine for SHARC ADSP-21364, callable from VisualDSP++ C.
// Simultaneously compute sine and cosine of a given angle.
// Polynomial approximation developed by Flemming Pedersen of CERN.  
// This implementation by Lippold Haken of Haken Audio, March 2010, October 2012.
//
// C prototype for this function:
// typedef struct { float sin, cos; } SinCos;
// void sinCos( float fRadians, SinCos *result );
//
// 23 cycles to execute this function:
//    18 cycles for the computations
//     5 cycles for compiler environment overhead.

#include <def21364.h>

     // Data table for sinCos function.
    .section/dm/DOUBLE32 seg_dmda;
    sinCosData:
    .global sinCosData;
    .type sinCosData,STT_OBJECT;
    .var =

         // Integer 3 for "mod 4" operation.
         0x00000003, 0x00000003,

         // 1.0 for neutralizing final cos multiply.
         0x3F800000, 0x3F800000,

         // Coefficients for sin and cos polynomials.
         0xBB96BD89, // SQ7 for sin polynomial   -4.6002309092153379e-003
         0xBCA75707, // CQ6 for cos polynomial   -2.0427240364907607e-002
         0x3DA32F1D, // SQ5 for sin polynomial    7.9679708649230657e-002
         0x3E81D8BD, // CQ4 for cos polynomial    2.5360671639164339e-001
         0xBF255DDD, // SQ3 for sin polynomial   -6.4596348437163809e-001
         0xBF9DE9D1, // CQ2 for cos polynomial   -1.2336979844380824e+000
         0x3FC90FDB, // SQ1 for sin polynomial    1.5707963267948966e+000
         0x3F800000, // CQ0 for cos polynomial    1.0000000000000000e+000

         // Sign adjustment table, indexed by integer quadrant.
         0x3F800000, 0x3F800000,    //  1.0, 1.0
         0x3F800000, 0xBF800000,    //  1.0,-1.0
         0xBF800000, 0xBF800000,    // -1.0,-1.0
         0xBF800000, 0x3F800000;    // -1.0, 1.0

    sinCosData.end:

    .section/pm/DOUBLE32 seg_pmco;
    _sinCos:

    // Registers in VisualDSP environment:
        // M5,M13   = 0
        // M6,M14   = 1
        // M7,M15   = -1
        // F4       = first function argument (input angle)
        // R8       = pointer to address for storing sin,cos result
        // R0,R1,R2,R4,R8,R12,I4,I12,I13,M4 need not be preserved
        // I6,I7    = stack and frame pointers
        //            C calling routine has: "CJUMP (DB); DM(I7,M7)=R2; DM(I7,M7)=PC;"
        //            CJUMP does these operations: "R2=I6, I6=I7"
        //            Before exiting must do RFRAME: "I7=I6, I6=DM(0,I6)" 

    // Preamble.
        SF4 = F4;
        BIT SET MODE1 SIMD;

    // Pointer to constants.
        I4 = sinCosData;

    // Compute integer quadrant, fractional quadrant, fractional quadrant squared.
        // Quadrant "q" = fRadians * 2./Pi
        // Quadrant values (0. .. 4.) correspond to (0. .. 2Pi)
        R1 = 0x3F22F983;                            // F1 = 2./Pi
        F0 = F1 * F4,       I12 = DM(M7,I6);        // I12 is execution return address
        // Integer quadrant = round(q) mod 4    (0..3)
        // Since MODE1 TRUNC bit is zero, FIX implements round-to-nearest.
        R1 = FIX F0,        R2  = DM(I4,2);
        R2 = R1 AND R2,     F12 = DM(I4,2);
        // Fractional quadrant "Xq" = q - round(q)    (-.5 .. .5)
        // Fractional quadrant values (-.5 .. .5) correspond to (-Pi/4..Pi/4)
        F1 = FLOAT R1,      F4 = DM(I4,2);
        F1 = F0 - F1,       M4 = R2;
        // Set SZ if integer quadrant is even.
        R2 = LSHIFT R2 BY 31;
        // Fractional quadrant squared "Xq2" = Xq * Xq
        // Set SF1 = 1.0 to avoid final Xq multiply for cosine polynomial.
        F2 = F1 * F1,       F12 <-> SF1;

    // Registers involved in sin and cos computation:
        // F1     = fractional quadrant Xq 
        // SF1    = 1.0
        // F2,SF2 = fractional quadrant squared Xq2 
        // F4     = SQ7 
        // SF4    = CQ6 
        // M4     = integer quadrant
        // SZ     = indicates integer quadrant is even
        // DM(I4) = pairs of cosine and sine coefficients

    // Compute sine polynomial (PEx) and cosine polynomial (PEy).
        // Xq2*SQ7                          Xq2*CQ6
        F0 = F2 * F4,       F4 = DM(I4,2);
        // SQ5+Xq2*SQ7                      CQ4+Xq2*CQ6
        F0 = F0 + F4;
        // Xq2(SQ5+Xq2*SQ7)                 Xq2(CQ4+Xq2*CQ6)
        F0 = F0 * F2,       F4 = DM(I4,2);
        // SQ3+Xq2(SQ5+Xq2*SQ7)             CQ2+Xq2(CQ4+Xq2*CQ6)
        F0 = F0 + F4,       F4 = DM(I4,2);
        // Xq2(SQ3+Xq2(SQ5+Xq2*SQ7))        Xq2(CQ2+Xq2(CQ4+Xq2*CQ6))
        F0 = F0 * F2,       MODIFY(I4,M4);
        // SQ1+Xq2(SQ3+Xq2(SQ5+Xq2*SQ7)))   CQ0+Xq2(CQ2+Xq2(CQ4+Xq2*CQ6)))
        F0 = F0 + F4,       F4 = DM(M4,I4);
        // Xq(SQ1+Xq2(SQ3+Xq2(SQ5+Xq2*SQ7))))     no change on PEy
        F0 = F0 * F1,       I4 = R8;        // I4 is data return address 

    // Registers:
        // F0     = sine result so far
        // SF0    = cosine result so far
        // F4,SF4 = +/-1 for inverting sign for quadrant
        // SZ     = set for integer quadrant even

    // Invert and swap sin/cos according to integer quadrant:
    //  quadrant F0      SF0
    //  0 = 00   SIN     COS
    //  1 = 01   COS    -SIN   swap results, invert sine
    //  2 = 10  -SIN    -COS   invert signs
    //  3 = 11  -COS     SIN   swap results, invert cosine
        IF NOT SZ F0 <-> SF0;               // swap sin,cos if odd quadrant
        RFRAME;                             // stack frame management before exit
        JUMP (M14,I12) (DB), F0 = F0 * F4;  // negate sin/cos if needed
        DM(M5,I4) = F0;                     // store sin at I4, cos (SF0) at I4+1
        BIT CLR MODE1 SIMD;                 // end SIMD during JUMP (DB) stall

    ._sinCos.end:
    .global _sinCos;
    .type _sinCos,STT_FUNC;

Fractional Delay Line implementation

Gabriel Rivas ● January 27, 2011●3 comments ● Coded in C 
/****************DELAY.C*******************************/
#include "delay.h"
#include "math.h"

#define MAX_BUF_SIZE 64000

/*****************************************************************************
*       Fractional delay line implementation in C:
*
*                    ---------[d_mix]--------------------------
*                    |                                         |
*                    |                                         |
*                    |x1                                       v
*     xin ------>[+]----->[z^-M]--[interp.]----[d_fw]-------->[+]-----> yout 
*                 ^                         |  
*                 |                         |
*                 |----------[d_fb]<--------|   
*******************************************************************************/

double d_buffer[MAX_BUF_SIZE];

/*
This interface defines the delay object
*/
static struct fract_delay {
    double d_mix;       /*delay blend parameter*/
    short d_samples;	/*delay duration in samples*/
    double d_fb;	    /*feedback volume*/
    double d_fw;	    /*delay tap mix volume*/
    double n_fract;     /*fractional part of the delay*/
    double *rdPtr;      /*delay read pointer*/
    double *wrtPtr;     /*delay write pointer*/
};

static struct fract_delay del;

/*
This function is used to initialize the delay object
*/
void Delay_Init(double delay_samples,double dfb,double dfw, double dmix) {
	Delay_set_delay(delay_samples);
	Delay_set_fb(dfb);
	Delay_set_fw(dfw);
	Delay_set_mix(dmix);	
	del.wrtPtr = &d_buffer[MAX_BUF_SIZE-1];
}

/*
These functions are used as interface to the delay object,
so there's not direct access to the delay object from
external modules
*/
void Delay_set_fb(double val) {
    del.d_fb = val;
}

void Delay_set_fw(double val) {
    del.d_fw = val;
}

void Delay_set_mix(double val) {
    del.d_mix = val;
}

void Delay_set_delay(double n_delay) {
    /*Get the integer part of the delay*/
    del.d_samples = (short)floor(n_delay);

    /*gets the fractional part of the delay*/
    del.n_fract = (n_delay - del.d_samples);	
}

double Delay_get_fb(void) {
    return del.d_fb;
}

double Delay_get_fw(void) {
    return del.d_fw;
}

double Delay_get_mix(void) {
    return del.d_mix;
}

/*
This is the main delay task,
*/
double Delay_task(double xin) {
	double yout;
	double * y0; 
	double * y1;
	double x1;
	double x_est;
    
    /*Calculates current read pointer position*/
	del.rdPtr = del.wrtPtr - (short)del.d_samples;
	
	/*Wraps read pointer*/
	if (del.rdPtr < d_buffer) {
		del.rdPtr += MAX_BUF_SIZE-1;
	}
	
	/*Linear interpolation to estimate the delay + the fractional part*/
	y0 = del.rdPtr-1;
	y1 = del.rdPtr;

	if (y0 < d_buffer) {
	    y0 += MAX_BUF_SIZE-1;
	}

	x_est = (*(y0) - *(y1))*del.n_fract + *(y1);

    /*Calculate next value to store in buffer*/
    x1 = xin + x_est*del.d_fb;
    
	/*Store value in buffer*/
	*(del.wrtPtr) = x1;
  
    /*Output value calculation*/
	yout = x1*del.d_mix + x_est*del.d_fw;

    /*Increment delat write pointer*/
	del.wrtPtr++;

    /*Wraps delay write pointer*/
	if ((del.wrtPtr-&d_buffer[0]) > MAX_BUF_SIZE-1) {
		del.wrtPtr = &d_buffer[0];
	}
	return yout;
}
/***********DELAY.h*************************************/
#ifndef __DELAY_H__
#define __DELAY_H__

void Delay_Init(double delay_samples,double dfb,double dfw, double dmix);
void Delay_set_fb(double val);
void Delay_set_fw(double val);
void Delay_set_mix(double val);
void Delay_set_delay(double n_delay);
double Delay_get_fb(void);
double Delay_get_fw(void);
double Delay_get_mix(void);
double Delay_task(double xin);

#endif
/*****USAGE EXAMPLE****************************************/
void main(void) {
    double xin;
    double yout;
    Delay_Init(85.6,0.7,0.7,1);

    while(1) { 
        if (new_sample_flag()) {
            /*When there's new sample at your ADC or CODEC input*/
            /*Read the sample*/
            xin = read_sample();
            /*Apply the Delay_task function to the sample*/
            yout = Delay_task(xin);

            /*Send the output value to your ADC or codec output*/
            write_output(yout);
        }				
    }
}

TI C281x Timer Configuration

Emmanuel ● January 20, 2011 ● Coded in C for the TI F28x
**********************************************************************/
#include "DSP281x_Device.h"
/**********************************************************************
* Function: InitTimer()
*
* Description: Initialize the CPU Timer 0 at a rate of 2 Hz
**********************************************************************/
void InitTimer(void)
{	
	//CpuTimer0Regs.TIM.all = 0x0;        //Timer0 Counter Register
	
	CpuTimer0Regs.PRD.all = 0x047868C0;       //Period Register
	
	CpuTimer0Regs.TCR.all = 0x4830;       //Control Register
/*
bit 15		0:		CPU-Timer Interupt Flag
bit 14		1:		Timer Interrupt Enable
bit 13-12   0's:	Reserved
bit 11-10	10:		CPU-Timer Emulation Modes
bit 9-6		0's:	Reserved
bit 5		0:		CPU-Timer Reload bit
bit 4		0:		CPU-Timer Status Bit
bit 3-0		0's:	Reserved
*/
	
	CpuTimer0Regs.TPR.all = 0x0000;       //Prescale Register
	CpuTimer0Regs.TPRH.all = 0x0000;       //Prescale Register High
	
	
	PieCtrlRegs.PIEIER1.bit.INTx7 = 1; 		//Interruption enabled
	IER |= 0x0001;							//PIE group enabled
	
	StartCpuTimer0();//Reset of the Timer
	
}

Basic Kalman filter

● January 20, 2011●14 comments ● Coded in Scilab 
// Clear initialisation 
clf()

// Time vector
t =[0:0.5:100];
len_t = length(t);
dt = 100 / len_t;

// Real values x: position v: velocity a: acceleration
x_vrai = 0 * t;
v_vrai = 0 * t;
a_vrai = 0 * t;

// Meseared values.
x_mes = 0 * t;
v_mes = 0 * t;
a_mes = 0 * t;

// Initialisation
for i = 0.02 * len_t: 0.3 * len_t,
  a_vrai(i) = 1;
end

for i = 0.5 * len_t: 0.7 * len_t,
  a_vrai(i) = 2;
end

// State of kalman filter
etat_vrai = [ x_vrai(1);  v_vrai(1); a_vrai(1) ];

A = [ 1 dt dt*dt/2; 0 1 dt; 0 0 1];//transition matrice
for i = 2:length(t),
  etat_vrai = [ x_vrai(i-1);  v_vrai(i-1); a_vrai(i) ];
  etat_vrai = A * etat_vrai;
  x_vrai(i) = etat_vrai(1);  
  v_vrai(i) = etat_vrai(2);
end

max_brt_x = 200;
max_brt_v = 10;
max_brt_a = 0.3;

// Random noise
x_brt = grand(1, len_t, 'unf', -max_brt_x, max_brt_x);
v_brt = grand(1, len_t, 'unf', -max_brt_v, max_brt_v);
a_brt = grand(1, len_t, 'unf', -max_brt_a, max_brt_a);

// Compute meas
x_mes = x_vrai + x_brt;
v_mes = v_vrai + v_brt;
a_mes = a_vrai + a_brt;

// START 
x_est = 0 * t;
v_est = 0 * t;
a_est = 0 * t;

etat_mes = [ 0; 0; 0 ];
etat_est = [ 0; 0; 0 ];
inn = [ 0; 0; 0 ];
// Matrix of covariance of the bruit.
// independant noise -> null except for diagonal, 
// and on the diagonal == sigma^2.
// VAR(aX+b) = a^2 * VAR(X);
// VAR(X) = E[X*X] - E[X]*E[X]
R = [ max_brt_x*max_brt_x 0 0; 0 max_brt_v*max_brt_v 0; 0 0 max_brt_a*max_brt_a ];

Q = [1 0 0; 0 1 0; 0 0 1];
P = Q;
for i = 2:length(t),
  // Init : measured state
  etat_mes = [ x_mes(i);  v_mes(i); a_mes(i) ];
  
  // Innovation: measured state -  estimated state
  inn = etat_mes - etat_est;
  
  // Covariance of the innovation
  S = P + R;
  
  // Gain
  K = A * inv(S);
  
  // Estimated state
  etat_est = A * etat_est + K * inn;
  
  // Covariance of prediction error
  // C = identity
  P = A * P * A' + Q - A * P * inv(S) * P * A';
  
  // End: estimated state
  x_est(i) = etat_est(1);
  v_est(i) = etat_est(2); 
  a_est(i) = etat_est(3); 

end

// Blue : real
// Gree : noised
// Red: estimated
subplot(311)
plot(t, a_vrai, t, a_mes, t, a_est);  
subplot(312)
plot(t, v_vrai, t, v_mes, t, v_est);  
subplot(313)
plot(t, x_vrai, t, x_mes, t, x_est);

Block processing adaptative notch filter

Hernán Ordiales ● December 17, 2010 ● Coded in C++ 
bool Do(const Audio& input, const Audio& reference, Audio& output)
{
	const unsigned size = input.GetSize();
	const DataArray& in = input.GetBuffer();
	const DataArray& ref = reference.GetBuffer();
	DataArray& out = output.GetBuffer();

	const unsigned M = mConfig.GetFilterLength();
	const TData mu = mConfig.GetStep();
	const unsigned L = M-1;
	
	std::vector<TData> &wn = _filterCoef;

	TData r[M];
	for (unsigned k=0;k<size;k++)
	{
		TData acum = 0.;
		for (unsigned n=0, i=k;n<M;n++,i++)
		{
			if (i<L)
				r[n] = _oldRef[i];
			else
				r[n] = ref[i-L];
			acum += wn[n]*r[n]; // interference estimation
		}
		
		if (k<L)
			out[k] = _oldIn[k]-acum; // ECG signal estimation
		else
			out[k] = in[k-L]-acum; // ECG signal estimation
			
		for (unsigned n=0;n<M;n++) wn[n]+=mu*r[n]*out[k]; // new filter taps
	}

	for (unsigned k=0;k<L;k++)
	{
		_oldRef[k] = ref[size-L+k];
		_oldIn[k] = in[size-L+k];
	}

	return true;
}

Resampling based on FFT

Vashishth ● December 16, 2010●2 comments ● Coded in Matlab 
% FFTRESAMPLE Resample a real signal by the ratio p/q
function y = fftResample(x,p,q)

% --- take FFT of signal ---
f = fft(x);

% --- resize in the FFT domain ---
% add/remove the highest frequency components such that len(f) = len2
len1 = length(f);
len2 = round(len1*p/q);
lby2 = 1+len1/2;
if len2 < len1
  % remove some high frequency samples
  d = len1-len2;
  f = f(:);
  f(floor(lby2-(d-1)/2:lby2+(d-1)/2)) = [];
elseif len2 > len1
  % add some high frequency zero samples
  d = len2-len1;
  lby2 = floor(lby2);
  f = f(:);
  f = [f(1:lby2); zeros(d,1); f(lby2+1:end)];
end

% --- take the IFFT ---
% odd number of sample removal/addition may make the FFT of a real signal
% asymmetric and artificially introduce imaginary components - we take the
% real value of the IFFT to remove these, at the cost of not being able to
% reample complex signals
y = real(ifft(f));

2-D Ping Pong Game.

Vashishth ● December 16, 2010 ● Coded in Matlab 
%By vkc
function RunGame

    hMainWindow = figure(...
        'Color', [1 1 1],...
        'Name', 'Game Window',...
        'Units', 'pixels',...
        'Position',[100 100 800 500]);

    
    
    img = imread('ball.bmp','bmp');
    [m,n,c] = size(img);
    hBall = axes(...
        'parent', hMainWindow,...
        'color', [1 1 1],...
        'visible', 'off',...
        'units', 'pixels',...
        'position', [345, 280, n, m] );
    hBallImage = imshow( img );
    set(hBallImage, 'parent', hBall, 'visible', 'off' );
    ballSpeed = 3;
    ballDirection = 0;
    hTempBall = axes( ...
        'parent', hMainWindow,...
        'units', 'pixels',...
        'color', 'none',...
        'visible', 'off',...
        'position', get(hBall, 'position' ) );
    
    
    img = imread('paddle.bmp','bmp');
    [m,n,c] = size(img);
    
    hRightPaddle = axes(...
        'parent', hMainWindow,...
        'color', 'none',...
        'visible', 'off',...
        'units', 'pixels',...
        'position', [650 - 10, 250 - 50, n, m] );
    hRightPaddleImage = imshow( img );
    set(hRightPaddleImage, 'parent', hRightPaddle, 'visible', 'off' );
    targetY = 200;
    t = timer(  'TimerFcn', @UpdateRightPaddleAI,...
                'StartDelay', 10 );
            start(t)
    
    

    hLeftPaddle = axes(...
        'parent', hMainWindow,...
        'color', 'none',...
        'visible', 'off',...
        'units', 'pixels',...
        'position', [50, 250 - 50, n, m] );
    hLeftPaddleImage = imshow( img );
    set(hLeftPaddleImage, 'parent', hLeftPaddle, 'visible', 'off' );

    hBottomWall = axes(...
        'parent', hMainWindow,...
        'color', [1 1 1],...
        'units', 'pixels',...
        'visible', 'off',...
        'position', [0 40 700 10] );
    patch( [0 700 700 0], [0 0 10 10], 'b' );    
    
    hTopWall = axes(...
        'parent', hMainWindow,...
        'color', [1 1 1],...
        'units', 'pixels',...
        'visible', 'off',...
        'position', [0 450 700 10] );
    patch( [0 700 700 0], [0 0 10 10], 'b' );
    
    rightScore = 0;
    leftScore = 0;
    hRightScore = axes(...
        'parent', hMainWindow,...
        'color', 'none',...
        'visible', 'off',...
        'units', 'pixels',...
        'position', [650 485 50 10] );
    hLeftScore = axes(...
        'parent', hMainWindow,...
        'color', 'none',...
        'visible', 'off',...
        'units', 'pixels',...
        'position', [30 485 50 10] );
    hRightScoreText = text( 0, 0, '0', 'parent', hRightScore,...
        'visible', 'on', 'color', [1 1 1] );
    hLeftScoreText = text( 0, 0, '0', 'parent', hLeftScore,...
        'visible', 'on', 'color', [1 1 1] );
    
    
    
    hQuitButton = uicontrol(...
        'string', 'Quit',...
        'position', [325 475 50 20],...
        'Callback', @QuitButton_CallBack );
    
    hStartButton = uicontrol(...
        'string', 'Start',...
        'position', [325 240 50 20],...
        'Callback',@StartButton_CallBack );

    playing = true; 
    

    while playing == true
        

        UpdateBall()
        UpdateLeftPaddle()
        UpdateRightPaddle()

        CheckForScore()

        pause( 0.01 )
    end
    stop(t)
    close( hMainWindow )
    
    
    
    
    
    
    
    

    
    
    function UpdateBall
        
       pos = get( hBall, 'position' );
       ballX = pos(1,1);
       ballY = pos(1,2);
       
       ballDirection = NormalizeAngle( ballDirection );
      
       
       % check for collisions with the walls
       if ( ballY > 450 - 10 ) && ( ballDirection > 0 ) && ( ballDirection < 180 )
            if ( ballDirection > 90 )
                ballDirection = ballDirection + 2 * ( 180 - ballDirection );
            else
                ballDirection = ballDirection - 2 * ballDirection;
            end
       elseif ( ballY < 50 ) && ( ballDirection > 180 ) && ( ballDirection < 360 )
            if ( ballDirection > 270 )
                ballDirection = ballDirection + 2 * ( 360 - ballDirection );
            else
                ballDirection = ballDirection - 2 * ( ballDirection - 180 );
            end
       end
       
       % check for collisions with the paddles
       
       if ( ballDirection > 90 && ballDirection < 270 )
           
            leftPaddlePos = get( hLeftPaddle, 'position' );
            leftX = leftPaddlePos(1,1);
            leftY = leftPaddlePos(1,2);
          
            if(     (ballX < leftX + 10)...
                &&  (ballX > leftX + 5)...
                &&  (ballY + 10 > leftY)...
                &&  (ballY < leftY + 100)     )
                
                if ( ballDirection < 180 )
                    ballDirection = 180 - ballDirection;
                elseif( ballDirection > 180 )
                    ballDirection = 180 - ballDirection;
                end
            end
       else
            rightPaddlePos = get( hRightPaddle, 'position' );
            rightX = rightPaddlePos(1,1);
            rightY = rightPaddlePos(1,2);
            
            if(     (ballX + 10 > rightX)...
                &&  (ballX + 10 < rightX + 5)...
                &&  (ballY > rightY)...
                &&  (ballY < rightY + 100)  )
                
                if ( ballDirection < 90 )
                    ballDirection = 180 - ballDirection;
                elseif( ballDirection > 270 )
                    ballDirection = 180 - ballDirection;
                end
            end
       end
           
       MoveObject( hBall, ballSpeed, ballDirection );
        
    end

    
    function UpdateRightPaddle()

         
         speed = 5;
         pos = get( hRightPaddle, 'position' );
         rightY = pos(1,2);
         
         
         
         if( rightY + 5 < targetY - 50 && rightY < 400 - 50 )
             MoveObject( hRightPaddle, speed, 90 )
         elseif( rightY - 5 > targetY - 50 && rightY > 50 )
             MoveObject( hRightPaddle, speed, 270 )
         end
      
         pos = get( hRightPaddle, 'position' );
         rightY = pos( 1,2);
         if( rightY > 400 - 50 )
             rightY = 350;
         elseif( rightY < 50 )
             rightY = 50;
         end
         
         
        if( strcmp( get( t, 'Running' ), 'off' ) )
            start(t)
        end
    
    end

    function UpdateRightPaddleAI( ob, data )
        
        % calculate where the ball will colide.
        tempBallDirection = NormalizeAngle( ballDirection ); 
        if( tempBallDirection < 90 || tempBallDirection > 270 && ballSpeed > 0 )
           
            ballPos = get( hBall, 'position' );
            set( hTempBall, 'position', ballPos );
            ballX = ballPos(1,1);            
            while( ballX < 650 - 10  )
                
                ballPos = get( hTempBall, 'position' );
                ballX = ballPos(1,1); 
                ballY = ballPos(1,2);
                MoveObject( hTempBall, 20, tempBallDirection )

               % check for temp ball collision with walls.
               if ( ballY > 450 - 10 ) 
                   if ( tempBallDirection > 0 ) 
                       if ( tempBallDirection < 180 )    
                            tempBallDirection = 360 - tempBallDirection;
                       end
                   end
               elseif ( ballY < 60 ) 
                   if( tempBallDirection > 180 ) 
                       if( tempBallDirection < 360 )
                            tempBallDirection = 360 - tempBallDirection;
                       end
                   end
               end

%                     line( 0, 0, 'marker', '*', 'parent', hTempBall )
%                     pause( 0.0005 )
            end

            pos = get( hTempBall, 'position' );
            ballY = pos(1,2);
            targetY = ballY + ( rand * 150 ) - 75;
            
        end
    end

    function UpdateLeftPaddle()    
        scr = get( hMainWindow, 'position' );
        screenX = scr(1,1);
        screenY = scr(1,2);
        screenH = scr(1,4);
        
        mouse = get(0, 'PointerLocation' );
        y = mouse(1,2) - screenY;
        
        if( y > 100 && y < 400 )
            paddlePos = get( hLeftPaddle, 'position' ); 
            paddlePos(1,2) = y - 50; 
            set( hLeftPaddle, 'position', paddlePos );
        elseif( y > 400 )
            paddlePos = get( hLeftPaddle, 'position' ); 
            paddlePos(1,2) = 400 - 50; 
            set( hLeftPaddle, 'position', paddlePos ); 
        elseif( y < 100 )
            paddlePos = get( hLeftPaddle, 'position' ); 
            paddlePos(1,2) = 100 - 50; 
            set( hLeftPaddle, 'position', paddlePos );
        end
        
    end

    function CheckForScore()
        
       pos = get( hBall, 'position' );
       xpos = pos(1,1);
       ypos = pos(1,2);
       
       if ( xpos < 5 )
           set( hBallImage, 'visible', 'off' )
           rightScore = rightScore + 1;
           set ( hRightScoreText, 'string', num2str( rightScore ) )
           pause( .5 )
           ResetBall()
       elseif ( xpos + 10 > 695 )
           set( hBallImage, 'visible', 'off' )
           leftScore = leftScore + 1;
           set ( hLeftScoreText, 'string', num2str( leftScore ) )
           pause( .5 )
           ResetBall()
       end
       
    end
        
    function ResetBall
        
        pos = get( hBall, 'position' );
        pos(1,1) = 345;
        pos(1,2) = 255 + floor( rand*100 ) - 50;
        set( hBall, 'position', pos )
        ballSpeed = 4;
        ballDirection =  ( (rand(1) < 0.5) * 180 ) ...          % 0 or 180
                       + ( 45 + (rand(1) < 0.5) * -90 ) ...     % + 45 or - 45
                       + ( floor( rand * 40 ) - 20 );           % + -20 to 20
        set( hBallImage, 'visible', 'on' )
        pause(1)
        
    end
    

    function MoveObject( hInstance, speed, direction )

        p = get( hInstance, 'position' );

        x = p( 1, 1 );
        y = p( 1, 2 );
        
        x = x + cosd( direction ) * speed;
        y = y + sind( direction ) * speed;

        p( 1, 1 ) = x;
        p( 1, 2 ) = y;

        set( hInstance, 'position', p )

    end

    function SetObjectPosition( hObject, x, y )
       
       pos = get( hObject, 'position' );
       pos(1,1) = x;
       pos(1,2) = y;
       set( hObject, 'position', pos )
        
    end

    function a = NormalizeAngle( angle )
        
        while angle > 360
            angle = angle - 360;
        end
        
        while angle < 0
            angle = angle + 360;
        end
        a = angle;

    end

    function QuitButton_CallBack( hObject, eventData )
        playing = false;
    end

    function StartButton_CallBack( hObject, eventData )
        set( hObject, 'visible', 'off' );
        set( hLeftPaddleImage, 'visible', 'on' )
        set( hRightPaddleImage, 'visible', 'on' )
        ResetBall();
    end

end

Passive direction finding

● December 15, 2010 ● Coded in Matlab 
clc
clear all
close all

fs=20e7;
T=1/fs;

t=0:T:0.0000002;
f=10e6;
scanagle_deg=60;
step_deg=6;

theta=-deg2rad(scanagle_deg):deg2rad(step_deg):deg2rad(scanagle_deg);

for i=1:length(theta);

ant1=2*sin(2*pi*f*t);
ant2=2*sin(2*pi*f*t+theta(i));

[my,mx]=max(ant1);

sum_ant(i)=ant1(mx)+ant2(mx);
diff_ant(i)=ant1(mx)-ant2(mx);
ratio_ant(i)=diff_ant(i)/sum_ant(i);

% if diff_ant(i)==0
%     diff_ant(i)=0.1;
% end
% ratio1_ant(i)=sum_ant(i)/diff_ant(i);

end

% subplot(311)
% plot(t,ant1,t,ant2)
subplot(211)
plot(rad2deg(theta),sum_ant,rad2deg(theta),diff_ant)
subplot(212)
plot(rad2deg(theta),ratio_ant)

%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%

sim_ant1=2*sin(2*pi*f*t);
sim_ant2=2*sin(2*pi*f*t+deg2rad(30));

[smy,smx]=max(sim_ant1);

%%%%also take for same sample value of data

sim_sum_ant=sim_ant1(mx)+sim_ant2(mx);
sim_diff_ant=sim_ant1(mx)-sim_ant2(mx);
sim_ratio_ant=sim_diff_ant/sim_sum_ant

passive Sum difference method for finding bearing

● December 15, 2010 ● Coded in Matlab 
clc
clear all
close all

fs=20e7;
T=1/fs;

t=0:T:0.0000002;
f=10e6;
scanagle_deg=60; % Range covered by seeker antenna
step_deg=6;

% for ploting original arrays of sum, diff and ratio
theta=-deg2rad(scanagle_deg):deg2rad(step_deg):deg2rad(scanagle_deg);

for i=1:length(theta);

ant1=1*sin(2*pi*f*t);               %Antenna 1
ant2=1*sin(2*pi*f*t+theta(i));      %Antenna 2

[my,mx]=max(ant1);

sum_ant(i)=ant1(mx)+ant2(mx);               %Sum of antennas
diff_ant(i)=ant1(mx)-ant2(mx);              %diff of antennas
ratio_ant(i)=diff_ant(i)/sum_ant(i);        %ratio

end

% subplot(311)
% plot(t,ant1,t,ant2)
% subplot(211)
% plot(rad2deg(theta),sum_ant,rad2deg(theta),diff_ant)
% subplot(212)
% plot(rad2deg(theta),ratio_ant)

% 
% figure
% subplot(211)
% plot(rad2deg(theta),sumn_ant,rad2deg(theta),diffn_ant)
% subplot(212)
% plot(rad2deg(theta),ration_ant)

%%%%%%%%%%%%%%%%Recieved signal at antenna for DOA%%%%%%%%%%%%%%%%%%%%%%%%%%

A_ant1=2;                   % Amplitude of antenna 1
A_ant2=3;                   % Amplitude of antenna 2

DOA_angle_target=20;        % DOA of TARGET

sim_ant11=A_ant1*sin(2*pi*f*t);                         % Simulated antenna 1
sim_ant22=A_ant2*sin(2*pi*f*t+deg2rad(DOA_angle_target));   % Simulated antenna 2

sim_ant1=sim_ant11/max(sim_ant11);          %normalization
sim_ant2=sim_ant22/max(sim_ant22);

[smy,smx]=max(sim_ant1);

%%%%also take for same sample value of data

sim_sum_ant=sim_ant1(smx)+sim_ant2(smx);
sim_diff_ant=sim_ant1(smx)-sim_ant2(smx);
sim_ratio_ant=sim_diff_ant/sim_sum_ant;

%%%%%%%%%%%%% Polynomial fitting of ratio_ant of 6th order%%%%%%%%%%%%
% Error in DOA is obtained because of this
% if polynomial fitting is removed or more accurate results are obtained
% DOA of target can be found with greater accuracy
% Polynomial was obtained though curve fitting toolbox
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%

p1 =  1.047e-008;
p2 = -6.907e-007;
p3 =  2.383e-005;
p4 =  -0.0004914;
p5 =    0.008555;
p6 =    -0.09626;
p7 =      0.4215;
x=0;

for ii=1:2200
    x=x+0.01;
fitted_model_rat_ant(ii) = p1*x^6 + p2*x^5 + p3*x^4 + p4*x^3 + p5*x^2 + p6*x + p7;

end

theta_new=-scanagle_deg:.0545:scanagle_deg;    % Theta for ploting fitted model
figure
plot(theta_new(1:2200),fitted_model_rat_ant)

c1=ceil(sim_ratio_ant*7000);           % For comparison of sim_ratio ant and model
c2=ceil(fitted_model_rat_ant*7000);    % Different threshold Threshold can be chosen 
[r,c,v]= find(c2==c1);

detected_theta=(c.*0.0545)-60       %theta from curve fitting model

if(A_ant1>A_ant2)           % condition for checking which angle was correct
    correct_theta=detected_theta(1)
elseif(A_ant1<A_ant2)
    correct_theta=detected_theta(2)
else
     correct_theta=detected_theta
end

Correlation on DSP KIT TMS320C6713 Simulator

● December 15, 2010 ● Coded in C++ for the TI C67x
#include <stdio.h>
#include <math.h>

double pi=3.14159;

float x[50],y[50];
float r[100];

void correlation(float *x,float *y, int nx, int ny);

int main()
{   
   

int ii;
for(ii=0;ii<50  ;ii++)
 {
	 x[ii]=sin(2.0*pi*ii*3/50);
 	 y[ii]=sin(2.0*pi*ii*3/50);

 }

correlation(x,y,50,50);

printf("Complete.\n");

return 0;
}

void correlation(float *x,float *y, int nx, int ny)
{
	int n=50,delay=0,maxdelay=50;
	int i,j;
   double mx,my,sx,sy,sxy,denom;
  
   
   /* Calculate the mean of the two series x[], y[] */
   mx = 0;
   my = 0;   
	   for (i=0;i<n;i++) 
		   {
			  mx += x[i];
			  my += y[i];
		   }
   mx /= n;
   my /= n;

   /* Calculate the denominator */
   sx = 0;
   sy = 0;
	   for (i=0;i<n;i++) 
		   {
			  sx += (x[i] - mx) * (x[i] - mx);
			  sy += (y[i] - my) * (y[i] - my);
		   }
   denom = sqrt(sx*sy);

   /* Calculate the correlation series */
   for (delay=-maxdelay;delay<maxdelay;delay++) 
	   {
		  sxy = 0;
			  for (i=0;i<n;i++) 
				  {
					 j = i + delay;
	 if (j < 0 || j >= n)
						continue;
	 else
						sxy += (x[i] - mx) * (y[j] - my);
					 // Or should it be (?)
					/* if (j < 0 || j >= n)
						sxy += (x[i] - mx) * (-my);
					 else
						sxy += (x[i] - mx) * (y[j] - my);*/
					 
				  }
r[delay+maxdelay]= ( sxy / denom);