complex interpolatePoint(const signalVector &inSig,
float ix)
{
float fracOffset = ix - floor(ix);
signalVector *sincVector = fetchSincVector(fracOffset);
signalVector::iterator sincPtr = sincVector->begin();
int start = (int) (floor(ix) - SINCWAVEFORMSIZE/2);
if (start < 0) {
sincPtr += (-start);
start = 0;
}
int end = (int) (floor(ix) + (SINCWAVEFORMSIZE/2)+1);
if ((unsigned) end > inSig.size()-1) end = inSig.size()-1;
complex pVal = 0.0;
if (!inSig.isRealOnly()) {
for (int i = start; i <= end; i++) {
pVal += inSig[i] * sincPtr->real();
sincPtr++;
}
}
else {
for (int i = start; i <= end; i++) {
pVal += inSig[i].real() * sincPtr->real();
sincPtr++;
}
}
return pVal;
}
int RadioInterface::radioifyVector(signalVector &wVector,
size_t chan, bool zero)
{
if (zero)
sendBuffer[chan]->zero(wVector.size());
else
sendBuffer[chan]->write((float *) wVector.begin(), wVector.size());
return wVector.size();
}
int RadioInterface::radioifyVector(signalVector &wVector,
float *retVector,
bool zero)
{
if (zero) {
memset(retVector, 0, wVector.size() * 2 * sizeof(float));
return wVector.size();
}
memcpy(retVector, wVector.begin(), wVector.size() * 2 * sizeof(float));
return wVector.size();
}
int RadioInterface::unRadioifyVector(float *floatVector,
signalVector& newVector)
{
int i;
signalVector::iterator itr = newVector.begin();
for (i = 0; i < newVector.size(); i++) {
*itr++ = Complex<float>(floatVector[2 * i + 0],
floatVector[2 * i + 1]);
}
return newVector.size();
}
complex peakDetect(const signalVector &rxBurst,
float *peakIndex,
float *avgPwr)
{
complex maxVal = 0.0;
float maxIndex = -1;
float sumPower = 0.0;
for (unsigned int i = 0; i < rxBurst.size(); i++) {
float samplePower = rxBurst[i].norm2();
if (samplePower > maxVal.real()) {
maxVal = samplePower;
maxIndex = i;
}
sumPower += samplePower;
}
// interpolate around the peak
// to save computation, we'll use early-late balancing
float earlyIndex = maxIndex-1;
float lateIndex = maxIndex+1;
float incr = 0.5;
while (incr > 1.0/1024.0) {
complex earlyP = interpolatePoint(rxBurst,earlyIndex);
complex lateP = interpolatePoint(rxBurst,lateIndex);
if (earlyP < lateP)
earlyIndex += incr;
else if (earlyP > lateP)
earlyIndex -= incr;
else break;
incr /= 2.0;
lateIndex = earlyIndex + 2.0;
}
maxIndex = earlyIndex + 1.0;
maxVal = interpolatePoint(rxBurst,maxIndex);
if (peakIndex!=NULL)
*peakIndex = maxIndex;
if (avgPwr!=NULL)
*avgPwr = (sumPower-maxVal.norm2()) / (rxBurst.size()-1);
return maxVal;
}
int RadioInterface::unRadioifyVector(float *floatVector,
signalVector& newVector)
{
signalVector::iterator itr = newVector.begin();
if (newVector.size() > recvCursor) {
LOG(ALERT) << "Insufficient number of samples in receive buffer";
return -1;
}
for (size_t i = 0; i < newVector.size(); i++) {
*itr++ = Complex<float>(floatVector[2 * i + 0],
floatVector[2 * i + 1]);
}
return newVector.size();
}
bool energyDetect(signalVector &rxBurst,
unsigned windowLength,
float detectThreshold,
float *avgPwr)
{
signalVector::const_iterator windowItr = rxBurst.begin(); //+rxBurst.size()/2 - 5*windowLength/2;
float energy = 0.0;
if (windowLength < 0) windowLength = 20;
if (windowLength > rxBurst.size()) windowLength = rxBurst.size();
for (unsigned i = 0; i < windowLength; i++) {
energy += windowItr->norm2();
windowItr+=4;
}
if (avgPwr) *avgPwr = energy/windowLength;
LOG(DEEPDEBUG) << "detected energy: " << energy/windowLength;
return (energy/windowLength > detectThreshold*detectThreshold);
}
void RadioInterface::driveTransmitRadio(signalVector &radioBurst) {
if (!mOn) return;
USRPifyVector(radioBurst, sendBuffer+sendCursor, powerScaling);
sendCursor += (radioBurst.size()*2);
pushBuffer();
}
void RadioInterface::driveTransmitRadio(signalVector &radioBurst, bool zeroBurst) {
if (!mOn) return;
radioifyVector(radioBurst, sendBuffer + 2 * sendCursor, powerScaling, zeroBurst);
sendCursor += radioBurst.size();
pushBuffer();
}
complex interpolatePoint(const signalVector &inSig,
float ix)
{
int start = (int) (floor(ix) - 10);
if (start < 0) start = 0;
int end = (int) (floor(ix) + 11);
if ((unsigned) end > inSig.size()-1) end = inSig.size()-1;
complex pVal = 0.0;
if (!inSig.isRealOnly()) {
for (int i = start; i < end; i++)
pVal += inSig[i] * sinc(M_PI_F*(i-ix));
}
else {
for (int i = start; i < end; i++)
pVal += inSig[i].real() * sinc(M_PI_F*(i-ix));
}
return pVal;
}
int RadioInterface::radioifyVector(signalVector &wVector,
float *retVector,
float scale,
bool zero)
{
int i;
signalVector::iterator itr = wVector.begin();
if (zero) {
memset(retVector, 0, wVector.size() * 2 * sizeof(float));
return wVector.size();
}
for (i = 0; i < wVector.size(); i++) {
retVector[2 * i + 0] = itr->real() * scale;
retVector[2 * i + 1] = itr->imag() * scale;
itr++;
}
return wVector.size();
}
bool detectRACHBurst(signalVector &rxBurst,
float detectThreshold,
int samplesPerSymbol,
complex *amplitude,
float* TOA)
{
//static complex staticData[500];
//signalVector correlatedRACH(staticData,0,rxBurst.size());
signalVector correlatedRACH(rxBurst.size());
correlate(&rxBurst,gRACHSequence->sequenceReversedConjugated,&correlatedRACH,NO_DELAY,true);
float meanPower;
complex peakAmpl = peakDetect(correlatedRACH,TOA,&meanPower);
float valleyPower = 0.0;
// check for bogus results
if ((*TOA < 0.0) || (*TOA > correlatedRACH.size())) {
*amplitude = 0.0;
return false;
}
complex *peakPtr = correlatedRACH.begin() + (int) rint(*TOA);
LOG(DEBUG) << "RACH corr: " << correlatedRACH;
float numSamples = 0.0;
for (int i = 57*samplesPerSymbol; i <= 107*samplesPerSymbol;i++) {
if (peakPtr+i >= correlatedRACH.end())
break;
valleyPower += (peakPtr+i)->norm2();
numSamples++;
}
if (numSamples < 2) {
*amplitude = 0.0;
return false;
}
float RMS = sqrtf(valleyPower/(float) numSamples)+0.00001;
float peakToMean = peakAmpl.abs()/RMS;
LOG(DEBUG) << "RACH peakAmpl=" << peakAmpl << " RMS=" << RMS << " peakToMean=" << peakToMean;
*amplitude = peakAmpl/(gRACHSequence->gain);
*TOA = (*TOA) - gRACHSequence->TOA - 8*samplesPerSymbol;
LOG(DEBUG) << "RACH thresh: " << peakToMean;
return (peakToMean > detectThreshold);
}
void delayVector(signalVector &wBurst,
float delay)
{
int intOffset = (int) floor(delay);
float fracOffset = delay - intOffset;
// do fractional shift first, only do it for reasonable offsets
if (fabs(fracOffset) > 1e-2) {
// create sinc function
static complex staticData[21];
signalVector sincVector(staticData,0,21);
sincVector.isRealOnly(true);
signalVector::iterator sincBurstItr = sincVector.begin();
for (int i = 0; i < 21; i++)
*sincBurstItr++ = (complex) sinc(M_PI_F*(i-10-fracOffset));
static complex shiftedData[300];
signalVector shiftedBurst(shiftedData,0,wBurst.size());
convolve(&wBurst,&sincVector,&shiftedBurst,NO_DELAY);
wBurst.clone(shiftedBurst);
}
if (intOffset < 0) {
intOffset = -intOffset;
signalVector::iterator wBurstItr = wBurst.begin();
signalVector::iterator shiftedItr = wBurst.begin()+intOffset;
while (shiftedItr < wBurst.end())
*wBurstItr++ = *shiftedItr++;
while (wBurstItr < wBurst.end())
*wBurstItr++ = 0.0;
}
else {
signalVector::iterator wBurstItr = wBurst.end()-1;
signalVector::iterator shiftedItr = wBurst.end()-1-intOffset;
while (shiftedItr >= wBurst.begin())
*wBurstItr-- = *shiftedItr--;
while (wBurstItr >= wBurst.begin())
*wBurstItr-- = 0.0;
}
}
float vectorPower(const signalVector &x)
{
return vectorNorm2(x)/x.size();
}
// Assumes symbol-spaced sampling!!!
// Based upon paper by Al-Dhahir and Cioffi
bool designDFE(signalVector &channelResponse,
float SNRestimate,
int Nf,
signalVector **feedForwardFilter,
signalVector **feedbackFilter)
{
signalVector G0(Nf);
signalVector G1(Nf);
signalVector::iterator G0ptr = G0.begin();
signalVector::iterator G1ptr = G1.begin();
signalVector::iterator chanPtr = channelResponse.begin();
int nu = channelResponse.size()-1;
*G0ptr = 1.0/sqrtf(SNRestimate);
for(int j = 0; j <= nu; j++) {
*G1ptr = chanPtr->conj();
G1ptr++; chanPtr++;
}
signalVector *L[Nf];
signalVector::iterator Lptr;
float d;
for(int i = 0; i < Nf; i++) {
d = G0.begin()->norm2() + G1.begin()->norm2();
L[i] = new signalVector(Nf+nu);
Lptr = L[i]->begin()+i;
G0ptr = G0.begin(); G1ptr = G1.begin();
while ((G0ptr < G0.end()) && (Lptr < L[i]->end())) {
*Lptr = (*G0ptr*(G0.begin()->conj()) + *G1ptr*(G1.begin()->conj()) )/d;
Lptr++;
G0ptr++;
G1ptr++;
}
complex k = (*G1.begin())/(*G0.begin());
if (i != Nf-1) {
signalVector G0new = G1;
scaleVector(G0new,k.conj());
addVector(G0new,G0);
signalVector G1new = G0;
scaleVector(G1new,k*(-1.0));
addVector(G1new,G1);
delayVector(G1new,-1.0);
scaleVector(G0new,1.0/sqrtf(1.0+k.norm2()));
scaleVector(G1new,1.0/sqrtf(1.0+k.norm2()));
G0 = G0new;
G1 = G1new;
}
}
*feedbackFilter = new signalVector(nu);
L[Nf-1]->segmentCopyTo(**feedbackFilter,Nf,nu);
scaleVector(**feedbackFilter,(complex) -1.0);
conjugateVector(**feedbackFilter);
signalVector v(Nf);
signalVector::iterator vStart = v.begin();
signalVector::iterator vPtr;
*(vStart+Nf-1) = (complex) 1.0;
for(int k = Nf-2; k >= 0; k--) {
Lptr = L[k]->begin()+k+1;
vPtr = vStart + k+1;
complex v_k = 0.0;
for (int j = k+1; j < Nf; j++) {
v_k -= (*vPtr)*(*Lptr);
vPtr++; Lptr++;
}
*(vStart + k) = v_k;
}
*feedForwardFilter = new signalVector(Nf);
signalVector::iterator w = (*feedForwardFilter)->begin();
for (int i = 0; i < Nf; i++) {
delete L[i];
complex w_i = 0.0;
int endPt = ( nu < (Nf-1-i) ) ? nu : (Nf-1-i);
vPtr = vStart+i;
chanPtr = channelResponse.begin();
for (int k = 0; k < endPt+1; k++) {
w_i += (*vPtr)*(chanPtr->conj());
vPtr++; chanPtr++;
}
*w = w_i/d;
w++;
}
return true;
}