public int Read(ref ComplexF[] dest, int cnt)
{
int free_cnt = ReadSpace();
if (free_cnt == 0) return 0;
int to_read = cnt > free_cnt ? free_cnt : cnt;
int cnt2 = rptr + to_read;
int n1 = 0, n2 = 0;
if (cnt2 > size)
{
n1 = size - rptr;
n2 = cnt2 & mask;
}
else
{
n1 = to_read;
n2 = 0;
}
Array.Copy(buf, rptr, dest, 0, n1);
rptr = (rptr + n1) & mask;
if (n2 != 0)
{
Array.Copy(buf, rptr, dest, n1, n2);
rptr = (rptr + n2) & mask;
}
return to_read;
}
private ComplexF CsclF(ComplexF x, float a)
{
ComplexF z;
z.Re = x.Re * a;
z.Im = x.Im * a;
return z;
}
public void Exchange_samples(ref ComplexF[] output, ref ComplexF[] monitor, int buflen)
{
if (MainForm.output_ring_buf.ReadSpace() < buflen)
{
while (MainForm.output_ring_buf.ReadSpace() < buflen)
{
if (run_transmiter)
{
audio_event.Set();
Thread.Sleep(1);
}
else
return;
}
}
EnterCriticalSection(cs_audio);
MainForm.output_ring_buf.Read(ref iq_buffer, buflen);
correctIQ(ref iq_buffer);
MainForm.mon_ring_buf.Read(ref mon_iq_buffer, buflen);
Array.Copy(iq_buffer, output, buflen);
Array.Copy(mon_iq_buffer, monitor, buflen);
LeaveCriticalSection(cs_audio);
}
//-----------------------------------------------------------------------------------
//-----------------------------------------------------------------------------------
/// <summary>
/// Determine whether two complex numbers are almost (i.e. within the tolerance) equivalent.
/// </summary>
/// <param name="a"></param>
/// <param name="b"></param>
/// <param name="tolerance"></param>
/// <returns></returns>
public static bool IsEqual( ComplexF a, ComplexF b, float tolerance )
{
return
( Math.Abs( a.Re - b.Re ) < tolerance ) &&
( Math.Abs( a.Im - b.Im ) < tolerance );
}
private ComplexF CmulF(ComplexF x, ComplexF y)
{
ComplexF z;
z.Re = x.Re * y.Re - x.Im * y.Im;
z.Im = x.Im * y.Re + x.Re * y.Im;
return z;
}
private double carg(ComplexF x)
{
return Math.Atan2(x.Im, x.Re);
}
/// <summary>
/// Compute a 1D fast Fourier transform of a dataset of complex numbers.
/// </summary>
/// <param name="data"></param>
/// <param name="length"></param>
/// <param name="direction"></param>
public void FFT_Quick( ComplexF[] data, int length, FourierDirection direction )
{
/*if( data == null ) {
throw new ArgumentNullException( "data" );
}
if( data.Length < length ) {
throw new ArgumentOutOfRangeException( "length", length, "must be at least as large as 'data.Length' parameter" );
}
if( IsPowerOf2( length ) == false ) {
throw new ArgumentOutOfRangeException( "length", length, "must be a power of 2" );
}*/
SyncLookupTableLength( length );
int ln = Log2( length );
// reorder array
ReorderArray( data );
// successive doubling
int N = 1;
int signIndex = ( direction == FourierDirection.Forward ) ? 0 : 1;
for( int level = 1; level <= ln; level ++ ) {
int M = N;
N <<= 1;
float[] uRLookup = _uRLookupF[ level, signIndex ];
float[] uILookup = _uILookupF[ level, signIndex ];
for( int j = 0; j < M; j ++ ) {
float uR = uRLookup[j];
float uI = uILookup[j];
for( int even = j; even < length; even += N ) {
int odd = even + M;
float r = data[ odd ].Re;
float i = data[ odd ].Im;
float odduR = r * uR - i * uI;
float odduI = r * uI + i * uR;
r = data[ even ].Re;
i = data[ even ].Im;
data[ even ].Re = r + odduR;
data[ even ].Im = i + odduI;
data[ odd ].Re = r - odduR;
data[ odd ].Im = i - odduI;
}
}
}
}
/// <summary>
/// Compute a 1D fast Fourier transform of a dataset of complex numbers.
/// </summary>
/// <param name="data"></param>
/// <param name="direction"></param>
public void FFT( ComplexF[] data, FourierDirection direction )
{
if( data == null ) {
throw new ArgumentNullException( "data" );
}
FFT(data, data.Length, direction );
}
private float Cabs(ComplexF z)
{
return (float)Math.Sqrt(z.Re * z.Re + z.Im * z.Im);
}
private void send_symbol(int index, int sym)
{
float delta, mon_delta;
float ival, qval;
ComplexF symbol = new ComplexF();
int i;
try
{
if (trx.modem[index].qpsk && trx.reverse)
sym = (4 - sym) & 3;
/* differential QPSK modulation - top bit flipped */
switch (sym)
{
case 0:
symbol.Re = -1.0f; /* 180 degrees */
symbol.Im = 0.0f;
break;
case 1:
symbol.Re = 0.0f; /* 270 degrees */
symbol.Im = -1.0f;
break;
case 2:
symbol.Re = 1.0f; /* 0 degrees */
symbol.Im = 0.0f;
break;
case 3:
symbol.Re = 0.0f; /* 90 degrees */
symbol.Im = 1.0f;
break;
}
symbol = CmulF(trx.modem[index].tx_prevsymbol, symbol);
delta = (float)(TWOPI * (trx.modem[index].tx_frequency) / trx.modem[index].tx_samplerate);
mon_delta = (float)(TWOPI * trx.mon_frequency / Audio.SampleRate);
if (trx.modem[index].qpsk)
{
for (i = 0; i < trx.modem[index].tx_symbollen; i++)
{
ival = (float)(trx.modem[index].txshape[i] * trx.modem[index].tx_prevsymbol.Re +
(1.0 - trx.modem[index].txshape[i]) * symbol.Re);
qval = (float)(trx.modem[index].txshape[i] * trx.modem[index].tx_prevsymbol.Im +
(1.0 - trx.modem[index].txshape[i]) * symbol.Im);
if (ptr >= 2048)
{
ptr = 0;
Array.Copy(output, 0, Tx[index].input_buf, 2048, 2048);
Array.Copy(Tx[index].input_buf, Tx[index].filter.vector, 4096);
fft.FFT(Tx[index].filter.vector, 4096, FourierDirection.Forward);
for (int j = 0; j < 4096; j++)
Tx[index].filter.vector[j] = CmulF(Tx[index].filter.vector[j],
Tx[index].filter.zfvec[j]);
fft.FFT(Tx[index].filter.vector, 4096, FourierDirection.Backward);
Array.Copy(Tx[index].input_buf, 2048, Tx[index].input_buf, 0, 2048);
if (MainForm.output_ring_buf.WriteSpace() < 2048)
{
if (run_transmiter)
{
while (MainForm.output_ring_buf.WriteSpace() < 2048)
audio_event.WaitOne(1);
}
}
EnterCriticalSection(cs_audio);
MainForm.output_ring_buf.Write(Tx[index].filter.vector, 2048);
LeaveCriticalSection(cs_audio);
}
output[ptr].Re = (float)((ival * (float)Math.Sin(trx.modem[index].tx_phaseacc) +
qval * (float)Math.Cos(trx.modem[index].tx_phaseacc)));
output[ptr].Im = -output[ptr].Re;
ptr++;
trx.mon_outbuf[i].Re = (float)(ival * (float)Math.Cos(trx.modem[index].mon_phaseacc));
trx.mon_outbuf[i].Im = (float)(qval * (float)Math.Sin(trx.modem[index].mon_phaseacc));
trx.modem[index].tx_phaseacc += delta;
trx.modem[index].mon_phaseacc += mon_delta;
if (trx.modem[index].tx_phaseacc > M_PI)
trx.modem[index].tx_phaseacc -= TWOPI;
if (trx.modem[index].mon_phaseacc > M_PI)
trx.modem[index].mon_phaseacc -= TWOPI;
}
}
else
{
for (i = 0; i < trx.modem[index].tx_symbollen; i++)
{
ival = (float)(trx.modem[index].txshape[i] * trx.modem[index].tx_prevsymbol.Re +
(1.0 - trx.modem[index].txshape[i]) * symbol.Re);
if (ptr >= 2048)
{
ptr = 0;
Array.Copy(output, 0, Tx[index].input_buf, 2048, 2048);
Array.Copy(Tx[index].input_buf, Tx[index].filter.vector, 4096);
fft.FFT(Tx[index].filter.vector, 4096, FourierDirection.Forward);
for (int j = 0; j < 4096; j++)
Tx[index].filter.vector[j] = CmulF(Tx[index].filter.vector[j],
Tx[index].filter.zfvec[j]);
fft.FFT(Tx[index].filter.vector, 4096, FourierDirection.Backward);
Array.Copy(Tx[index].input_buf, 2048, Tx[index].input_buf, 0, 2048);
if (MainForm.output_ring_buf.WriteSpace() < 2048)
{
if (run_transmiter)
{
while (MainForm.output_ring_buf.WriteSpace() < 2048)
audio_event.WaitOne(1);
}
}
EnterCriticalSection(cs_audio);
MainForm.output_ring_buf.Write(Tx[index].filter.vector, 2048);
LeaveCriticalSection(cs_audio);
}
output[ptr].Re = (float)(ival * (float)Math.Cos(trx.modem[index].tx_phaseacc));
output[ptr].Im = -output[ptr].Re; // (float)(ival * (float)Math.Sin(trx.modem[index].tx_phaseacc));
ptr++;
trx.mon_outbuf[i].Im = (float)(ival * (float)Math.Cos(trx.modem[index].mon_phaseacc));
trx.mon_outbuf[i].Re = (float)(ival * (float)Math.Sin(trx.modem[index].mon_phaseacc));
trx.modem[index].tx_phaseacc += delta;
trx.modem[index].mon_phaseacc += mon_delta;
if (trx.modem[index].tx_phaseacc > M_PI)
trx.modem[index].tx_phaseacc -= TWOPI;
if (trx.modem[index].mon_phaseacc > M_PI)
trx.modem[index].mon_phaseacc -= TWOPI;
}
}
if (MainForm.output_ring_buf.WriteSpace() < trx.modem[index].tx_symbollen)
{
if (run_transmiter)
{
while (MainForm.output_ring_buf.WriteSpace() < trx.modem[index].tx_symbollen)
audio_event.WaitOne(10);
}
}
EnterCriticalSection(cs_audio);
MainForm.mon_ring_buf.Write(trx.mon_outbuf, trx.modem[index].tx_symbollen);
LeaveCriticalSection(cs_audio);
/* save the current symbol */
trx.modem[index].tx_prevsymbol = symbol;
}
catch (Exception ex)
{
Debug.Write(ex.ToString());
}
}
private void rx_symbol(int index, ComplexF symbol)
{
try
{
double error;
int n;
if ((trx.modem[index].phase = carg(ccor(trx.modem[index].rx_prevsymbol, symbol))) < 0)
trx.modem[index].phase += TWOPI;
trx.modem[index].rx_prevsymbol = symbol;
if (trx.modem[index].qpsk)
{
trx.modem[index].bits = ((int)(trx.modem[index].phase / M_PI_2 + 0.5)) & 3;
n = 4;
}
else
{
trx.modem[index].bits = (((int)(trx.modem[index].phase / M_PI + 0.5)) & 1) << 1;
n = 2;
}
trx.modem[index].quality.Re = (float)decayavg(trx.modem[index].quality.Re,
Math.Cos(n * trx.modem[index].phase), 50.0);
trx.modem[index].quality.Im = (float)decayavg(trx.modem[index].quality.Im,
Math.Sin(n * trx.modem[index].phase), 50.0);
/*trx.modem[index].quality.Re = (float)(0.02f * Math.Cos(n * trx.modem[index].phase) +
0.98f * trx.modem[index].quality.Re);
trx.modem[index].quality.Im = (float)(0.02f * Math.Sin(n * trx.modem[index].phase) +
0.98f * trx.modem[index].quality.Im);*/
trx.modem[index].dcdshreg = (trx.modem[index].dcdshreg << 2) | (uint)trx.modem[index].bits;
switch (trx.modem[index].dcdshreg)
{
case 0xAAAAAAAA: /* DCD on by preamble */
trx.modem[index].dcd = true;
trx.modem[index].quality.Re = 1;
trx.modem[index].quality.Im = 0;
trx.acquire = 0;
break;
case 0: /* DCD off by postamble */
trx.modem[index].dcd = false;
trx.modem[index].quality.Re = 0;
trx.modem[index].quality.Im = 0;
break;
default:
double pwr = Math.Abs(trx.modem[index].quality.Re * trx.modem[index].quality.Re +
trx.modem[index].quality.Im * trx.modem[index].quality.Im);
if (pwr > (trx.squelch * sql))
{
trx.modem[index].dcd = true;
}
else
{
trx.modem[index].dcd = false;
}
break;
}
if (index == 0)
{
if (trx.modem[index].dcd == true)
{
MainForm.lblPSKDCDCh1.BackColor = Color.Green;
}
else
{
MainForm.lblPSKDCDCh1.BackColor = Color.Red;
}
}
else if (index == 1)
{
if (trx.modem[index].dcd == true)
{
MainForm.lblPSKDCDCh2.BackColor = Color.Green;
}
else
{
MainForm.lblPSKDCDCh2.BackColor = Color.Red;
}
}
if (trx.modem[index].dcd == true || trx.squelchon == false)
{
if (trx.modem[index].qpsk)
rx_qpsk(index, trx.modem[index].bits);
else
{
if (trx.modem[index].bits > 0)
rx_bit(index, 0);
else
rx_bit(index, 1);
}
if (trx.modem[index].dcd)
{
error = (trx.modem[index].phase - trx.modem[index].bits * M_PI / 2);
if (error < M_PI / 2)
error += TWOPI;
if (error > M_PI / 2)
error -= TWOPI;
error *= (double)(trx.modem[index].rx_samplerate / (trx.modem[index].rx_symbollen * TWOPI));
trx.modem[index].rx_frequency -= (error / 16.0);
trx.modem[index].tx_frequency -= (error / 16.0);
if (trx.modem[index].rx_frequency > MainForm.PSKPitch + 7.0)
{
trx.modem[index].rx_frequency = MainForm.PSKPitch + 7.0;
trx.modem[index].tx_frequency = tx_if_shift + 7.0;
}
else if (trx.modem[index].rx_frequency < MainForm.PSKPitch - 7.0)
{
trx.modem[index].rx_frequency = MainForm.PSKPitch - 7.0;
trx.modem[index].tx_frequency = tx_if_shift - 7.0;
}
if (trx.afcon)
{
double difference = 0.0;
pll++;
if (pll > 20)
{
switch (index)
{
case 0:
double diff = trx.modem[index].rx_frequency - MainForm.PSKPitch;
if (diff > 1.0)
{
difference = 0.000001;
MainForm.Invoke(new CrossThreadSetText(MainForm.CommandCallback), "VFO", 1,
difference.ToString());
}
else if (diff < -1.0)
{
difference = -0.000001;
MainForm.Invoke(new CrossThreadSetText(MainForm.CommandCallback), "VFO", 1,
difference.ToString());
}
break;
case 1:
diff = trx.modem[index].rx_frequency - MainForm.PSKPitch;
if (diff > 1.0)
{
difference = 0.000001;
MainForm.Invoke(new CrossThreadSetText(MainForm.CommandCallback), "VFO", 2,
difference.ToString());
}
else if (diff < -1.0)
{
difference = -0.000001;
MainForm.Invoke(new CrossThreadSetText(MainForm.CommandCallback), "VFO", 2,
difference.ToString());
}
break;
}
pll = 0;
}
}
}
}
}
catch (Exception ex)
{
Debug.Write(ex.ToString());
}
}
private int RXprocess(int index, int len)
{
try
{
double delta;
ComplexF z = new ComplexF();
ComplexF z2 = new ComplexF();
int i, j = 0;
string dcd = "";
int ptr = 0;
delta = TWOPI * trx.modem[index].rx_frequency / trx.modem[index].rx_samplerate;
while (len-- > 0)
{
trx.modem[index].rx_phaseacc += delta;
if (trx.modem[index].rx_phaseacc >= M_PI)
trx.modem[index].rx_phaseacc -= TWOPI;
// Filter and downsample by 16 or 8
if (trx.modem[index].decimate >= trx.modem[index].decimate_ratio - 1)
{
trx.modem[index].decimate = 0;
double sum, ampsum;
int idx;
z2.Re = 0.0f;
z2.Im = 0.0f;
// NCO mixer
if (index == 0)
{
z.Re = (float)(buffer_ch1[j] * Math.Cos(trx.modem[index].rx_phaseacc));
z.Im = (float)(buffer_ch1[j] * Math.Sin(trx.modem[index].rx_phaseacc));
}
else if (index == 1)
{
z.Re = (float)(buffer_ch2[j] * Math.Cos(trx.modem[index].rx_phaseacc));
z.Im = (float)(buffer_ch2[j] * Math.Sin(trx.modem[index].rx_phaseacc));
}
// MAC filtering
trx.modem[index].ibuffer[trx.modem[index].pointer] = z.Re;
trx.modem[index].qbuffer[trx.modem[index].pointer] = z.Im;
trx.modem[index].pointer++;
ptr = trx.modem[index].pointer - trx.modem[index].length;
for (i = 0; i < trx.modem[index].length; i++)
{
z2.Re += (float)(trx.modem[index].ibuffer[ptr] * trx.modem[index].filter[i]);
z2.Im += (float)(trx.modem[index].qbuffer[ptr] * trx.modem[index].filter[i]);
ptr++;
}
if (trx.modem[index].pointer >= trx.modem[index].FIRBufferLen)
{
Array.Copy(trx.modem[index].ibuffer, (trx.modem[index].FIRBufferLen - trx.modem[index].length),
trx.modem[index].ibuffer, 0, trx.modem[index].length);
Array.Copy(trx.modem[index].qbuffer, (trx.modem[index].FIRBufferLen - trx.modem[index].length),
trx.modem[index].qbuffer, 0, trx.modem[index].length);
trx.modem[index].pointer = trx.modem[index].length;
}
// sync correction
idx = (int)Math.Min(trx.modem[index].bitclk, 15);
idx = Math.Max(0, idx);
trx.modem[index].syncbuf[idx] = 0.9 * trx.modem[index].syncbuf[idx] + 0.1 *
Math.Sqrt(z2.Re * z2.Re + z2.Im * z2.Im);
sum = 0.0;
ampsum = 0.0;
for (i = 0; i < 8; i++)
{
sum += trx.modem[index].syncbuf[i] - trx.modem[index].syncbuf[i + 8];
ampsum += trx.modem[index].syncbuf[i] + trx.modem[index].syncbuf[i + 8];
}
sum = (ampsum == 0 ? 0 : sum / ampsum);
if (sum == float.NaN)
sum = 0.0;
trx.modem[index].bitclk -= sum / 5.0f;
// bit clock
trx.modem[index].bitclk += 1;
if (trx.modem[index].bitclk >= 16.0)
{
trx.modem[index].bitclk -= 16.0;
rx_symbol(index, z2);
}
}
else
trx.modem[index].decimate++;
j++;
}
dcd = Math.Round(MainForm.PSKPitch - trx.modem[index].rx_frequency, 3).ToString("f3");
dcd = dcd.PadRight(3, '0');
MainForm.Invoke(new CrossThreadSetText(MainForm.CommandCallback), "Set text", 100 + index, dcd);
return 0;
}
catch (Exception ex)
{
Debug.Write(ex.ToString());
return -1;
}
}
private unsafe float normalize_vec_COMPLEX(ComplexF[] z, int n, float scale)
{
if (z != null && n > 0)
{
int i;
float big = -(float)1e15;
for (i = 0; i < n; i++)
{
float a = Cabs(z[i]);
big = Math.Max(big, a);
}
if (big > 0.0)
{
float scl = (float)(scale / big);
for (i = 0; i < n; i++)
z[i] = CsclF(z[i], scl);
return scl;
}
else return 0.0f;
}
return 0.0f;
}
private ComplexFIR Create_FIR_Bandpass_COMPLEX(float lofreq, float hifreq, float samplerate, int size)
{
ComplexFIR p = new ComplexFIR();
ComplexF[] h;
float[] w;
float fc, ff, midpoint;
int i, msize;
float pi = 3.14159265358928f;
float twopi = 2 * pi;
h = new ComplexF[size];
if ((lofreq < -(samplerate / 2.0)) || (hifreq > (samplerate / 2.0)) || (hifreq <= lofreq))
return p;
else if (size < 1)
return p;
else
{
msize = size - 1;
midpoint = (float)(0.5f * msize);
p = Create_FIR_COMPLEX(size);
p.frq.lo = lofreq;
p.frq.hi = hifreq;
h = p.coef;
w = new float[size];
w = makewindow(size, w);
lofreq /= samplerate;
hifreq /= samplerate;
fc = (float)((hifreq - lofreq) / 2.0f);
ff = (float)((lofreq + hifreq) * pi);
for (i = 0; i < size; i++)
{
float k = (float)i - midpoint;
float tmp, phs = ff * k;
if ((float)i != midpoint)
tmp = (float)((Math.Sin(twopi * k * fc) / (pi * k)) * w[i]);
else
tmp = (float)(2.0f * fc);
tmp *= 2.0f;
h[i].Re = (float)(tmp * Math.Cos(phs));
h[i].Im = (float)(tmp * Math.Sin(phs));
}
return p;
}
}
private unsafe void CreateTXFilter(int index, int samplerate, double low_freq, double high_freq)
{
try
{
int fftlen = Tx[index].filter.fftlen;
int ncoef = Tx[index].filter.buflen + 1;
ComplexF[] vector = new ComplexF[fftlen];
Tx[index].filter.coef = Create_FIR_Bandpass_COMPLEX((float)low_freq,
(float)high_freq, samplerate, ncoef);
for (int i = 0; i < ncoef; i++)
{
vector[fftlen - ncoef + i] = Tx[index].filter.coef.coef[i];
}
fft.FFT(vector, fftlen, FourierDirection.Forward);
normalize_vec_COMPLEX(vector, Tx[index].filter.fftlen,
Tx[index].filter.scale);
for (int i = 0; i < fftlen; i++)
{
Tx[index].filter.zfvec[i] = vector[i];
}
}
catch (Exception ex)
{
Debug.Write(ex.ToString());
}
}
public void Clear()
{
ComplexF[] zero = new ComplexF[size];
Array.Clear(zero, 0, size);
Array.Copy(zero, buf, size);
}
private float Utility(ref ComplexF[] iq, float phase, float gain)
{
float[] spectrum = new float[4096];
Array.Copy(iq, 0, fftPtr, 2048, 2048);
//Array.Copy(zero, 0, fftPtr, 0, 2048);
for (var i = 0; i < 2048; i++)
{
fftPtr[i].Im += phase * fftPtr[i].Re;
fftPtr[i].Re *= gain;
fftPtr[i].Re *= window[i];
fftPtr[i].Im *= window[i];
}
fft.FFT_Quick(fftPtr, 4096, FourierDirection.Forward);
SpectrumPower(ref fftPtr, ref spectrumPtr, 4096, 50.0f);
Array.Copy(spectrumPtr, 0, spectrum, 2048, 2048);
Array.Copy(spectrumPtr, 2048, spectrum, 0, 2048);
/*Array.Copy(spectrumPtr, 0, DX.new_display_data, 2048, 2048);
Array.Copy(spectrumPtr, 2048, DX.new_display_data, 0, 2048);
Array.Copy(DX.new_display_data, DX.new_waterfall_data, 4096);*/
var result = 0.0f;
for (var i = 0; i < 4096 / 2; i++)
{
var distanceFromCenter = 4096 / 2 - i;
if (distanceFromCenter > (0.05f * 4096 / 2))
{
result += Math.Abs(spectrum[i] - spectrum[4096 - 2 - i]);
}
}
Array.Copy(iq, 0, fftPtr, 0, 2048);
return result;
}
private double carg(ComplexF x)
{
double z;
z = Math.Atan2(x.Im, x.Re);
return z;
}
/// <summary>
/// Compute a 3D fast fourier transform on a data set of complex numbers
/// </summary>
/// <param name="data"></param>
/// <param name="xLength"></param>
/// <param name="yLength"></param>
/// <param name="zLength"></param>
/// <param name="direction"></param>
public void FFT3( ComplexF[] data, int xLength, int yLength, int zLength, FourierDirection direction )
{
if( data == null ) {
throw new ArgumentNullException( "data" );
}
if( data.Length < xLength*yLength*zLength ) {
throw new ArgumentOutOfRangeException( "data.Length", data.Length, "must be at least as large as 'xLength * yLength * zLength' parameter" );
}
if( IsPowerOf2( xLength ) == false ) {
throw new ArgumentOutOfRangeException( "xLength", xLength, "must be a power of 2" );
}
if( IsPowerOf2( yLength ) == false ) {
throw new ArgumentOutOfRangeException( "yLength", yLength, "must be a power of 2" );
}
if( IsPowerOf2( zLength ) == false ) {
throw new ArgumentOutOfRangeException( "zLength", zLength, "must be a power of 2" );
}
int xInc = 1;
int yInc = xLength;
int zInc = xLength * yLength;
if( xLength > 1 ) {
SyncLookupTableLength( xLength );
for( int z = 0; z < zLength; z ++ ) {
for( int y = 0; y < yLength; y ++ ) {
int xStart = y * yInc + z * zInc;
LinearFFT_Quick( data, xStart, xInc, xLength, direction );
}
}
}
if( yLength > 1 ) {
SyncLookupTableLength( yLength );
for( int z = 0; z < zLength; z ++ ) {
for( int x = 0; x < xLength; x ++ ) {
int yStart = z * zInc + x * xInc;
LinearFFT_Quick( data, yStart, yInc, yLength, direction );
}
}
}
if( zLength > 1 ) {
SyncLookupTableLength( zLength );
for( int y = 0; y < yLength; y ++ ) {
for( int x = 0; x < xLength; x ++ ) {
int zStart = y * yInc + x * xInc;
LinearFFT_Quick( data, zStart, zInc, zLength, direction );
}
}
}
}
//-----------------------------------------------------------------------------------
//-----------------------------------------------------------------------------------
/// <summary>
/// Determine whether two complex numbers are almost (i.e. within the tolerance) equivalent.
/// </summary>
/// <param name="a"></param>
/// <param name="b"></param>
/// <param name="tolerance"></param>
/// <returns></returns>
static public bool IsEqual(ComplexF a, ComplexF b, float tolerance)
{
return
((Math.Abs(a.Re - b.Re) < tolerance) &&
(Math.Abs(a.Im - b.Im) < tolerance));
}
private void LinearFFT( ComplexF[] data, int start, int inc, int length, FourierDirection direction )
{
Debug.Assert( data != null );
Debug.Assert( start >= 0 );
Debug.Assert( inc >= 1 );
Debug.Assert( length >= 1 );
Debug.Assert( ( start + inc * ( length - 1 ) ) < data.Length );
// copy to buffer
ComplexF[] buffer = null;
LockBufferCF( length, ref buffer );
int j = start;
for( int i = 0; i < length; i ++ ) {
buffer[ i ] = data[ j ];
j += inc;
}
FFT(buffer, length, direction );
// copy from buffer
j = start;
for( int i = 0; i < length; i ++ ) {
data[ j ] = buffer[ i ];
j += inc;
}
UnlockBufferCF( ref buffer );
}
public float Mag()
{
ComplexF z = this;
return((float)Math.Sqrt(z.Re * z.Re + z.Im * z.Im));
}
ComplexF ccor(ComplexF x, ComplexF y)
{
ComplexF z;
z.Re = x.Re * y.Re + x.Im * y.Im;
z.Im = x.Re * y.Im - x.Im * y.Re;
return z;
}
/// <summary>
/// Create a complex number based on an existing complex number
/// </summary>
/// <param name="c"></param>
public ComplexF(ComplexF c)
{
this.Re = c.Re;
this.Im = c.Im;
}
private void correctIQ(int index, ref ComplexF[] buffer)
{
int i;
for (i = 0; i < 2048; i++)
{
buffer[i].Im += trx.modem[index].tx_phase * buffer[i].Re;
buffer[i].Re *= trx.modem[index].tx_gain;
}
}
private void LockBufferCF( int length, ref ComplexF[] buffer )
{
Debug.Assert( length >= 0 );
Debug.Assert( _bufferCFLocked == false );
_bufferCFLocked = true;
if( length != _bufferCF.Length ) {
_bufferCF = new ComplexF[ length ];
}
buffer = _bufferCF;
}
/// <summary>
/// Create a complex number based on an existing complex number
/// </summary>
/// <param name="c"></param>
public ComplexF( ComplexF c )
{
this.Re = c.Re;
this.Im = c.Im;
}
private void ReorderArray( ComplexF[] data )
{
Debug.Assert( data != null );
int length = data.Length;
Debug.Assert( IsPowerOf2( length ) == true );
Debug.Assert( length >= cMinLength );
Debug.Assert( length <= cMaxLength );
int[] reversedBits = GetReversedBits( Log2( length ) );
for( int i = 0; i < length; i ++ ) {
int swap = reversedBits[ i ];
if( swap > i ) {
ComplexF temp = data[ i ];
data[ i ] = data[ swap ];
data[ swap ] = temp;
}
}
}
private void correctIQ(ref ComplexF[] buffer)
{
int i;
for (i = 0; i < 2048; i++)
{
buffer[i].Im += tx_phase * buffer[i].Re;
buffer[i].Re *= tx_gain;
}
}
private void Swap( ref ComplexF a, ref ComplexF b )
{
ComplexF temp = a;
a = b;
b = temp;
}
public int Write(ComplexF[] src, int cnt)
{
int free_cnt = WriteSpace();
if (free_cnt == 0 || free_cnt < 0)
return 0;
else if (free_cnt < cnt)
cnt = free_cnt;
int to_write = cnt > free_cnt ? free_cnt : cnt;
int cnt2 = wptr + to_write;
int n1 = 0, n2 = 0;
if (cnt2 > size)
{
n1 = size - wptr;
n2 = cnt2 & mask;
}
else
{
n1 = to_write;
n2 = 0;
}
Array.Copy(src, 0, buf, wptr, n1);
wptr = (wptr + n1) & mask;
if (n2 != 0)
{
Array.Copy(src, n1, buf, wptr, n2);
wptr = (wptr + n2) & mask;
}
return to_write;
}
private void UnlockBufferCF( ref ComplexF[] buffer )
{
Debug.Assert( _bufferCF == buffer );
Debug.Assert( _bufferCFLocked == true );
_bufferCFLocked = false;
buffer = null;
}
/// <summary>
/// Compute a 1D fast Fourier transform of a dataset of complex numbers.
/// </summary>
/// <param name="data"></param>
/// <param name="length"></param>
/// <param name="direction"></param>
public void FFT(ComplexF[] data, int length, FourierDirection direction )
{
try
{
if (data == null)
{
throw new ArgumentNullException("data");
}
if (data.Length < length)
{
throw new ArgumentOutOfRangeException("length", length, "must be at least as large as 'data.Length' parameter");
}
if (IsPowerOf2(length) == false)
{
throw new ArgumentOutOfRangeException("length", length, "must be a power of 2");
}
SyncLookupTableLength(length);
int ln = Log2(length);
// reorder array
ReorderArray(data);
// successive doubling
N = 1;
int signIndex = (direction == FourierDirection.Forward) ? 0 : 1;
for (level = 1; level <= ln; level++)
{
M = N;
N <<= 1;
uRLookup = _uRLookupF[level, signIndex];
uILookup = _uILookupF[level, signIndex];
for (int j = 0; j < M; j++)
{
uR = uRLookup[j];
uI = uILookup[j];
for (even = j; even < length; even += N)
{
odd = even + M;
r = data[odd].Re;
i = data[odd].Im;
odduR = r * uR - i * uI;
odduI = r * uI + i * uR;
r = data[even].Re;
i = data[even].Im;
data[even].Re = r + odduR;
data[even].Im = i + odduI;
data[odd].Re = r - odduR;
data[odd].Im = i - odduI;
}
}
}
}
catch (Exception ex)
{
Debug.Write(ex.ToString());
}
}
public void Clear()
{
ComplexF[] zero = new ComplexF[size];
Array.Clear(zero, 0, size);
Array.Copy(zero, buf, size);
}
