dsp.cpp

QAM module to use in Java with an easy interface and powerful performance

//---------------------------------------------------------------------------


#pragma hdrstop

#include "DSP.h"
#include "Settings.h"
//---------------------------------------------------------------------------

#pragma package(smart_init)

//---------------------------------------------------------------------------


__int64 GetMachineCycleCount()
{
        __int64 cycles;
        _asm rdtsc; // won't work on 486 or below - only pentium or above

        _asm lea ebx,cycles;
        _asm mov [ebx],eax;
        _asm mov [ebx+4],edx;
        return cycles;
}


__fastcall WaveTable::WaveTable()
{
        WTstep=((double)gSettings.DemodMajor.Freq)*WTSIZE/(gSettings.Common.Samplerate);
        WTptr=0;
        FractionOfSampleItPassesBy=0.0;
}

WaveTable::WaveTable(int freq)
{
        WTstep=((double)freq)*WTSIZE/(gSettings.Common.Samplerate);
        WTptr=0;
        FractionOfSampleItPassesBy=0.0;
}



WaveTable::~WaveTable()
{

}

double __fastcall WaveTable::DistancebetweenWT(double WTptr1, double WTptr2)
{
        double dif1=WTptr1-WTptr2;
        double dif2=WTptr2-WTptr1;
        if(dif1<0)dif1+=WTSIZE;
        if(dif2<0)dif2+=WTSIZE;
        double dif=dif1;
        if(dif2<dif1)dif=-dif2;
        dif=dif/WTSIZE;
        return dif;
}

void __fastcall WaveTable::WTnextFrame()
{
        WTptr+=WTstep;
	if(((int)WTptr)>=WTSIZE) WTptr-=WTSIZE;
}

double __fastcall  WaveTable::WTSinValue()
{
        return gSettings.Trig.SinWT[(int)WTptr];
}

double __fastcall  WaveTable::WTSinValue(double PlusFractOfSample)
{
        double ts=WTptr;
        ts+=PlusFractOfSample*WTstep;
        if(((int)ts)>=WTSIZE) ts-=WTSIZE;
        return gSettings.Trig.SinWT[(int)(ts)];
}

double __fastcall WaveTable::WTCosValue()
{
        return gSettings.Trig.CosWT[(int)WTptr];
}

double __fastcall  WaveTable::WTCosValue(double PlusFractOfSample)
{
        double ts=WTptr;
        ts+=PlusFractOfSample*WTstep;
        if(((int)ts)>=WTSIZE) ts-=WTSIZE;
        return gSettings.Trig.CosWT[(int)(ts)];
}

void __fastcall WaveTable::WTsetFreq(int freq)
{
        WTstep=((double)freq)*WTSIZE/(gSettings.Common.Samplerate);
        WTptr=0.0;
}

void __fastcall WaveTable::SetFreq(double freq)
{
        WTstep=(freq)*((double)WTSIZE)/(gSettings.Common.Samplerate);
        WTptr=0.0;
}

double WaveTable::GetFreqTest()
{
        return WTstep;
}

bool __fastcall WaveTable::IfPassesPointNextTime()
{
        FractionOfSampleItPassesBy=WTptr+(WTstep-WTSIZE);
        if(FractionOfSampleItPassesBy<0)return false;
        FractionOfSampleItPassesBy/=WTstep;
        return true;
}

bool __fastcall WaveTable::IfPassesPointNextTime(double FractionOfWave)
{
        FractionOfSampleItPassesBy=(FractionOfWave*WTSIZE)-WTptr;
        if(FractionOfSampleItPassesBy<0)FractionOfSampleItPassesBy+=WTSIZE;
        if(FractionOfSampleItPassesBy<WTstep)
        {
                FractionOfSampleItPassesBy=(WTstep-FractionOfSampleItPassesBy)/WTstep;
                return true;
        }
        return false;
}

bool __fastcall WaveTable::IfPassesPointNextTime_frames(double NumberOfFrames)
{
        FractionOfSampleItPassesBy=NumberOfFrames-WTptr;
        if(FractionOfSampleItPassesBy<0)FractionOfSampleItPassesBy+=WTSIZE;
        if(FractionOfSampleItPassesBy<WTstep)
        {
                FractionOfSampleItPassesBy=(WTstep-FractionOfSampleItPassesBy)/WTstep;
                return true;
        }
        return false;
}

void __fastcall WaveTable::SetWTptr(double FractionOfWave,double PlusNumberOfFrames)
{
        while(FractionOfWave>=1)FractionOfWave-=1;
        while(FractionOfWave<0)FractionOfWave+=1;
        WTptr=(FractionOfWave*WTSIZE);
        WTptr+=PlusNumberOfFrames*WTstep;
        while(((int)WTptr)>=WTSIZE)WTptr-=WTSIZE;
        while(((int)WTptr)<0)WTptr+=WTSIZE;
}


