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;
}
float vectorNorm2(const signalVector &x)
{
signalVector::const_iterator xPtr = x.begin();
float Energy = 0.0;
for (;xPtr != x.end();xPtr++) {
Energy += xPtr->norm2();
}
return Energy;
}
/** in-place conjugation */
void conjugateVector(signalVector &x)
{
if (x.isRealOnly()) return;
signalVector::iterator xP = x.begin();
signalVector::iterator xPEnd = x.end();
while (xP < xPEnd) {
*xP = xP->conj();
xP++;
}
}
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();
}
void RadioInterface::unRadioifyVector(short *shortVector, signalVector& newVector)
{
signalVector::iterator itr = newVector.begin();
short *shortItr = shortVector;
while (itr < newVector.end()) {
*itr++ = Complex<float>(*(shortItr),*(shortItr+1));
//LOG(DEEPDEBUG) << (*(itr-1));
shortItr += 2;
}
}
// in-place addition!!
bool addVector(signalVector &x,
signalVector &y)
{
signalVector::iterator xP = x.begin();
signalVector::iterator yP = y.begin();
signalVector::iterator xPEnd = x.end();
signalVector::iterator yPEnd = y.end();
while ((xP < xPEnd) && (yP < yPEnd)) {
*xP = *xP + *yP;
xP++; yP++;
}
return true;
}
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;
}
void GMSKReverseRotate(signalVector &x) {
signalVector::iterator xPtr= x.begin();
signalVector::iterator rotPtr = GMSKReverseRotation->begin();
if (x.isRealOnly()) {
while (xPtr < x.end()) {
*xPtr = *rotPtr++ * (xPtr->real());
xPtr++;
}
}
else {
while (xPtr < x.end()) {
*xPtr = *rotPtr++ * (*xPtr);
xPtr++;
}
}
}
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 offsetVector(signalVector &x,
complex offset)
{
signalVector::iterator xP = x.begin();
signalVector::iterator xPEnd = x.end();
if (!x.isRealOnly()) {
while (xP < xPEnd) {
*xP += offset;
xP++;
}
}
else {
while (xP < xPEnd) {
*xP = xP->real() + offset;
xP++;
}
}
}
void scaleVector(signalVector &x,
complex scale)
{
signalVector::iterator xP = x.begin();
signalVector::iterator xPEnd = x.end();
if (!x.isRealOnly()) {
while (xP < xPEnd) {
*xP = *xP * scale;
xP++;
}
}
else {
while (xP < xPEnd) {
*xP = xP->real() * scale;
xP++;
}
}
}
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();
}
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;
}
}
void RadioInterface::unUSRPifyVector(short *shortVector, signalVector& newVector)
{
signalVector::iterator itr = newVector.begin();
short *shortItr = shortVector;
// need to flip I and Q from USRP
#ifndef SWLOOPBACK
#define FLIP_IQ 1
#else
#define FLIP_IQ 0
#endif
while (itr < newVector.end()) {
*itr++ = Complex<float>(usrp_to_host_short(*(shortItr+FLIP_IQ)),
usrp_to_host_short(*(shortItr+1-FLIP_IQ)));
//LOG(DEEPDEBUG) << (*(itr-1));
shortItr += 2;
}
}
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();
}
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;
}
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);
}
bool analyzeTrafficBurst(signalVector &rxBurst,
unsigned TSC,
float detectThreshold,
int samplesPerSymbol,
complex *amplitude,
float *TOA,
unsigned maxTOA,
bool requestChannel,
signalVector **channelResponse,
float *channelResponseOffset)
{
assert(TSC<8);
assert(amplitude);
assert(TOA);
assert(gMidambles[TSC]);
if (maxTOA < 3*samplesPerSymbol) maxTOA = 3*samplesPerSymbol;
unsigned spanTOA = maxTOA;
if (spanTOA < 5*samplesPerSymbol) spanTOA = 5*samplesPerSymbol;
unsigned startIx = (66-spanTOA)*samplesPerSymbol;
unsigned endIx = (66+16+spanTOA)*samplesPerSymbol;
unsigned windowLen = endIx - startIx;
unsigned corrLen = 2*maxTOA+1;
unsigned expectedTOAPeak = (unsigned) round(gMidambles[TSC]->TOA + (gMidambles[TSC]->sequenceReversedConjugated->size()-1)/2);
signalVector burstSegment(rxBurst.begin(),startIx,windowLen);
static complex staticData[200];
signalVector correlatedBurst(staticData,0,corrLen);
correlate(&burstSegment, gMidambles[TSC]->sequenceReversedConjugated,
&correlatedBurst, CUSTOM,true,
expectedTOAPeak-maxTOA,corrLen);
float meanPower;
*amplitude = peakDetect(correlatedBurst,TOA,&meanPower);
float valleyPower = 0.0; //amplitude->norm2();
complex *peakPtr = correlatedBurst.begin() + (int) rint(*TOA);
// check for bogus results
if ((*TOA < 0.0) || (*TOA > correlatedBurst.size())) {
*amplitude = 0.0;
return false;
}
int numRms = 0;
for (int i = 2*samplesPerSymbol; i <= 5*samplesPerSymbol;i++) {
if (peakPtr - i >= correlatedBurst.begin()) {
valleyPower += (peakPtr-i)->norm2();
numRms++;
}
if (peakPtr + i < correlatedBurst.end()) {
valleyPower += (peakPtr+i)->norm2();
numRms++;
}
}
if (numRms < 2) {
// check for bogus results
*amplitude = 0.0;
return false;
}
float RMS = sqrtf(valleyPower/(float)numRms)+0.00001;
float peakToMean = (amplitude->abs())/RMS;
// NOTE: Because ideal TSC is 66 symbols into burst,
// the ideal TSC has an +/- 180 degree phase shift,
// due to the pi/4 frequency shift, that
// needs to be accounted for.
*amplitude = (*amplitude)/gMidambles[TSC]->gain;
*TOA = (*TOA) - (maxTOA);
LOG(DEBUG) << "TCH peakAmpl=" << amplitude->abs() << " RMS=" << RMS << " peakToMean=" << peakToMean << " TOA=" << *TOA;
LOG(DEBUG) << "autocorr: " << correlatedBurst;
if (requestChannel && (peakToMean > detectThreshold)) {
float TOAoffset = maxTOA; //gMidambles[TSC]->TOA+(66*samplesPerSymbol-startIx);
delayVector(correlatedBurst,-(*TOA));
// midamble only allows estimation of a 6-tap channel
signalVector channelVector(6*samplesPerSymbol);
float maxEnergy = -1.0;
int maxI = -1;
for (int i = 0; i < 7; i++) {
if (TOAoffset+(i-5)*samplesPerSymbol + channelVector.size() > correlatedBurst.size()) continue;
if (TOAoffset+(i-5)*samplesPerSymbol < 0) continue;
correlatedBurst.segmentCopyTo(channelVector,(int) floor(TOAoffset+(i-5)*samplesPerSymbol),channelVector.size());
float energy = vectorNorm2(channelVector);
if (energy > 0.95*maxEnergy) {
maxI = i;
maxEnergy = energy;
}
}
*channelResponse = new signalVector(channelVector.size());
correlatedBurst.segmentCopyTo(**channelResponse,(int) floor(TOAoffset+(maxI-5)*samplesPerSymbol),(*channelResponse)->size());
scaleVector(**channelResponse,complex(1.0,0.0)/gMidambles[TSC]->gain);
LOG(DEEPDEBUG) << "channelResponse: " << **channelResponse;
if (channelResponseOffset)
*channelResponseOffset = 5*samplesPerSymbol-maxI;
}
return (peakToMean > detectThreshold);
}
// 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;
}
float vectorPower(const signalVector &x)
{
return vectorNorm2(x)/x.size();
}