Hibbian
/
qtsoundmodem
1
0
Fork 0
Code Issues 1 Pull Requests Packages Projects Releases Wiki Activity
qtsoundmodem/SoundInput.c

5254 lines
168 KiB
C
Raw Normal View History

New upstream version 0.0.0.67
2023-09-04 19:06:44 +01:00
// ARDOP Modem Decode Sound Samples
#include <math.h>
#include "ARDOPC.h"
#pragma warning(disable : 4244) // Code does lots of float to int
#ifndef TEENSY
#define MEMORYARQ
#endif
#undef PLOTWATERFALL
#ifdef PLOTWATERFALL
#define WHITE 0xffff
#define Tomato 0xffff
#define Orange 0xffff
#define Khaki 0xffff
#define Cyan 0xffff
#define DeepSkyBlue 0
#define RoyalBlue 0
#define Navy 0
#define Black 0
#endif
#ifdef TEENSY
#define PKTLED LED3 // flash when packet received
extern unsigned int PKTLEDTimer;
#endif
//#define max(x, y) ((x) > (y) ? (x) : (y))
//#define min(x, y) ((x) < (y) ? (x) : (y))
void SendFrametoHost(unsigned char *data, unsigned dlen);
void CheckandAdjustRXLevel(int maxlevel, int minlevel, BOOL Force);
void mySetPixel(unsigned char x, unsigned char y, unsigned int Colour);
void clearDisplay();
void updateDisplay();
VOID L2Routine(UCHAR * Packet, int Length, int FrameQuality, int totalRSErrors, int NumCar, int pktRXMode);
void RemoveProcessedOFDMData();
BOOL CheckCRC16(unsigned char * Data, int Length);
void DrawAxes(int Qual, const char * Frametype, char * Mode);
extern int lastmax, lastmin; // Sample Levels
char strRcvFrameTag[32];
BOOL blnLeaderFound = FALSE;
int intLeaderRcvdMs = 1000; // Leader length??
extern int intLastRcvdFrameQuality;
extern int intReceivedLeaderLen;
extern UCHAR bytLastReceivedDataFrameType;
extern int NErrors;
extern BOOL blnBREAKCmd;
extern UCHAR bytLastACKedDataFrameType;
extern int intARQDefaultDlyMs;
unsigned int tmrFinalID;
extern BOOL PKTCONNECTED;
extern int LastDemodType;
extern int pktRXMode;
extern int RXOFDMMode;
extern BOOL blnBusyStatus;
BOOL blnLastBusyStatus;
int BusyCount;
short intPriorMixedSamples[120]; // a buffer of 120 samples to hold the prior samples used in the filter
int intPriorMixedSamplesLength = 120; // size of Prior sample buffer
// While searching for leader we must save unprocessed samples
// We may have up to 720 left, so need 1920
short rawSamples[2400]; // Get Frame Type need 2400 and we may add 1200
extern int rawSamplesLength;
int maxrawSamplesLength;
short intFilteredMixedSamples[3500]; // Get Frame Type need 2400 and we may add 1200
int intFilteredMixedSamplesLength = 0;
int MaxFilteredMixedSamplesLength = 0;
int intFrameType= 0; // Type we are decoding
int LastDataFrameType = 0; // Last data frame processed (for Memory ARQ, etc)
char strDecodeCapture[256];
// Frame type parameters
int intCenterFreq = 1500;
float floatCarFreq; //(was int) // Are these the same ??
int intNumCar;
int intSampPerSym;
int intBaud;
int intDataLen;
int intRSLen;
int intSampleLen;
int DataRate = 0; // For SCS Reporting
int intDataPtr;
int intDataBytesPerCar;
BOOL blnOdd;
char strType[18] = "";
char strMod[16] = "";
UCHAR bytMinQualThresh;
int intPSKMode;
int intSymbolsPerByte = 4;
// ARDOP V2 has max 10 carriers and 160 (120 + 40RS) per carrier
#define MAX_RAW_LENGTH 163 // Len Byte + Data + RS + CRC I think!
#define MAX_RAW_LENGTH_FSK 43 // MAX FSK 32 data + 8 RS
// OFDM is MAXCAR * 100
// 10 carrier 16QAM id 10 * 160
#define MAX_DATA_LENGTH MAXCAR * 100 // I think! (OFDM 16QAM)
// intToneMags should be an array with one row per carrier.
// and 16 * max bytes data (2 bits per symbol, 4 samples per symbol in 4FSK.
// but as 600 Baud frames are very long (750 bytes), but only one carrier
// may be better to store as scalar and calculate offsets into it for each carrier
// treat 600 as 3 * 200, but scalar still may be better
// Needs 64K if ints + another 64 for MEM ARQ. (maybe able to store as shorts)
// 48K would do if we use a scalar (600 baud, 750 bytes)
// Max is 4 carrier, 83 bytes or 1 carrier 762 (or treat as 3 * 253)
// Could just about do this on Teensy 3.6 or Nucleo F7
// looks like we have 4 samples for each 2 bits, which means 16 samples per byte.
// ARDOP 2 only has one and two carrier FSK modes
// Teensy is rather short of RAM, but as we never receive FSK and PSK
// at the same time we can use same data area (saves about 20K)
int intToneMagsIndex[2];
// Same here
int intSumCounts[MAXCAR]; // number in above arrays
int intToneMagsLength;
unsigned char goodCarriers = 0; // Carriers we have already decoded
// We always collect all phases for PSK and QAM so we can do phase correction
// Max PSK frame is 83, 4 samples per byte = 332
// Max 16QAM frame is 163, 2 samples per byte = 326
// OFDM frames are shorter, so carriers 11 - 17 could have smaller sample buffers
// This is a bit complicated, but allows smaller buffers for the OFDM carriers (Needed for Teensy)
//short intPhases[MAXCAR][332] = {0};
short QAMPhases[10][332]; // 6640 bytes
short OFDMPhases[MAXCAR - 10][232]; // Need 232 = (PSK2 8 * 29); 15312
short * Phaseptrs[MAXCAR] =
{&QAMPhases[0][0], &QAMPhases[1][0], &QAMPhases[2][0], &QAMPhases[3][0], &QAMPhases[4][0],
&QAMPhases[5][0], &QAMPhases[6][0], &QAMPhases[7][0], &QAMPhases[8][0], &QAMPhases[9][0],
&OFDMPhases[0][0], &OFDMPhases[1][0], &OFDMPhases[2][0], &OFDMPhases[3][0], &OFDMPhases[4][0],
&OFDMPhases[5][0], &OFDMPhases[6][0], &OFDMPhases[7][0], &OFDMPhases[8][0], &OFDMPhases[9][0],
&OFDMPhases[10][0], &OFDMPhases[11][0], &OFDMPhases[12][0], &OFDMPhases[13][0], &OFDMPhases[14][0],
&OFDMPhases[15][0], &OFDMPhases[16][0], &OFDMPhases[17][0], &OFDMPhases[18][0], &OFDMPhases[19][0],
&OFDMPhases[20][0], &OFDMPhases[21][0], &OFDMPhases[22][0], &OFDMPhases[23][0], &OFDMPhases[24][0],
&OFDMPhases[25][0], &OFDMPhases[26][0], &OFDMPhases[27][0], &OFDMPhases[28][0], &OFDMPhases[29][0],
&OFDMPhases[30][0], &OFDMPhases[31][0], &OFDMPhases[32][0]};
short ** intPhases = &Phaseptrs[0];
short QAMMags[10][332];
short OFDMMags[MAXCAR - 10][232];
short * Magptrs[MAXCAR] =
{&QAMMags[0][0], &QAMMags[1][0], &QAMMags[2][0], &QAMMags[3][0], &QAMMags[4][0],
&QAMMags[5][0], &QAMMags[6][0], &QAMMags[7][0], &QAMMags[8][0], &QAMMags[9][0],
&OFDMMags[0][0], &OFDMMags[1][0], &OFDMMags[2][0], &OFDMMags[3][0], &OFDMMags[4][0],
&OFDMMags[5][0], &OFDMMags[6][0], &OFDMMags[7][0], &OFDMMags[8][0], &OFDMMags[9][0],
&OFDMMags[10][0], &OFDMMags[11][0], &OFDMMags[12][0], &OFDMMags[13][0], &OFDMMags[14][0],
&OFDMMags[15][0], &OFDMMags[16][0], &OFDMMags[17][0], &OFDMMags[18][0], &OFDMMags[19][0],
&OFDMMags[20][0], &OFDMMags[21][0], &OFDMMags[22][0], &OFDMMags[23][0], &OFDMMags[24][0],
&OFDMMags[25][0], &OFDMMags[26][0], &OFDMMags[27][0], &OFDMMags[28][0], &OFDMMags[29][0],
&OFDMMags[30][0], &OFDMMags[31][0], &OFDMMags[32][0]};
short ** intMags = &Magptrs[0];
//int Tones[2][16 * MAX_RAW_LENGTH_FSK];
int intToneMags[4][16 * MAX_RAW_LENGTH_FSK] = {0}; // Need one per carrier
// We need 5504 bytes for FSK but can overlay on PSK data areas
//int * Toneptrs[2] = {(int *)&Tones[0][0], (int *)&Tones[1][0]};
//int ** intToneMags = &Toneptrs[0];
#ifdef MEMORYARQ
// Enough RAM for memory ARQ so keep all samples for FSK and a copy of tones or phase/amplitude
int intToneMagsAvg[2][332]; //???? FSK Tone averages
short intCarPhaseAvg[MAXCAR][332]; // array to accumulate phases for averaging (Memory ARQ)
short intCarMagAvg[MAXCAR][332]; // array to accumulate mags for averaging (Memory ARQ)
#endif
//219 /3 * 8= 73 * 8 = 584
//163 * 4 = 652
// If we do Mem ARQ we will need a fair amount of RAM
int intPhasesLen;
// Received Frame
UCHAR bytData[128 * 80]; // Max OFDM Window
int frameLen;
int totalRSErrors;
// for comparing with CarrierOK
const char Good[MAXCAR] = {1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1}; // All Good
const char Bad[MAXCAR] = {0}; // All bad
// We need one raw buffer per carrier
// This can be optimized quite a bit to save space
// We can probably overlay on bytData
// If we still have 600 baud modes may need a lot more for first
// Note OFDM doesn't need one per carrier so only need 10
UCHAR bytFrameData[10][MAX_RAW_LENGTH + 10]; // Received chars
char CarrierOk[MAXCAR]; // RS OK Flags per carrier
int RepeatedFrame = 0; // set if this dats frame is a repeat
int charIndex = 0; // Index into received chars
int SymbolsLeft; // number still to decode
int DummyCarrier = 0; // pseudo carrier used for long 600 baud frames
UCHAR * Decode600Buffer;
BOOL PSKInitDone = FALSE;
BOOL blnSymbolSyncFound, blnFrameSyncFound;
extern UCHAR bytLastARQSessionID;
extern UCHAR bytCurrentFrameType;
extern int intShiftUpDn;
extern const char ARQSubStates[10][11];
extern int intLastARQDataFrameToHost;
// dont think I need it short intRcvdSamples[12000]; // 1 second. May need to optimise
float dblOffsetLastGoodDecode = 0;
int dttLastGoodFrameTypeDecode = -20000;
float dblOffsetHz = 0;;
int dttLastLeaderDetect;
extern int intRmtLeaderMeasure;
extern BOOL blnARQConnected;
extern BOOL blnPending;
extern UCHAR bytPendingSessionID;
extern UCHAR bytSessionID;
int dttLastGoodFrameTypeDecod;
int dttStartRmtLeaderMeasure;
char lastGoodID[11] = "";
int GotBitSyncTicks;
int intARQRTmeasuredMs;
float dbl2Pi = 2 * M_PI;
float dblSNdBPwr;
float dblNCOFreq = 3000; // nominal NC) frequency
float dblNCOPhase = 0;
float dblNCOPhaseInc = 2 * M_PI * 3000 / 12000; // was dblNCOFreq
float dblPwrSNPower_dBPrior = 0;
float dblPhaseDiff1_2Avg; // an initial value of -10 causes initialization in AcquireFrameSyncRSBAvg
int intMFSReadPtr = 0; // reset the MFSReadPtr offset 30 to accomodate the filter delay
int RcvdSamplesLen = 0; // Samples in RX buffer
float dblPhaseDiff1_2Avg;
int intPhaseError = 0;
BOOL Acquire2ToneLeaderSymbolFraming();
BOOL SearchFor2ToneLeader4(short * intNewSamples, int Length, float * dblOffsetHz, int * intSN);
BOOL AcquireFrameSyncRSB();
BOOL AcquireFrameSyncRSBAvg();
int Acquire4FSKFrameType();
void DemodulateFrame(int intFrameType);
void Demod1Car4FSKChar(int Start, UCHAR * Decoded, int Carrier);
VOID Track1Car4FSK(short * intSamples, int * intPtr, int intSampPerSymbol, float intSearchFreq, int intBaud, UCHAR * bytSymHistory);
VOID Decode1CarPSK(int Carrier, BOOL OFDM);
int EnvelopeCorrelator();
int EnvelopeCorrelatorNew();
BOOL DecodeFrame(int intFrameType, UCHAR * bytData);
void Update4FSKConstellation(int * intToneMags, int * intQuality);
void Update16FSKConstellation(int * intToneMags, int * intQuality);
void Update8FSKConstellation(int * intToneMags, int * intQuality);
void ProcessPingFrame(char * bytData);
int Compute4FSKSN();
void DemodPSK();
BOOL DemodQAM();
BOOL DemodOFDM();
BOOL Decode4FSKOFDMACK();
void PrintCarrierFlags()
{
char Msg[128];
if (intNumCar == 1)
Debugprintf("MEMARQ Flags %d", CarrierOk[0]);
else if (intNumCar == 2)
Debugprintf("MEMARQ Flags %d %d", CarrierOk[0], CarrierOk[1]);
else
{
sprintf(Msg, "MEMARQ Flags %d %d %d %d %d %d %d %d %d %d %d %d %d %d %d %d %d %d %d %d %d %d %d %d %d %d %d %d %d %d %d %d %d %d %d %d %d %d %d %d %d %d %d",
CarrierOk[0], CarrierOk[1], CarrierOk[2], CarrierOk[3], CarrierOk[4], CarrierOk[5], CarrierOk[6], CarrierOk[7], CarrierOk[8], CarrierOk[9],
CarrierOk[10], CarrierOk[11], CarrierOk[12], CarrierOk[13], CarrierOk[14], CarrierOk[15], CarrierOk[16], CarrierOk[17], CarrierOk[18], CarrierOk[19],
CarrierOk[20], CarrierOk[21], CarrierOk[22], CarrierOk[23], CarrierOk[24], CarrierOk[25], CarrierOk[26], CarrierOk[27], CarrierOk[28], CarrierOk[29],
CarrierOk[30], CarrierOk[31], CarrierOk[32], CarrierOk[33], CarrierOk[34], CarrierOk[35], CarrierOk[36], CarrierOk[37], CarrierOk[38], CarrierOk[39],
CarrierOk[40], CarrierOk[41], CarrierOk[42]);
Msg[12 + 2 * intNumCar] = 0;
Debugprintf(Msg);
}
}
// Function to determine if frame type is short control frame
BOOL IsShortControlFrame(UCHAR bytType)
{
switch (intFrameType)
{
case DataNAK:
case DataNAKLoQ:
case ConRejBusy:
case ConRejBW:
case ConAck:
case DISCFRAME:
case BREAK:
case END:
case IDLEFRAME:
case DataACK:
case DataACKHiQ:
return TRUE;
}
return FALSE;
}
BOOL IsConReqFrame(UCHAR bytType)
{
switch (bytType)
{
case ConReq200:
case ConReq500:
case ConReq2500:
case OConReq500:
case OConReq2500:
return TRUE;
}
return FALSE;
}
// Function to determine if it is a data frame (Even OR Odd)
BOOL IsDataFrame(UCHAR intFrameType)
{
const char * String = Name(intFrameType);
if (intFrameType == PktFrameHeader)
return TRUE;
if (String == NULL || String[0] == 0)
return FALSE;
if (strstr(String, ".E") || strstr(String, ".O"))
return TRUE;
return FALSE;
}
// Subroutine to clear all mixed samples
void ClearAllMixedSamples()
{
intFilteredMixedSamplesLength = 0;
intMFSReadPtr = 0;
rawSamplesLength = 0; // Clear saved
}
// Subroutine to Initialize mixed samples
void InitializeMixedSamples()
{
// Measure the time from release of PTT to leader detection of reply.
intARQRTmeasuredMs = min(10000, Now - dttStartRTMeasure); //?????? needs work
intPriorMixedSamplesLength = 120; // zero out prior samples in Prior sample buffer
intFilteredMixedSamplesLength = 0; // zero out the FilteredMixedSamples array
intMFSReadPtr = 0; // reset the MFSReadPtr offset 30 to accomodate the filter delay
}
// Subroutine to discard all sampled prior to current intRcvdSamplesRPtr
void DiscardOldSamples()
{
// This restructures the intRcvdSamples array discarding all samples prior to intRcvdSamplesRPtr
//not sure why we need this !!
/*
if (RcvdSamplesLen - intRcvdSamplesRPtr <= 0)
RcvdSamplesLen = intRcvdSamplesRPtr = 0;
else
{
// This is rather slow. I'd prefer a cyclic buffer. Lets see....
memmove(intRcvdSamples, &intRcvdSamples[intRcvdSamplesRPtr], (RcvdSamplesLen - intRcvdSamplesRPtr)* 2);
RcvdSamplesLen -= intRcvdSamplesRPtr;
intRcvdSamplesRPtr = 0;
}
*/
}
// Subroutine to apply 2000 Hz filter to mixed samples
float xdblZin_1 = 0, xdblZin_2 = 0, xdblZComb= 0; // Used in the comb generator
// The resonators
float xdblZout_0[29] = {0.0f}; // resonator outputs
float xdblZout_1[29] = {0.0f}; // resonator outputs delayed one sample
float xdblZout_2[29] = {0.0f}; // resonator outputs delayed two samples
float xdblCoef[29] = {0.0}; // the coefficients
float xdblR = 0.9995f; // insures stability (must be < 1.0) (Value .9995 7/8/2013 gives good results)
int xintN = 120; //Length of filter 12000/100
void FSMixFilter2500Hz(short * intMixedSamples, int intMixedSamplesLength)
{
// assumes sample rate of 12000
// implements 27 100 Hz wide sections (~2500 Hz wide @ - 30dB centered on 1500 Hz)
// FSF (Frequency Selective Filter) variables
// This works on intMixedSamples, len intMixedSamplesLength;
// Filtered data is appended to intFilteredMixedSamples
float dblRn;
float dblR2;
float dblZin = 0;
int i, j;
float intFilteredSample = 0; // Filtered sample
if (intFilteredMixedSamplesLength < 0)
Debugprintf("Corrupt intFilteredMixedSamplesLength");
dblRn = powf(xdblR, xintN);
dblR2 = powf(xdblR, 2);
// Initialize the coefficients
if (xdblCoef[28] == 0)
{
for (i = 2; i <= 28; i++)
{
xdblCoef[i] = 2 * xdblR * cosf(2 * M_PI * i / xintN); // For Frequency = bin i
}
}
for (i = 0; i < intMixedSamplesLength; i++)
{
intFilteredSample = 0;
if (i < xintN)
dblZin = intMixedSamples[i] - dblRn * intPriorMixedSamples[i];
else
dblZin = intMixedSamples[i] - dblRn * intMixedSamples[i - xintN];
//Compute the Comb
xdblZComb = dblZin - xdblZin_2 * dblR2;
xdblZin_2 = xdblZin_1;
xdblZin_1 = dblZin;
// Now the resonators
for (j = 2; j <= 28; j++) // calculate output for 3 resonators
{
xdblZout_0[j] = xdblZComb + xdblCoef[j] * xdblZout_1[j] - dblR2 * xdblZout_2[j];
xdblZout_2[j] = xdblZout_1[j];
xdblZout_1[j] = xdblZout_0[j];
//' scale each by transition coeff and + (Even) or - (Odd)
//' Resonators 2 and 13 scaled by .389 get best shape and side lobe supression
//' Scaling also accomodates for the filter "gain" of approx 60.
if (j == 2 || j == 28)
intFilteredSample += 0.389f * xdblZout_0[j];
else if ((j & 1) == 0)
intFilteredSample += xdblZout_0[j];
else
intFilteredSample -= xdblZout_0[j];
}
intFilteredSample = intFilteredSample * 0.00833333333f;
intFilteredMixedSamples[intFilteredMixedSamplesLength++] = intFilteredSample; // rescales for gain of filter
}
// update the prior intPriorMixedSamples array for the next filter call
memmove(intPriorMixedSamples, &intMixedSamples[intMixedSamplesLength - xintN], intPriorMixedSamplesLength * 2);
if (intFilteredMixedSamplesLength > MaxFilteredMixedSamplesLength)
MaxFilteredMixedSamplesLength = intFilteredMixedSamplesLength;
if (intFilteredMixedSamplesLength > 3500)
Debugprintf("Corrupt intFilteredMixedSamplesLength %d", intFilteredMixedSamplesLength);
}
// Function to apply 150Hz filter used in Envelope correlator
void Filter150Hz(short * intFilterOut)
{
// assumes sample rate of 12000
// implements 3 100 Hz wide sections (~150 Hz wide @ - 30dB centered on 1500 Hz)
// FSF (Frequency Selective Filter) variables
static float dblR = 0.9995f; // insures stability (must be < 1.0) (Value .9995 7/8/2013 gives good results)
static int intN = 120; //Length of filter 12000/100
static float dblRn;
static float dblR2;
static float dblCoef[17] = {0.0}; // the coefficients
float dblZin = 0, dblZin_1 = 0, dblZin_2 = 0, dblZComb= 0; // Used in the comb generator
// The resonators
float dblZout_0[17] = {0.0}; // resonator outputs
float dblZout_1[17] = {0.0}; // resonator outputs delayed one sample
float dblZout_2[17] = {0.0}; // resonator outputs delayed two samples
int i, j;
float FilterOut = 0; // Filtered sample
float largest = 0;
dblRn = powf(dblR, intN);
dblR2 = powf(dblR, 2);
// Initialize the coefficients
if (dblCoef[17] == 0)
{
for (i = 14; i <= 16; i++)
{
dblCoef[i] = 2 * dblR * cosf(2 * M_PI * i / intN); // For Frequency = bin i
}
}
for (i = 0; i < 480; i++)
{
if (i < intN)
dblZin = intFilteredMixedSamples[intMFSReadPtr + i] - dblRn * 0; // no prior mixed samples
else
dblZin = intFilteredMixedSamples[intMFSReadPtr + i] - dblRn * intFilteredMixedSamples[intMFSReadPtr + i - intN];
// Compute the Comb
dblZComb = dblZin - dblZin_2 * dblR2;
dblZin_2 = dblZin_1;
dblZin_1 = dblZin;
// Now the resonators
for (j = 14; j <= 16; j++) // calculate output for 3 resonators
{
dblZout_0[j] = dblZComb + dblCoef[j] * dblZout_1[j] - dblR2 * dblZout_2[j];
dblZout_2[j] = dblZout_1[j];
dblZout_1[j] = dblZout_0[j];
// scale each by transition coeff and + (Even) or - (Odd)
// Scaling also accomodates for the filter "gain" of approx 120.
// These transition coefficients fairly close to optimum for WGN 0db PSK4, 100 baud (yield highest average quality) 5/24/2014
if (j == 14 || j == 16)
FilterOut = 0.2f * dblZout_0[j]; // this transisiton minimizes ringing and peaks
else
FilterOut -= dblZout_0[j];
}
intFilterOut[i] = (int)ceil(FilterOut * 0.00833333333); // rescales for gain of filter
}
}
// Function to apply 75Hz filter used in Envelope correlator
void Filter75Hz(short * intFilterOut, BOOL blnInitialise, int intSamplesToFilter)
{
// assumes sample rate of 12000
// implements 3 100 Hz wide sections (~150 Hz wide @ - 30dB centered on 1500 Hz)
// FSF (Frequency Selective Filter) variables
static float dblR = 0.9995f; // insures stability (must be < 1.0) (Value .9995 7/8/2013 gives good results)
static int intN = 240; //Length of filter 12000/50 - delays output 120 samples from input
static float dblRn;
static float dblR2;
static float dblCoef[3] = {0.0}; // the coefficients
float dblZin = 0, dblZin_1 = 0, dblZin_2 = 0, dblZComb= 0; // Used in the comb generator
// The resonators
float dblZout_0[3] = {0.0}; // resonator outputs
float dblZout_1[3] = {0.0}; // resonator outputs delayed one sample
float dblZout_2[3] = {0.0}; // resonator outputs delayed two samples
int i, j;
float FilterOut = 0; // Filtered sample
float largest = 0;
dblRn = powf(dblR, intN);
dblR2 = powf(dblR, 2);
// Initialize the coefficients
if (dblCoef[2] == 0)
{
for (i = 0; i <= 3; i++)
{
dblCoef[i] = 2 * dblR * cosf(2 * M_PI * (29 + i)/ intN); // For Frequency = bin 29, 30, 31
}
}
for (i = 0; i < intSamplesToFilter; i++)
{
if (i < intN)
dblZin = intFilteredMixedSamples[intMFSReadPtr + i] - dblRn * 0; // no prior mixed samples
else
dblZin = intFilteredMixedSamples[intMFSReadPtr + i] - dblRn * intFilteredMixedSamples[intMFSReadPtr + i - intN];
// Compute the Comb
dblZComb = dblZin - dblZin_2 * dblR2;
dblZin_2 = dblZin_1;
dblZin_1 = dblZin;
// Now the resonators
for (j = 0; j < 3; j++) // calculate output for 3 resonators
{
dblZout_0[j] = dblZComb + dblCoef[j] * dblZout_1[j] - dblR2 * dblZout_2[j];
dblZout_2[j] = dblZout_1[j];
dblZout_1[j] = dblZout_0[j];
// scale each by transition coeff and + (Even) or - (Odd)
// Scaling also accomodates for the filter "gain" of approx 120.
// These transition coefficients fairly close to optimum for WGN 0db PSK4, 100 baud (yield highest average quality) 5/24/2014
if (j == 0 || j == 2)
FilterOut -= 0.39811f * dblZout_0[j]; // this transisiton minimizes ringing and peaks
else
FilterOut += dblZout_0[j];
}
intFilterOut[i] = (int)ceil(FilterOut * 0.0041f); // rescales for gain of filter
}
}
// Subroutine to Mix new samples with NCO to tune to nominal 1500 Hz center with reversed sideband and filter.
void MixNCOFilter(short * intNewSamples, int Length, float dblOffsetHz)
{
// Correct the dimension of intPriorMixedSamples if needed (should only happen after a bandwidth setting change).
int i;
short intMixedSamples[2400]; // All we need at once ( I hope!) // may need to be int
int intMixedSamplesLength = 0; //size of intMixedSamples
if (Length == 0)
return;
// Nominal NCO freq is 3000 Hz to downmix intNewSamples (NCO - Fnew) to center of 1500 Hz (invertes the sideband too)
dblNCOFreq = 3000 + dblOffsetHz;
dblNCOPhaseInc = dblNCOFreq * dbl2Pi / 12000;
intMixedSamplesLength = Length;
for (i = 0; i < Length; i++)
{
intMixedSamples[i] = (int)ceilf(intNewSamples[i] * cosf(dblNCOPhase)); // later may want a lower "cost" implementation of "Cos"
dblNCOPhase += dblNCOPhaseInc;
if (dblNCOPhase > dbl2Pi)
dblNCOPhase -= dbl2Pi;
}
// showed no significant difference if the 2000 Hz filer used for all bandwidths.
// printtick("Start Filter");
FSMixFilter2500Hz(intMixedSamples, intMixedSamplesLength); // filter through the FS filter (required to reject image from Local oscillator)
// printtick("Done Filter");
// save for analysys
// WriteSamples(&intFilteredMixedSamples[oldlen], Length);
// WriteSamples(intMixedSamples, Length);
}
// Function to Correct Raw demodulated data with Reed Solomon FEC
int CorrectRawDataWithRS(UCHAR * bytRawData, UCHAR * bytCorrectedData, int intDataLen, int intRSLen, int bytFrameType, int Carrier)
{
BOOL blnRSOK;
BOOL FrameOK;
BOOL OK;
//Dim bytNoRS(1 + intDataLen + 2 - 1) As Byte ' 1 byte byte Count, Data, 2 byte CRC
//Array.Copy(bytRawData, 0, bytNoRS, 0, bytNoRS.Length)
if (CarrierOk[Carrier]) // Already decoded this carrier?
{
// Athough we have already checked the data, it may be in the wrong place
// in the buffer if another carrier was decoded wrong.
memcpy(bytCorrectedData, &bytRawData[1], bytRawData[0] + 1); // Extra char in case OFDM
if (strFrameType[LastDataFrameType][0] == 'O')
Debugprintf("[CorrectRawDataWithRS] Carrier %d already decoded Block %d, Len %d", Carrier, bytRawData[1], bytRawData[0]);
else
Debugprintf("[CorrectRawDataWithRS] Carrier %d already decoded Len %d", Carrier, bytRawData[0]);
return bytRawData[0]; // don't do it again
}
if (strFrameType[intFrameType][0] == 'O')
OK = CheckCRC16(bytRawData, intDataLen + 1);
else
OK = CheckCRC16FrameType(bytRawData, intDataLen + 1, bytFrameType);
// As crc can fail also check returned lenght is reasonable
if (OK && bytRawData[0] <= intDataLen) // No RS correction needed // return the actual data
{
memcpy(bytCorrectedData, &bytRawData[1], bytRawData[0] + 1);
if (strFrameType[intFrameType][0] == 'O')
Debugprintf("[CorrectRawDataWithRS] Carrier %d OK without RS, Block %d Len = %d", Carrier, bytRawData[1], bytRawData[0]);
else
Debugprintf("[CorrectRawDataWithRS] Carrier %d OK without RS, Len = %d", Carrier, bytRawData[0]);
CarrierOk[Carrier] = TRUE;
return bytRawData[0];
}
// Try correcting with RS Parity
FrameOK = RSDecode(bytRawData, intDataLen + 3 + intRSLen, intRSLen, &blnRSOK);
if (blnRSOK)
{}
// Debugprintf("RS Says OK without correction");
else
if (FrameOK)
{}
// Debugprintf("RS Says OK after %d correction(s)", NErrors);
else
{
Debugprintf("[intFrameType] RS Says Can't Correct");
goto returnBad;
}
if (FrameOK)
{
if (strFrameType[intFrameType][0] == 'O')
OK = CheckCRC16(bytRawData, intDataLen + 1);
else
OK = CheckCRC16FrameType(bytRawData, intDataLen + 1, bytFrameType);
if (OK && bytRawData[0] <= intDataLen) // Now OK - return the actual data
{
int intFailedByteCnt = 0;
if (strFrameType[intFrameType][0] == 'O')
Debugprintf("[CorrectRawDataWithRS] Carrier %d OK with RS %d corrections, Block %d, Len = %d", Carrier, NErrors, bytRawData[1], bytRawData[0]);
else
Debugprintf("[CorrectRawDataWithRS] Carrier %d OK with RS %d corrections, Len = %d", Carrier, NErrors, bytRawData[0]);
totalRSErrors += NErrors;
memcpy(bytCorrectedData, &bytRawData[1], bytRawData[0] + 1);
CarrierOk[Carrier] = TRUE;
return bytRawData[0];
}
Debugprintf("[CorrectRawDataWithRS] Carrier %d RS says ok but CRC still bad", Carrier);
}
// return uncorrected data without byte count or RS Parity
returnBad:
memcpy(bytCorrectedData, &bytRawData[1], intDataLen + 1);
CarrierOk[Carrier] = FALSE;
return intDataLen;
}
// Subroutine to process new samples as received from the sound card via Main.ProcessCapturedData
// Only called when not transmitting
double dblPhaseInc; // in milliradians
short intNforGoertzel[MAXCAR];
short intPSKPhase_1[MAXCAR], intPSKPhase_0[MAXCAR];
short intCP[MAXCAR]; // Cyclic prefix offset
float dblFreqBin[MAXCAR];
BOOL CheckFrameTypeParity(int intTonePtr, int * intToneMags);
void ARDOPProcessNewSamples(short * Samples, int nSamples)
{
BOOL blnFrameDecodedOK = FALSE;
// LookforUZ7HOLeader(Samples, nSamples);
// printtick("Start afsk");
// DemodAFSK(Samples, nSamples);
// printtick("End afsk");
// return;
if (ProtocolState == FECSend)
return;
// Append new data to anything in rawSamples
memcpy(&rawSamples[rawSamplesLength], Samples, nSamples * 2);
rawSamplesLength += nSamples;
if (rawSamplesLength > maxrawSamplesLength)
maxrawSamplesLength = rawSamplesLength;
if (rawSamplesLength >= 2400)
Debugprintf("Corrupt rawSamplesLength %d", rawSamplesLength);
nSamples = rawSamplesLength;
Samples = rawSamples;
rawSamplesLength = 0;
// printtick("Start Busy");
if (nSamples >= 1024)
UpdateBusyDetector(Samples);
// printtick("Done Busy");
// it seems that searchforleader runs on unmixed and unfilered samples
// Searching for leader
if (State == SearchingForLeader)
{
// Search for leader as long as 960 samples (8 symbols) available
// printtick("Start Leader Search");
if (nSamples >= 1200)
{
if (ProtocolState == FECSend)
return;
}
while (State == SearchingForLeader && nSamples >= 1200)
{
int intSN;
blnLeaderFound = SearchFor2ToneLeader4(Samples, nSamples, &dblOffsetHz, &intSN);
// blnLeaderFound = SearchFor2ToneLeader2(Samples, nSamples, &dblOffsetHz, &intSN);
if (blnLeaderFound)
{
// Debugprintf("Got Leader");
dttLastLeaderDetect = Now;
nSamples -= 480;
Samples += 480; // !!!! needs attention !!!
InitializeMixedSamples();
State = AcquireSymbolSync;
}
else
{
if (SlowCPU)
{
nSamples -= 480;
Samples += 480; // advance pointer 2 symbols (40 ms) ' reduce CPU loading
}
else
{
nSamples -= 240;
Samples += 240; // !!!! needs attention !!!
}
}
}
if (State == SearchingForLeader)
{
// Save unused samples
memmove(rawSamples, Samples, nSamples * 2);
rawSamplesLength = nSamples;
// printtick("End Leader Search");
return;
}
}
// Got leader
// At this point samples haven't been processed, and are in Samples, len nSamples
// I'm going to filter all samples into intFilteredMixedSamples.
// printtick("Start Mix");
MixNCOFilter(Samples, nSamples, dblOffsetHz); // Mix and filter new samples (Mixing consumes all intRcvdSamples)
nSamples = 0; // all used
// printtick("Done Mix Samples");
// Acquire Symbol Sync
if (State == AcquireSymbolSync)
{
if ((intFilteredMixedSamplesLength - intMFSReadPtr) > 960)
{
blnSymbolSyncFound = Acquire2ToneLeaderSymbolFraming(); // adjust the pointer to the nominal symbol start based on phase
if (blnSymbolSyncFound)
State = AcquireFrameSync;
else
{
// Rick's Latest code (2.0.3) advances pointer instead of clearing samples
// DiscardOldSamples();
// ClearAllMixedSamples();
intMFSReadPtr += 240; // advance the MFSReadPointer one symbol and try to search for leader again.
State = SearchingForLeader;
return;
}
// printtick("Got Sym Sync");
}
}
// Acquire Frame Sync
if (State == AcquireFrameSync)
{
blnFrameSyncFound = AcquireFrameSyncRSB();
// Remove used samples
intFilteredMixedSamplesLength -= intMFSReadPtr;
if (intFilteredMixedSamplesLength < 0)
Debugprintf("Corrupt intFilteredMixedSamplesLength");
memmove(intFilteredMixedSamples,
&intFilteredMixedSamples[intMFSReadPtr], intFilteredMixedSamplesLength * 2);
intMFSReadPtr = 0;
if (blnFrameSyncFound)
{
State = AcquireFrameType;
// Have frame Sync. Remove used samples from buffer
printtick("Got Frame Sync");
}
else if ((Now - dttLastLeaderDetect) > 1000) // no Frame sync within 1000 ms (may want to make this limit a funciton of Mode and leaders)
{
DiscardOldSamples();
ClearAllMixedSamples();
State = SearchingForLeader;
printtick("frame sync timeout");
}
/*
else if (intPhaseError > 2)
{
DiscardOldSamples();
ClearAllMixedSamples();
State = SearchingForLeader;
printtick("frame sync timeout");
}
// else
// printtick("no frame sync");
*/
}
// Acquire Frame Type
if (State == AcquireFrameType)
{
// printtick("getting frame type");
intFrameType = Acquire4FSKFrameType();
if (intFrameType == -2)
{
// sprintf(Msg, "not enough %d %d", intFilteredMixedSamplesLength, intMFSReadPtr);
// printtick(Msg);
return; // insufficient samples
}
if (intFrameType == -1) // poor decode quality (large decode distance)
{
State = SearchingForLeader;
ClearAllMixedSamples();
DiscardOldSamples();
Debugprintf("poor frame type decode");
// stcStatus.BackColor = SystemColors.Control
// stcStatus.Text = ""
// stcStatus.ControlName = "lblRcvFrame"
// queTNCStatus.Enqueue(stcStatus)
}
else
{
// Get Frame info and Initialise Demodulate variables
// We've used intMFSReadPtr samples, so remove from Buffer
// sprintf(Msg, "Got Frame Type %x", intFrameType);
// printtick(Msg);
blnLastBusyStatus = 1;
blnBusyStatus = 1;
BusyCount = 10;
intFilteredMixedSamplesLength -= intMFSReadPtr;
if (intFilteredMixedSamplesLength < 0)
Debugprintf("Corrupt intFilteredMixedSamplesLength");
memmove(intFilteredMixedSamples,
&intFilteredMixedSamples[intMFSReadPtr], intFilteredMixedSamplesLength * 2);
intMFSReadPtr = 0;
if (!FrameInfo(intFrameType, &blnOdd, &intNumCar, strMod, &intBaud, &intDataLen, &intRSLen, &bytMinQualThresh, strType))
{
printtick("bad frame type");
State = SearchingForLeader;
ClearAllMixedSamples();
DiscardOldSamples();
return;
}
if (IsShortControlFrame(intFrameType))
{
// Frame has no data so is now complete
DrawRXFrame(1, Name(intFrameType));
// See if IRStoISS shortcut can be invoked
// prepare for next
DiscardOldSamples();
ClearAllMixedSamples();
State = SearchingForLeader;
blnFrameDecodedOK = TRUE;
Debugprintf("[DecodeFrame] Frame: %s ", Name(intFrameType));
DecodeCompleteTime = Now;
goto ProcessFrame;
}
DrawRXFrame(0, Name(intFrameType));
if (intBaud == 25)
intSampPerSym = 480;
else if (intBaud == 50)
intSampPerSym = 240;
else if (intBaud == 55)
intSampPerSym = 216;
else if (intBaud == 100)
intSampPerSym = 120;
else if (intBaud == 167)
intSampPerSym = 72;
else if (intBaud == 600)
intSampPerSym = 20;
if (IsDataFrame(intFrameType))
SymbolsLeft = intDataLen + intRSLen + 3; // Data has crc + length byte
else if (intFrameType == OFDMACK)
SymbolsLeft = intDataLen + intRSLen + 2; // CRC but no len
else
SymbolsLeft = intDataLen + intRSLen; // No CRC
if (intDataLen == 600)
SymbolsLeft += 6; // 600 baud has 3 * RS Blocks
// Save data rate for PTC reporting
if (Rate[intFrameType] > 0)
DataRate = Rate[intFrameType];
intToneMagsLength = 16 * SymbolsLeft; // 4 tones, 2 bits per set
memset(intToneMagsIndex, 0, sizeof(intToneMagsIndex));
charIndex = 0;
PSKInitDone = 0;
frameLen = 0;
totalRSErrors = 0;
DummyCarrier = 0; // pseudo carrier used for long 600 baud frames
Decode600Buffer = &bytFrameData[0][0];
State = AcquireFrame;
// if a data frame, and not the same frame type as last, reinitialise
// correctly received carriers byte and memory ARQ fields
// if (IsDataFrame(intFrameType) && LastDataFrameType != intFrameType)
if (intFrameType == PktFrameHeader || intFrameType == PktFrameData)
{
memset(CarrierOk, 0, sizeof(CarrierOk));
memset(intSumCounts, 0, sizeof(intSumCounts));
#ifdef MEMORYARQ
memset(intToneMagsAvg, 0, sizeof(intToneMagsAvg));
memset(intCarPhaseAvg, 0, sizeof(intCarPhaseAvg));
memset(intCarMagAvg, 0, sizeof(intCarMagAvg));
#endif
LastDataFrameType = intFrameType;
}
else if (LastDataFrameType != intFrameType)
{
if (strFrameType[LastDataFrameType][0] == 'O')
{
// OFDM Frame, We know the ISS received the last ack, so can remove any data passed to host.
// We need to do that, as new frame block numbers will start from first unacked block.
if (intFrameType == OFDMACK)
RepeatedFrame = FALSE;
RepeatedFrame = FALSE;
RemoveProcessedOFDMData();
}
Debugprintf("New frame type - MEMARQ flags reset");
memset(CarrierOk, 0, sizeof(CarrierOk));
if (IsDataFrame(intFrameType))
LastDataFrameType = intFrameType;
// note that although we only do mem arq if enough RAM we
// still skip decoding carriers that have been received;
#ifdef MEMORYARQ
memset(intSumCounts, 0, sizeof(intSumCounts));
memset(intToneMagsAvg, 0, sizeof(intToneMagsAvg));
memset(intCarPhaseAvg, 0, sizeof(intCarPhaseAvg));
memset(intCarMagAvg, 0, sizeof(intCarMagAvg));
#endif
}
else
{
// Repeated frame. OFDM needs to know, as we may have passed data to host.
if (IsDataFrame(intFrameType))
RepeatedFrame = TRUE;
}
PrintCarrierFlags();
}
}
// Acquire Frame
if (State == AcquireFrame)
{
// Call DemodulateFrame for each set of samples
DemodulateFrame(intFrameType);
if (State == AcquireFrame)
// We haven't got it all yet so wait for more samples
return;
// We have the whole frame, so process it
// printtick("got whole frame");
LastDemodType = intFrameType;
if (strcmp (strMod, "4FSK") == 0)
Update4FSKConstellation(&intToneMags[0][0], &intLastRcvdFrameQuality);
else if (strcmp (strMod, "16FSK") == 0)
Update16FSKConstellation(&intToneMags[0][0], &intLastRcvdFrameQuality);
else if (strcmp (strMod, "8FSK") == 0)
Update8FSKConstellation(&intToneMags[0][0], &intLastRcvdFrameQuality);
// PSK and QAM quality done in Decode routines
Debugprintf("Qual = %d", intLastRcvdFrameQuality);
// This mechanism is to skip actual decoding and reply/change state...no need to decode
blnFrameDecodedOK = DecodeFrame(intFrameType, bytData);
ProcessFrame:
if (!blnFrameDecodedOK)
DrawRXFrame(2, Name(intFrameType));
if (intFrameType == PktFrameData)
{
#ifdef TEENSY
SetLED(PKTLED, TRUE); // Flash LED
PKTLEDTimer = Now + 200; // for 200 mS
#endif
return;
}
if (blnFrameDecodedOK)
{
// Set input level if supported
#ifdef HASPOTS
CheckandAdjustRXLevel(lastmax, lastmin, TRUE);
#endif
if (AccumulateStats)
if (IsDataFrame(intFrameType))
if (strstr (strMod, "PSK"))
intGoodPSKFrameDataDecodes++;
else if (strstr (strMod, "QAM"))
intGoodQAMFrameDataDecodes++;
else if (strstr (strMod, "OFDM"))
intGoodOFDMFrameDataDecodes++;
else
intGoodFSKFrameDataDecodes++;
#ifdef TEENSY
if (IsDataFrame(intFrameType))
{
SetLED(PKTLED, TRUE); // Flash LED
PKTLEDTimer = Now + 400; // For 400 Ms
}
#endif
}
else
{
// Bad decode
if (AccumulateStats)
if (IsDataFrame(intFrameType))
if (strstr(strMod, "PSK"))
intFailedPSKFrameDataDecodes++;
else if (strstr(strMod, "QAM"))
intFailedQAMFrameDataDecodes++;
else if (strstr(strMod, "OFDM"))
intFailedOFDMFrameDataDecodes++;
else
intFailedFSKFrameDataDecodes++;
// Debug.WriteLine("[DecodePSKData2] bytPass = " & Format(bytPass, "X"))
}
skipDecode:
State = SearchingForLeader;
ClearAllMixedSamples();
DiscardOldSamples();
return;
}
}
// Subroutine to compute Goertzel algorithm and return Real and Imag components for a single frequency bin
void GoertzelRealImag(short intRealIn[], int intPtr, int N, float m, float * dblReal, float * dblImag)
{
// intRealIn is a buffer at least intPtr + N in length
// N need not be a power of 2
// m need not be an integer
// Computes the Real and Imaginary Freq values for bin m
// Verified to = FFT results for at least 10 significant digits
// Timings for 1024 Point on Laptop (64 bit Core Duo 2.2 Ghz)
// GoertzelRealImag .015 ms Normal FFT (.5 ms)
// assuming Goertzel is proportional to N and FFT time proportional to Nlog2N
// FFT:Goertzel time ratio ~ 3.3 Log2(N)
// Sanity check
//if (intPtr < 0 Or (intRealIn.Length - intPtr) < N Then
// dblReal = 0 : dblImag = 0 : Exit Sub
// End If
float dblZ_1 = 0.0f, dblZ_2 = 0.0f, dblW = 0.0f;
float dblCoeff = 2 * cosf(2 * M_PI * m / N);
int i;
for (i = 0; i <= N; i++)
{
if (i == N)
dblW = dblZ_1 * dblCoeff - dblZ_2;
else
dblW = intRealIn[intPtr] + dblZ_1 * dblCoeff - dblZ_2;
dblZ_2 = dblZ_1;
dblZ_1 = dblW;
intPtr++;
}
*dblReal = 2 * (dblW - cosf(2 * M_PI * m / N) * dblZ_2) / N; // scale results by N/2
*dblImag = 2 * (sinf(2 * M_PI * m / N) * dblZ_2) / N; // scale results by N/2 (this sign agrees with Scope DSP phase values)
}
// Subroutine to compute Goertzel algorithm and return Real and Imag components for a single frequency bin with a Hanning Window function
float dblHanWin[120];
float dblHanAng;
int HanWinLen = 0;
float dblHannWin[480];
float dblHannAng;
// Subroutine to compute Goertzel algorithm and return Real and Imag components for a single frequency bin with a Hann Window function for N a multiple of 120
void GoertzelRealImagHann120(short intRealIn[], int intPtr, int N, float m, float * dblReal, float * dblImag)
{
// This version precomputes the raised cosine (Hann or Hanning) window and uses it for any length that is a multiple of 120 samples
// intRealIn is a buffer at least intPtr + N in length
// N must be 960 to use this function
// Hann coefficients are approximated for N>120 but should be close
// m need not be an integer
// Computes the Real and Imaginary Freq values for bin m
// Verified to = FFT results for at least 10 significant digits
// Timings for 1024 Point on Laptop (64 bit Core Duo 2.2 Ghz)
// GoertzelRealImag .015 ms Normal FFT (.5 ms)
// assuming Goertzel is proportional to N and FFT time proportional to Nlog2N
// FFT:Goertzel time ratio ~ 3.3 Log2(N)
float dblZ_1 = 0.0f, dblZ_2 = 0.0f, dblW = 0.0f;
float dblCoeff = 2 * cosf(2 * M_PI * m / N);
int i;
int intM = N / 120; // No if 120 sample blocks
if (HanWinLen != N) //if there is any change in N this is then recalculate the Hanning Window...this mechanism reduces use of Cos
{
HanWinLen = N;
dblHanAng = 2 * M_PI / 120;
for (i = 0; i < 60; i++)
{
dblHanWin[i] = 0.5 - 0.5 * cosf(i * dblHanAng + dblHanAng);
}
}
for (i = 0; i <= N; i++)
{
if (i == N)
dblW = dblZ_1 * dblCoeff - dblZ_2;
else if (i < (N / 2)) // ist half of 120 sample block
// looks like we use values 0 ti 59 then 59 down to 0
dblW = intRealIn[intPtr] * dblHanWin[(i /intM) % 60] + dblZ_1 * dblCoeff - dblZ_2;
else
dblW = intRealIn[intPtr] * dblHanWin[59 - ((i /intM) % 60)] + dblZ_1 * dblCoeff - dblZ_2;
dblZ_2 = dblZ_1;
dblZ_1 = dblW;
intPtr++;
}
*dblReal = 2 * (dblW - cosf(2 * M_PI * m / N) * dblZ_2) / N; // scale results by N/2
*dblImag = 2 * (sinf(2 * M_PI * m / N) * dblZ_2) / N; // scale results by N/2 (this sign agrees with Scope DSP phase values)
}
void GoertzelRealImagHann960(short intRealIn[], int intPtr, int N, float m, float * dblReal, float * dblImag)
{
// This version precomputes the raised cosine (Hann or Hanning) window and uses it for any length that is a multiple of 120 samples
// intRealIn is a buffer at least intPtr + N in length
// N must be a multiple of 120 to use this function
// Hann coefficients are approximated for N>120 but should be close
// m need not be an integer
// Computes the Real and Imaginary Freq values for bin m
// Verified to = FFT results for at least 10 significant digits
// Timings for 1024 Point on Laptop (64 bit Core Duo 2.2 Ghz)
// GoertzelRealImag .015 ms Normal FFT (.5 ms)
// assuming Goertzel is proportional to N and FFT time proportional to Nlog2N
// FFT:Goertzel time ratio ~ 3.3 Log2(N)
float dblZ_1 = 0.0f, dblZ_2 = 0.0f, dblW = 0.0f;
float dblCoeff = 2 * cosf(2 * M_PI * m / N);
int i;
int intM = N / 120; // No if 120 sample blocks
if (dblHannWin[479] < 0.5) //if there is any change in N this is then recalculate the Hanning Window...this mechanism reduces use of Cos
{
dblHannAng = 2 * M_PI / 960;
for (i = 0; i < 480; i++)
{
dblHannWin[i] = 0.5 - 0.5 * cosf(i * dblHannAng + dblHannAng);
}
}
for (i = 0; i <= N; i++)
{
if (i == N)
dblW = dblZ_1 * dblCoeff - dblZ_2;
else if (i < (N / 2)) // ist half of 120 sample block
// looks like we use values 0 ti 59 then 59 down to 0
dblW = intRealIn[intPtr] * dblHannWin[(i /intM) % 60] + dblZ_1 * dblCoeff - dblZ_2;
else
dblW = intRealIn[intPtr] * dblHannWin[479 - ((i /intM) % 60)] + dblZ_1 * dblCoeff - dblZ_2;
dblZ_2 = dblZ_1;
dblZ_1 = dblW;
intPtr++;
}
*dblReal = 2 * (dblW - cosf(2 * M_PI * m / N) * dblZ_2) / N; // scale results by N/2
*dblImag = 2 * (sinf(2 * M_PI * m / N) * dblZ_2) / N; // scale results by N/2 (this sign agrees with Scope DSP phase values)
}
void GoertzelRealImagHanning(short intRealIn[], int intPtr, int N, float m, float * dblReal, float * dblImag)
{
// intRealIn is a buffer at least intPtr + N in length
// N need not be a power of 2
// m need not be an integer
// Computes the Real and Imaginary Freq values for bin m
// Verified to = FFT results for at least 10 significant digits
// Timings for 1024 Point on Laptop (64 bit Core Duo 2.2 Ghz)
// GoertzelRealImag .015 ms Normal FFT (.5 ms)
// assuming Goertzel is proportional to N and FFT time proportional to Nlog2N
// FFT:Goertzel time ratio ~ 3.3 Log2(N)
// Sanity check
float dblZ_1 = 0.0f, dblZ_2 = 0.0f, dblW = 0.0f;
float dblCoeff = 2 * cosf(2 * M_PI * m / N);
int i;
if (HanWinLen != N) //if there is any change in N this is then recalculate the Hanning Window...this mechanism reduces use of Cos
{
HanWinLen = N;
dblHanAng = 2 * M_PI / (N - 1);
for (i = 0; i < N; i++)
{
dblHanWin[i] = 0.5 - 0.5 * cosf(i * dblHanAng);
}
}
for (i = 0; i <= N; i++)
{
if (i == N)
dblW = dblZ_1 * dblCoeff - dblZ_2;
else
dblW = intRealIn[intPtr] * dblHanWin[i] + dblZ_1 * dblCoeff - dblZ_2;
dblZ_2 = dblZ_1;
dblZ_1 = dblW;
intPtr++;
}
*dblReal = 2 * (dblW - cosf(2 * M_PI * m / N) * dblZ_2) / N; // scale results by N/2
*dblImag = 2 * (sinf(2 * M_PI * m / N) * dblZ_2) / N; // scale results by N/2 (this sign agrees with Scope DSP phase values)
}
float dblHamWin[1200];
float dblHamAng;
int HamWinLen = 0;
void GoertzelRealImagHamming(short intRealIn[], int intPtr, int N, float m, float * dblReal, float * dblImag)
{
// intRealIn is a buffer at least intPtr + N in length
// N need not be a power of 2
// m need not be an integer
// Computes the Real and Imaginary Freq values for bin m
// Verified to = FFT results for at least 10 significant digits
// Timings for 1024 Point on Laptop (64 bit Core Duo 2.2 Ghz)
// GoertzelRealImag .015 ms Normal FFT (.5 ms)
// assuming Goertzel is proportional to N and FFT time proportional to Nlog2N
// FFT:Goertzel time ratio ~ 3.3 Log2(N)
// Sanity check
float dblZ_1 = 0.0f, dblZ_2 = 0.0f, dblW = 0.0f;
float dblCoeff = 2 * cosf(2 * M_PI * m / N);
int i;
if (HamWinLen != N) //if there is any cHamge in N this is then recalculate the Hanning Window...this mechanism reduces use of Cos
{
HamWinLen = N;
dblHamAng = 2 * M_PI / (N - 1);
for (i = 0; i < N; i++)
{
dblHamWin[i] = 0.54f - 0.46f * cosf(i * dblHamAng);
}
}
for (i = 0; i <= N; i++)
{
if (i == N)
dblW = dblZ_1 * dblCoeff - dblZ_2;
else
dblW = intRealIn[intPtr] * dblHamWin[i] + dblZ_1 * dblCoeff - dblZ_2;
dblZ_2 = dblZ_1;
dblZ_1 = dblW;
intPtr++;
}
*dblReal = 2 * (dblW - cosf(2 * M_PI * m / N) * dblZ_2) / N; // scale results by N/2
*dblImag = 2 * (sinf(2 * M_PI * m / N) * dblZ_2) / N; // scale results by N/2 (this sign agrees with Scope DSP phase values)
}
// Function to interpolate spectrum peak using Quinn algorithm
float QuinnSpectralPeakLocator(float XkM1Re, float XkM1Im, float XkRe, float XkIm, float XkP1Re, float XkP1Im)
{
// based on the Quinn algorithm in Streamlining Digital Processing page 139
// Alpha1 = Re(Xk-1/Xk)
// Alpha2 = Re(Xk+1/Xk)
//Delta1 = Alpha1/(1 - Alpha1)
//'Delta2 = Alpha2/(1 - Alpha2)
// if Delta1 > 0 and Delta2 > 0 then Delta = Delta2 else Delta = Delta1
// should be within .1 bin for S:N > 2 dB
float dblDenom = powf(XkRe, 2) + powf(XkIm, 2);
float dblAlpha1;
float dblAlpha2;
float dblDelta1;
float dblDelta2;
dblAlpha1 = ((XkM1Re * XkRe) + (XkM1Im * XkIm)) / dblDenom;
dblAlpha2 = ((XkP1Re * XkRe) + (XkP1Im * XkIm)) / dblDenom;
dblDelta1 = dblAlpha1 / (1 - dblAlpha1);
dblDelta2 = dblAlpha2 / (1 - dblAlpha2);
if (dblDelta1 > 0 && dblDelta2 > 0)
return dblDelta2;
else
return dblDelta1;
}
// Function to interpolate spectrum peak using simple interpolation
float SpectralPeakLocator(float XkM1Re, float XkM1Im, float XkRe, float XkIm, float XkP1Re, float XkP1Im, float * dblCentMag, char * Win)
{
// Use this for Windowed samples instead of QuinnSpectralPeakLocator
float dblLeftMag, dblRightMag;
*dblCentMag = sqrtf(powf(XkRe, 2) + powf(XkIm, 2));
dblLeftMag = sqrtf(powf(XkM1Re, 2) + powf(XkM1Im, 2));
dblRightMag = sqrtf(powf(XkP1Re, 2) + powf(XkP1Im, 2));
//Factor 1.22 empirically determine optimum for Hamming window
// For Hanning Window use factor of 1.36
// For Blackman Window use factor of 1.75
if (strcmp(Win, "Blackman"))
return 1.75 * (dblRightMag - dblLeftMag) / (dblLeftMag + *dblCentMag + dblRightMag); // Optimized for Hamming Window
if (strcmp(Win, "Hann"))
return 1.36 * (dblRightMag - dblLeftMag) / (dblLeftMag + *dblCentMag + dblRightMag); // Optimized for Hamming Window
if (strcmp(Win, "Hamming"))
return 1.22 * (dblRightMag - dblLeftMag) / (dblLeftMag + *dblCentMag + dblRightMag); // Optimized for Hamming Window
return 0;
}
// Function to detect and tune the 50 baud 2 tone leader (for all bandwidths) Updated version of SearchFor2ToneLeader2
float dblPriorFineOffset = 1000.0f;
BOOL SearchFor2ToneLeader3(short * intNewSamples, int Length, float * dblOffsetHz, int * intSN)
{
// This version uses 10Hz bin spacing. Hamming window on Goertzel, and simple spectral peak interpolator
// It requires about 50% more CPU time when running but produces more sensive leader detection and more accurate tuning
// search through the samples looking for the telltail 50 baud 2 tone pattern (nominal tones 1475, 1525 Hz)
// Find the offset in Hz (due to missmatch in transmitter - receiver tuning
// Finds the S:N (power ratio of the tones 1475 and 1525 ratioed to "noise" averaged from bins at 1425, 1450, 1550, and 1575Hz)
float dblGoertzelReal[56];
float dblGoertzelImag[56];
float dblMag[56];
float dblPower, dblLeftMag, dblRightMag;
float dblMaxPeak = 0.0, dblMaxPeakSN = 0.0, dblBinAdj;
int intInterpCnt = 0; // the count 0 to 3 of the interpolations that were < +/- .5 bin
int intIatMaxPeak = 0;
float dblAlpha = 0.3f; // Works well possibly some room for optimization Changed from .5 to .3 on Rev 0.1.5.3
float dblInterpretThreshold= 1.0f; // Good results June 6, 2014 (was .4) ' Works well possibly some room for optimization
float dblFilteredMaxPeak = 0;
int intStartBin, intStopBin;
float dblLeftCar, dblRightCar, dblBinInterpLeft, dblBinInterpRight, dblCtrR, dblCtrI, dblLeftP, dblRightP;
float dblLeftR[3], dblLeftI[3], dblRightR[3], dblRightI[3];
int i;
int Ptr = 0;
float dblAvgNoisePerBin, dblCoarsePwrSN, dblBinAdj1475, dblBinAdj1525, dblCoarseOffset = 1000;
float dblTrialOffset, dblPowerEarly, dblSNdBPwrEarly;
if ((Length) < 1200)
return FALSE; // ensure there are at least 1200 samples (5 symbols of 240 samples)
// if ((Now - dttLastGoodFrameTypeDecode > 20000) && TuningRange > 0)
{
// this is the full search over the full tuning range selected. Uses more CPU time and with possibly larger deviation once connected.
intStartBin = ((200 - TuningRange) / 10);
intStopBin = 55 - intStartBin;
dblMaxPeak = 0;
// Generate the Power magnitudes for up to 56 10 Hz bins (a function of MCB.TuningRange)
for (i = intStartBin; i <= intStopBin; i++)
{
// note hamming window reduces end effect caused by 1200 samples (not an even multiple of 240) but spreads response peaks
GoertzelRealImagHamming(intNewSamples, Ptr, 1200, i + 122.5f, &dblGoertzelReal[i], &dblGoertzelImag[i]);
dblMag[i] = powf(dblGoertzelReal[i], 2) + powf(dblGoertzelImag[i], 2); // dblMag(i) in units of power (V^2)
}
// Search the bins to locate the max S:N in the two tone signal/avg noise.
for (i = intStartBin + 5; i <= intStopBin - 10; i++) // ' +/- MCB.TuningRange from nominal
{
dblPower = sqrtf(dblMag[i] * dblMag[i + 5]); // using the product to minimize sensitivity to one strong carrier vs the two tone
// sqrt converts back to units of power from Power ^2
// don't use center noise bin as too easily corrupted by adjacent carriers
dblAvgNoisePerBin = (dblMag[i - 5] + dblMag[i - 3] + dblMag[i + 8] + dblMag[i + 10]) / 4; // Simple average
dblMaxPeak = dblPower / dblAvgNoisePerBin;
if (dblMaxPeak > dblMaxPeakSN)
{
dblMaxPeakSN = dblMaxPeak;
dblCoarsePwrSN = 10 * log10f(dblMaxPeak);
intIatMaxPeak = i + 122;
}
}
// Do the interpolation based on the two carriers at nominal 1475 and 1525Hz
if (((intIatMaxPeak - 123) >= intStartBin) && ((intIatMaxPeak - 118) <= intStopBin)) // check to ensure no index errors
{
// Interpolate the adjacent bins using QuinnSpectralPeakLocator
dblBinAdj1475 = SpectralPeakLocator(
dblGoertzelReal[intIatMaxPeak - 123], dblGoertzelImag[intIatMaxPeak - 123],
dblGoertzelReal[intIatMaxPeak - 122], dblGoertzelImag[intIatMaxPeak - 122],
dblGoertzelReal[intIatMaxPeak - 121], dblGoertzelImag[intIatMaxPeak - 121], &dblLeftMag, "Hamming");
if (dblBinAdj1475 < dblInterpretThreshold && dblBinAdj1475 > -dblInterpretThreshold)
{
dblBinAdj = dblBinAdj1475;
intInterpCnt += 1;
}
dblBinAdj1525 = SpectralPeakLocator(
dblGoertzelReal[intIatMaxPeak - 118], dblGoertzelImag[intIatMaxPeak - 118],
dblGoertzelReal[intIatMaxPeak - 117], dblGoertzelImag[intIatMaxPeak - 117],
dblGoertzelReal[intIatMaxPeak - 116], dblGoertzelImag[intIatMaxPeak - 116], &dblRightMag, "Hamming");
if (dblBinAdj1525 < dblInterpretThreshold && dblBinAdj1525 > -dblInterpretThreshold)
{
dblBinAdj += dblBinAdj1525;
intInterpCnt += 1;
}
if (intInterpCnt == 0)
{
dblPriorFineOffset = 1000.0f;
return FALSE;
}
else
{
dblBinAdj = dblBinAdj / intInterpCnt; // average the offsets that are within 1 bin
dblCoarseOffset = 10.0f * (intIatMaxPeak + dblBinAdj - 147); // compute the Coarse tuning offset in Hz
}
}
else
{
dblPriorFineOffset = 1000.0f;
return FALSE;
}
}
// Drop into Narrow Search
if (dblCoarseOffset < 999)
dblTrialOffset = dblCoarseOffset; // use the CoarseOffset calculation from above
else
dblTrialOffset = *dblOffsetHz; // use the prior offset value
if (fabsf(dblTrialOffset) > TuningRange && TuningRange > 0)
{
dblPriorFineOffset = 1000.0f;
return False;
}
dblLeftCar = 147.5f + dblTrialOffset / 10.0f; // the nominal positions of the two tone carriers based on the last computerd dblOffsetHz
dblRightCar = 152.5f + dblTrialOffset / 10.0f;
// Calculate 4 bins total for Noise values in S/N computation (calculate average noise) ' Simple average of noise bins
GoertzelRealImagHamming(intNewSamples, Ptr, 1200, 142.5f + dblTrialOffset / 10.0f, &dblCtrR, &dblCtrI); // nominal center -75 Hz
dblAvgNoisePerBin = powf(dblCtrR, 2) + powf(dblCtrI, 2);
GoertzelRealImagHamming(intNewSamples, Ptr, 1200, 145.0f + dblTrialOffset / 10.0f, &dblCtrR, &dblCtrI); // center - 50 Hz
dblAvgNoisePerBin += powf(dblCtrR, 2) + powf(dblCtrI, 2);
GoertzelRealImagHamming(intNewSamples, Ptr, 1200, 155.0 + dblTrialOffset / 10.0f, &dblCtrR, &dblCtrI); // center + 50 Hz
dblAvgNoisePerBin += powf(dblCtrR, 2) + powf(dblCtrI, 2);
GoertzelRealImagHamming(intNewSamples, Ptr, 1200, 157.5 + dblTrialOffset / 10.0f, &dblCtrR, &dblCtrI); // center + 75 Hz
dblAvgNoisePerBin += powf(dblCtrR, 2) + powf(dblCtrI, 2);
dblAvgNoisePerBin = dblAvgNoisePerBin * 0.25f; // simple average, now units of power
// Calculate one bin above and below the two nominal 2 tone positions for Quinn Spectral Peak locator
GoertzelRealImagHamming(intNewSamples, Ptr, 1200, dblLeftCar - 1, &dblLeftR[0], &dblLeftI[0]);
GoertzelRealImagHamming(intNewSamples, Ptr, 1200, dblLeftCar, &dblLeftR[1], &dblLeftI[1]);
dblLeftP = powf(dblLeftR[1], 2) + powf(dblLeftI[1], 2);
GoertzelRealImagHamming(intNewSamples, Ptr, 1200, dblLeftCar + 1, &dblLeftR[2], &dblLeftI[2]);
GoertzelRealImagHamming(intNewSamples, Ptr, 1200, dblRightCar - 1, &dblRightR[0], &dblRightI[0]);
GoertzelRealImagHamming(intNewSamples, Ptr, 1200, dblRightCar, &dblRightR[1], &dblRightI[1]);
dblRightP = powf(dblRightR[1], 2) + powf(dblRightI[1], 2);
GoertzelRealImag(intNewSamples, Ptr, 1200, dblRightCar + 1, &dblRightR[2], &dblRightI[2]);
// Calculate the total power in the two tones
// This mechanism designed to reject single carrier but average both carriers if ratios is less than 4:1
if (dblLeftP > 4 * dblRightP)
dblPower = dblRightP;
else if (dblRightP > 4 * dblLeftP)
dblPower = dblLeftP;
else
dblPower = sqrtf(dblLeftP * dblRightP);
dblSNdBPwr = 10 * log10f(dblPower / dblAvgNoisePerBin);
// Early leader detect code to calculate S:N on the first 2 symbols)
// concept is to allow more accurate framing and sync detection and reduce false leader detects
GoertzelRealImag(intNewSamples, Ptr, 480, 57.0f + dblTrialOffset / 25.0f, &dblCtrR, &dblCtrI); // nominal center -75 Hz
dblAvgNoisePerBin = powf(dblCtrR, 2) + powf(dblCtrI, 2);
GoertzelRealImag(intNewSamples, Ptr, 480, 58.0f + dblTrialOffset / 25.0f, &dblCtrR, &dblCtrI); // nominal center -75 Hz
dblAvgNoisePerBin += powf(dblCtrR, 2) + powf(dblCtrI, 2);
GoertzelRealImag(intNewSamples, Ptr, 480, 62.0f + dblTrialOffset / 25.0f, &dblCtrR, &dblCtrI); // nominal center -75 Hz
dblAvgNoisePerBin += powf(dblCtrR, 2) + powf(dblCtrI, 2);
GoertzelRealImag(intNewSamples, Ptr, 480, 63.0f + dblTrialOffset / 25.0f, &dblCtrR, &dblCtrI); // nominal center -75 Hz
dblAvgNoisePerBin = max(1000.0f, 0.25 * (dblAvgNoisePerBin + powf(dblCtrR, 2) + powf(dblCtrI, 2))); // average of 4 noise bins
dblLeftCar = 59 + dblTrialOffset / 25; // the nominal positions of the two tone carriers based on the last computerd dblOffsetHz
dblRightCar = 61 + dblTrialOffset / 25;
GoertzelRealImag(intNewSamples, Ptr, 480, dblLeftCar, &dblCtrR, &dblCtrI); // LEFT carrier
dblLeftP = powf(dblCtrR, 2) + powf(dblCtrI, 2);
GoertzelRealImag(intNewSamples, Ptr, 480, dblRightCar, &dblCtrR, &dblCtrI); // Right carrier
dblRightP = powf(dblCtrR, 2) + powf(dblCtrI, 2);
// the following rejects a single tone carrier but averages the two tones if ratio is < 4:1
if (dblLeftP > 4 * dblRightP)
dblPowerEarly = dblRightP;
else if (dblRightP > 4 * dblLeftP)
dblPowerEarly = dblLeftP;
else
dblPowerEarly = sqrtf(dblLeftP * dblRightP);
dblSNdBPwrEarly = 10 * log10f(dblPowerEarly / dblAvgNoisePerBin);
// End of Early leader detect test code
if (dblSNdBPwr > (4 + Squelch) && dblSNdBPwrEarly > Squelch && (dblAvgNoisePerBin > 100.0f || dblPriorFineOffset != 1000.0f)) // making early threshold = lower (after 3 dB compensation for bandwidth)
{
// Debugprintf("Fine Search S:N= %f dB, Early S:N= %f dblAvgNoisePerBin %f ", dblSNdBPwr, dblSNdBPwrEarly, dblAvgNoisePerBin);
// Calculate the interpolation based on the left of the two tones
dblBinInterpLeft = SpectralPeakLocator(dblLeftR[0], dblLeftI[0], dblLeftR[1], dblLeftI[1], dblLeftR[2], dblLeftI[2], &dblLeftMag, "Hamming");
// And the right of the two tones
dblBinInterpRight = SpectralPeakLocator(dblRightR[0], dblRightI[0], dblRightR[1], dblRightI[1], dblRightR[2], dblRightI[2], &dblRightMag, "Hamming");
// Weight the interpolated values in proportion to their magnitudes
dblBinInterpLeft = dblBinInterpLeft * dblLeftMag / (dblLeftMag + dblRightMag);
dblBinInterpRight = dblBinInterpRight * dblRightMag / (dblLeftMag + dblRightMag);
#ifdef ARMLINUX
{
int x = round(dblBinInterpLeft); // odd, but PI doesnt print floats properly
int y = round(dblBinInterpRight);
// Debugprintf(" SPL Left= %d SPL Right= %d Offset %f, LeftMag %f RightMag %f", x, y, *dblOffsetHz, dblLeftMag, dblRightMag);
}
#else
// Debugprintf(" SPL Left= %f SPL Right= %f, Offset %f, LeftMag %f RightMag %f",
// dblBinInterpLeft, dblBinInterpRight, *dblOffsetHz, dblLeftMag, dblRightMag);
#endif
if (fabsf(dblBinInterpLeft + dblBinInterpRight) < 1.0) // sanity check for the interpolators
{
if (dblBinInterpLeft + dblBinInterpRight > 0) // consider different bounding below
*dblOffsetHz = dblTrialOffset + min((dblBinInterpLeft + dblBinInterpRight) * 10.0f, 3); // average left and right, adjustment bounded to +/- 3Hz max
else
*dblOffsetHz = dblTrialOffset + max((dblBinInterpLeft + dblBinInterpRight) * 10.0f, -3);
// Note the addition of requiring a second detect with small offset dramatically reduces false triggering even at Squelch values of 3
// The following demonstrated good detection down to -10 dB S:N with squelch = 3 and minimal false triggering.
// Added rev 0.8.2.2 11/6/2016 RM
if (abs(dblPriorFineOffset - *dblOffsetHz) < 2.9f)
{
Debugprintf("Prior-Offset= %f", (dblPriorFineOffset - *dblOffsetHz));
// Capture power for debugging ...note: convert to 3 KHz noise bandwidth from 25Hz or 12.Hz for reporting consistancy.
Debugprintf("Ldr; S:N(3KHz) Early= %f dB, Full %f dB, Offset= %f Hz: ", dblSNdBPwrEarly - 20.8f, dblSNdBPwr - 24.77f, *dblOffsetHz);
dttStartRmtLeaderMeasure = Now;
if (AccumulateStats)
{
dblLeaderSNAvg = ((dblLeaderSNAvg * intLeaderDetects) + dblSNdBPwr) / (1 + intLeaderDetects);
intLeaderDetects++;
}
dblNCOFreq = 3000 + *dblOffsetHz; // Set the NCO frequency and phase inc for mixing
dblNCOPhaseInc = dbl2Pi * dblNCOFreq / 12000;
dttLastLeaderDetect = dttStartRmtLeaderMeasure = Now;
State = AcquireSymbolSync;
*intSN = dblSNdBPwr - 24.77; // 23.8dB accomodates ratio of 3Kz BW:10 Hz BW (10Log 3000/10 = 24.77)
// don't advance the pointer here
dblPriorFineOffset = 1000.0f;
return TRUE;
}
else
dblPriorFineOffset = *dblOffsetHz;
// always use 1 symbol inc when looking for next minimal offset
}
}
return FALSE;
}
BOOL SearchFor2ToneLeader4(short * intNewSamples, int Length, float * dblOffsetHz, int * intSN)
{
// This version uses 12.5 Hz bin spacing. Blackman window on Goertzel, and simple spectral peak interpolator optimized for Blackman
// Blackman selected for maximum rejection (about 60 dB) of the other two-tone bin 50 Hz (4 x 12.5 Hz bins) away.
// search through the samples looking for the telltail 50 baud 2 tone pattern (nominal tones 1475, 1525 Hz)
// Find the offset in Hz (due to missmatch in transmitter - receiver tuning
// Finds the S:N (power ratio of the tones 1475 and 1525 ratioed to "noise" averaged from bins at 1425, 1450, 1550, and 1575Hz)
float dblGoertzelReal[45];
float dblGoertzelImag[45];
float dblMag[45];
float dblPower, dblPwrSNdB, dblLeftMag, dblRightMag, dblAvgNoisePerBinAtPeak;
float dblRealL, dblRealR, dblImagL, dblImagR;
float dblMaxPeak = 0.0, dblMaxPeakSN = 0.0, dblMagWindow;
int intInterpCnt = 0; // the count 0 to 3 of the interpolations that were < +/- .5 bin
int intIatMaxPeak = 0;
float dblAlpha = 0.3f; // Works well possibly some room for optimization Changed from .5 to .3 on Rev 0.1.5.3
float dblInterpretThreshold= 1.0f; // Good results June 6, 2014 (was .4) ' Works well possibly some room for optimization
float dblFilteredMaxPeak = 0;
int intStartBin, intStopBin;
int i;
int Ptr = 0;
float dblAvgNoisePerBin, dblBinAdj1475, dblBinAdj1525, dblCoarseOffset = 1000;
float dblOffset = 1000; // initialize to impossible value
// This should allow tunning from nominal bins at 1425Hz to 1575Hz +/- 200 Hz tuning range
if ((Length) < 1200)
return FALSE; // ensure there are at least 1200 samples (5 symbols of 240 samples)
// if ((Now - dttLastGoodFrameTypeDecode > 20000) && TuningRange > 0)
// {
// // this is the full search over the full tuning range selected. Uses more CPU time and with possibly larger deviation once connected.
intStartBin = ((200 - TuningRange) / 12.5);
intStopBin = 44 - intStartBin;
dblMaxPeak = 0;
dblMagWindow = 0;
dblMaxPeakSN = -100;
// Generate the Power magnitudes for up to 56 10 Hz bins (a function of MCB.TuningRange)
for (i = intStartBin; i <= intStopBin; i++)
{
// note Blackman window reduced end effect but looses sensitivity so sticking with Hann window
// Test of 4/22/2018 indicated accurate Hann window (960) gives about 1-2 dB more sensitivity than Blackman window
GoertzelRealImagHann960(intNewSamples, Ptr, 960, i + 98, &dblGoertzelReal[i], &dblGoertzelImag[i]);
dblMag[i] = powf(dblGoertzelReal[i], 2) + powf(dblGoertzelImag[i], 2); // dblMag(i) in units of power (V^2)
dblMagWindow += dblMag[i];
}
// Search the bins to locate the max S:N in the two tone signal/avg noise.
for (i = intStartBin + 4; i <= intStopBin - 8; i++) // ' +/- MCB.TuningRange from nominal
{
dblPower = sqrtf(dblMag[i] * dblMag[i + 4]); // using the product to minimize sensitivity to one strong carrier vs the two tone
// sqrt converts back to units of power from Power ^2
// don't use center 7 noise bins as too easily corrupted by adjacent two-tone carriers
dblAvgNoisePerBin = (dblMagWindow - (dblMag[i - 1] + dblMag[i] + dblMag[i + 1] + dblMag[i + 2] + dblMag[i + 3] + dblMag[i + 4] + dblMag[i + 5])) / (intStopBin - (intStartBin + 7));
dblMaxPeak = dblPower / dblAvgNoisePerBin;
if (dblMaxPeak > dblMaxPeakSN)
{
dblMaxPeakSN = dblMaxPeak;
dblAvgNoisePerBinAtPeak = max(dblAvgNoisePerBin, 1000.0f);
intIatMaxPeak = i + 98;
}
}
dblMaxPeakSN = (dblMag[intIatMaxPeak - 98] + dblMag[intIatMaxPeak - 94]) / dblAvgNoisePerBinAtPeak;
dblPwrSNdB = 10.0f * log10f(dblMaxPeakSN);
// Check aquelch
if ((dblPwrSNdB > (3 * Squelch)) && dblPwrSNPower_dBPrior > (3 * Squelch))
{
// Do the interpolation based on the two carriers at nominal 1475 and 1525Hz
if (((intIatMaxPeak - 99) >= intStartBin) && ((intIatMaxPeak - 103) <= intStopBin)) // check to ensure no index errors
{
// Interpolate the adjacent bins using QuinnSpectralPeakLocator
dblBinAdj1475 = SpectralPeakLocator(
dblGoertzelReal[intIatMaxPeak - 99], dblGoertzelImag[intIatMaxPeak - 99],
dblGoertzelReal[intIatMaxPeak - 98], dblGoertzelImag[intIatMaxPeak - 98],
dblGoertzelReal[intIatMaxPeak - 97], dblGoertzelImag[intIatMaxPeak - 97], &dblLeftMag, "Hann");
dblBinAdj1525 = SpectralPeakLocator(
dblGoertzelReal[intIatMaxPeak - 95], dblGoertzelImag[intIatMaxPeak - 95],
dblGoertzelReal[intIatMaxPeak - 94], dblGoertzelImag[intIatMaxPeak - 94],
dblGoertzelReal[intIatMaxPeak - 93], dblGoertzelImag[intIatMaxPeak - 93], &dblRightMag, "Hann");
// Weight the offset calculation by the magnitude of the dblLeftMag and dblRightMag carriers
dblOffset = 12.5 * (intIatMaxPeak + dblBinAdj1475 * dblLeftMag / (dblLeftMag + dblRightMag) + dblBinAdj1525 * dblRightMag / (dblLeftMag + dblRightMag) - 118); // compute the Coarse tuning offset in Hz
if (fabsf(dblOffset) > (TuningRange + 7)) // Was 7 caused tuning problems
{
dblPwrSNPower_dBPrior = dblPwrSNdB;
return False;
}
// recompute the S:N based on the interpolated bins and average with computation 1 and 2 symbols in the future
// Use of Hann window increases sensitivity slightly (1-2 dB)
GoertzelRealImagHann120(intNewSamples, 0, 960, intIatMaxPeak + dblOffset / 12.5, &dblRealL, &dblImagL);
GoertzelRealImagHann120(intNewSamples, 0, 960, intIatMaxPeak + 4 + dblOffset / 12.5, &dblRealR, &dblImagR);
dblMaxPeakSN = (powf(dblRealL, 2) + powf(dblImagL, 2) + powf(dblRealR, 2) + powf(dblImagR, 2)) / dblAvgNoisePerBinAtPeak;
// now compute for 120 samples later
GoertzelRealImagHann120(intNewSamples, 120, 960, intIatMaxPeak + dblOffset / 12.5, &dblRealL, &dblImagL);
GoertzelRealImagHann120(intNewSamples, 120, 960, intIatMaxPeak + 4 + dblOffset / 12.5, &dblRealR, &dblImagR);
dblMaxPeakSN += (powf(dblRealL, 2) + powf(dblImagL, 2) + powf(dblRealR, 2) + powf(dblImagR, 2)) / dblAvgNoisePerBinAtPeak;
// and a third 240 samples later
GoertzelRealImagHann120(intNewSamples, 240, 960, intIatMaxPeak + dblOffset / 12.5, &dblRealL, &dblImagL);
GoertzelRealImagHann120(intNewSamples, 240, 960, intIatMaxPeak + 4 + dblOffset / 12.5, &dblRealR, &dblImagR);
dblMaxPeakSN += (powf(dblRealL, 2) + powf(dblImagL, 2) + powf(dblRealR, 2) + powf(dblImagR, 2)) / dblAvgNoisePerBinAtPeak;
dblMaxPeakSN = dblMaxPeakSN / 3; // average the dblMaxPeakSN over the three calculations
// ???? Calc Twice ????
dblMaxPeakSN = (powf(dblRealL, 2) + powf(dblImagL, 2) + powf(dblRealR, 2) + powf(dblImagR, 2)) / dblAvgNoisePerBinAtPeak;
dblPwrSNdB = 10 * log10f(dblMaxPeakSN);
if (dblPwrSNdB > 3 * Squelch) // This average power now includes two samples from symbols +120 and + 240 samples
{
//strDecodeCapture = "Ldr; S:N(3KHz) Prior=" & Format(dblPwrSNPower_dBPrior, "#.0") & "dB, Current=" & Format(dblPwrSNdB, "#.0") & "dB, Offset=" & Format(dblOffset, "##0.00") & "Hz "
Debugprintf("Ldr; S:N(3KHz) Avg= %f dB, Offset== %f Hz", dblPwrSNdB, dblOffset);
dttStartRmtLeaderMeasure = Now;
if (AccumulateStats)
{
dblLeaderSNAvg = ((dblLeaderSNAvg * intLeaderDetects) + dblPwrSNdB) / (1 + intLeaderDetects);
intLeaderDetects += 1;
}
*dblOffsetHz = dblOffset;
dblNCOFreq = 3000 + *dblOffsetHz; // Set the NCO frequency and phase inc for mixing
dblNCOPhaseInc = dbl2Pi * dblNCOFreq / 12000;
// don't advance the pointer here
State = AcquireSymbolSync;
dttLastLeaderDetect = Now;
dblPhaseDiff1_2Avg = 10; // initialize to 10 to cause initialization of exponential averager in AcquireFrameSyncRSBAvg
*intSN = round(dblPwrSNdB - 20.8); // 20.8dB accomodates ratio of 3Kz BW: (effective Blackman Window bandwidth of ~25 Hz)
return True;
}
else
{
return False;
}
}
}
dblPwrSNPower_dBPrior = dblPwrSNdB;
return FALSE;
}
// Function to look at the 2 tone leader and establishes the Symbol framing using envelope search and minimal phase error.
BOOL Acquire2ToneLeaderSymbolFraming()
{
float dblCarPh;
float dblReal, dblImag;
int intLocalPtr = intMFSReadPtr; // try advancing one symbol to minimize initial startup errors
float dblAbsPhErr;
float dblMinAbsPhErr = 5000; // initialize to an excessive value
int intIatMinErr;
float dblPhaseAtMinErr;
int intAbsPeak = 0;
int intJatPeak = 0;
int i;
// Use Phase of 1500 Hz leader to establish symbol framing. Nominal phase is 0 or 180 degrees
if ((intFilteredMixedSamplesLength - intLocalPtr) < 960)
return FALSE; // not enough
intLocalPtr = intMFSReadPtr + EnvelopeCorrelatorNew(); // should position the pointer at the symbol boundary
if (intLocalPtr < intMFSReadPtr)
return False; // use negative value of EnvelopeCorrelator to indicate insufficient correlation.
// Check 2 samples either side of the intLocalPtr for minimum phase error.(closest to Pi or -Pi)
// Could be as much as .4 Radians (~70 degrees) depending on sampling positions.
for (i = -2; i <= 2; i++) // 0 To 0 ' -2 To 2 ' for just 5 samples
{
// using the full symbol seemed to work best on weak Signals (0 to -5 dB S/N) June 15, 2015
GoertzelRealImagHann120(intFilteredMixedSamples, intLocalPtr + i, 240, 30, &dblReal, &dblImag); // Carrier at 1500 Hz nominal Positioning
dblCarPh = atan2f(dblImag, dblReal);
dblAbsPhErr = fabsf(dblCarPh - (ceil(dblCarPh / M_PI) * M_PI));
if (dblAbsPhErr < dblMinAbsPhErr)
{
dblMinAbsPhErr = dblAbsPhErr;
intIatMinErr = i;
dblPhaseAtMinErr = dblCarPh;
}
}
intMFSReadPtr = intLocalPtr + intIatMinErr;
Debugprintf("[Acquire2ToneLeaderSymbolFraming] intIatMinError= %d, Leader Length %d mS", intIatMinErr, Now - dttLastLeaderDetect);
State = AcquireFrameSync;
if (AccumulateStats)
intLeaderSyncs++;
//Debug.WriteLine(" [Acquire2ToneLeaderSymbolSync] iAtMinError = " & intIatMinErr.ToString & " Ptr = " & intMFSReadPtr.ToString & " MinAbsPhErr = " & Format(dblMinAbsPhErr, "#.00"))
//Debug.WriteLine(" [Acquire2ToneLeaderSymbolSync] Ph1500 @ MinErr = " & Format(dblPhaseAtMinErr, "#.000"))
//strDecodeCapture &= "Framing; iAtMinErr=" & intIatMinErr.ToString & ", Ptr=" & intMFSReadPtr.ToString & ", MinAbsPhErr=" & Format(dblMinAbsPhErr, "#.00") & ": "
intPhaseError = 0;
return TRUE;
}
// Function to establish symbol sync
int EnvelopeCorrelator()
{
// Compute the two symbol correlation with the Two tone leader template.
// slide the correlation one sample and repeat up to 240 steps
// keep the point of maximum or minimum correlation...and use this to identify the the symbol start.
float dblCorMax = -1000000.0f; // Preset to excessive values
float dblCorMin = 1000000.0f;
int intJatMax = 0, intJatMin = 0;
float dblCorSum, dblCorProduct, dblCorMaxProduct = 0.0;
int i,j;
short int75HzFiltered[720];
if (intFilteredMixedSamplesLength < intMFSReadPtr + 720)
return -1;
Filter75Hz(int75HzFiltered, TRUE, 720); // This filter appears to help reduce avg decode distance (10 frames) by about 14%-19% at WGN-5 May 3, 2015
for (j = 0; j < 360; j++) // Over 1.5 symbols
{
dblCorSum = 0;
for (i = 0; i < 240; i++) // over 1 50 baud symbol (may be able to reduce to 1 symbol)
{
dblCorProduct = int50BaudTwoToneLeaderTemplate[i] * int75HzFiltered[120 + i + j]; // note 120 accomdates filter delay of 120 samples
dblCorSum += dblCorProduct;
if (fabsf(dblCorProduct) > dblCorMaxProduct)
dblCorMaxProduct = fabsf(dblCorProduct);
}
if (fabsf(dblCorSum) > dblCorMax)
{
dblCorMax = fabsf(dblCorSum);
intJatMax = j;
}
}
if (AccumulateStats)
{
dblAvgCorMaxToMaxProduct = (dblAvgCorMaxToMaxProduct * intEnvelopeCors + (dblCorMax / dblCorMaxProduct)) / (intEnvelopeCors + 1);
intEnvelopeCors++;
}
// if (dblCorMax > 40 * dblCorMaxProduct)
{
Debugprintf("EnvelopeCorrelator CorMax:MaxProd= %f J= %d", dblCorMax / dblCorMaxProduct, intJatMax);
return intJatMax;
}
// else
// return -1;
}
int EnvelopeCorrelatorNew()
{
// Compute the two symbol correlation with the Two tone leader template.
// slide the correlation one sample and repeat up to 240 steps
// keep the point of maximum or minimum correlation...and use this to identify the the symbol start.
float dblCorMax = -1000000.0f; // Preset to excessive values
float dblCorMin = 1000000.0f;
int intJatMax = 0, intJatMin = 0;
float dblCorSum, dblCorProduct, dblCorMaxProduct = 0.0;
int i,j;
short int75HzFiltered[960];
if (intFilteredMixedSamplesLength < intMFSReadPtr + 960)
return -1;
Filter75Hz(int75HzFiltered, TRUE, 960); // This filter appears to help reduce avg decode distance (10 frames) by about 14%-19% at WGN-5 May 3, 2015
for (j = 360; j < 600; j++) // Over 2 symbols
{
dblCorSum = 0;
for (i = 0; i < 240; i++) // over 1 50 baud symbol (may be able to reduce to 1 symbol)
{
dblCorProduct = int50BaudTwoToneLeaderTemplate[i] * int75HzFiltered[120 + i + j]; // note 120 accomdates filter delay of 120 samples
dblCorSum += dblCorProduct;
if (fabsf(dblCorProduct) > dblCorMaxProduct)
dblCorMaxProduct = fabsf(dblCorProduct);
}
if (fabsf(dblCorSum) > dblCorMax)
{
dblCorMax = fabsf(dblCorSum);
intJatMax = j;
}
}
if (AccumulateStats)
{
dblAvgCorMaxToMaxProduct = (dblAvgCorMaxToMaxProduct * intEnvelopeCors + (dblCorMax / dblCorMaxProduct)) / (intEnvelopeCors + 1);
intEnvelopeCors++;
}
if (dblCorMax > 40 * dblCorMaxProduct)
{
Debugprintf("EnvelopeCorrelator CorMax:MaxProd= %f J= %d", dblCorMax / dblCorMaxProduct, intJatMax);
return intJatMax;
}
Debugprintf("EnvelopeCorrelator failed %d", dblCorMax / dblCorMaxProduct);
return -1;
}
// Function to acquire the Frame Sync for all Frames
BOOL AcquireFrameSyncRSB()
{
// Two improvements could be incorporated into this function:
// 1) Provide symbol tracking until the frame sync is found (small corrections should be less than 1 sample per 4 symbols ~2000 ppm)
// 2) Ability to more accurately locate the symbol center (could be handled by symbol tracking 1) above.
// This is for acquiring FSKFrameSync After Mixing Tones Mirrored around 1500 Hz. e.g. Reversed Sideband
// Frequency offset should be near 0 (normally within +/- 1 Hz)
// Locate the sync Symbol which has no phase change from the prior symbol (BPSK leader @ 1500 Hz)
int intLocalPtr = intMFSReadPtr;
int intAvailableSymbols = (intFilteredMixedSamplesLength - intMFSReadPtr) / 240;
float dblPhaseSym1; //' phase of the first symbol
float dblPhaseSym2; //' phase of the second symbol
float dblPhaseSym3; //' phase of the third symbol
float dblReal, dblImag;
float dblPhaseDiff12, dblPhaseDiff23;
int i;
if (intAvailableSymbols < 3)
return FALSE; // must have at least 360 samples to search
// Calculate the Phase for the First symbol
GoertzelRealImag(intFilteredMixedSamples, intLocalPtr, 240, 30, &dblReal, &dblImag); // Carrier at 1500 Hz nominal Positioning with no cyclic prefix
dblPhaseSym1 = atan2f(dblImag, dblReal);
intLocalPtr += 240; // advance one symbol
GoertzelRealImag(intFilteredMixedSamples, intLocalPtr, 240, 30, &dblReal, &dblImag); // Carrier at 1500 Hz nominal Positioning with no cyclic prefix
dblPhaseSym2 = atan2f(dblImag, dblReal);
intLocalPtr += 240; // advance one symbol
for (i = 0; i <= intAvailableSymbols - 3; i++)
{
// Compute the phase of the next symbol
GoertzelRealImag(intFilteredMixedSamples, intLocalPtr, 240, 30, &dblReal, &dblImag); // Carrier at 1500 Hz nominal Positioning with no cyclic prefix
dblPhaseSym3 = atan2f(dblImag, dblReal);
// Compute the phase differences between sym1-sym2, sym2-sym3
dblPhaseDiff12 = dblPhaseSym1 - dblPhaseSym2;
if (dblPhaseDiff12 > M_PI) // bound phase diff to +/- Pi
dblPhaseDiff12 -= dbl2Pi;
else if (dblPhaseDiff12 < -M_PI)
dblPhaseDiff12 += dbl2Pi;
dblPhaseDiff23 = dblPhaseSym2 - dblPhaseSym3;
if (dblPhaseDiff23 > M_PI) // bound phase diff to +/- Pi
dblPhaseDiff23 -= dbl2Pi;
else if (dblPhaseDiff23 < -M_PI)
dblPhaseDiff23 += dbl2Pi;
if (fabsf(dblPhaseDiff12) > 0.6667f * M_PI && fabsf(dblPhaseDiff23) < 0.3333f * M_PI) // Tighten the margin to 60 degrees
{
// intPSKRefPhase = (short)dblPhaseSym3 * 1000;
intLeaderRcvdMs = (int)ceil((intLocalPtr - 30) / 12); // 30 is to accomodate offset of inital pointer for filter length.
intMFSReadPtr = intLocalPtr + 240; // Position read pointer to start of the symbol following reference symbol
if (AccumulateStats)
intFrameSyncs += 1; // accumulate tuning stats
//strDecodeCapture &= "Sync; Phase1>2=" & Format(dblPhaseDiff12, "0.00") & " Phase2>3=" & Format(dblPhaseDiff23, "0.00") & ": "
return TRUE; // pointer is pointing to first 4FSK data symbol. (first symbol of frame type)
}
else
{
dblPhaseSym1 = dblPhaseSym2;
dblPhaseSym2 = dblPhaseSym3;
intLocalPtr += 240; // advance one symbol
}
}
intMFSReadPtr = intLocalPtr - 480; // back up 2 symbols for next attempt (Current Sym2 will become new Sym1)
return FALSE;
}
// Function to acquire the Frame Sync for all Frames using exponential averaging
int AcquireFrameSyncRSBAvg()
{
// This new routine uses exponential averaging on the ptr reference leader phases to minimize noise contribution
// Needs optimization of filter values and decision thresholds with actual simulator at low S:N and multipath.
// This is for acquiring FSKFrameSync After Mixing Tones Mirrored around 1500 Hz. e.g. Reversed Sideband
// Frequency offset should be near 0 (normally within +/- 1 Hz)
// Locate the sync Symbol which has no phase change from the prior symbol (50 baud BPSK leader @ 1500 Hz)
int intLocalPtr = intMFSReadPtr;
int intAvailableSymbols = (intFilteredMixedSamplesLength - intMFSReadPtr) / 240;
float dblPhaseSym1; //' phase of the first symbol
float dblPhaseSym2; //' phase of the second symbol
float dblPhaseSym3; //' phase of the third symbol
float dblReal, dblImag;
float dblPhaseDiff12, dblPhaseDiff23;
int i;
if (intAvailableSymbols < 3)
return FALSE; // must have at least 360 samples to search
// Calculate the Phase for the First symbol
GoertzelRealImagHann120(intFilteredMixedSamples, intLocalPtr, 240, 30, &dblReal, &dblImag); // Carrier at 1500 Hz nominal Positioning with no cyclic prefix
dblPhaseSym1 = atan2f(dblImag, dblReal);
intLocalPtr += 240; // advance one symbol
GoertzelRealImagHann120(intFilteredMixedSamples, intLocalPtr, 240, 30, &dblReal, &dblImag); // Carrier at 1500 Hz nominal Positioning with no cyclic prefix
dblPhaseSym2 = atan2f(dblImag, dblReal);
intLocalPtr += 240; // advance one symbol
for (i = 0; i <= intAvailableSymbols - 3; i++)
{
// Compute the phase of the next symbol
GoertzelRealImagHann120(intFilteredMixedSamples, intLocalPtr, 240, 30, &dblReal, &dblImag); // Carrier at 1500 Hz nominal Positioning with no cyclic prefix
dblPhaseSym3 = atan2f(dblImag, dblReal);
// Compute the phase differences between sym1-sym2, sym2-sym3
dblPhaseDiff12 = dblPhaseSym1 - dblPhaseSym2;
if (dblPhaseDiff12 > M_PI) // bound phase diff to +/- Pi
dblPhaseDiff12 -= dbl2Pi;
else if (dblPhaseDiff12 < -M_PI)
dblPhaseDiff12 += dbl2Pi;
if (dblPhaseDiff1_2Avg > 9)
dblPhaseDiff1_2Avg = fabsf(dblPhaseDiff12); // initialize the difference average after a prior detect
else
dblPhaseDiff1_2Avg = 0.75 * dblPhaseDiff1_2Avg + 0.25 * fabsf(dblPhaseDiff12); // exponential average
dblPhaseDiff23 = dblPhaseSym2 - dblPhaseSym3;
if (dblPhaseDiff23 > M_PI) // bound phase diff to +/- Pi
dblPhaseDiff23 -= dbl2Pi;
else if (dblPhaseDiff23 < -M_PI)
dblPhaseDiff23 += dbl2Pi;
if (fabsf(dblPhaseDiff1_2Avg ) > (0.83333 * M_PI) && fabsf(dblPhaseDiff23) < (0.25f * M_PI)) // Margin ~30 deg and 45 degrees
{
intLeaderRcvdMs = (int)ceil((intLocalPtr - 30) / 12); // 30 is to accomodate offset of inital pointer for filter length.
intMFSReadPtr = intLocalPtr + 240; // Position read pointer to start of the symbol following reference symbol
if (AccumulateStats)
intFrameSyncs += 1; // accumulate tuning stats
//strDecodeCapture &= "Sync; Phase1>2=" & Format(dblPhaseDiff12, "0.00") & " Phase2>3=" & Format(dblPhaseDiff23, "0.00") & ": "
// dttLastLeaderSync = Now;
dblPwrSNPower_dBPrior = -1000; // Reset the prior Leader power to small value to insure minimum of two symbol passes on next leader detect.
return TRUE; // pointer is pointing to first 4FSK data symbol. (first symbol of frame type)
}
// The following looks for phase errors (which should nomimally be Pi or 180 deg) and counts errors
// abandoning search on the second error, Then advancing the main intMFSReadPtr one symbol (240 samples) and returning to SearchingForLeader state.
if (fabsf(dblPhaseDiff1_2Avg) < (0.6667 * M_PI) || fabsf(dblPhaseDiff23) < (0.6667 * M_PI)) // Margin 60 deg
{
intPhaseError += 1;
dblPhaseSym1 = dblPhaseSym2;
dblPhaseSym2 = dblPhaseSym3;
intLocalPtr += 240; // advance one symbol
// if (intPhaseError > 1) // This bailout mechanism for sync failure is superior and doesn't make any assumptions about leader length
// {
// intMFSReadPtr += 240; // advance the MFSReadPointer one symbol and try to search for leader again.
// State = SearchingForLeader;
// return False;
// }
}
else
{
// keep searching available samples
dblPhaseSym1 = dblPhaseSym2;
dblPhaseSym2 = dblPhaseSym3;
intLocalPtr += 240; // advance one symbol
}
}
intMFSReadPtr = intLocalPtr - 480; // back up 2 symbols for next attempt (Current Sym2 will become new Sym1)
return FALSE;
}
// Function to Demod FrameType4FSK
BOOL DemodFrameType4FSK(int intPtr, short * intSamples, int * intToneMags)
{
float dblReal, dblImag;
int i;
if ((intFilteredMixedSamplesLength - intPtr) < 1920) // 8 symbols
return FALSE;
intToneMagsLength = 8;
for (i = 0; i < 8; i++)
{
GoertzelRealImagHann120(intSamples, intPtr, 240, 1350 / 50.0f, &dblReal, &dblImag);
intToneMags[4 * i] = (int)powf(dblReal, 2) + powf(dblImag, 2);
GoertzelRealImagHann120(intSamples, intPtr, 240, 1450 / 50.0f, &dblReal, &dblImag);
intToneMags[1 + 4 * i] = (int)powf(dblReal, 2) + powf(dblImag, 2);
GoertzelRealImagHann120(intSamples, intPtr, 240, 1550 / 50.0f, &dblReal, &dblImag);
intToneMags[2 + 4 * i] = (int)powf(dblReal, 2) + powf(dblImag, 2);
GoertzelRealImagHann120(intSamples, intPtr, 240, 1650 / 50.0f, &dblReal, &dblImag);
intToneMags[3 + 4 * i] = (int)powf(dblReal, 2) + powf(dblImag, 2);
intPtr += 240;
}
return TRUE;
}
// Function to compute the "distance" from a specific bytFrame Xored by bytID using 1 symbol parity
float ComputeDecodeDistance(int intTonePtr, int * intToneMags, UCHAR bytFrameType, UCHAR bytID)
{
// intTonePtr is the offset into the Frame type symbols. 0 for first Frame byte 16 = (4 x 4) for second frame byte
float dblDistance = 0;
int int4ToneSum;
int intToneIndex;
UCHAR bytMask = 0x30;
int j, k;
for (j = 0; j <= 3; j++) // over 4 symbols
{
int4ToneSum = 0;
for (k = 0; k <=3; k++)
{
int4ToneSum += intToneMags[intTonePtr + (4 * j) + k];
}
if (int4ToneSum == 0)
int4ToneSum = 1; // protects against possible overflow
if (j < 3)
intToneIndex = ((bytFrameType ^ bytID) & bytMask) >> (4 - 2 * j);
else
intToneIndex = ComputeTypeParity(bytFrameType ^ bytID);
dblDistance += 1.0f - ((1.0f * intToneMags[intTonePtr + (4 * j) + (3 - intToneIndex)]) / (1.0f * int4ToneSum));
bytMask = bytMask >> 2;
}
dblDistance = dblDistance / 4; // normalize back to 0 to 1 range
return dblDistance;
}
// A function to check the parity symbol used in the frame type decoding
BOOL CheckTypeParity(UCHAR bytFrameType)
{
// Returns True if Parity OK
UCHAR bytMask = 0x30; // Look at only 6 bits of data (values only 0 to 63)
UCHAR bytParitySum = 3;
UCHAR bytSym = 0;
int k;
for (k = 0; k < 3; k++)
{
bytSym = (bytMask & bytFrameType) >> (2 * (2 - k));
bytParitySum = bytParitySum ^ bytSym;
bytMask = bytMask >> 2;
}
return bytParitySum == ((bytFrameType & 0x0C0) >> 6);
}
// Function to check Parity of frame type bytes
UCHAR GetFrameTypeByte(int intTonePtr, int * intToneMags)
{
// Demodulate the byte pointed to postion of tone PTR and return it
UCHAR bytData = 0, bytParity, bytSym;
int intIndex = intTonePtr;
int j;
for (j = 0; j < 4; j++)
{
// for each 4FSK symbol (2 bits) in a byte
if (intToneMags[intIndex] > intToneMags[intIndex + 1] && intToneMags[intIndex] > intToneMags[intIndex + 2] && intToneMags[intIndex] > intToneMags[intIndex + 3])
bytSym = 3;
else if (intToneMags[intIndex + 1] > intToneMags[intIndex] && intToneMags[intIndex + 1] > intToneMags[intIndex + 2] && intToneMags[intIndex + 1] > intToneMags[intIndex + 3])
bytSym = 2;
else if (intToneMags[intIndex + 2] > intToneMags[intIndex] && intToneMags[intIndex + 2] > intToneMags[intIndex + 1] && intToneMags[intIndex + 2] > intToneMags[intIndex + 3])
bytSym = 1;
else
bytSym = 0;
if (j < 3)
bytData = (bytData << 2) + bytSym;
else
bytParity = bytSym << 6;
intIndex += 4;
}
return bytData | bytParity;
}
BOOL CheckFrameTypeParity(int intTonePtr, int * intToneMags)
{
// Demodulate the byte pointed to postion of tone PTR and check Parity Return True if OK
UCHAR bytData = GetFrameTypeByte(intTonePtr, intToneMags);
return CheckTypeParity(bytData);
}
// Function to compute the frame type by selecting the minimal distance from all valid frame types.
int MinimalDistanceFrameType(int * intToneMags, UCHAR bytSessionID)
{
float dblMinDistance1 = 5; // minimal distance for the first byte initialize to large value
float dblMinDistance2 = 5; // minimal distance for the second byte initialize to large value
float dblMinDistance3 = 5; // minimal distance for the second byte under exceptional cases initialize to large value
int intIatMinDistance1, intIatMinDistance2, intIatMinDistance3;
float dblDistance1, dblDistance2, dblDistance3;
int i;
strDecodeCapture[0] = 0;
if (ProtocolState == ISS)
{
bytValidFrameTypes = bytValidFrameTypesISS;
bytValidFrameTypesLength = bytValidFrameTypesLengthISS;
}
else
{
bytValidFrameTypes = bytValidFrameTypesALL;
bytValidFrameTypesLength = bytValidFrameTypesLengthALL;
}
// Search through all the valid frame types finding the minimal distance
// This looks like a lot of computation but measured < 1 ms for 135 iterations....RM 11/1/2016
for (i = 0; i < bytValidFrameTypesLength; i++)
{
dblDistance1 = ComputeDecodeDistance(0, intToneMags, bytValidFrameTypes[i], 0);
dblDistance2 = ComputeDecodeDistance(16, intToneMags, bytValidFrameTypes[i], bytSessionID);
if (blnPending)
dblDistance3 = ComputeDecodeDistance(16, intToneMags, bytValidFrameTypes[i], 0x3F);
else
dblDistance3 = ComputeDecodeDistance(16, intToneMags, bytValidFrameTypes[i], bytLastARQSessionID);
if (dblDistance1 < dblMinDistance1)
{
dblMinDistance1 = dblDistance1;
intIatMinDistance1 = bytValidFrameTypes[i];
}
if (dblDistance2 < dblMinDistance2)
{
dblMinDistance2 = dblDistance2;
intIatMinDistance2 = bytValidFrameTypes[i];
}
if (dblDistance3 < dblMinDistance3)
{
dblMinDistance3 = dblDistance3;
intIatMinDistance3 = bytValidFrameTypes[i];
}
}
Debugprintf("Frame Decode type %x %x %x Dist %.2f %.2f %.2f Sess %x pend %d conn %d lastsess %d",
intIatMinDistance1, intIatMinDistance2, intIatMinDistance3,
dblMinDistance1, dblMinDistance2, dblMinDistance3,
bytSessionID, blnPending, blnARQConnected, bytLastARQSessionID);
if (bytSessionID == 0x3F) // ' we are in a FEC QSO, monitoring an ARQ session or have not yet reached the ARQ Pending or Connected status
{
if (intIatMinDistance1 == intIatMinDistance2 && ((dblMinDistance1 < 0.3) || (dblMinDistance2 < 0.3)))
{
sprintf(strDecodeCapture, "%s MD Decode;2 ID=H%X, Type=H%X:%s, D1= %.2f, D2= %.2f",
strDecodeCapture, bytSessionID, intIatMinDistance1, Name(intIatMinDistance1), dblMinDistance1, dblMinDistance2);
Debugprintf("[Frame Type Decode OK ] %s", strDecodeCapture);
dblOffsetLastGoodDecode = dblOffsetHz;
return intIatMinDistance1;
}
if ((dblMinDistance1 < 0.3) && CheckFrameTypeParity(0, intToneMags) && IsDataFrame(intIatMinDistance1) ) // this would handle the case of monitoring an ARQ connection where the SessionID is not 0x3F
{
sprintf(strDecodeCapture, "%s MD Decode;3 ID=H%X, Type=H%X:%s, D1= %.2f, D2= %.2f",
strDecodeCapture, bytSessionID, intIatMinDistance1, Name(intIatMinDistance1), dblMinDistance1, dblMinDistance2);
Debugprintf("[Frame Type Decode OK ] %s", strDecodeCapture);
return intIatMinDistance1;
}
if ((dblMinDistance2 < 0.3) && CheckFrameTypeParity(16, intToneMags) && IsDataFrame(intIatMinDistance2)) // this would handle the case of monitoring an FEC transmission that failed above when the session ID is = 03F
{
sprintf(strDecodeCapture, "%s MD Decode;4 ID=H%X, Type=H%X:%s, D1= %.2f, D2= %.2f",
strDecodeCapture, bytSessionID, intIatMinDistance1, Name(intIatMinDistance2), dblMinDistance1, dblMinDistance2);
Debugprintf("[Frame Type Decode OK ] %s", strDecodeCapture);
return intIatMinDistance2;
}
return -1; // indicates poor quality decode so don't use
}
sprintf(strDecodeCapture, "%s MD Decode;12 Type1=H%X: Type2=H%X: , D1= %.2f, D2= %.2f",
strDecodeCapture, intIatMinDistance1 , intIatMinDistance2, dblMinDistance1, dblMinDistance2);
Debugprintf("[Frame Type Decode Fail] %s", strDecodeCapture);
return -1; // indicates poor quality decode so don't use
}
// Function to acquire the 4FSK frame type
int Acquire4FSKFrameType()
{
// intMFSReadPtr is pointing to start of first symbol of Frame Type (total of 8 4FSK symbols in frame type (2 bytes) + 1 parity symbol per byte
// returns -1 if minimal distance decoding is below threshold (low likelyhood of being correct)
// returns -2 if insufficient samples
// Else returns frame type 0-255
int NewType = 0;
char Offset[32];
if ((intFilteredMixedSamplesLength - intMFSReadPtr) < (240 * 8))
return -2; // Check for 8 available 4FSK Symbols
if (!DemodFrameType4FSK(intMFSReadPtr, intFilteredMixedSamples, &intToneMags[0][0]))
{
Update4FSKConstellation(&intToneMags[0][0], &intLastRcvdFrameQuality);
intMFSReadPtr += (240 * 8);
return -1;
}
intRmtLeaderMeasure = (Now - dttStartRmtLeaderMeasure);
dttLastGoodFrameTypeDecode = Now;
// Now do check received Tone array for testing minimum distance decoder
if (blnPending) // If we have a pending connection (btween the IRS first decode of ConReq until it receives a ConAck from the iSS)
NewType = MinimalDistanceFrameType(&intToneMags[0][0], bytPendingSessionID); // The pending session ID will become the session ID once connected)
else if (blnARQConnected) // If we are connected then just use the stcConnection.bytSessionID
NewType = MinimalDistanceFrameType(&intToneMags[0][0], bytSessionID);
else // not connected and not pending so use &FF (FEC or ARQ unconnected session ID
NewType = MinimalDistanceFrameType(&intToneMags[0][0], 0x3F);
sprintf(Offset, "Offset %5.1f", dblOffsetHz);
SendtoGUI('O', Offset, strlen(Offset));
if (NewType >= 0 && IsShortControlFrame(NewType)) // update the constellation if a short frame (no data to follow)
Update4FSKConstellation(&intToneMags[0][0], &intLastRcvdFrameQuality);
if (AccumulateStats)
if (NewType >= 0)
intGoodFSKFrameTypes++;
else
intFailedFSKFrameTypes++;
intMFSReadPtr += (240 * 8); // advance to read pointer to the next symbol (if there is one)
return NewType;
}
// Demodulate Functions. These are called repeatedly as samples arrive
// and buld a frame in static array bytFrameData
// Function to demodulate one carrier for all low baud rate 4FSK frame types
// Is called repeatedly to decode multitone modes
int Corrections = 0;
BOOL Demod1Car4FSK()
{
int Start = 0;
// We can't wait for the full frame as we don't have enough ram, so
// we do one character at a time, until we run out or end of frame
// Only continue if we have more than intSampPerSym * 4 chars
while (State == AcquireFrame)
{
if (intFilteredMixedSamplesLength < ((intSampPerSym * 4) + 20)) // allow for correcrions
{
// Move any unprocessessed data down buffer
// (while checking process - will use cyclic buffer eventually
// Debugprintf("Corrections %d", Corrections);
// If corrections is non-zero, we have to adjust
// number left
intFilteredMixedSamplesLength -= Corrections;
if (intFilteredMixedSamplesLength < 0)
Debugprintf("Corrupt intFilteredMixedSamplesLength");
Corrections = 0;
if (intFilteredMixedSamplesLength > 0)
memmove(intFilteredMixedSamples,
&intFilteredMixedSamples[Start], intFilteredMixedSamplesLength * 2);
return FALSE;
}
// If this is a multicarrier mode, we must call the
// decode char routing for each carrier
switch (intNumCar)
{
case 1:
intCenterFreq = 1500;
if (CarrierOk[0] == FALSE) // Don't redo if already decoded
Demod1Car4FSKChar(Start, bytFrameData[0], 0);
break;
case 2:
intCenterFreq = 1750;
if (CarrierOk[0] == FALSE)
Demod1Car4FSKChar(Start, bytFrameData[0], 0);
intCenterFreq = 1250;
if (CarrierOk[1] == FALSE)
Demod1Car4FSKChar(Start, bytFrameData[1], 1);
break;
/* case 4:
intCenterFreq = 2250;
if (CarrierOk[0] == FALSE)
Demod1Car4FSKChar(Start, bytFrameData1, 0);
intCenterFreq = 1750;
if (CarrierOk[1] == FALSE)
Demod1Car4FSKChar(Start, bytFrameData2, 1);
intCenterFreq = 1250;
if (CarrierOk[2] == FALSE)
Demod1Car4FSKChar(Start, bytFrameData3, 2);
intCenterFreq = 750;
if (CarrierOk[3] == FALSE)
Demod1Car4FSKChar(Start, bytFrameData4, 3);
break;
*/
}
charIndex++; // Index into received chars
SymbolsLeft--; // number still to decode
Start += intSampPerSym * 4; // 4 FSK bit pairs per byte
intFilteredMixedSamplesLength -= intSampPerSym * 4;
if (intFilteredMixedSamplesLength < 0)
Debugprintf("Corrupt intFilteredMixedSamplesLength");
if (SymbolsLeft == 0)
{
//- prepare for next
// If variable length packet frame header we only have header - leave rx running
if (intFrameType == PktFrameHeader)
{
State = SearchingForLeader;
// Save any unused samples
if (intFilteredMixedSamplesLength > 0 && Start > 0)
memmove(intFilteredMixedSamples,
&intFilteredMixedSamples[Start], intFilteredMixedSamplesLength * 2);
return TRUE;
}
DecodeCompleteTime = Now;
DiscardOldSamples();
ClearAllMixedSamples();
State = SearchingForLeader;
}
}
return TRUE;
}
// Function to demodulate one carrier for all low baud rate 4FSK frame types
void Demod1Car4FSKChar(int Start, UCHAR * Decoded, int Carrier)
{
// Converts intSamples to an array of bytes demodulating the 4FSK symbols with center freq intCenterFreq
// intPtr should be pointing to the approximate start of the first data symbol
// Updates bytData() with demodulated bytes
// Updates bytMinSymQuality with the minimum (range is 25 to 100) symbol making up each byte.
float dblReal, dblImag;
float dblSearchFreq;
float dblMagSum = 0;
float dblMag[4]; // The magnitude for each of the 4FSK frequency bins
UCHAR bytSym;
static UCHAR bytSymHistory[3];
int j;
UCHAR bytData = 0;
int * intToneMagsptr = &intToneMags[Carrier][intToneMagsIndex[Carrier]];
intToneMagsIndex[Carrier] += 16;
// ReDim intToneMags(4 * intNumOfSymbols - 1)
// ReDim bytData(intNumOfSymbols \ 4 - 1)
if (intBaud == 100)
dblSearchFreq = intCenterFreq + (1.5f * intBaud); // the highest freq (equiv to lowest sent freq because of sideband reversal)
else
dblSearchFreq = intCenterFreq + (3.0f * intBaud); // the highest freq (equiv to lowest sent freq because of sideband reversal)
// Do one symbol
for (j = 0; j < 4; j++) // for each 4FSK symbol (2 bits) in a byte
{
dblMagSum = 0;
if (intBaud == 100)
{
GoertzelRealImag(intFilteredMixedSamples, Start, intSampPerSym, dblSearchFreq / intBaud, &dblReal, &dblImag);
dblMag[0] = powf(dblReal,2) + powf(dblImag, 2);
dblMagSum += dblMag[0];
GoertzelRealImag(intFilteredMixedSamples, Start, intSampPerSym, (dblSearchFreq - intBaud) / intBaud, &dblReal, &dblImag);
dblMag[1] = powf(dblReal,2) + powf(dblImag, 2);
dblMagSum += dblMag[1];
GoertzelRealImag(intFilteredMixedSamples, Start, intSampPerSym, (dblSearchFreq - 2 * intBaud) / intBaud, &dblReal, &dblImag);
dblMag[2] = powf(dblReal,2) + powf(dblImag, 2);
dblMagSum += dblMag[2];
GoertzelRealImag(intFilteredMixedSamples, Start, intSampPerSym, (dblSearchFreq - 3 * intBaud) / intBaud, &dblReal,& dblImag);
dblMag[3] = powf(dblReal,2) + powf(dblImag, 2);
dblMagSum += dblMag[3];
}
else
{
dblMagSum = 0;
GoertzelRealImagHann120(intFilteredMixedSamples, Start, intSampPerSym, dblSearchFreq / intBaud, &dblReal, &dblImag);
dblMag[0] = powf(dblReal,2) + powf(dblImag, 2);
dblMagSum += dblMag[0];
GoertzelRealImagHann120(intFilteredMixedSamples, Start, intSampPerSym, (dblSearchFreq - 2 * intBaud) / intBaud, &dblReal, &dblImag);
dblMag[1] = powf(dblReal,2) + powf(dblImag, 2);
dblMagSum += dblMag[1];
GoertzelRealImagHann120(intFilteredMixedSamples, Start, intSampPerSym, (dblSearchFreq - 4 * intBaud) / intBaud, &dblReal, &dblImag);
dblMag[2] = powf(dblReal,2) + powf(dblImag, 2);
dblMagSum += dblMag[2];
GoertzelRealImagHann120(intFilteredMixedSamples, Start, intSampPerSym, (dblSearchFreq - 6 * intBaud) / intBaud, &dblReal,& dblImag);
dblMag[3] = powf(dblReal,2) + powf(dblImag, 2);
dblMagSum += dblMag[3];
}
if (dblMag[0] > dblMag[1] && dblMag[0] > dblMag[2] && dblMag[0] > dblMag[3])
bytSym = 0;
else if (dblMag[1] > dblMag[0] && dblMag[1] > dblMag[2] && dblMag[1] > dblMag[3])
bytSym = 1;
else if (dblMag[2] > dblMag[0] && dblMag[2] > dblMag[1] && dblMag[2] > dblMag[3])
bytSym = 2;
else
bytSym = 3;
bytData = (bytData << 2) + bytSym;
// !!!!!!! this needs attention !!!!!!!!
*intToneMagsptr++ = dblMag[0];
*intToneMagsptr++ = dblMag[1];
*intToneMagsptr++ = dblMag[2];
*intToneMagsptr++ = dblMag[3];
bytSymHistory[0] = bytSymHistory[1];
bytSymHistory[1] = bytSymHistory[2];
bytSymHistory[2] = bytSym;
// if ((bytSymHistory[0] != bytSymHistory[1]) && (bytSymHistory[1] != bytSymHistory[2]))
{
// only track when adjacent symbols are different (statistically about 56% of the time)
// this should allow tracking over 2000 ppm sampling rate error
// if (Start > intSampPerSym + 2)
// Track1Car4FSK(intFilteredMixedSamples, &Start, intSampPerSym, dblSearchFreq, intBaud, bytSymHistory);
}
Start += intSampPerSym; // advance the pointer one symbol
}
if (AccumulateStats)
intFSKSymbolCnt += 4;
Decoded[charIndex] = bytData;
return;
}
extern int intBW;
// Function to Demodulate Frame based on frame type
// Will be called repeatedly as new samples arrive
void DemodulateFrame(int intFrameType)
{
// Dim stcStatus As Status = Nothing
int intConstellationQuality = 0;
// ReDim bytData(-1)
strRcvFrameTag[0] = 0;
switch (intFrameType)
{
case ConReq200:
case ConReq500:
case ConReq2500:
case OConReq500:
case OConReq2500:
case PING:
case IDFRAME:
case PINGACK:
case CQ_de:
case PktFrameHeader: // Experimental Variable Length Frame
case OFDMACK:
Demod1Car4FSK();
return;
case PktFrameData: // Experimantal Variable Length Frame
if (strcmp(strMod, "4FSK") == 0)
Demod1Car4FSK();
else if (strcmp(strMod, "16QAM") == 0)
DemodQAM();
else
DemodPSK();
return;
}
switch (intFrameType & 0xFE) // Others are even/odd data frames
{
case D4FSK_500_50_E:
case D4FSK_1000_50_E:
Demod1Car4FSK();
break;
case D4PSK_200_50_E:
case D4PSK_200_100_E:
case D4PSK_500_50_E:
case D4PSK_500_100_E:
case D4PSKR_2500_50_E:
case D4PSK_2500_50_E:
case D4PSK_2500_100_E:
DemodPSK();
break;
case D16QAM_200_100_E:
case D16QAMR_500_100_E:
case D16QAM_500_100_E:
case D16QAMR_2500_100_E:
case D16QAM_2500_100_E:
DemodQAM();
break;
case DOFDM_200_55_E:
case DOFDM_500_55_E:
case DOFDM_2500_55_E:
DemodOFDM();
break;
default:
Debugprintf("Unsupported frame type %x", intFrameType);
DiscardOldSamples();
ClearAllMixedSamples();
State = SearchingForLeader;
intFilteredMixedSamplesLength = 0; // Testing
}
}
int intSNdB = 0, intQuality = 0;
BOOL DecodeFrame(int xxx, UCHAR * bytData)
{
BOOL blnDecodeOK = FALSE;
char strCallerCallsign[10] = "";
char strTargetCallsign[10] = "";
char strIDCallSign[11] = "";
char strGridSQ[20] = "";
char Reply[80];
strRcvFrameTag[0] = 0;
//DataACK/NAK and short control frames
if (IsShortControlFrame(intFrameType)) // Short Control Frames
{
blnDecodeOK = TRUE;
DrawRXFrame(1, Name(intFrameType));
goto returnframe;
}
totalRSErrors = 0;
if (IsDataFrame(intFrameType))
PrintCarrierFlags();
switch (intFrameType)
{
case PktFrameHeader:
{
// Variable Length Packet Frame Header
// 6 bits Type 10 Bits Len
int Len;
int pktNumCar;
int pktDataLen;
int pktRSLen;
frameLen = CorrectRawDataWithRS(&bytFrameData[0][0], bytData, intDataLen, intRSLen, intFrameType, 0);
if (CarrierOk[0])
{
pktRXMode = bytFrameData[0][1] >> 2;
pktNumCar = pktCarriers[pktRXMode];
Len = ((bytFrameData[0][1] & 0x3) << 8) | bytFrameData[0][2];
}
// Now only using one carrier
// else if (CarrierOk[1])
// {
// pktRXMode = bytFrameData2[1] >> 5;
// pktNumCar = ((bytFrameData2[1] & 0x1c) >> 2) + 1;
// Len = ((bytFrameData2[1] & 0x3) << 8) | bytFrameData2[2];
// }
else
{
// Cant decode
DiscardOldSamples();
ClearAllMixedSamples();
break;
}
strcpy(strMod, &pktMod[pktRXMode][0]);
// Reset to receive rest of frame
pktDataLen = (Len + (pktNumCar - 1)) / pktNumCar; // Round up
// This must match the encode settings
pktRSLen = pktDataLen >> 2; // Try 25% for now
if (pktRSLen & 1)
pktRSLen++; // Odd RS bytes no use
if (pktRSLen < 4)
pktRSLen = 4; // At least 4
SymbolsLeft = pktDataLen + pktRSLen + 3; // Data has crc + length byte
State = AcquireFrame;
intFrameType = PktFrameData;
CarrierOk[1] = CarrierOk[0] = 0;
charIndex = 0;
frameLen = 0;
intPhasesLen = 0;
memset(intToneMagsIndex, 0, sizeof(intToneMagsIndex));
intDataLen = pktDataLen;
intRSLen = pktRSLen;
intNumCar = pktNumCar;
PSKInitDone = 0;
Debugprintf("Pkt Frame Header Type %s Len %d", strMod, Len);
strlop(strMod, '/');
blnDecodeOK = TRUE;
return 0;
}
case PktFrameData:
{
if (pktFSK[pktRXMode])
{
// Need to Check RS
frameLen = CorrectRawDataWithRS(&bytFrameData[0][0], bytData, intDataLen, intRSLen, intFrameType, 0);
if (intNumCar > 1)
frameLen += CorrectRawDataWithRS(&bytFrameData[1][0], &bytData[frameLen], intDataLen, intRSLen, intFrameType, 1);
if (intNumCar > 2)
{
frameLen += CorrectRawDataWithRS(&bytFrameData[2][0], &bytData[frameLen], intDataLen, intRSLen, intFrameType, 2);
frameLen += CorrectRawDataWithRS(&bytFrameData[3][0], &bytData[frameLen], intDataLen, intRSLen, intFrameType, 3);
}
}
if (memcmp(CarrierOk, Good, intNumCar) == 0)
{
blnDecodeOK = TRUE;
// Packet Data - if KISS interface ias active
// Pass to Host as KISS frame, else pass to
// Session code
// Data in bytData len in frameLen
ProcessPktFrame(0, bytData, frameLen);
// else
// L2Routine(bytData, frameLen, intLastRcvdFrameQuality, totalRSErrors, intNumCar, pktRXMode);
}
break;
}
default:
Debugprintf("Unrecognised frame type");
}
if (blnDecodeOK)
{
Debugprintf("[DecodeFrame] Frame: %s Decode PASS, Constellation Quality= %d", Name(intFrameType), intLastRcvdFrameQuality);
#ifdef PLOTCONSTELLATION
if (intFrameType >= 0x30 && intFrameType <= 0x38)
DrawDecode(lastGoodID); // ID or CONREQ
else
DrawDecode("PASS");
updateDisplay();
#endif
}
else
{
Debugprintf("[DecodeFrame] Frame: %s Decode FAIL, Constellation Quality= %d", Name(intFrameType), intLastRcvdFrameQuality);
#ifdef PLOTCONSTELLATION
DrawDecode("FAIL");
updateDisplay();
#endif
}
returnframe:
if (blnDecodeOK && IsDataFrame(intFrameType))
bytLastReceivedDataFrameType = intFrameType;
// if (DebugLog)
// if (blnDecodeOK)
// Debugprintf("[DecodeFrame] Frame: %s Decode PASS, Constellation Quality= %d", Name(intFrameType), intLastRcvdFrameQuality);
// else
// Debugprintf("[DecodeFrame] Frame: %s Decode FAIL, Constellation Quality= %d", Name(intFrameType), intLastRcvdFrameQuality);
return blnDecodeOK;
}
// Subroutine to update the 4FSK Constellation
void drawFastVLine(int x0, int y0, int length, int color);
void drawFastHLine(int x0, int y0, int length, int color);
void Update4FSKConstellation(int * intToneMags, int * intQuality)
{
// Subroutine to update bmpConstellation plot for 4FSK modes...
int intToneSum = 0;
int intMagMax = 0;
float dblPi4 = 0.25 * M_PI;
float dblDistanceSum = 0;
int intRad = 0;
int i, x, y;
int yCenter = 0;
int xCenter = 0;
#ifdef PLOTCONSTELLATION
int clrPixel;
int yCenter = (ConstellationHeight)/ 2;
int xCenter = (ConstellationWidth) / 2;
clearDisplay();
#endif
for (i = 0; i < intToneMagsLength; i += 4) // for the number of symbols represented by intToneMags
{
intToneSum = intToneMags[i] + intToneMags[i + 1] + intToneMags[i + 2] + intToneMags[i + 3];
if (intToneMags[i] > intToneMags[i + 1] && intToneMags[i] > intToneMags[i + 2] && intToneMags[i] > intToneMags[i + 3])
{
if (intToneSum > 0)
intRad = max(5, 42 - 80 * (intToneMags[i + 1] + intToneMags[i + 2] + intToneMags[i + 3]) / intToneSum);
dblDistanceSum += (42 - intRad);
intRad = (intRad * PLOTRADIUS) / 50; // rescale for OLED (50 instead of 42 as we rotate constellation 35 degrees
x = xCenter + intRad;
y = yCenter + intRad;
}
else if (intToneMags[i + 1] > intToneMags[i] && intToneMags[i + 1] > intToneMags[i + 2] && intToneMags[i + 1] > intToneMags[i + 3])
{
if (intToneSum > 0)
intRad = max(5, 42 - 80 * (intToneMags[i] + intToneMags[i + 2] + intToneMags[i + 3]) / intToneSum);
dblDistanceSum += (42 - intRad);
intRad = (intRad * PLOTRADIUS) / 50; // rescale for OLED (50 instead of 42 as we rotate constellation 35 degrees
x = xCenter + intRad;
y = yCenter - intRad;
}
else if (intToneMags[i + 2] > intToneMags[i] && intToneMags[i + 2] > intToneMags[i + 1] && intToneMags[i + 2] > intToneMags[i + 3])
{
if (intToneSum > 0)
intRad = max(5, 42 - 80 * (intToneMags[i + 1] + intToneMags[i] + intToneMags[i + 3]) / intToneSum);
dblDistanceSum += (42 - intRad);
intRad = (intRad * PLOTRADIUS) / 50; // rescale for OLED (50 instead of 42 as we rotate constellation 35 degrees
x = xCenter - intRad;
y = yCenter - intRad;
}
else if (intToneSum > 0)
{
intRad = max(5, 42 - 80 * (intToneMags[i + 1] + intToneMags[i + 2] + intToneMags[i]) / intToneSum);
dblDistanceSum += (42 - intRad);
intRad = (intRad * PLOTRADIUS) / 50; // rescale for OLED (50 instead of 42 as we rotate constellation 35 degrees
x = xCenter - intRad;
y = yCenter + intRad;
}
#ifdef PLOTCONSTELLATION
if (intRad < 15)
clrPixel = Tomato;
else if (intRad < 30)
clrPixel = Gold;
else
clrPixel = Lime;
mySetPixel(x, y, clrPixel);
#endif
}
*intQuality = 100 - (2.7f * (dblDistanceSum / (intToneMagsLength / 4))); // ' factor 2.7 emperically chosen for calibration (Qual range 25 to 100)
if (*intQuality < 0)
*intQuality = 0;
else if (*intQuality > 100)
*intQuality = 100;
if (AccumulateStats)
{
int4FSKQualityCnts += 1;
int4FSKQuality += *intQuality;
}
#ifdef PLOTCONSTELLATION
DrawAxes(*intQuality, shortName(intFrameType), strMod);
#endif
return;
}
// Subroutine to update the 16FSK constallation
void Update16FSKConstellation(int * intToneMags, int * intQuality)
{
// Subroutine to update bmpConstellation plot for 16FSK modes...
int intToneSum = 0;
float intMagMax = 0;
float dblDistanceSum = 0;
float dblPlotRotation = 0;
// Dim stcStatus As Status
int intRad;
// Dim clrPixel As System.Drawing.Color
int intJatMaxMag;
int i, j;
#ifdef PLOTCONSTELLATION
float dblRad;
float dblAng;
int x, y,clrPixel;
int yCenter = (ConstellationHeight - 2)/ 2;
int xCenter = (ConstellationWidth - 2) / 2;
clearDisplay();
#endif
for (i = 0; i< intToneMagsLength; i += 16) // for the number of symbols represented by intToneMags
{
intToneSum = 0;
intMagMax = 0;
for (j = 0; j < 16; j++)
{
if (intToneMags[i + j] > intMagMax)
{
intMagMax = intToneMags[i + j];
intJatMaxMag = j;
}
intToneSum += intToneMags[i + j];
}
intRad = max(5, 42 - 40 * (intToneSum - intMagMax) / intToneSum);
dblDistanceSum += (43 - intRad);
#ifdef PLOTCONSTELLATION
if (intRad < 15)
clrPixel = Tomato;
else if (intRad < 30)
clrPixel = Gold;
else
clrPixel = Lime;
// plot the symbols rotated to avoid the axis
intRad = (intRad * PLOTRADIUS) /42; // rescale for OLED
dblAng = M_PI / 16.0f + (intJatMaxMag * M_PI / 8);
x = xCenter + intRad * cosf(dblAng);
y = yCenter + intRad * sinf(dblAng);
mySetPixel(x, y, clrPixel);
#endif
}
*intQuality = max(0, (100 - 2.2 * (dblDistanceSum / (intToneMagsLength / 16)))); // factor 2.2 emperically chosen for calibration (Qual range 25 to 100)
// *intQuality = max(0, (100 - 1.0 * (dblDistanceSum / (intToneMagsLength / 16)))); // factor 2.2 emperically chosen for calibration (Qual range 25 to 100)
if(AccumulateStats)
{
int16FSKQualityCnts++;
int16FSKQuality += *intQuality;
}
#ifdef PLOTCONSTELLATION
DrawAxes(*intQuality, shortName(intFrameType), strMod);
#endif
}
// Subroutine to udpate the 8FSK Constellation
void Update8FSKConstellation(int * intToneMags, int * intQuality)
{
// Subroutine to update bmpConstellation plot for 8FSK modes...
int intToneSum = 0;
int intMagMax = 0;
float dblPi4 = 0.25 * M_PI;
float dblDistanceSum = 0;
int intRad = 0;
int i, j, intJatMaxMag;
#ifdef PLOTCONSTELLATION
float dblAng;
int yCenter = (ConstellationHeight - 2)/ 2;
int xCenter = (ConstellationWidth - 2) / 2;
unsigned short clrPixel = WHITE;
unsigned short x, y;
clearDisplay();
#endif
for (i = 0; i < intToneMagsLength; i += 8) // for the number of symbols represented by intToneMags
{
intToneSum = 0;
intMagMax = 0;
for (j = 0; j < 8; j++)
{
if (intToneMags[i + j] > intMagMax)
{
intMagMax = intToneMags[i + j];
intJatMaxMag = j;
}
intToneSum += intToneMags[i + j];
}
intRad = max(5, 42 - 40 * (intToneSum - intMagMax) / intToneSum);
dblDistanceSum += (43 - intRad);
#ifdef PLOTCONSTELLATION
if (intRad < 15)
clrPixel = Tomato;
else if (intRad < 30)
clrPixel = Gold;
else
clrPixel = Lime;
// plot the symbols rotated to avoid the axis
intRad = (intRad * PLOTRADIUS) /42; // rescale for OLED
dblAng = M_PI / 9.0f + (intJatMaxMag * M_PI / 4);
x = xCenter + intRad * cosf(dblAng);
y = yCenter + intRad * sinf(dblAng);
mySetPixel(x, y, clrPixel);
#endif
}
*intQuality = max(0, (100 - 2.0 * (dblDistanceSum / (intToneMagsLength / 8)))); // factor 2.0 emperically chosen for calibration (Qual range 25 to 100)
if(AccumulateStats)
{
int8FSKQualityCnts++;
int8FSKQuality += *intQuality;
}
#ifdef PLOTCONSTELLATION
DrawAxes(*intQuality, shortName(intFrameType), strMod);
#endif
return;
}
// Subroutine to Update the PhaseConstellation
int UpdatePhaseConstellation(short * intPhases, short * intMag, int intPSKPhase, BOOL blnQAM, BOOL OFDM)
{
// Subroutine to update bmpConstellation plot for PSK modes...
// Skip plotting and calculations of intPSKPhase(0) as this is a reference phase (9/30/2014)
float dblPhaseError;
float dblPhaseErrorSum = 0;
int intPSKIndex;
float intP = 0;
float dblRad = 0;
float dblAvgRad = 0;
float intMagMax = 0;
float dblPi4 = 0.25 * M_PI;
float dbPhaseStep;
float dblRadError = 0;
float dblPlotRotation = 0;
int intRadInner = 0, intRadOuter = 0;
float dblAvgRadOuter = 0, dblAvgRadInner = 0, dblRadErrorInner = 0, dblRadErrorOuter = 0;
int i,j, k, intQuality;
#ifdef PLOTCONSTELLATION
int intX, intY;
int yCenter = (ConstellationHeight - 2)/ 2;
int xCenter = (ConstellationWidth - 2) / 2;
unsigned short clrPixel = WHITE;
clearDisplay();
#endif
if (intPSKPhase == 4)
intPSKIndex = 0;
else
intPSKIndex = 1;
if (blnQAM)
{
intPSKPhase = 8;
intPSKIndex = 1;
dbPhaseStep = 2 * M_PI / intPSKPhase;
for (j = 1; j < intPhasesLen; j++) // skip the magnitude of the reference in calculation
{
intMagMax = max(intMagMax, intMag[j]); // find the max magnitude to auto scale
}
for (k = 1; k < intPhasesLen; k++)
{
if (intMag[k] < 0.75f * intMagMax)
{
dblAvgRadInner += intMag[k];
intRadInner++;
}
else
{
dblAvgRadOuter += intMag[k];
intRadOuter++;
}
}
dblAvgRadInner = dblAvgRadInner / intRadInner;
dblAvgRadOuter = dblAvgRadOuter / intRadOuter;
}
else
{
dbPhaseStep = 2 * M_PI / intPSKPhase;
for (j = 1; j < intPhasesLen; j++) // skip the magnitude of the reference in calculation
{
intMagMax = max(intMagMax, intMag[j]); // find the max magnitude to auto scale
dblAvgRad += intMag[j];
}
}
dblAvgRad = dblAvgRad / (intPhasesLen - 1); // the average radius
for (i = 1; i < intPhasesLen; i++) // Don't plot the first phase (reference)
{
intP = round((0.001f * intPhases[i]) / dbPhaseStep);
// compute the Phase and Radius errors
if (intMag[i] > (dblAvgRadInner + dblAvgRadOuter) / 2)
dblRadErrorOuter += fabsf(dblAvgRadOuter - intMag[i]);
else
dblRadErrorInner += fabsf(dblAvgRadInner - intMag[i]);
dblPhaseError = fabsf(((0.001 * intPhases[i]) - intP * dbPhaseStep)); // always positive and < .5 * dblPhaseStep
dblPhaseErrorSum += dblPhaseError;
#ifdef PLOTCONSTELLATION
dblRad = PLOTRADIUS * intMag[i] / intMagMax; // scale the radius dblRad based on intMagMax
intX = xCenter + dblRad * cosf(dblPlotRotation + intPhases[i] / 1000.0f);
intY = yCenter + dblRad * sinf(dblPlotRotation + intPhases[i] / 1000.0f);
if (intX > 0 && intY > 0)
if (intX != xCenter && intY != yCenter)
mySetPixel(intX, intY, Yellow); // don't plot on top of axis
#endif
}
if (blnQAM)
{
// intQuality = max(0, ((100 - 200 * (dblPhaseErrorSum / (intPhasesLen)) / dbPhaseStep))); // ignore radius error for (PSK) but include for QAM
intQuality = max(0, (1 - (dblRadErrorInner / (intRadInner * dblAvgRadInner) + dblRadErrorOuter / (intRadOuter * dblAvgRadOuter))) * (100 - 200 * (dblPhaseErrorSum / intPhasesLen) / dbPhaseStep));
// intQuality = max(0, ((100 - 200 * (dblPhaseErrorSum / (intPhasesLen)) / dbPhaseStep))); // ignore radius error for (PSK) but include for QAM
if (AccumulateStats)
{
if (OFDM)
{
intOFDMQualityCnts[RXOFDMMode] ++;
intOFDMQuality[RXOFDMMode] += intQuality;
intOFDMSymbolsDecoded += intPhasesLen;
}
else
{
intQAMQualityCnts += 1;
intQAMQuality += intQuality;
intQAMSymbolsDecoded += intPhasesLen;
}
}
}
else
{
intQuality = max(0, ((100 - 200 * (dblPhaseErrorSum / (intPhasesLen)) / dbPhaseStep))); // ignore radius error for (PSK) but include for QAM
if (AccumulateStats)
{
if (OFDM)
{
intOFDMQualityCnts[RXOFDMMode] ++;
intOFDMQuality[RXOFDMMode] += intQuality;
intOFDMSymbolsDecoded += intPhasesLen;
}
else
{
intPSKQualityCnts[intPSKIndex]++;
intPSKQuality[intPSKIndex] += intQuality;
intPSKSymbolsDecoded += intPhasesLen;
}
}
}
#ifdef PLOTCONSTELLATION
DrawAxes(intQuality, shortName(intFrameType), strMod);
#endif
return intQuality;
}
// Subroutine to track 1 carrier 4FSK. Used for both single and multiple simultaneous carrier 4FSK modes.
VOID Track1Car4FSK(short * intSamples, int * intPtr, int intSampPerSymbol, float dblSearchFreq, int intBaud, UCHAR * bytSymHistory)
{
// look at magnitude of the tone for bytHistory(1) 2 sample2 earlier and 2 samples later. and pick the maximum adjusting intPtr + or - 1
// this seems to work fine on test Mar 16, 2015. This should handle sample rate offsets (sender to receiver) up to about 2000 ppm
float dblReal, dblImag, dblMagEarly, dblMag, dblMagLate;
float dblBinToSearch = (dblSearchFreq - (intBaud * bytSymHistory[1])) / intBaud; // select the 2nd last symbol for magnitude comparison
GoertzelRealImag(intSamples, (*intPtr - intSampPerSymbol - 2), intSampPerSymbol, dblBinToSearch, &dblReal, &dblImag);
dblMagEarly = powf(dblReal, 2) + powf(dblImag, 2);
GoertzelRealImag(intSamples, (*intPtr - intSampPerSymbol), intSampPerSymbol, dblBinToSearch, &dblReal, &dblImag);
dblMag = powf(dblReal, 2) + powf(dblImag, 2);
GoertzelRealImag(intSamples, (*intPtr - intSampPerSymbol + 2), intSampPerSymbol, dblBinToSearch, &dblReal, &dblImag);
dblMagLate = powf(dblReal, 2) + powf(dblImag, 2);
if (dblMagEarly > dblMag && dblMagEarly > dblMagLate)
{
*intPtr --;
Corrections--;
if (AccumulateStats)
intAccumFSKTracking--;
}
else if (dblMagLate > dblMag && dblMagLate > dblMagEarly)
{
*intPtr ++;
Corrections++;
if (AccumulateStats)
intAccumFSKTracking++;
}
}
// Function to Decode one Carrier of PSK modulation
// Ideally want to be able to call on for each symbol, as I don't have the
// RAM to build whole frame
// Call for each set of 4 or 8 Phase Values
int pskStart = 0;
VOID Decode1CarPSK(int Carrier, BOOL OFDM)
{
unsigned int int24Bits;
UCHAR bytRawData;
int k;
int Len = intPhasesLen;
UCHAR * Decoded;
if (OFDM)
Decoded = &bytFrameData[0][0]; // Always uses same buffer
else
{
if (CarrierOk[Carrier])
return; // don't do it again
Decoded = &bytFrameData[Carrier][0];
}
pskStart = 0;
charIndex = 0;
while (Len >= 0)
{
// Phase Samples are in intPhases
switch (intPSKMode)
{
case 2: // process 8 sequential phases per byte (1 bits per phase)
for (k = 0; k < 8; k++)
{
if (k == 0)
bytRawData = 0;
else
bytRawData <<= 1;
if (intPhases[Carrier][pskStart] >= 1572 || intPhases[Carrier][pskStart]<= -1572)
bytRawData += 1;
pskStart++;
}
Decoded[charIndex++] = bytRawData;
Len -= 8;
break;
case 4: // process 4 sequential phases per byte (2 bits per phase)
for (k = 0; k < 4; k++)
{
if (k == 0)
bytRawData = 0;
else
bytRawData <<= 2;
if (intPhases[Carrier][pskStart] < 786 && intPhases[Carrier][pskStart] > -786)
{
} // Zero so no need to do anything
else if (intPhases[Carrier][pskStart] >= 786 && intPhases[Carrier][pskStart] < 2356)
bytRawData += 1;
else if (intPhases[Carrier][pskStart] >= 2356 || intPhases[Carrier][pskStart] <= -2356)
bytRawData += 2;
else
bytRawData += 3;
pskStart++;
}
Decoded[charIndex++] = bytRawData;
Len -= 4;
break;
case 8: // Process 8 sequential phases (3 bits per phase) for 24 bits or 3 bytes
// Status verified on 1 Carrier 8PSK with no RS needed for High S/N
// Assume we check for 8 available phase samples before being called
int24Bits = 0;
for (k = 0; k < 8; k++)
{
int24Bits <<= 3;
if (intPhases[Carrier][pskStart] < 393 && intPhases[Carrier][pskStart] > -393)
{
} // Zero so no need to do anything
else if (intPhases[Carrier][pskStart] >= 393 && intPhases[Carrier][pskStart] < 1179)
int24Bits += 1;
else if (intPhases[Carrier][pskStart] >= 1179 && intPhases[Carrier][pskStart] < 1965)
int24Bits += 2;
else if (intPhases[Carrier][pskStart] >= 1965 && intPhases[Carrier][pskStart] < 2751)
int24Bits += 3;
else if (intPhases[Carrier][pskStart] >= 2751 || intPhases[Carrier][pskStart] < -2751)
int24Bits += 4;
else if (intPhases[Carrier][pskStart] >= -2751 && intPhases[Carrier][pskStart] < -1965)
int24Bits += 5;
else if (intPhases[Carrier][pskStart] >= -1965 && intPhases[Carrier][pskStart] <= -1179)
int24Bits += 6;
else
int24Bits += 7;
pskStart ++;
}
Decoded[charIndex++] = int24Bits >> 16;
Decoded[charIndex++] = int24Bits >> 8;
Decoded[charIndex++] = int24Bits;
Len -= 8;
break;
case 16: // Process 2 sequential phases (4 bits per phase) for 1 bytes
for (k = 0; k < 2; k++)
{
if (k == 0)
bytRawData = 0;
else
bytRawData <<= 4;
if (intPhases[Carrier][pskStart] < 196 && intPhases[Carrier][pskStart] > -196)
{
} // Zero so no need to do anything
else if (intPhases[Carrier][pskStart] >= 196 && intPhases[Carrier][pskStart] < 589)
bytRawData += 1;
else if (intPhases[Carrier][pskStart] >= 589 && intPhases[Carrier][pskStart] < 981)
bytRawData += 2;
else if (intPhases[Carrier][pskStart] >= 981 && intPhases[Carrier][pskStart] < 1374)
bytRawData += 3;
else if (intPhases[Carrier][pskStart] >= 1374 && intPhases[Carrier][pskStart] < 1766)
bytRawData += 4;
else if (intPhases[Carrier][pskStart] >= 1766 && intPhases[Carrier][pskStart] < 2159)
bytRawData += 5;
else if (intPhases[Carrier][pskStart] >= 2159 && intPhases[Carrier][pskStart] < 2551)
bytRawData += 6;
else if (intPhases[Carrier][pskStart] >= 2551 && intPhases[Carrier][pskStart] < 2944)
bytRawData += 7;
else if (intPhases[Carrier][pskStart] >= 2944 || intPhases[Carrier][pskStart] < -2944)
bytRawData += 8;
else if (intPhases[Carrier][pskStart] >= -2944 && intPhases[Carrier][pskStart] < -2551)
bytRawData += 9;
else if (intPhases[Carrier][pskStart] >= -2551 && intPhases[Carrier][pskStart] < -2159)
bytRawData += 10;
else if (intPhases[Carrier][pskStart] >= -2159 && intPhases[Carrier][pskStart] < -1766)
bytRawData += 11;
else if (intPhases[Carrier][pskStart] >= -1766 && intPhases[Carrier][pskStart] < -1374)
bytRawData += 12;
else if (intPhases[Carrier][pskStart] >= -1374 && intPhases[Carrier][pskStart] < -981)
bytRawData += 13;
else if (intPhases[Carrier][pskStart] >= -981 && intPhases[Carrier][pskStart] < -589)
bytRawData += 14;
else
bytRawData += 15;
pskStart ++;
}
Decoded[charIndex++] = bytRawData;
Len -= 2;
break;
default:
return; //????
}
}
return;
}
// Function to compute PSK symbol tracking (all PSK modes, used for single or multiple carrier modes)
int Track1CarPSK(int floatCarFreq, int PSKMode, BOOL QAM, BOOL OFDM, float dblUnfilteredPhase, BOOL blnInit)
{
// This routine initializes and tracks the phase offset per symbol and adjust intPtr +/-1 when the offset creeps to a threshold value.
// adjusts (by Ref) intPtr 0, -1 or +1 based on a filtering of phase offset.
// this seems to work fine on test Mar 21, 2015. May need optimization after testing with higher sample rate errors.
// This should handle sample rate offsets (sender to receiver) up to about 2000 ppm
float dblAlpha = 0.3f; // low pass filter constant may want to optimize value after testing with large sample rate error.
// (Affects how much averaging is done) lower values of dblAlpha will minimize adjustments but track more slugishly.
float dblPhaseOffset;
static float dblTrackingPhase = 0;
static float dblModFactor;
static float dblRadiansPerSample; // range is .4188 @ car freq = 800 to 1.1195 @ car freq 2200
static float dblPhaseAtLastTrack;
static int intCountAtLastTrack;
static float dblFilteredPhaseOffset;
if (blnInit)
{
// dblFilterredPhase = dblUnfilteredPhase;
dblTrackingPhase = dblUnfilteredPhase;
if (PSKMode == 16)
dblModFactor = M_PI / 8;
else if (PSKMode == 8)
dblModFactor = M_PI / 4;
else if (PSKMode == 4)
dblModFactor = M_PI / 2;
else
dblModFactor = M_PI; // 2PSK
dblRadiansPerSample = (floatCarFreq * dbl2Pi) / 12000.0f;
dblPhaseOffset = dblUnfilteredPhase - dblModFactor * round(dblUnfilteredPhase / dblModFactor);
dblPhaseAtLastTrack = dblPhaseOffset;
dblFilteredPhaseOffset = dblPhaseOffset;
intCountAtLastTrack = 0;
return 0;
}
intCountAtLastTrack += 1;
dblPhaseOffset = dblUnfilteredPhase - dblModFactor * round(dblUnfilteredPhase / dblModFactor);
dblFilteredPhaseOffset = (1 - dblAlpha) * dblFilteredPhaseOffset + dblAlpha * dblPhaseOffset;
if ((dblFilteredPhaseOffset - dblPhaseAtLastTrack) > dblRadiansPerSample)
{
//Debug.WriteLine("Filtered>LastTrack: Cnt=" & intCountAtLastTrack.ToString & " Filtered = " & Format(dblFilteredPhaseOffset, "00.000") & " Offset = " & Format(dblPhaseOffset, "00.000") & " Unfiltered = " & Format(dblUnfilteredPhase, "00.000"))
dblFilteredPhaseOffset = dblPhaseOffset - dblRadiansPerSample;
dblPhaseAtLastTrack = dblFilteredPhaseOffset;
if (AccumulateStats)
{
if (OFDM)
{
intOFDMTrackAttempts++;
intAccumOFDMTracking--;
}
else
if (QAM)
{
intQAMTrackAttempts++;
intAccumQAMTracking--;
}
else
{
intPSKTrackAttempts++;
intAccumPSKTracking--;
}
}
return -1;
}
if ((dblPhaseAtLastTrack - dblFilteredPhaseOffset) > dblRadiansPerSample)
{
//'Debug.WriteLine("Filtered<LastTrack: Cnt=" & intCountAtLastTrack.ToString & " Filtered = " & Format(dblFilteredPhaseOffset, "00.000") & " Offset = " & Format(dblPhaseOffset, "00.000") & " Unfiltered = " & Format(dblUnfilteredPhase, "00.000"))
dblFilteredPhaseOffset = dblPhaseOffset + dblRadiansPerSample;
dblPhaseAtLastTrack = dblFilteredPhaseOffset;
if (AccumulateStats)
{
if (OFDM)
{
intOFDMTrackAttempts++;
intAccumOFDMTracking++;
}
else
if (QAM) // 16QAM" Then
{
intQAMTrackAttempts++;
intAccumQAMTracking++;
}
else
{
intPSKTrackAttempts++;
intAccumPSKTracking++;
}
}
return 1;
}
// 'Debug.WriteLine("Filtered Phase = " & Format(dblFilteredPhaseOffset, "00.000") & " Offset = " & Format(dblPhaseOffset, "00.000") & " Unfiltered = " & Format(dblUnfilteredPhase, "00.000"))
return 0;
}
// Function to compute the differenc of two angles
int ComputeAng1_Ang2(int intAng1, int intAng2)
{
// do an angle subtraction intAng1 minus intAng2 (in milliradians)
// Results always between -3142 and 3142 (+/- Pi)
int intDiff;
intDiff = intAng1 - intAng2;
if (intDiff < -3142)
intDiff += 6284;
else if (intDiff > 3142 )
intDiff -= 6284;
return intDiff;
}
// Subroutine to "rotate" the phases to try and set the average offset to 0.
void CorrectPhaseForTuningOffset(short * intPhase, int intPhaseLength, int intPSKMode)
{
// A tunning error of -1 Hz will rotate the phase calculation Clockwise ~ 64 milliradians (~4 degrees)
// This corrects for:
// 1) Small tuning errors which result in a phase bias (rotation) of then entire constellation
// 2) Small Transmitter/receiver drift during the frame by averaging and adjusting to constellation to the average.
// It only processes phase values close to the nominal to avoid generating too large of a correction from outliers: +/- 30 deg for 4PSK, +/- 15 deg for 8PSK
// Is very affective in handling initial tuning error.
short intPhaseMargin = 2095 / intPSKMode; // Compute the acceptable phase correction range (+/-30 degrees for 4 PSK)
short intPhaseInc = 6284 / intPSKMode;
int intTest;
int i;
int intOffset, intAvgOffset, intAvgOffsetBeginning, intAvgOffsetEnd;
int intAccOffsetCnt = 0, intAccOffsetCntBeginning = 0, intAccOffsetCntEnd = 0;
int intAccOffsetBeginning = 0, intAccOffsetEnd = 0, intAccOffset = 0;
// Note Rev 0.6.2.4 The following phase margin value increased from 2095 (120 deg) to 2793 (160 deg) yielded an improvement in decode at low S:N
intPhaseMargin = 2793 / intPSKMode; // Compute the acceptable phase correction range (+/-30 degrees for 4 PSK)
intPhaseInc = 6284 / intPSKMode;
// Compute the average offset (rotation) for all symbols within +/- intPhaseMargin of nominal
for (i = 0; i < intPhaseLength; i++)
{
intTest = (intPhase[i] / intPhaseInc);
intOffset = intPhase[i] - intTest * intPhaseInc;
if ((intOffset >= 0 && intOffset <= intPhaseMargin) || (intOffset < 0 && intOffset >= -intPhaseMargin))
{
intAccOffsetCnt += 1;
intAccOffset += intOffset;
if (i <= intPhaseLength / 4)
{
intAccOffsetCntBeginning += 1;
intAccOffsetBeginning += intOffset;
}
else if (i >= (3 * intPhaseLength) / 4)
{
intAccOffsetCntEnd += 1;
intAccOffsetEnd += intOffset;
}
}
}
if (intAccOffsetCnt > 0)
intAvgOffset = (intAccOffset / intAccOffsetCnt);
if (intAccOffsetCntBeginning > 0)
intAvgOffsetBeginning = (intAccOffsetBeginning / intAccOffsetCntBeginning);
if (intAccOffsetCntEnd > 0)
intAvgOffsetEnd = (intAccOffsetEnd / intAccOffsetCntEnd);
//Debugprintf("[CorrectPhaseForOffset] Beginning: %d End: %d Total: %d",
//intAvgOffsetBeginning, intAvgOffsetEnd, intAvgOffset);
if ((intAccOffsetCntBeginning > intPhaseLength / 8) && (intAccOffsetCntEnd > intPhaseLength / 8))
{
for (i = 0; i < intPhaseLength; i++)
{
intPhase[i] = intPhase[i] - ((intAvgOffsetBeginning * (intPhaseLength - i) / intPhaseLength) + (intAvgOffsetEnd * i / intPhaseLength));
if (intPhase[i] > 3142)
intPhase[i] -= 6284;
else if (intPhase[i] < -3142)
intPhase[i] += 6284;
}
Debugprintf("[CorrectPhaseForTuningOffset] AvgOffsetBeginning=%d AvgOffsetEnd=%d AccOffsetCnt=%d/%d",
intAvgOffsetBeginning, intAvgOffsetEnd, intAccOffsetCnt, intPhaseLength);
}
else if (intAccOffsetCnt > intPhaseLength / 2)
{
for (i = 0; i < intPhaseLength; i++)
{
intPhase[i] -= intAvgOffset;
if (intPhase[i] > 3142)
intPhase[i] -= 6284;
else if (intPhase[i] < -3142)
intPhase[i] += 6284;
}
Debugprintf("[CorrectPhaseForTuningOffset] AvgOffset=%d AccOffsetCnt=%d/%d",
intAvgOffset, intAccOffsetCnt, intPhaseLength);
}
}
// Function to Decode one Carrier of 16QAM modulation
// Call for each set of 4 or 8 Phase Values
short intCarMagThreshold[MAXCAR] = {0};
VOID Decode1CarQAM(int Carrier)
{
unsigned int intData;
int k;
float dblAlpha = 0.1f; // this determins how quickly the rolling average dblTrackingThreshold responds.
// dblAlpha value of .1 seems to work well...needs to be tested on fading channel (e.g. Multipath)
int Threshold = intCarMagThreshold[Carrier];
int Len = intPhasesLen;
UCHAR * Decoded = bytFrameData[Carrier];
if (CarrierOk[Carrier])
return; // don't do it again
pskStart = 0;
charIndex = 0;
// We calculated initial mag from reference symbol
// use filtered tracking of refernce phase amplitude
// (should be full amplitude value)
// On WGN this appears to improve decoding threshold about 1 dB 9/3/2016
while (Len >= 0)
{
// Phase Samples are in intPhases
intData = 0;
for (k = 0; k < 2; k++)
{
intData <<= 4;
if (intPhases[Carrier][pskStart] < 393 && intPhases[Carrier][pskStart] > -393)
{
} // Zero so no need to do anything
else if (intPhases[Carrier][pskStart] >= 393 && intPhases[Carrier][pskStart] < 1179)
intData += 1;
else if (intPhases[Carrier][pskStart] >= 1179 && intPhases[Carrier][pskStart] < 1965)
intData += 2;
else if (intPhases[Carrier][pskStart] >= 1965 && intPhases[Carrier][pskStart] < 2751)
intData += 3;
else if (intPhases[Carrier][pskStart] >= 2751 || intPhases[Carrier][pskStart] < -2751)
intData += 4;
else if (intPhases[Carrier][pskStart] >= -2751 && intPhases[Carrier][pskStart] < -1965)
intData += 5;
else if (intPhases[Carrier][pskStart] >= -1965 && intPhases[Carrier][pskStart] <= -1179)
intData += 6;
else
intData += 7;
if (intMags[Carrier][pskStart] < Threshold)
{
intData += 8; // add 8 to "inner circle" symbols.
Threshold = (Threshold * 900 + intMags[Carrier][pskStart] * 150) / 1000;
}
else
{
Threshold = ( Threshold * 900 + intMags[Carrier][pskStart] * 75) / 1000;
}
intCarMagThreshold[Carrier] = Threshold;
pskStart++;
}
Decoded[charIndex++] = intData;
Len -=2;
}
}
// Functions to demod all PSKData frames single or multiple carriers
VOID InitDemodPSK()
{
// Called at start of frame
int i;
float dblPhase, dblReal, dblImag;
intPSKMode = strMod[0] - '0';
PSKInitDone = TRUE;
intPhasesLen = 0;
if (intPSKMode == 8)
dblPhaseInc = 2 * M_PI * 1000 / 8;
else
dblPhaseInc = 2 * M_PI * 1000 / 4;
if (intBaud == 50)
intSampPerSym = 240;
else
intSampPerSym = 120;
if (intNumCar == 1)
floatCarFreq = 1500;
else
floatCarFreq = 1400 + (intNumCar / 2) * 200; // start at the highest carrier freq which is actually the lowest transmitted carrier due to Reverse sideband mixing
for (i= 0; i < intNumCar; i++)
{
if (intBaud == 50)
{
intCP[i] = 0;
intNforGoertzel[i] = 240;
dblFreqBin[i] = floatCarFreq / 50;
}
else if (intBaud == 100)
{
//Experimental use of Hanning Windowing
intNforGoertzel[i] = 120;
dblFreqBin[i] = floatCarFreq / 100;
intCP[i] = 0;
}
/* if (intBaud == 100 && floatCarFreq == 1500)
{
intCP[i] = 20; // These values selected for best decode percentage (92%) and best average 4PSK Quality (82) on MPP0dB channel
dblFreqBin[i] = floatCarFreq / 150;
intNforGoertzel[i] = 80;
}
else if (intBaud == 100)
{
intCP[i] = 28; // This value selected for best decoding percentage (56%) and best Averag 4PSK Quality (77) on mpg +5 dB
intNforGoertzel[i] = 60;
dblFreqBin[i] = floatCarFreq / 200;
}
else if (intBaud == 167)
{
intCP[i] = 6; // Need to optimize (little difference between 6 and 12 @ wgn5, 2 Car 500 Hz)
intNforGoertzel[i] = 60;
dblFreqBin[i] = floatCarFreq / 200;
}
*/
// Get initial Reference Phase
GoertzelRealImagHann120(intFilteredMixedSamples, 0, intNforGoertzel[i], dblFreqBin[i], &dblReal, &dblImag);
dblPhase = atan2f(dblImag, dblReal);
Track1CarPSK(floatCarFreq, strMod[0] - '0', FALSE, FALSE, dblPhase, TRUE);
intPSKPhase_1[i] = -1000 * dblPhase; // negative sign compensates for phase reverse after mixing
// Set initial mag from Reference Phase (which should be full power)
// Done here as well as in initQAM for pkt where we may switch mode midpacket
intCarMagThreshold[i] = sqrtf(powf(dblReal, 2) + powf(dblImag, 2));
intCarMagThreshold[i] *= 0.75;
floatCarFreq -= 200; // Step through each carrier Highest to lowest which is equivalent to lowest to highest before RSB mixing.
}
}
int Demod1CarPSKChar(int Start, int Carrier);
void SavePSKSamples(int i);
short WeightedAngleAvg(short intAng1, short intAng2);
int CheckCarrierPairPSK(int Base, int Dup, int frameLen)
{
int i, Len;
Debugprintf("DemodPSK Carriers %d and %d", Base, Dup);
Decode1CarPSK(Base, FALSE);
Len = CorrectRawDataWithRS(&bytFrameData[Base][0], &bytData[frameLen], intDataLen, intRSLen, intFrameType, Base);
if (CarrierOk[Base])
{
// No need to decode 2nd
CarrierOk[Dup] = 1; // So FrameOk test is passed
return Len + frameLen;
}
Debugprintf("DemodPSK Carrier %d bad, trying %d", Base, Dup);
Decode1CarPSK(Dup, FALSE); // Decode Dup carrier
Len = CorrectRawDataWithRS(&bytFrameData[Dup][0], &bytData[frameLen], intDataLen, intRSLen, intFrameType, Base); // Save as carrier 1
if (CarrierOk[Base])
{
CarrierOk[Dup] = 1; // So FrameOk test is passed
bytFrameData[Base][0] = Len;
memcpy(&bytFrameData[Base][1], &bytData[frameLen], Len); // Any retry will use first copy without new decode
return Len + frameLen;
}
// Try to average phases for the two carriers
Debugprintf("DemodPSK both bad, trying average");
for (i = 0; i <intPhasesLen; i++)
{
intPhases[Base][i] = WeightedAngleAvg(intPhases[Base][i], intPhases[Dup][i]);
}
// Try decode again on averages
Decode1CarPSK(Base, FALSE);
Len = CorrectRawDataWithRS(&bytFrameData[Base][0], &bytData[frameLen], intDataLen, intRSLen, intFrameType, Base);
if (CarrierOk[Base])
CarrierOk[Dup] = 1; // So FrameOk test is passed
return Len + frameLen;
}
void DemodPSK()
{
int Used[MAXCAR] = {0}, Carrier;
int Start = 0, i;
int MemARQRetries = 0;
// We can't wait for the full frame as we don't have enough RAM, so
// we do one DMA Buffer at a time, until we run out or end of frame
// Only continue if we have enough samples
intPSKMode = strMod[0] - '0';
while (State == AcquireFrame)
{
if (intFilteredMixedSamplesLength < intPSKMode * intSampPerSym + 10) // allow for a few phase corrections
{
// Move any unprocessessed data down buffer
// (while checking process - will use cyclic buffer eventually
if (intFilteredMixedSamplesLength > 0 && Start > 0)
memmove(intFilteredMixedSamples,
&intFilteredMixedSamples[Start], intFilteredMixedSamplesLength * 2);
return;
}
if (PSKInitDone == 0) // First time through
{
if (intFilteredMixedSamplesLength < 2 * intPSKMode * intSampPerSym + 10)
return; // Wait for at least 2 chars worth
InitDemodPSK();
intFilteredMixedSamplesLength -= intSampPerSym;
if (intFilteredMixedSamplesLength < 0)
Debugprintf("Corrupt intFilteredMixedSamplesLength");
Start += intSampPerSym;
}
// If this is a multicarrier mode, we must call the
// decode char routing for each carrier
if (intNumCar == 1)
floatCarFreq = 1500;
else
floatCarFreq = 1400 + (intNumCar / 2) * 200; // start at the highest carrier freq which is actually the lowest transmitted carrier due to Reverse sideband mixing
Used[0] = Demod1CarPSKChar(Start, 0);
if (intNumCar > 1)
{
intPhasesLen -= intPSKMode;
floatCarFreq -= 200; // Step through each carrier Highest to lowest which is equivalent to lowest to highest before RSB mixing.
Used[1] = Demod1CarPSKChar(Start, 1);
}
if (intNumCar > 2)
{
intPhasesLen -= intPSKMode;
floatCarFreq -= 200; // Step through each carrier Highest to lowest which is equivalent to lowest to highest before RSB mixing.
Used[2] = Demod1CarPSKChar(Start, 2);
intPhasesLen -= intPSKMode;
floatCarFreq -= 200; // Step through each carrier Highest to lowest which is equivalent to lowest to highest before RSB mixing.
Used[3] = Demod1CarPSKChar(Start, 3);
}
if (intNumCar > 4)
{
intPhasesLen -= intPSKMode;
floatCarFreq -= 200; // Step through each carrier Highest to lowest which is equivalent to lowest to highest before RSB mixing.
Used[4] = Demod1CarPSKChar(Start, 4);
intPhasesLen -= intPSKMode;
floatCarFreq -= 200; // Step through each carrier Highest to lowest which is equivalent to lowest to highest before RSB mixing.
Used[5] = Demod1CarPSKChar(Start, 5);
intPhasesLen -= intPSKMode;
floatCarFreq -= 200; // Step through each carrier Highest to lowest which is equivalent to lowest to highest before RSB mixing.
Used[6] = Demod1CarPSKChar(Start, 6);
intPhasesLen -= intPSKMode;
floatCarFreq -= 200; // Step through each carrier Highest to lowest which is equivalent to lowest to highest before RSB mixing.
Used[7] = Demod1CarPSKChar(Start, 7);
intPhasesLen -= intPSKMode;
floatCarFreq -= 200; // Step through each carrier Highest to lowest which is equivalent to lowest to highest before RSB mixing.
Used[8] = Demod1CarPSKChar(Start, 8);
intPhasesLen -= intPSKMode;
floatCarFreq -= 200; // Step through each carrier Highest to lowest which is equivalent to lowest to highest before RSB mixing.
Used[9] = Demod1CarPSKChar(Start, 9);
}
if (intPSKMode == 4)
SymbolsLeft--; // number still to decode
else
SymbolsLeft -=3;
// If/when we reenable phase correstion we can take average of Used values.
// ?? Should be also keep start value per carrier ??
Start += Used[0];
intFilteredMixedSamplesLength -= Used[0];
if (intFilteredMixedSamplesLength < 0)
Debugprintf("Corrupt intFilteredMixedSamplesLength");
if (SymbolsLeft > 0)
continue;
// Decode the phases
DecodeCompleteTime = Now;
// CorrectPhaseForTuningOffset(&intPhases[0][0], intPhasesLen, strMod);
// if (intNumCar > 1)
// CorrectPhaseForTuningOffset(&intPhases[1][0], intPhasesLen, strMod);
if (intNumCar > 2)
{
// CorrectPhaseForTuningOffset(&intPhases[2][0], intPhasesLen, strMod);
// CorrectPhaseForTuningOffset(&intPhases[3][0], intPhasesLen, strMod);
}
if (intNumCar > 4)
{
// CorrectPhaseForTuningOffset(&intPhases[4][0], intPhasesLen, strMod);
// CorrectPhaseForTuningOffset(&intPhases[5][0], intPhasesLen, strMod);
// CorrectPhaseForTuningOffset(&intPhases[6][0], intPhasesLen, strMod);
// CorrectPhaseForTuningOffset(&intPhases[7][0], intPhasesLen, strMod);
}
// Rick uses the last carier for Quality
intLastRcvdFrameQuality = UpdatePhaseConstellation(&intPhases[intNumCar - 1][0], &intMags[intNumCar - 1][0], strMod[0] - '0', FALSE, FALSE);
// prepare for next
State = SearchingForLeader;
DiscardOldSamples();
ClearAllMixedSamples();
if (strchr(strMod, 'R'))
{
// Robust Mode - data is repeated (1-2 or 1-6, 2-7, etc
if (intNumCar == 2)
{
frameLen = CheckCarrierPairPSK(0, 1, 0);
return;
}
//Only have 2 or 10 (500 or 2500 modes)
frameLen = CheckCarrierPairPSK(0, 5, 0);
frameLen = CheckCarrierPairPSK(1, 6, frameLen);
frameLen = CheckCarrierPairPSK(2, 7, frameLen);
frameLen = CheckCarrierPairPSK(3, 8, frameLen);
frameLen = CheckCarrierPairPSK(4, 9, frameLen);
return;
}
// Non -robust
frameLen = 0;
for (i = 0; i < intNumCar; i++)
{
Decode1CarPSK(i, FALSE);
frameLen += CorrectRawDataWithRS(&bytFrameData[i][0], &bytData[frameLen], intDataLen, intRSLen, intFrameType, i);
}
// If variable length packet frame header we only have header - leave rx running
if (intFrameType == PktFrameHeader)
{
State = SearchingForLeader;
// Save any unused samples
if (intFilteredMixedSamplesLength > 0 && Start > 0)
memmove(intFilteredMixedSamples,
&intFilteredMixedSamples[Start], intFilteredMixedSamplesLength * 2);
return;
}
#ifdef MEMORYARQ
for (Carrier = 0; Carrier < intNumCar; Carrier++)
{
if (!CarrierOk[Carrier])
{
// Decode error - save data for MEM ARQ
SavePSKSamples(Carrier);
if (intSumCounts[Carrier] > 1)
{
Decode1CarQAM(Carrier); // try to decode based on the WeightedAveragePhases
MemARQRetries++;
}
}
}
if (MemARQRetries)
{
// We've retryed to decode - see if ok now
int OKNow = TRUE;
Debugprintf("DemodPSK retry RS on MEM ARQ Corrected frames");
frameLen = 0;
for (Carrier = 0; Carrier < intNumCar; Carrier++)
{
frameLen += CorrectRawDataWithRS(bytFrameData[Carrier], bytData, intDataLen, intRSLen, intFrameType, Carrier);
if (CarrierOk[Carrier] == 0)
OKNow = FALSE;
}
if (OKNow && AccumulateStats)
intGoodPSKSummationDecodes++;
}
#endif
}
return;
}
// Function to demodulate one carrier for all PSK frame types
int Demod1CarPSKChar(int Start, int Carrier)
{
// Converts intSample to an array of differential phase and magnitude values for the Specific Carrier Freq
// intPtr should be pointing to the approximate start of the first reference/training symbol (1 of 3)
// intPhase() is an array of phase values (in milliradians range of 0 to 6283) for each symbol
// intMag() is an array of Magnitude values (not used in PSK decoding but for constellation plotting or QAM decoding)
// Objective is to use Minimum Phase Error Tracking to maintain optimum pointer position
// This is called for one DMA buffer of samples (normally 1200)
float dblReal, dblImag;
int intMiliRadPerSample = floatCarFreq * M_PI / 6;
int i;
int intNumOfSymbols = intPSKMode;
int origStart = Start;;
if (CarrierOk[Carrier]) // Already decoded this carrier?
{
intPhasesLen += intNumOfSymbols;
return intSampPerSym * intNumOfSymbols;
}
for (i = 0; i < intNumOfSymbols; i++)
{
GoertzelRealImag(intFilteredMixedSamples, Start, intNforGoertzel[Carrier], dblFreqBin[Carrier], &dblReal, &dblImag);
// GoertzelRealImagHann120(intFilteredMixedSamples, Start, intNforGoertzel[Carrier], dblFreqBin[Carrier], &dblReal, &dblImag);
intMags[Carrier][intPhasesLen] = sqrtf(powf(dblReal, 2) + powf(dblImag, 2));
intPSKPhase_0[Carrier] = -1000 * atan2f(dblImag, dblReal);
intPhases[Carrier][intPhasesLen] = (ComputeAng1_Ang2(intPSKPhase_0[Carrier], intPSKPhase_1[Carrier]));
/*
if (Carrier == 0)
{
Corrections = Track1CarPSK(floatCarFreq, strMod, atan2f(dblImag, dblReal), FALSE);
if (Corrections != 0)
{
Start += Corrections;
if (intCP[i] == 0)
GoertzelRealImagHanning(intFilteredMixedSamples, Start, intNforGoertzel[Carrier], dblFreqBin[Carrier], &dblReal, &dblImag);
else
GoertzelRealImag(intFilteredMixedSamples, Start + intCP[Carrier], intNforGoertzel[Carrier], dblFreqBin[Carrier], &dblReal, &dblImag);
intPSKPhase_0[Carrier] = 1000 * atan2f(dblImag, dblReal);
}
}
*/
intPSKPhase_1[Carrier] = intPSKPhase_0[Carrier];
intPhasesLen++;
Start += intSampPerSym;
}
if (AccumulateStats)
intPSKSymbolCnt += intNumOfSymbols;
return (Start - origStart); // Symbols we've consumed
}
VOID InitDemodQAM()
{
// Called at start of frame
int i;
float dblPhase, dblReal, dblImag;
intPSKMode = 8; // 16QAM uses 8 PSK
dblPhaseInc = 2 * M_PI * 1000 / 8;
intPhasesLen = 0;
PSKInitDone = TRUE;
intSampPerSym = 120;
if (intNumCar == 1)
floatCarFreq = 1500;
else
floatCarFreq = 1400 + (intNumCar / 2) * 200; // start at the highest carrier freq which is actually the lowest transmitted carrier due to Reverse sideband mixing
for (i= 0; i < intNumCar; i++)
{
// Only 100 Hz for QAM
intCP[i] = 0;
intNforGoertzel[i] = 120;
dblFreqBin[i] = floatCarFreq / 100;
// Get initial Reference Phase
GoertzelRealImagHanning(intFilteredMixedSamples, intCP[i], intNforGoertzel[i], dblFreqBin[i], &dblReal, &dblImag);
dblPhase = atan2f(dblImag, dblReal);
// Set initial mag from Reference Phase (which should be full power)
intCarMagThreshold[i] = sqrtf(powf(dblReal, 2) + powf(dblImag, 2));
intCarMagThreshold[i] *= 0.75;
Track1CarPSK(floatCarFreq, 8, TRUE, FALSE, dblPhase, TRUE);
intPSKPhase_1[i] = 1000 * dblPhase;
floatCarFreq -= 200; // Step through each carrier Highest to lowest which is equivalent to lowest to highest before RSB mixing.
}
}
int Demod1CarQAMChar(int Start, int Carrier);
// Function to average two angles using magnitude weighting
short WeightedAngleAvg(short intAng1, short intAng2)
{
// Ang1 and Ang 2 are in the range of -3142 to + 3142 (miliradians)
// works but should come up with a routine that avoids Sin, Cos, Atan2
// Modified in Rev 0.3.5.1 to "weight" averaging by intMag1 and intMag2 (why!!!)
float dblSumX, dblSumY;
dblSumX = cosf(intAng1 / 1000.0) + cosf(intAng2 / 1000.0);
dblSumY = sinf(intAng1 / 1000.0) + sinf(intAng2 / 1000.0);
return (1000 * atan2f(dblSumY, dblSumX));
}
#ifdef MEMORYARQ
void SaveQAMSamples(int i)
{
int m;
if (intSumCounts[i] == 0)
{
// First try - initialize Sum counts Phase average and Mag Average
for (m = 0; m < intPhasesLen; m++)
{
intCarPhaseAvg[i][m] = intPhases[i][m];
intCarMagAvg[i][m] = intMags[i][m];
}
}
else
{
for (m = 0; m < intPhasesLen; m++)
{
intCarPhaseAvg[i][m] = WeightedAngleAvg(intCarPhaseAvg[i][m], intPhases[i][m]);
intPhases[i][m] = intCarPhaseAvg[i][m];
// Use simple weighted average for Mags
intCarMagAvg[i][m] = (intCarMagAvg[i][m] * intSumCounts[i] + intMags[i][m]) / (intSumCounts[i] + 1);
intMags[i][m] = intCarMagAvg[i][m];
}
}
intSumCounts[i]++;
}
void SavePSKSamples(int i)
{
int m;
if (intSumCounts[i] == 0)
{
// First try - initialize Sum counts Phase average and Mag Average
for (m = 0; m < intPhasesLen; m++)
{
intCarPhaseAvg[i][m] = intPhases[i][m];
}
}
else
{
for (m = 0; m < intPhasesLen; m++)
{
intCarPhaseAvg[i][m] = WeightedAngleAvg(intCarPhaseAvg[i][m], intPhases[i][m]);
intPhases[i][m] = intCarPhaseAvg[i][m];
}
}
intSumCounts[i]++;
}
#endif
int CheckCarrierPair(int Base, int Dup, int frameLen)
{
int i, Len;
Debugprintf("DemodQAMR Carriers %d and %d", Base, Dup);
Decode1CarQAM(Base);
Len = CorrectRawDataWithRS(&bytFrameData[Base][0], &bytData[frameLen], intDataLen, intRSLen, intFrameType, Base);
if (CarrierOk[Base])
{
// No need to decode 2nd
CarrierOk[Dup] = 1; // So FrameOk test is passed
Debugprintf("DemodQAMR Returning Len %d", Len);
return Len + frameLen;
}
Debugprintf("DemodQAMR Carrier %d bad, trying %d", Base, Dup);
Decode1CarQAM(Dup); // Decode Dup carrier
Len = CorrectRawDataWithRS(&bytFrameData[Dup][0], &bytData[frameLen], intDataLen, intRSLen, intFrameType, Base); // Save as carrier 1
if (CarrierOk[Base])
{
CarrierOk[Dup] = 1; // So FrameOk test is passed
bytFrameData[Base][0] = Len;
memcpy(&bytFrameData[Base][1], &bytData[frameLen], Len); // Any retry will use first copy without new decode
Debugprintf("DemodQAMR Returning Len %d", Len);
return Len + frameLen;
}
// Try to average phases for the two carriers
Debugprintf("DemodQAMR both bad, trying average");
for (i = 0; i <intPhasesLen; i++)
{
intPhases[Base][i] = WeightedAngleAvg(intPhases[Base][i], intPhases[Dup][i]);
intMags[Base][i] = (intMags[Base][i] + intMags[Dup][i]) / 2;
}
// Try decode again on averages
Decode1CarQAM(Base);
Len = CorrectRawDataWithRS(&bytFrameData[Base][0], &bytData[frameLen], intDataLen, intRSLen, intFrameType, Base);
if (CarrierOk[Base])
CarrierOk[Dup] = 1; // So FrameOk test is passed
Debugprintf("DemodQAMR Returning Len %d", Len);
return Len + frameLen;
}
BOOL DemodQAM()
{
int Used = 0;
int Start = 0;
int i, MemARQOk = 0;
// We can't wait for the full frame as we don't have enough RAM, so
// we do one DMA Buffer at a time, until we run out or end of frame
// Only continue if we have enough samples
while (State == AcquireFrame)
{
if (intFilteredMixedSamplesLength < 8 * intSampPerSym + 10) // allow for a few phase corrections
{
// Move any unprocessessed data down buffer
// (while checking process - will use cyclic buffer eventually
if (intFilteredMixedSamplesLength > 0)
memmove(intFilteredMixedSamples,
&intFilteredMixedSamples[Start], intFilteredMixedSamplesLength * 2);
return FALSE;
}
if (PSKInitDone == 0) // First time through
{
if (intFilteredMixedSamplesLength < 9 * intSampPerSym + 10)
return FALSE; // Wait for at least 2 chars worth
InitDemodQAM();
intFilteredMixedSamplesLength -= intSampPerSym;
Start += intSampPerSym;
}
// If this is a multicarrier mode, we must call the
// decode char routine for each carrier
if (intNumCar == 1)
floatCarFreq = 1500;
else
floatCarFreq = 1400 + (intNumCar / 2) * 200; // start at the highest carrier freq which is actually the lowest transmitted carrier due to Reverse sideband mixing
Used = Demod1CarQAMChar(Start, 0); // demods 2 phase values - enough for one char
if (intNumCar > 1)
{
intPhasesLen -= 2;
floatCarFreq -= 200; // Step through each carrier Highest to lowest which is equivalent to lowest to highest before RSB mixing.
Demod1CarQAMChar(Start, 1);
}
if (intNumCar > 2)
{
intPhasesLen -= 2;
floatCarFreq -= 200; // Step through each carrier Highest to lowest which is equivalent to lowest to highest before RSB mixing.
Demod1CarQAMChar(Start, 2);
intPhasesLen -= 2;
floatCarFreq -= 200; // Step through each carrier Highest to lowest which is equivalent to lowest to highest before RSB mixing.
Demod1CarQAMChar(Start, 3);
}
if (intNumCar > 4)
{
intPhasesLen -= 2;
floatCarFreq -= 200; // Step through each carrier Highest to lowest which is equivalent to lowest to highest before RSB mixing.
Demod1CarQAMChar(Start, 4);
intPhasesLen -= 2;
floatCarFreq -= 200; // Step through each carrier Highest to lowest which is equivalent to lowest to highest before RSB mixing.
Demod1CarQAMChar(Start, 5);
intPhasesLen -= 2;
floatCarFreq -= 200; // Step through each carrier Highest to lowest which is equivalent to lowest to highest before RSB mixing.
Demod1CarQAMChar(Start, 6);
intPhasesLen -= 2;
floatCarFreq -= 200; // Step through each carrier Highest to lowest which is equivalent to lowest to highest before RSB mixing.
Demod1CarQAMChar(Start, 7);
intPhasesLen -= 2;
floatCarFreq -= 200; // Step through each carrier Highest to lowest which is equivalent to lowest to highest before RSB mixing.
Demod1CarQAMChar(Start, 8);
intPhasesLen -= 2;
floatCarFreq -= 200; // Step through each carrier Highest to lowest which is equivalent to lowest to highest before RSB mixing.
Demod1CarQAMChar(Start, 9);
}
SymbolsLeft--; // number still to decode - we've done one
Start += Used;
intFilteredMixedSamplesLength -= Used;
if (SymbolsLeft <= 0)
{
// Frame complete - decode it
DecodeCompleteTime = Now;
// CorrectPhaseForTuningOffset(&intPhases[0][0], intPhasesLen, strMod);
// if (intNumCar > 1)
// CorrectPhaseForTuningOffset(&intPhases[1][0], intPhasesLen, strMod);
if (intNumCar > 2)
{
// CorrectPhaseForTuningOffset(&intPhases[2][0], intPhasesLen, strMod);
// CorrectPhaseForTuningOffset(&intPhases[3][0], intPhasesLen, strMod);
}
if (intNumCar > 4)
{
// CorrectPhaseForTuningOffset(&intPhases[4][0], intPhasesLen, strMod);
// CorrectPhaseForTuningOffset(&intPhases[5][0], intPhasesLen, strMod);
// CorrectPhaseForTuningOffset(&intPhases[6][0], intPhasesLen, strMod);
// CorrectPhaseForTuningOffset(&intPhases[7][0], intPhasesLen, strMod);
}
intLastRcvdFrameQuality = UpdatePhaseConstellation(&intPhases[intNumCar - 1][0], &intMags[intNumCar - 1][0], 8, TRUE, FALSE);
// prepare for next so we can exit when we have finished decode
DiscardOldSamples();
ClearAllMixedSamples();
State = SearchingForLeader;
if (strchr(strMod, 'R'))
{
// Robust Mode - data is repeated (1-2 or 1-6, 2-7, etc
if (intNumCar == 2)
{
frameLen = CheckCarrierPair(0, 1, 0);
return TRUE;
}
//Only have 2 or 10 (500 or 2500 modes)
frameLen = CheckCarrierPair(0, 5, 0);
frameLen = CheckCarrierPair(1, 6, frameLen);
frameLen = CheckCarrierPair(2, 7, frameLen);
frameLen = CheckCarrierPair(3, 8, frameLen);
frameLen = CheckCarrierPair(4, 9, frameLen);
return TRUE;
}
// Non -robust
Decode1CarQAM(0);
frameLen = CorrectRawDataWithRS(&bytFrameData[0][0], bytData, intDataLen, intRSLen, intFrameType, 0);
if (intNumCar > 1)
{
Decode1CarQAM(1);
frameLen += CorrectRawDataWithRS(&bytFrameData[1][0], &bytData[frameLen], intDataLen, intRSLen, intFrameType, 1);
}
if (intNumCar > 2)
{
Decode1CarQAM(2);
Decode1CarQAM(3);
frameLen += CorrectRawDataWithRS(&bytFrameData[2][0], &bytData[frameLen], intDataLen, intRSLen, intFrameType, 2);
frameLen += CorrectRawDataWithRS(&bytFrameData[3][0], &bytData[frameLen], intDataLen, intRSLen, intFrameType, 3);
}
if (intNumCar > 4)
{
Decode1CarQAM(4);
Decode1CarQAM(5);
Decode1CarQAM(6);
Decode1CarQAM(7);
Decode1CarQAM(8);
Decode1CarQAM(9);
frameLen += CorrectRawDataWithRS(&bytFrameData[4][0], &bytData[frameLen], intDataLen, intRSLen, intFrameType, 4);
frameLen += CorrectRawDataWithRS(&bytFrameData[5][0], &bytData[frameLen], intDataLen, intRSLen, intFrameType, 5);
frameLen += CorrectRawDataWithRS(&bytFrameData[6][0], &bytData[frameLen], intDataLen, intRSLen, intFrameType, 6);
frameLen += CorrectRawDataWithRS(&bytFrameData[7][0], &bytData[frameLen], intDataLen, intRSLen, intFrameType, 7);
frameLen += CorrectRawDataWithRS(&bytFrameData[8][0], &bytData[frameLen], intDataLen, intRSLen, intFrameType, 8);
frameLen += CorrectRawDataWithRS(&bytFrameData[9][0], &bytData[frameLen], intDataLen, intRSLen, intFrameType, 9);
}
// Check Data
if (memcmp(CarrierOk, Good, intNumCar) == 0)
return TRUE;
// Bad decode if we have Memory ARQ try it
#ifdef MEMORYARQ
for (i = 0; i < intNumCar; i++)
{
if (!CarrierOk[i] && intFrameType != PktFrameHeader)
{
// Decode error - save data for MEM ARQ
SaveQAMSamples(i);
if (intSumCounts[0] > 1)
{
MemARQOk = 1;
Decode1CarQAM(i); // try to decode based on the WeightedAveragePhases
}
}
}
if (MemARQOk == 0) // Havent averaged yet
return TRUE;
// We've tried to correct - see if it worked
Debugprintf("DemodQAM Trying MEM ARQ");
// Non -robust
frameLen = 0;
for (i = 0; i < intNumCar; i++)
{
frameLen += CorrectRawDataWithRS(&bytFrameData[i][0], &bytData[frameLen], intDataLen, intRSLen, intFrameType, i);
}
// Check Data
if (memcmp(CarrierOk, Good, intNumCar) == 0)
{
Debugprintf("DemodQAM MEM ARQ Corrected frame");
intGoodQAMSummationDecodes++;
}
#endif
}
}
return TRUE;
}
int Demod1CarQAMChar(int Start, int Carrier)
{
// Converts intSample to an array of differential phase and magnitude values for the Specific Carrier Freq
// intPtr should be pointing to the approximate start of the first reference/training symbol (1 of 3)
// intPhase() is an array of phase values (in milliradians range of 0 to 6283) for each symbol
// intMag() is an array of Magnitude values (not used in PSK decoding but for constellation plotting or QAM decoding)
// Objective is to use Minimum Phase Error Tracking to maintain optimum pointer position
// This is called for one DMA buffer of samples (normally 1200)
float dblReal, dblImag;
int intMiliRadPerSample = floatCarFreq * M_PI / 6;
int i;
int intNumOfSymbols = 2;
int origStart = Start;;
if (CarrierOk[Carrier]) // Already decoded this carrier?
{
intPhasesLen += intNumOfSymbols;
return intSampPerSym * intNumOfSymbols;
}
for (i = 0; i < intNumOfSymbols; i++)
{
// GoertzelRealImag(intFilteredMixedSamples, Start + intCP[Carrier], intNforGoertzel[Carrier], dblFreqBin[Carrier], &dblReal, &dblImag);
GoertzelRealImagHanning(intFilteredMixedSamples, Start + intCP[Carrier], intNforGoertzel[Carrier], dblFreqBin[Carrier], &dblReal, &dblImag);
intMags[Carrier][intPhasesLen] = sqrtf(powf(dblReal, 2) + powf(dblImag, 2));
intPSKPhase_0[Carrier] = 1000 * atan2f(dblImag, dblReal);
intPhases[Carrier][intPhasesLen] = -(ComputeAng1_Ang2(intPSKPhase_0[Carrier], intPSKPhase_1[Carrier]));
/*
if (Carrier == 0)
{
Corrections = Track1CarPSK(floatCarFreq, strMod, atan2f(dblImag, dblReal), FALSE);
if (Corrections != 0)
{
Start += Corrections;
// GoertzelRealImag(intFilteredMixedSamples, Start + intCP[Carrier], intNforGoertzel[Carrier], dblFreqBin[Carrier], &dblReal, &dblImag);
GoertzelRealImagHanning(intFilteredMixedSamples, Start + intCP[Carrier], intNforGoertzel[Carrier], dblFreqBin[Carrier], &dblReal, &dblImag);
intPSKPhase_0[Carrier] = 1000 * atan2f(dblImag, dblReal);
}
}
*/
intPSKPhase_1[Carrier] = intPSKPhase_0[Carrier];
intPhasesLen++;
Start += intSampPerSym;
}
if (AccumulateStats)
intQAMSymbolCnt += intNumOfSymbols;
return (Start - origStart); // Symbols we've consumed
}
extern int bytQDataInProcessLen;
// function to decode one carrier from tones (used to decode from Averaged intToneMags)
BOOL Decode1Car4FSKFromTones(UCHAR * bytData, int intToneMags)
{
// Decodes intToneMags() to an array of bytes
// Updates bytData() with decoded
/*
UCHAR bytSym;
int intIndex;
ReDim bytData(intToneMags.Length \ 16 - 1)
For i As Integer = 0 To bytData.Length - 1 ' For each data byte
intIndex = 16 * i
For j As Integer = 0 To 3 ' for each 4FSK symbol (2 bits) in a byte
If intToneMags(intIndex) > intToneMags(intIndex + 1) And intToneMags(intIndex) > intToneMags(intIndex + 2) And intToneMags(intIndex) > intToneMags(intIndex + 3) Then
bytSym = 0
ElseIf intToneMags(intIndex + 1) > intToneMags(intIndex) And intToneMags(intIndex + 1) > intToneMags(intIndex + 2) And intToneMags(intIndex + 1) > intToneMags(intIndex + 3) Then
bytSym = 1
ElseIf intToneMags(intIndex + 2) > intToneMags(intIndex) And intToneMags(intIndex + 2) > intToneMags(intIndex + 1) And intToneMags(intIndex + 2) > intToneMags(intIndex + 3) Then
bytSym = 2
Else
bytSym = 3
End If
bytData(i) = (bytData(i) << 2) + bytSym
intIndex += 4
Next j
Next i
Return True
End Function ' Decode1Car4FSKFromTones
*/
return TRUE;
}
/* ' Function to decode one carrier from tones (used to decode from Averaged intToneMags)
Private Function Decode1Car8FSKFromTones(ByRef bytData() As Byte, ByRef intToneMags() As Int32) As Boolean
' Decodes intToneMags() to an array of bytes
' Updates bytData() with decoded
Dim bytSym As Byte
Dim intThreeBytes As Int32
ReDim bytData(3 * intToneMags.Length \ 64 - 1)
Dim intMaxMag As Int32
For i As Integer = 0 To (bytData.Length \ 3) - 1 ' For each group of 3 bytes data byte
intThreeBytes = 0
For j As Integer = 0 To 7 ' for each group of 8 symbols (24 bits)
intMaxMag = 0
For k As Integer = 0 To 7 ' for each of 8 possible tones per symbol
If intToneMags((i * 64) + 8 * j + k) > intMaxMag Then
intMaxMag = intToneMags((i * 64) + 8 * j + k)
bytSym = k
End If
Next k
intThreeBytes = (intThreeBytes << 3) + bytSym
Next j
bytData(3 * i) = (intThreeBytes And &HFF0000) >> 16
bytData(3 * i + 1) = (intThreeBytes And &HFF00) >> 8
bytData(3 * i + 2) = (intThreeBytes And &HFF)
Next i
Return True
End Function ' Decode1Car8FSKFromTones
' Function to decode one carrier from tones (used to decode from Averaged intToneMags)
Private Function Decode1Car16FSKFromTones(ByRef bytData() As Byte, ByRef intToneMags() As Int32) As Boolean
' Decodes intToneMags() to an array of bytes
' Updates bytData() with decoded tones
Dim bytSym As Byte
Dim intMaxMag As Int32
ReDim bytData(intToneMags.Length \ 32 - 1)
For i As Integer = 0 To bytData.Length - 1 ' For each data byte
For j As Integer = 0 To 1 ' for each 16FSK symbol (4 bits) in a byte
intMaxMag = 0
For k As Integer = 0 To 15
If intToneMags(i * 32 + 16 * j + k) > intMaxMag Then
intMaxMag = intToneMags(i * 32 + 16 * j + k)
bytSym = k
End If
Next k
bytData(i) = (bytData(i) << 4) + bytSym
Next j
Next i
Return True
End Function ' Decode1Car16FSKFromTones
*/
// Subroutine to update the Busy detector when not displaying Spectrum or Waterfall (graphics disabled)
extern int LastBusyCheck;
extern BOOL blnBusyStatus;
int intWaterfallRow = 0;
void UpdateBusyDetector(short * bytNewSamples)
{
float dblReF[1024];
float dblImF[1024];
float dblMag[206];
float dblMagAvg = 0;
int intTuneLineLow, intTuneLineHi, intDelta;
int i;
int BusyFlag;
if (ProtocolState > DISC) // ' Only process busy when in DISC state
return;
if (State != SearchingForLeader)
return; // only when looking for leader
if (Now - LastBusyCheck < 100)
return;
LastBusyCheck = Now;
FourierTransform(1024, bytNewSamples, &dblReF[0], &dblImF[0], FALSE);
for (i = 0; i < 206; i++)
{
// starting at ~300 Hz to ~2700 Hz Which puts the center of the signal in the center of the window (~1500Hz)
dblMag[i] = powf(dblReF[i + 25], 2) + powf(dblImF[i + 25], 2); // first pass
dblMagAvg += dblMag[i];
}
// Not sure about this as we use variable bandwidth frames. For now use 500
intDelta = (500 / 2 + TuningRange) / 11.719f;
intTuneLineLow = max((103 - intDelta), 3);
intTuneLineHi = min((103 + intDelta), 203);
// At the moment we only get here what seaching for leader,
// but if we want to plot spectrum we should call
// it always
BusyFlag = BusyDetect3(dblMag, intTuneLineLow, intTuneLineHi);
if (BusyFlag == 0)
{
if (BusyCount == 0)
blnBusyStatus = 0;
else
BusyCount--;
}
else
{
blnBusyStatus = 1;
BusyCount = 10; // Try delaying busy off a bit
}
if (blnBusyStatus && !blnLastBusyStatus)
{
Debugprintf("BUSY TRUE");
}
// stcStatus.Text = "True"
// queTNCStatus.Enqueue(stcStatus)
// 'Debug.WriteLine("BUSY TRUE @ " & Format(DateTime.UtcNow, "HH:mm:ss"))
else if (blnLastBusyStatus && !blnBusyStatus)
{
Debugprintf("BUSY FALSE");
}
blnLastBusyStatus = blnBusyStatus;
}