FIR::FIR(int _NumberOfPoints)
{
        int i;
        points=0;buff=0;

        NumberOfPoints=_NumberOfPoints;
        buffsize=NumberOfPoints+1;
        points=new double[NumberOfPoints];
        for(i=0;i<NumberOfPoints;i++)points[i]=0;
        buff=new double[buffsize];
        for(i=0;i<buffsize;i++)buff[i]=0;
        ptr=0;
        outsum=0;
}

FIR::~FIR()
{
        if(points)delete [] points;
        if(buff)delete [] buff;
}

double __fastcall FIR::FIRUpdateAndProcess(double sig)
{
        buff[ptr]=sig;
        ptr++;ptr%=buffsize;
        int tptr=ptr;
        outsum=0;
        for(int i=0;i<NumberOfPoints;i++)
        {
                outsum+=points[i]*buff[tptr];
                tptr++;tptr%=buffsize;
        }
        return outsum;
}

void __fastcall FIR::FIRUpdate(double sig)
{
        buff[ptr]=sig;
        ptr++;ptr%=buffsize;
}

double __fastcall FIR::FIRProcess(double FractionOfSampleOffset)
{
        double nextp=FractionOfSampleOffset;
        double thisp=1-nextp;
        int tptr=ptr;
        int nptr=ptr;
        outsum=0;
        nptr++;nptr%=buffsize;
        for(int i=0;i<NumberOfPoints;i++)
        {
                outsum+=points[i]*(buff[tptr]*thisp+buff[nptr]*nextp);
                tptr=nptr;
                nptr++;nptr%=buffsize;
        }
        return outsum;
}

double __fastcall FIR::FIRUpdateAndProcess(double sig, double FractionOfSampleOffset)
{
        buff[ptr]=sig;
        ptr++;ptr%=buffsize;
        double nextp=FractionOfSampleOffset;
        double thisp=1-nextp;
        int tptr=ptr;
        int nptr=ptr;
        outsum=0;
        nptr++;nptr%=buffsize;
        for(int i=0;i<NumberOfPoints;i++)
        {
                outsum+=points[i]*(buff[tptr]*thisp+buff[nptr]*nextp);
                tptr=nptr;
                nptr++;nptr%=buffsize;
        }
        return outsum;
}

void __fastcall FIR::FIRSetPoint(int point, double value)
{
        if(point<0)return;
        if(point>=NumberOfPoints) return;
        points[point]=value;
}

__fastcall RootRaisedCosine::RootRaisedCosine(int NumberOfPoints, double freq, double beta, FIR *Filter)
{
        if((NumberOfPoints%2)!=0){NumberOfPoints-=1;Filter->FIRSetPoint(NumberOfPoints,0);}
        double T=gSettings.Common.Samplerate/freq;
        double fi;
	for(int i=0;i<NumberOfPoints;i++)
        {
                if(i==(NumberOfPoints/2)) Filter->FIRSetPoint(i,(4*beta+M_PI-M_PI*beta)/(M_PI*sqrt(T)));
                 else
                 {
                        fi=(((double)i)-((double)NumberOfPoints)/2);
                        Filter->FIRSetPoint(i,(4.0*beta/(M_PI*sqrt(T))*(cos((1+beta)*M_PI*fi/T)+T/(4*beta*fi)*sin((1-beta)*M_PI*fi/T))/(1.0-pow(4*beta*fi/T,2))));
                 }
        }
}

__fastcall RootRaisedCosine::RootRaisedCosine(int NumberOfPoints, double freq, double beta, double *Points)
{
        if((NumberOfPoints%2)!=0){NumberOfPoints-=1;Points[NumberOfPoints]=0;}
        double T=gSettings.Common.Samplerate/freq;
        double fi;
	for(int i=0;i<NumberOfPoints;i++)
        {
                if(i==(NumberOfPoints/2)) Points[i]=(4*beta+M_PI-M_PI*beta)/(M_PI*sqrt(T));
                 else
                 {
                        fi=(((double)i)-((double)NumberOfPoints)/2);
                        if((1.0-pow(4.0*beta*fi/T,2))==0)Points[i]=(beta*((M_PI-2.0)*cos(M_PI/(4.0*beta))+(M_PI+2.0)*sin(M_PI/(4.0*beta)))/(M_PI*sqrt(2.0*T)));
                         else Points[i]=(4.0*beta/(M_PI*sqrt(T))*(cos((1.0+beta)*M_PI*fi/T)+T/(4.0*beta*fi)*sin((1.0-beta)*M_PI*fi/T))/(1.0-pow(4.0*beta*fi/T,2)));
                 }
        }
}

