/****************************************************************
KPIT Cummins Infosystems Ltd, Pune, India. - 10-Mar-2003.
This program is distributed in the hope that it will be useful,
but WITHOUT ANY WARRANTY; without even the implied warranty of
MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.
*****************************************************************/
#include "7047.h"
void InitSH()
{
PFC.PECRL1.WORD=0;
PFC.PECRL2.WORD=0;
PFC.PEIORL.WORD=0xFFFF;//PE I/O 書き出しモード
PFC.PBIOR.WORD=0XFFFF;
PFC.PDIORL.WORD=0xFFFF;
MST.CR2.BIT._AD0=0; // この設定がないと、AD変換器が動かない
AD0.ADCR.BIT.ADST=0;
AD0.ADCR.BYTE=0;
AD0.ADCSR.BYTE=0;
AD0.ADCR.BIT.CKS=0x3;
AD0.ADCR.BIT.ADST=1; //start AD
// TODO: add hardware initialisation code here
}
#define N 81
short fir[N]={
// 次数＝80 タップ数＝81 Q15
// サンプリング＝ 22.81KHz
// 帯域端周波数 = 0.3 KHz
-126,-267,47,-174,-3,-192,-11,-233,-13,-283,
-14,-341,-14,-408,-14,-487,-14,-578,-13,-685,
-13,-812,-12,-965,-11,-1155,-10,-1397,-9,-1718,
-8,-2170,-6,-2864,-5,-4089,-3,-6903,-2,-20844,
0,20844,2,6903,3,4089,5,2864,6,2170,
8,1718,9,1397,10,1155,11,965,12,812,
13,685,13,578,14,487,14,408,14,341,
14,283,13,233,11,192,3,174,-47,267,
126
};
short data_up[2*N]={0};
//short data_low[N]={0};
short sin_up[2*N]={0};
//short sin_low[N]={0};
inline void kakunou(int j,short data3,int k)
{
data_up[k-1-j]=data_up[2*k-1-j]=data3; //下から格納する
// data_low[k-1-j]=data3; //同じ
}
inline void sin_kakunou(int j,short data3,int k)
{
sin_up[k-1-j]=sin_up[2*k-1-j]=data3;
//sin_low[k-1-j]=data3;
}
inline int macw( short *a,short *b )
{
int c;
asm("clrmac");
asm("mov.l %0,r2\n"
"mov.l %1,r3"::"m" (a),"m" (b) );
asm(".rept 81 \n" //ここのNだけは、手動で変える事
"mac.w @r2+,@r3+\n"
".endr");
asm("sts MACL,r4\n" //積和MACの下位だけr4に移す。MACHに溢れたら、どうするんや？
"mov.l r4,%0":"=m" (c));
return c; //int Cを返す
}
//-----------------
short data=0;
int imag_y_chukan;
short imag_y;
short real_y;
int output;
int data_chukan=0;
short moto_data=0;
short out=0;
int i=0;
// a1=2cos(2*PI*6000/94523)*2^14
const int sinw=8912/60 ;//Q15 a1=sinw 1KHz sampling 22.814Hz
const int const1_cosd=4941; //Q17 b0=1-cosd
//-----------------
int main(void)
{
InitSH();
i=0;
//r9 =1-cosd 2^17
//r10=new sinw 2^15
//r11=new sin(w-d) 2^15
//sinw 初期値 2^15
//sin(w-deltaw)=0:初期値
//1_cosd 定数 2^17
//initialize
asm("mov.l %0,r9"::"m" (const1_cosd) ); //r9=1-cosd 2^17
asm("mov.l %0,r10"::"m" (sinw) ); //r10=sinw
asm("mov #0,r11" ); //r11=sin(w-deltaw)=0
asm("mov.l r9, @-r15\n" //PUSH r9 [1-cosd]
"mov.l r10, @-r15\n" //PUSH r10 [sinw]
"mov.l r11, @-r15" ); //PUSH r11 [sin(w-d)]
//-----------------------------------------------------------------
while(1)
{
// PE.DRL.BIT.B12=1; //DA変換器用タイミング
if (AD0.ADCSR.BIT.ADF)
{
AD0.ADCSR.BIT.ADF=0;
AD0.ADCR.BIT.ADST=0;
data=AD0.ADDR0.WORD;
data -=0x82C0;//無入力時のAD0の値
data=data>>6;//上位１０ビットが有効
//AD変換の結果をhilbert変換器に通す---------------------------------------------------------------------
kakunou(i,data,N);
data_chukan=macw( (data_up+N-1-i),fir);
data=data_chukan>>15;
moto_data=data_up[N-1-i+(N-1)/2];//N-1-iから40ポイント前の点＝中点
moto_data = moto_data*33/50; //振幅のスケーリング。 * 0.65と小数点にすると、極端に遅くなるので注意！
//-----------------------------------------------------------------
asm("mov.l @r15+,r11"); //POP r11 [sin(w-d)]
asm("mov.l @r15+,r10"); //POP r10 [sinw]
asm("mov.l @r15+,r9"); //POP r9 [1-cosd]
asm("mov r9,r1"); //r1=r9=1-cosd r9 保存される
asm("mov r10,r0"); //r0=r10=sinw
asm("muls r1,r0"); //(1-cosd)*sinw --to MACL 2^32
asm("shll16 r0"); // r0=r0*2^16 sinw 2^31
asm("shll r0"); // r0=r0*2 sinw 2^32
asm("sts MACL,r1"); //積をr1に移す r1=(1-cosd)*sinw
//r0=sinw(2^32)
//--------------------------------------------------------------------
asm( "sub r1,r0"); //r0=sinw-(1-cosd)*sinw=cosd*sinw
asm("shlr16 r0"); //上の結果をシフト 2^32--2^16=2^16 = 2*(cosd*sinw)
2^15
asm("exts.w r0,r1"); //r1=r0この結果を、符号拡張してr1に保存
asm("sub r11,r1"); //2cosd*sinw-sin(w-deltaw)
asm("mov r10,r11"); //r11=new sin(w-deltaw)
asm("mov r1,r10" ); //r10=new sinw
asm("mov.l r9,@-r15"); //PUSH r9 [1-cos]
asm("mov.l r10,@-r15");//PUSH r10 [sinw]
asm("mov.l r11, @-r15");//PUSH r11 [sin(w-d)]
asm("mov.w r10,%0":"=m" (out) );
sin_kakunou(i,out,N);
//作成したsin波を、hilbert変換器に通し、虚部を求める
imag_y_chukan=macw( (sin_up+N-1-i),fir);
imag_y=imag_y_chukan>>15;
real_y=sin_up[N-1-i+(N-1)/2];//N-1-iから40ポイント前の点＝中点
real_y=real_y>>1; //レベル合わせ
//実部、虚部同士の積を計算する
output=moto_data*real_y - data*imag_y;
output +=150000;
output =output>>9;
//合成したsin波と、hilbeｒｔ変換器による虚部の確認用
/* if(i%2) output=real_y;
else output=imag_y;
output =(output+600)/2; */
//元データとhilbert変換器による虚部の確認用
/* if(i%2) output=moto_data;
else output=data;
output =(output+500);*/
//出力する
PE.DRL.WORD =output;
//後処理
i++;
if( i==N) i=0;
AD0.ADCR.BIT.ADST=1;
}
// PE.DRL.BIT.B12=0; //DA変換器用タイミング
}
return 1;
}