public APFFT(int N)
{
hanning=new double[N];
for (int i = 0; i < N; i++)
{
hanning[i] = 0.5 - 0.5 * Math.Cos(2 * (i+1) * Math.PI / N);
}
convHann=new double[2*N-1];
for (int i = 0; i < 2 * N - 1; i++)
{
double sum = 0;
for (int j = 0; j < N; j++)
{
int index = i - j;
if (index >= 0 && index < N)
{
sum += hanning[j] * hanning[index];
}
}
convHann[i] = sum;
}
double sumHann = 0;
for (int i = 0; i < 2 * N - 1; i++)
{
sumHann += convHann[i];
}
for (int i = 0; i < 2 * N - 1; i++)
{
convHann[i] = convHann[i] / sumHann;
}
dotData = new double[2 * N - 1];
fftPlan = new FFTPlan(N);
this.NUM = N;
fftData1 = new float[N * 2];
fftData2 = new float[N * 2];
util = new DSPUtil();
}
public APFFT(int N)
{
hanning = new double[N];
for (int i = 0; i < N; i++)
{
hanning[i] = 0.5 - 0.5 * Math.Cos(2 * (i + 1) * Math.PI / N);
}
convHann = new double[2 * N - 1];
for (int i = 0; i < 2 * N - 1; i++)
{
double sum = 0;
for (int j = 0; j < N; j++)
{
int index = i - j;
if (index >= 0 && index < N)
{
sum += hanning[j] * hanning[index];
}
}
convHann[i] = sum;
}
double sumHann = 0;
for (int i = 0; i < 2 * N - 1; i++)
{
sumHann += convHann[i];
}
for (int i = 0; i < 2 * N - 1; i++)
{
convHann[i] = convHann[i] / sumHann;
}
dotData = new double[2 * N - 1];
fftPlan = new FFTPlan(N);
this.NUM = N;
fftData1 = new float[N * 2];
fftData2 = new float[N * 2];
util = new DSPUtil();
}
private void ProcDSP()
{
DSPUtil util = new DSPUtil();
FFTPlan fftPlan = null;
float[] data1 = null;
float[] data2 = null;
float[] fftData = null;
float[] dataSpectrum = null;
int PeakNum = 6;
float[] ypDiff = new float[PeakNum - 1];
float[] peakVal = new float[PeakNum];
int[] peakIndexLeft = new int[PeakNum];
int[] peakIndexLeftTmp = new int[PeakNum];
int[] peakIndexRight = new int[PeakNum];
float[] dcacAmpl = new float[2];
float[] amplDense = new float[250];
// float[] amplDenseDC = new float[25];
float dcAmpl = 0;
float acAmpl = 0;
float prevCarrier = -1;
float prevLow = -1;
int diffCnt = 0;
float carrierFreq = 0;
float lowFreq = 0;
float standard = 0;
SignalArgs args;
List<float> curveList = new List<float>();
bool curveStart = false;
float[] ypList = new float[] { 8.5f, 9.0f, 9.5f, 11.0f, 12.5f, 13.5f, 15.0f, 16.5f, 17.5f, 18.5f, 20.0f, 21.5f, 22.5f, 23.5f, 24.5f, 26.0f };
Queue<float> queueACCurve = new Queue<float>();
int maxCurveCnt = 0;
int MAXPT=40*25;
int PRENUM = 5;
this.adBlock = new ADBlock(SampleRate, SampleRate);
try
{
while (true)
{
float[] adData = adBlock.GetAdData(-1);
if (SignalArgsChanged == null)
{
eventDSP.Set();
continue;
}
if (adData == null) continue;
if (DecodeCurve>0) //计算道岔曲线 ，不计算载频和低频
{
int calCnt = (int)(1000 / this.TimeInterval);
int amplCnt = this.SampleRate / calCnt; //40ms一个点
for (int i = 0; i < calCnt; i++)
{
if (DecodeCurve == 1)
{
amplDense[i] = util.CalACAmpl(adData, i * amplCnt, amplCnt);
amplDense[i] = CalRealVal(amplDense[i], 50); //道岔电流都是交流50Hz
}
else if (DecodeCurve == 2)
{
amplDense[i] = util.CalDCAmpl(adData, i * amplCnt, amplCnt);
amplDense[i] = CalRealVal(amplDense[i], 0);
}
if (curveStart == false)
{
if (queueACCurve.Count >= PRENUM)
{
float[] prevVals = queueACCurve.ToArray();
for (int j = 0; j < prevVals.Length - 1; j++)
{
if (prevVals[j] > 0.001f)
{
float rate = (prevVals[prevVals.Length - 1] - prevVals[j]) / prevVals[j];
if (rate > 5)
{
curveStart = true;
curveList.AddRange(prevVals);
maxCurveCnt = MAXPT;
break;
}
}
}
}
}
else
{
curveList.Add(amplDense[i]);
if (amplDense[i] < 0.06f)
{
if (maxCurveCnt >= MAXPT)
{
maxCurveCnt = curveList.Count + 5;
}
}
if (curveList.Count >= maxCurveCnt) //最大40秒
{
args = new SignalArgs(recordFile.TimeExport.AddMilliseconds(-1 * this.TimeInterval * curveList.Count + i *1000f/ calCnt), curveList.ToArray());
SignalArgsChanged(this, args);
curveStart = false;
maxCurveCnt = 0;
curveList.Clear();
queueACCurve.Clear();
}
}
if (queueACCurve.Count >= PRENUM)
{
queueACCurve.Dequeue();
}
queueACCurve.Enqueue(amplDense[i]);
}
eventDSP.Set();
continue;
}
if (fftPlan == null)
{
fftPlan = new FFTPlan(SampleRate);
data1 = new float[SampleRate * 2];
data2 = new float[SampleRate * 2];
fftData = new float[SampleRate * 2];
dataSpectrum = new float[SampleRate];
}
//将实数变为复数
for (int i = 0; i < adData.Length; i++)
{
data1[i * 2] = adData[i];
data1[i * 2 + 1] = 0;
}
fftPlan.FFTForward(data1, fftData);
Array.Copy(fftData, data2, data2.Length);
//计算频谱
int spectrumLen = adData.Length / 2;
for (int i = 0; i < spectrumLen; i++)
{
dataSpectrum[i] = (float)Math.Sqrt(data2[i * 2] * data2[i * 2] + data2[i * 2 + 1] * data2[i * 2 + 1]);
}
//计算交直流幅度
util.CalDCACAmpl(adData, 0, this.SampleRate, dcacAmpl);
dcAmpl = CalRealVal(dcacAmpl[0], 0);
acAmpl = CalRealVal(dcacAmpl[1], 0);
int ignoreLow =DecodeFM?400: 5;
int ignoreHigh = DecodeFM ? 3000 : this.SampleRate / 2;
util.FindComplexPeaks(data2, ignoreLow, ignoreHigh, peakVal, peakIndexLeft); //忽略直流信息
// int freqCenter= util.CalFreqCenter(dataSpectrum, 400);
if (peakIndexLeft[0] < 0)
{
args = new SignalArgs(recordFile.TimeExport, dcAmpl, acAmpl, 0, 0);
SignalArgsChanged(this, args);
eventDSP.Set();
continue; //不存在极点
}
float freq = peakIndexLeft[0];
acAmpl = CalAmplByFreq(fftData, (int)freq);
acAmpl = CalRealVal(acAmpl, freq);
if (DecodeFM == false)
{
args = new SignalArgs(recordFile.TimeExport, dcAmpl, acAmpl, 0, 0);
SignalArgsChanged(this, args);
eventDSP.Set();
continue;
}
if (DecodeFM)
{
if (acAmpl < 0.1)
{
eventDSP.Set();
continue;
}
bool matchYP = true;
bool matchUM71 = true;
float freqShift = 0;
int underSampleCount = 1;
for (int i = 0; i < peakIndexLeft.Length; i++)
{
if ((peakIndexLeft[i] < 1600 - 200) || (peakIndexLeft[i] > 2600 + 200))
{
matchUM71 = false;
}
if ((peakIndexLeft[i] < 550 - 100) || (peakIndexLeft[i] > 850 + 100))
{
matchYP = false;
}
}
if ((matchYP == false) && (matchUM71 == false))
{
carrierFreq = peakIndexLeft[0];
lowFreq = 0;
}
if (matchYP)
{
float tmpShiftDiff = float.MaxValue;
for (int i = 0; i < peakIndexLeft.Length - 2; i++)
{
for (int j = i + 1; j < peakIndexLeft.Length - 1; j++)
{
float tmpShift = (peakIndexLeft[i] + peakIndexLeft[j] ) / 2f;
if (Math.Abs(tmpShift - 550) < tmpShiftDiff)
{
freqShift = tmpShift;
tmpShiftDiff = Math.Abs(tmpShift - 550);
}
if (Math.Abs(tmpShift - 650) < tmpShiftDiff)
{
freqShift = tmpShift;
tmpShiftDiff = Math.Abs(tmpShift - 650);
}
if (Math.Abs(tmpShift - 750) < tmpShiftDiff)
{
freqShift = tmpShift;
tmpShiftDiff = Math.Abs(tmpShift - 750);
}
if (Math.Abs(tmpShift - 850) < tmpShiftDiff)
{
freqShift = tmpShift;
tmpShiftDiff = Math.Abs(tmpShift - 850);
}
}
}
util.FindComplexPeaks(data2, (int)freqShift, ignoreHigh, peakVal, peakIndexLeft);
standard = Math.Abs(peakIndexLeft[0] - peakIndexLeft[1]);
underSampleCount = 30;
}
if (matchUM71)
{
freqShift = peakIndexLeft[0] - 40;
underSampleCount = 40;
}
util.ShiftSignal(data1, freqShift, this.SampleRate); //频谱搬移
util.ComplexFilter(data1, data2); //滤波
util.UnderSample(data2, underSampleCount); //欠采样
fftPlan.FFTForward(data2, data1); //频谱分析
if (matchYP)
{
int signalLength = data1.Length / 2;
util.FindComplexPeaks(data1, 0, signalLength / 2, peakVal, peakIndexLeft);
float diffMinYp = 1.5f; //最大不能差1.5
lowFreq = -1;
for (int j = 0; j < peakIndexLeft.Length - 1; j++)
{
for (int k = j+1; k < peakIndexLeft.Length; k++)
{
float tmpLow = Math.Abs(peakIndexLeft[k] - peakIndexLeft[j]) * 1f / underSampleCount;
if (tmpLow > 8 && tmpLow < 28)
{
if (Math.Abs(tmpLow - standard) < diffMinYp)
{
lowFreq = tmpLow;
diffMinYp = Math.Abs(tmpLow - standard);
}
}
}
if (lowFreq > 0) break;
}
util.FindComplexPeaks(data1, signalLength / 2, signalLength, peakVal, peakIndexRight);
carrierFreq = freqShift;
}
if (matchUM71)
{
util.FindComplexPeaks(data1, peakVal, peakIndexLeft);
carrierFreq = freqShift + peakIndexLeft[0] * 1f / underSampleCount;
lowFreq = Math.Abs(peakIndexLeft[1] - peakIndexLeft[2]) / 2f / underSampleCount;
if (lowFreq < 9 || lowFreq > 31)
{
lowFreq = -1;
}
}
}
acAmpl = CalAmplByFreq(fftData, (int)carrierFreq);
acAmpl = CalRealVal(acAmpl, carrierFreq);
if (Math.Abs(carrierFreq - prevCarrier) > 10 || Math.Abs(lowFreq - prevLow) > 1)
{
diffCnt++;
if (diffCnt >= 2)
{
prevCarrier = carrierFreq;
prevLow = lowFreq;
diffCnt = 0;
}
else
{
carrierFreq = prevCarrier;
lowFreq = prevLow;
}
}
else
{
prevCarrier = carrierFreq;
prevLow = lowFreq;
diffCnt = 0;
}
args = new SignalArgs(recordFile.TimeExport, dcAmpl, acAmpl, carrierFreq, lowFreq);
SignalArgsChanged(this, args);
eventDSP.Set();
}
}
catch (Exception)
{
if (fftPlan != null)
{
fftPlan.Dispose();
}
}
}
