367 lines
12 KiB
C
367 lines
12 KiB
C
|
|
|
|
#ifdef WIN32
|
|
#define WIN32_LEAN_AND_MEAN // Exclude rarely-used stuff from Windows headers
|
|
#define _CRT_SECURE_NO_DEPRECATE
|
|
|
|
#include <windows.h>
|
|
#endif
|
|
|
|
#include "ARDOPC.h"
|
|
|
|
VOID SortSignals2(float * dblMag, int intStartBin, int intStopBin, int intNumBins, float * dblAVGSignalPerBin, float * dblAVGBaselinePerBin);
|
|
|
|
int LastBusyOn;
|
|
int LastBusyOff;
|
|
|
|
BOOL blnLastBusy = FALSE;
|
|
|
|
float dblAvgStoNSlowNarrow;
|
|
float dblAvgStoNFastNarrow;
|
|
float dblAvgStoNSlowWide;
|
|
float dblAvgStoNFastWide;
|
|
int intLastStart = 0;
|
|
int intLastStop = 0;
|
|
int intBusyOnCnt = 0; // used to filter Busy ON detections
|
|
int intBusyOffCnt = 0; // used to filter Busy OFF detections
|
|
int dttLastBusyTrip = 0;
|
|
int dttPriorLastBusyTrip = 0;
|
|
int dttLastBusyClear = 0;
|
|
int dttLastTrip;
|
|
extern float dblAvgPk2BaselineRatio, dblAvgBaselineSlow, dblAvgBaselineFast;
|
|
int intHoldMs = 5000;
|
|
|
|
|
|
VOID ClearBusy()
|
|
{
|
|
dttLastBusyTrip = Now;
|
|
dttPriorLastBusyTrip = dttLastBusyTrip;
|
|
dttLastBusyClear = dttLastBusyTrip + 610; // This insures test in ARDOPprotocol ~ line 887 will work
|
|
dttLastTrip = dttLastBusyTrip -intHoldMs; // This clears the busy detect immediatly (required for scanning when re enabled by Listen=True
|
|
blnLastBusy = False;
|
|
intBusyOnCnt = 0;
|
|
intBusyOffCnt = 0;
|
|
intLastStart = 0;
|
|
intLastStop = 0; // This will force the busy detector to ignore old averages and initialze the rolling average filters
|
|
}
|
|
|
|
/*
|
|
// Function to implement a busy detector based on 1024 point FFT
|
|
|
|
BOOL BusyDetect2(float * dblMag, int intStart, int intStop) // this only called while searching for leader ...once leader detected, no longer called.
|
|
{
|
|
// each bin is about 12000/1024 or 11.72 Hz
|
|
// this only called while searching for leader ...once leader detected, no longer called.
|
|
// First sort signals and look at highes signals:baseline ratio..
|
|
|
|
float dblAVGSignalPerBinNarrow, dblAVGSignalPerBinWide, dblAVGBaselineNarrow, dblAVGBaselineWide;
|
|
float dblFastAlpha = 0.4f;
|
|
float dblSlowAlpha = 0.2f;
|
|
float dblAvgStoNNarrow, dblAvgStoNWide;
|
|
int intNarrow = 8; // 8 x 11.72 Hz about 94 z
|
|
int intWide = ((intStop - intStart) * 2) / 3; //* 0.66);
|
|
int blnBusy = FALSE;
|
|
float dblAvgStoNSlowNarrow = 0;
|
|
float dblAvgStoNFastNarrow = 0;
|
|
float dblAvgStoNSlowWide = 0;
|
|
float dblAvgStoNFastWide = 0;
|
|
|
|
// First narrow band (~94Hz)
|
|
|
|
SortSignals(dblMag, intStart, intStop, intNarrow, &dblAVGSignalPerBinNarrow, &dblAVGBaselineNarrow);
|
|
|
|
if (intLastStart == intStart && intLastStop == intStop)
|
|
{
|
|
dblAvgStoNSlowNarrow = (1 - dblSlowAlpha) * dblAvgStoNSlowNarrow + dblSlowAlpha * dblAVGSignalPerBinNarrow / dblAVGBaselineNarrow;
|
|
dblAvgStoNFastNarrow = (1 - dblFastAlpha) * dblAvgStoNFastNarrow + dblFastAlpha * dblAVGSignalPerBinNarrow / dblAVGBaselineNarrow;
|
|
}
|
|
else
|
|
{
|
|
dblAvgStoNSlowNarrow = dblAVGSignalPerBinNarrow / dblAVGBaselineNarrow;
|
|
dblAvgStoNFastNarrow = dblAVGSignalPerBinNarrow / dblAVGBaselineNarrow;
|
|
intLastStart = intStart;
|
|
intLastStop = intStop;
|
|
}
|
|
|
|
dblAvgStoNNarrow = max(dblAvgStoNSlowNarrow, dblAvgStoNFastNarrow); // computes fast attack, slow release
|
|
|
|
// Wide band (66% ofr current bandwidth)
|
|
|
|
SortSignals(dblMag, intStart, intStop, intWide, &dblAVGSignalPerBinWide, &dblAVGBaselineWide);
|
|
|
|
if (intLastStart == intStart && intLastStop == intStop)
|
|
{
|
|
dblAvgStoNSlowWide = (1 - dblSlowAlpha) * dblAvgStoNSlowWide + dblSlowAlpha * dblAVGSignalPerBinWide / dblAVGBaselineWide;
|
|
dblAvgStoNFastWide = (1 - dblFastAlpha) * dblAvgStoNFastWide + dblFastAlpha * dblAVGSignalPerBinWide / dblAVGBaselineWide;
|
|
}
|
|
else
|
|
{
|
|
dblAvgStoNSlowWide = dblAVGSignalPerBinWide / dblAVGBaselineWide;
|
|
dblAvgStoNFastWide = dblAVGSignalPerBinWide / dblAVGBaselineWide;
|
|
intLastStart = intStart;
|
|
intLastStop = intStop;
|
|
}
|
|
|
|
dblAvgStoNNarrow = max(dblAvgStoNSlowNarrow, dblAvgStoNFastNarrow); // computes fast attack, slow release
|
|
dblAvgStoNWide = max(dblAvgStoNSlowWide, dblAvgStoNFastWide); // computes fast attack, slow release
|
|
|
|
// Preliminary calibration...future a function of bandwidth and BusyDet.
|
|
|
|
switch (ARQBandwidth)
|
|
{
|
|
case B200MAX:
|
|
case B200FORCED:
|
|
if (dblAvgStoNNarrow > 1.5 * BusyDet|| dblAvgStoNWide > 2.5 * BusyDet)
|
|
blnBusy = True;
|
|
break;
|
|
|
|
case B500MAX:
|
|
case B500FORCED:
|
|
if (dblAvgStoNNarrow > 1.5 * BusyDet || dblAvgStoNWide > 2.5 * BusyDet)
|
|
blnBusy = True;
|
|
break;
|
|
|
|
case B1000MAX:
|
|
case B1000FORCED:
|
|
|
|
if (dblAvgStoNNarrow > 1.4 * BusyDet || dblAvgStoNWide > 2 * BusyDet)
|
|
blnBusy = True;
|
|
break;
|
|
|
|
case B2000MAX:
|
|
case B2000FORCED:
|
|
if (dblAvgStoNNarrow > 1.4 * BusyDet || dblAvgStoNWide > 2 * BusyDet)
|
|
blnBusy = True;
|
|
}
|
|
|
|
if (blnBusy) // This used to skip over one call busy nuisance trips. Busy must be present at least 2 consecutive times to be reported
|
|
{
|
|
intBusyOnCnt += 1;
|
|
intBusyOffCnt = 0;
|
|
if (intBusyOnCnt > 1)
|
|
blnBusy = True;
|
|
else if (!blnBusy)
|
|
{
|
|
intBusyOffCnt += 1;
|
|
intBusyOnCnt = 0;
|
|
if (intBusyOffCnt > 3)
|
|
blnBusy = False;
|
|
}
|
|
}
|
|
if (blnLastBusy == False && blnBusy)
|
|
{
|
|
int x = round(dblAvgStoNNarrow); // odd, but PI doesnt print floats properly
|
|
int y = round(dblAvgStoNWide);
|
|
|
|
blnLastBusy = True;
|
|
LastBusyOn = Now;
|
|
#ifdef __ARM_ARCH
|
|
WriteDebugLog(LOGDEBUG, "[BusyDetect2: BUSY ON StoN Narrow = %d StoN Wide %d", x, y);
|
|
#else
|
|
WriteDebugLog(LOGDEBUG, "[BusyDetect2: BUSY ON StoN Narrow = %f StoN Wide %f", dblAvgStoNNarrow, dblAvgStoNWide);
|
|
#endif
|
|
}
|
|
else if (blnLastBusy == True && !blnBusy)
|
|
{
|
|
int x = round(dblAvgStoNNarrow); // odd, but PI doesnt print floats properly
|
|
int y = round(dblAvgStoNWide);
|
|
|
|
blnLastBusy = False;
|
|
LastBusyOff = Now;
|
|
#ifdef __ARM_ARCH
|
|
WriteDebugLog(LOGDEBUG, "[BusyDetect2: BUSY OFF StoN Narrow = %d StoN Wide %d", x, y);
|
|
#else
|
|
WriteDebugLog(LOGDEBUG, "[BusyDetect2: BUSY OFF StoN Narrow = %f StoN Wide %f", dblAvgStoNNarrow, dblAvgStoNWide);
|
|
#endif
|
|
}
|
|
|
|
return blnLastBusy;
|
|
}
|
|
|
|
|
|
*/
|
|
|
|
|
|
|
|
|
|
BOOL BusyDetect3(float * dblMag, int intStart, int intStop) // this only called while searching for leader ...once leader detected, no longer called.
|
|
{
|
|
// each bin is about 12000/1024 or 11.72 Hz
|
|
// this only called while searching for leader ...once leader detected, no longer called.
|
|
// First sort signals and look at highes signals:baseline ratio..
|
|
|
|
float dblAVGSignalPerBinNarrow, dblAVGSignalPerBinWide, dblAVGBaselineNarrow, dblAVGBaselineWide;
|
|
float dblSlowAlpha = 0.2f;
|
|
float dblAvgStoNNarrow = 0, dblAvgStoNWide = 0;
|
|
int intNarrow = 8; // 8 x 11.72 Hz about 94 z
|
|
int intWide = ((intStop - intStart) * 2) / 3; //* 0.66);
|
|
int blnBusy = FALSE;
|
|
int BusyDet4th = BusyDet * BusyDet * BusyDet * BusyDet;
|
|
|
|
|
|
// First sort signals and look at highest signals:baseline ratio..
|
|
// First narrow band (~94Hz)
|
|
|
|
SortSignals2(dblMag, intStart, intStop, intNarrow, &dblAVGSignalPerBinNarrow, &dblAVGBaselineNarrow);
|
|
|
|
if (intLastStart == intStart && intLastStop == intStop)
|
|
dblAvgStoNNarrow = (1 - dblSlowAlpha) * dblAvgStoNNarrow + dblSlowAlpha * dblAVGSignalPerBinNarrow / dblAVGBaselineNarrow;
|
|
else
|
|
{
|
|
// This initializes the Narrow average after a bandwidth change
|
|
|
|
dblAvgStoNNarrow = dblAVGSignalPerBinNarrow / dblAVGBaselineNarrow;
|
|
intLastStart = intStart;
|
|
intLastStop = intStop;
|
|
}
|
|
|
|
// Wide band (66% of current bandwidth)
|
|
|
|
SortSignals2(dblMag, intStart, intStop, intWide, &dblAVGSignalPerBinWide, &dblAVGBaselineWide);
|
|
|
|
if (intLastStart == intStart && intLastStop == intStop)
|
|
dblAvgStoNWide = (1 - dblSlowAlpha) * dblAvgStoNWide + dblSlowAlpha * dblAVGSignalPerBinWide / dblAVGBaselineWide;
|
|
else
|
|
{
|
|
// This initializes the Wide average after a bandwidth change
|
|
|
|
dblAvgStoNWide = dblAVGSignalPerBinWide / dblAVGBaselineWide;
|
|
intLastStart = intStart;
|
|
intLastStop = intStop;
|
|
}
|
|
|
|
// Preliminary calibration...future a function of bandwidth and BusyDet.
|
|
|
|
switch (ARQBandwidth)
|
|
{
|
|
case XB200:
|
|
blnBusy = (dblAvgStoNNarrow > (3 + 0.008 * BusyDet4th)) || (dblAvgStoNWide > (5 + 0.02 * BusyDet4th));
|
|
break;
|
|
|
|
case XB500:
|
|
blnBusy = (dblAvgStoNNarrow > (3 + 0.008 * BusyDet4th) )|| (dblAvgStoNWide > (5 + 0.02 * BusyDet4th));
|
|
break;
|
|
|
|
case XB2500:
|
|
blnBusy = (dblAvgStoNNarrow > (3 + 0.008 * BusyDet4th)) || (dblAvgStoNWide > (5 + 0.016 * BusyDet4th));
|
|
}
|
|
|
|
if (BusyDet == 0)
|
|
blnBusy = FALSE; // 0 Disables check ?? Is this the best place to do this?
|
|
|
|
// WriteDebugLog(LOGDEBUG, "Busy %d Wide %f Narrow %f", blnBusy, dblAvgStoNWide, dblAvgStoNNarrow);
|
|
|
|
if (blnBusy)
|
|
{
|
|
// This requires multiple adjacent busy conditions to skip over one nuisance Busy trips.
|
|
// Busy must be present at least 3 consecutive times ( ~250 ms) to be reported
|
|
|
|
intBusyOnCnt += 1;
|
|
intBusyOffCnt = 0;
|
|
if (intBusyOnCnt > 3)
|
|
dttLastTrip = Now;
|
|
}
|
|
else
|
|
{
|
|
intBusyOffCnt += 1;
|
|
intBusyOnCnt = 0;
|
|
}
|
|
|
|
if (blnLastBusy == False && intBusyOnCnt >= 3)
|
|
{
|
|
dttPriorLastBusyTrip = dttLastBusyTrip; // save old dttLastBusyTrip for use in BUSYBLOCKING function
|
|
dttLastBusyTrip = Now;
|
|
blnLastBusy = True;
|
|
}
|
|
else
|
|
{
|
|
if (blnLastBusy && (Now - dttLastTrip) > intHoldMs && intBusyOffCnt >= 3)
|
|
{
|
|
dttLastBusyClear = Now;
|
|
blnLastBusy = False;
|
|
}
|
|
}
|
|
return blnLastBusy;
|
|
}
|
|
|
|
VOID SortSignals(float * dblMag, int intStartBin, int intStopBin, int intNumBins, float * dblAVGSignalPerBin, float * dblAVGBaselinePerBin)
|
|
{
|
|
// puts the top intNumber of bins between intStartBin and intStopBin into dblAVGSignalPerBin, the rest into dblAvgBaselinePerBin
|
|
// for decent accuracy intNumBins should be < 75% of intStopBin-intStartBin)
|
|
|
|
float dblAVGSignal[200] = {0};//intNumBins
|
|
float dblAVGBaseline[200] = {0};//intStopBin - intStartBin - intNumBins
|
|
|
|
float dblSigSum = 0;
|
|
float dblTotalSum = 0;
|
|
int intSigPtr = 0;
|
|
int intBasePtr = 0;
|
|
int i, j, k;
|
|
|
|
for (i = 0; i < intNumBins; i++)
|
|
{
|
|
for (j = intStartBin; j <= intStopBin; j++)
|
|
{
|
|
if (i == 0)
|
|
{
|
|
dblTotalSum += dblMag[j];
|
|
if (dblMag[j] > dblAVGSignal[i])
|
|
dblAVGSignal[i] = dblMag[j];
|
|
}
|
|
else
|
|
{
|
|
if (dblMag[j] > dblAVGSignal[i] && dblMag[j] < dblAVGSignal[i - 1])
|
|
dblAVGSignal[i] = dblMag[j];
|
|
}
|
|
}
|
|
}
|
|
|
|
for(k = 0; k < intNumBins; k++)
|
|
{
|
|
dblSigSum += dblAVGSignal[k];
|
|
}
|
|
*dblAVGSignalPerBin = dblSigSum / intNumBins;
|
|
*dblAVGBaselinePerBin = (dblTotalSum - dblSigSum) / (intStopBin - intStartBin - intNumBins + 1);
|
|
}
|
|
|
|
BOOL compare(const void *p1, const void *p2)
|
|
{
|
|
float x = *(const float *)p1;
|
|
float y = *(const float *)p2;
|
|
|
|
if (x < y)
|
|
return -1; // Return -1 if you want ascending, 1 if you want descending order.
|
|
else if (x > y)
|
|
return 1; // Return 1 if you want ascending, -1 if you want descending order.
|
|
|
|
return 0;
|
|
}
|
|
|
|
VOID SortSignals2(float * dblMag, int intStartBin, int intStopBin, int intNumBins, float * dblAVGSignalPerBin, float * dblAVGBaselinePerBin)
|
|
{
|
|
// puts the top intNumber of bins between intStartBin and intStopBin into dblAVGSignalPerBin, the rest into dblAvgBaselinePerBin
|
|
// for decent accuracy intNumBins should be < 75% of intStopBin-intStartBin)
|
|
|
|
// This version uses a native sort function which is much faster and reduces CPU loading significantly on wide bandwidths.
|
|
|
|
float dblSort[202];
|
|
float dblSum1 = 0, dblSum2 = 0;
|
|
int numtoSort = (intStopBin - intStartBin) + 1, i;
|
|
|
|
memcpy(dblSort, &dblMag[intStartBin], numtoSort * sizeof(float));
|
|
|
|
qsort((void *)dblSort, numtoSort, sizeof(float), compare);
|
|
|
|
for (i = numtoSort -1; i >= 0; i--)
|
|
{
|
|
if (i >= (numtoSort - intNumBins))
|
|
dblSum1 += dblSort[i];
|
|
else
|
|
dblSum2 += dblSort[i];
|
|
}
|
|
|
|
*dblAVGSignalPerBin = dblSum1 / intNumBins;
|
|
*dblAVGBaselinePerBin = dblSum2 / (intStopBin - intStartBin - intNumBins - 1);
|
|
}
|
|
|
|
|