RootRaisedCosine::~RootRaisedCosine()
{

}

__fastcall SymbolTimer::SymbolTimer()
{
        SetCallsPerSymbol(2);    //default value
}

void __fastcall SymbolTimer::CaculateNextRightPos()
{
        while(Pos>=FramesPerRightFrame)Pos-=FramesPerRightFrame;
        OverSampleCnt++;OverSampleCnt%=CallsPerSymbol;
        if(OverSampleCnt==0)FirstFrame=true;
         else FirstFrame=false;
}


void __fastcall SymbolTimer::AdjustByFractionOfCall(double Value)
{
        Pos-=FramesPerRightFrame*Value;
        if(Pos>=FramesPerRightFrame)
        {
                int p=floor(Pos/FramesPerRightFrame);
                Pos-=((double)p)*FramesPerRightFrame;
                OverSampleCnt+=p;OverSampleCnt%=CallsPerSymbol;
                if(OverSampleCnt==0)FirstFrame=true;
                 else FirstFrame=false;
        }
        if(Pos<=-FramesPerRightFrame)
        {
                int p=floor(-Pos/FramesPerRightFrame);
                Pos+=((double)p)*FramesPerRightFrame;
                OverSampleCnt+=p;OverSampleCnt%=CallsPerSymbol;
                if(OverSampleCnt==0)FirstFrame=true;
                 else FirstFrame=false;
        }   
}

void __fastcall SymbolTimer::SetCallsPerSymbol(int Value)
{
        FirstFrame=true;
        OverSampleCnt=0;
        CallsPerSymbol=Value;
        Freq=gSettings.DemodMajor.Freq;
        FramesPerRightFrame=((gSettings.Common.Samplerate)*(gSettings.DemodMajor.Gamma))/((gSettings.DemodMajor.Freq)*((double)CallsPerSymbol));
        Pos=0.0;
}

void __fastcall SymbolTimer::SetFrequency(double Value)
{
        Freq=Value;
        FramesPerRightFrame=((gSettings.Common.Samplerate)*(gSettings.DemodMajor.Gamma))/((Value)*((double)CallsPerSymbol));
}

double AngleRetriver::Angle(PtComplex a)
{
  PtComplex b;
  b.re=1;
  b.im=0;
  double w;
  w=vectorlength(a);
  if(w==0) return 0;
  a.re=a.re/w;
  a.im=a.im/w;
  double cosine = a.re * b.re + a.im * b.im ;
  // rounding errors might make dotproduct out of range for cosine
  if (cosine > 1) cosine = 1;
  else if (cosine < -1) cosine = -1;

  if ((b.re * a.im - b.im * a.re) < 0)
    return -acos(cosine);
  else
    return acos(cosine);

}

double AngleRetriver::ApproxAngle(PtComplex a)  //this might have a bug in it???
{
  PtComplex b;
  b.re=1;
  b.im=0;
  double w;
  w=Approxvectorlength(a);
  if(w==0) return 0;
  a.re=a.re/w;
  a.im=a.im/w;
  double cosine = a.re * b.re + a.im * b.im ;

        int i=(int)(((cosine+1.0)*WTSIZE_1)/2.0);
        if (i>WTSIZE_1)i=WTSIZE_1;
         else if(i<0)i=0;
        if ((a.re * b.im - a.im * b.re) < 0)return -(gSettings.Trig.CosINV[i]);
         else return (gSettings.Trig.CosINV[i]);
}

double AngleRetriver::vectorlength(PtComplex v)
{
  double sz;
  sz=v.re*v.re + v.im*v.im;
  sz=sqrt(sz);
  return sz;
}

double AngleRetriver::Approxvectorlength(PtComplex v)
{
  return ApproxSqrt(v.re*v.re + v.im*v.im);
}

double AngleRetriver::ApproxSqrt(double y)
{
        float x=(float)y;
        float xhalf = 0.5f*x;
        int i = *(int *)&x;
        i = 0x5f3759df - (i >> 1);
        x = *(float *)&i;
        x = x * (1.5f - xhalf * x * x);
        return 1/((double)x);
}

__fastcall LPFIIR::LPFIIR(double Fc)
{
        double ftmp=Fc;
        double x=exp(-14.445*ftmp);
        a[0]=pow((1.0-x),4);
        b[1]=4.0*x;
        b[2]=-6.0*x*x;
        b[3]=4.0*pow(x,3);
        b[4]=-pow(x,4);
}

double __fastcall LPFIIR::Update(double Val)
{
        y[0]=y[1];
        y[1]=y[2];
        y[2]=y[3];
        y[3]=y[4];
        y[4]=a[0]*Val+b[1]*y[3]+b[2]*y[2]+b[3]*y[1]+b[4]*y[0];
        return y[4];
}