/****************************************************************
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.405KHz
// 帯域端周波数 = 0.3 KHz
-148,-221,39,-169,-4,-185,-15,-223,-18,-272,
-20,-329,-20,-396,-20,-475,-20,-566,-19,-673,
-19,-801,-18,-955,-16,-1146,-15,-1389,-13,-1711,
-11,-2164,-9,-2859,-7,-4085,-5,-6901,-2,-20843,
0,20843,2,6901,5,4085,7,2859,9,2164,
11,1711,13,1389,15,1146,16,955,18,801,
19,673,19,566,20,475,20,396,20,329,
20,272,18,223,15,185,4,169,-39,221,
148
};
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;
//************************************************************
const int sinw=9069/120 ;//Q15 a1=sinw 1KHz sampling 22.405Hz
const int const1_cosd=5120; //Q17 b0=1-cosd
//************************************************************
int main(void)
{
InitSH();
i=0;
//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>>14;
moto_data=data_up[N-1-i+(N-1)/2];//N-1-iから40ポイント前の点＝中点
moto_data = moto_data*66/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*3 - data*imag_y;
output +=200000;
output =output>>9;
//****************************************
//合成したsin波と、hilbeｒｔ変換器による虚部の確認用
/*if(i%2) output=real_y;
else output=imag_y;
output =(output+600); */
//元データと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;
}