dmac2isr.c

信号fm调制与解调的dsp实现

#include <codec.h>
#include <firlab.h>
#include "math.h"


void Fmdemodulate(s16 *inbufpd,s16 *outbufpd,s16 lengh);
void phasedetect(s16 *inbufp,s16*outbufp,s16 lengh);

void Fmmodulate(s16 *inbuf,s16 *outbuf, s16 lengh);

extern	unsigned int channel;      /*IN: DMA channel number              */
extern	unsigned int dmsefc;       /*IN: Value to set sync & fr reg      */
extern	unsigned int dmmcr;        /*IN: Value to set mode control reg   */
extern	unsigned int dmctr;	   	  /*IN: Value to set element count reg  */
extern	unsigned int src_page;     /*IN: Value to set source page reg    */
extern	unsigned int src_addr;     /*IN: Value to set source addr reg    */
extern	unsigned int dst_page;     /*IN: Value to set dest page reg      */
extern	unsigned int dst_addr;     /*IN: Value to set dest addr reg      */
s16 LENGH=1024;
s16 outbuf[1024];
s16 carcos[1024];
s16 carsin[1024];
int inbufI[1024];
int inbufQ[1024];
int Ilpfout[1024];
int Qlpfout[1024];
int outbuffp[1024];
s16 outbufr[1024];

extern int buffer[0x1400];
extern int out_buffer[0x500];//
extern int lpfh[82];
extern int delaybuff[82]; 
extern int frame; 
extern int flag;
extern int currbuff;
extern int *delayptr1;

void	init_dma3(void)
{
	    	while(DMPREC&0x0008) {};	//DMA3的传送是否结束
	    
	    	/* Initialize DMA channel 3 */

			dmsefc = ((DSYNC_REVT1 <<12));
    		//dmmcr=((AUTOINIT_ENABLE << 15) | (DINM_ENABLE << 14) | (IMOD_HALFBLOCK <<13) | (CTMOD_DEC <<12) | (INDEXMODE_NOMOD << 8) | (SPACE_DATA << 6) | (INDEXMODE_INC << 2) | (SPACE_DATA));
    		dmmcr = 0x4141;			//AUTOINIT=0,DINM=1,IMOD=0,CTMOD=0,SIND=0x001;DMS=0x01,DIND=0x000,DMD=0x01
    		dmctr = 0x3FF;			//256 element/frame
    		src_addr = (unsigned int) &buffer+0xc00+((unsigned int)currbuff*0x400);
    		dst_addr = DXR1_ADDR(HANDSET_CODEC);

    		dma_init(DMA_CH3, dmsefc, dmmcr, dmctr, SPACE_DATA, src_addr, SPACE_DATA, dst_addr);

		/* Set number of frames for channel 3 */
    
    		DMA_FRAMECOUNT(DMA_CH3, 0);		//2 frame/block
    		
    		/* Enable channel 3 */

    		DMA_ENABLE(DMA_CH3);
}

interrupt void DMAC2ISR(void)
{
	int *inp_buffer,*out_buffer; 
	int i;

	frame++;
	
	switch(frame)
	{
	  case 3:
	  //init_dma3();
	  case 2:
		init_dma3();
	    	break;
	  case 1:
	    if (flag)	//不是最早数据帧
	    	init_dma3();
	    
	    else		//是最早数据帧
	    	flag=0xaa;
	    break;
	  
	  default:
	    break;
	}
	
	inp_buffer=buffer+(frame-1)*0x400;
	
	if (frame==3)	frame=0;
	
	out_buffer=buffer+0xc00+currbuff*0x400;
	LENGH=1024;
	Fmmodulate(inp_buffer, outbuf, LENGH);
	phasedetect(outbuf,outbufr,LENGH);
	Fmdemodulate(outbufr,outbuffp,LENGH);
	
	
	for(i=0;i<1024;i++)
	{ 
	  out_buffer[i]=100*outbuffp[i];
	  
	}
	
	//fir(inp_buffer,lpfh,out_buffer,&delayptr1,8,256);
	

		
	
	currbuff^=1;
    i=122;
}



void Fmmodulate(s16 *inbuf,s16 *outbuf, s16 lengh)
 {  
    s16 i;
    
    f32 kf=1;
    f32 finbuf[2];
    f32 foutbuf;
    f32 temp1;

    for(i=0;i<lengh;i++)
    {
    
 
      if (i==0)
      {temp1=kf*(((float)inbuf[i])/32768.0)/2;}
      else
      {
      finbuf[0]=((float)inbuf[i])/32768.0;
      finbuf[1]=((float)inbuf[i-1])/32768.0;
      temp1=temp1+kf*(finbuf[0]+finbuf[1])/2;
      }
  
      foutbuf=sin(2*3.1415926*1000*i/16000+temp1);
      
      outbuf[i]=(s16)(foutbuf*32768.0); 
    }
 }
void phasedetect(s16 *inbufp,s16*outbufp,s16 lengh)
{
   s16 i;
   float temp;
   temp=(8*((float)inbufp[1])/32768.0-((float)inbufp[2])/32768.0)/12;
   outbuf[0]=(s16)((temp-2*3.1415926*4000)*32768);
   temp=(8*((float)inbufp[1])/32768.0-((float)inbufp[2])/32768.0-8*((float)inbufp[0])/32768.0)/12;
   outbufp[1]=(s16)((temp-2*3.1415926*4000)*32768);
   for(i=2;i<lengh;i++)
   {
    temp=(((float)inbufp[i-2])/32768.0+8*((float)inbufp[i+1])/32768.0-((float)inbufp[i+2])/32768.0-8*((float)inbufp[i-1])/32768.0)/12;
    outbufp[i]=(s16)((temp)*32768);
     
   }
}
void Fmdemodulate(s16 *inbufpd,s16 *outbufpd,s16 lengh)
{   
    s16 i;
      
    f32 loutbuf;
    for (i=0;i<lengh;i++)
    {
      inbufI[i]=(((((float)inbufpd[i])/32768.0)*(((float)carcos[i])/32768.0))*32768);
      //inbufQ[i]=(((((float)inbufpd[i])/32768.0)*(((float)carsin[i])/32768.0))*32768); 
    }
    
    fir(inbufI,lpfh,Ilpfout,&delayptr1,82,1024);
    //fir(inbufQ,lpfh,Qlpfout,&delayptr1Q,82,1024);
    for (i=0;i<lengh;i++)
    {
      loutbuf=(float)(Ilpfout[i]/32768.0)*(float)(Ilpfout[i]/32768.0);//+(float)(Qlpfout[i]/32768.0)*(float)(Qlpfout[i]/32768.0);
      outbufpd[i]=(s16)((sqrt(loutbuf)-0.19)*32768);
    }
    
    
}