//
// Derived from fldigi rdid.cxx by John G8BPQ September 22
//
// ----------------------------------------------------------------------------
//
// rsid.cxx
//
// Copyright (C) 2008-2012
// Dave Freese, W1HKJ
// Copyright (C) 2009-2012
// Stelios Bounanos, M0GLD
// Copyright (C) 2012
// John Douyere, VK2ETA
//
// This file is part of fldigi.
//
// Fldigi is free software: you can redistribute it and/or modify
// it under the terms of the GNU General Public License as published by
// the Free Software Foundation, either version 3 of the License, or
// (at your option) any later version.
//
// Fldigi is distributed in the hope that it will be useful,
// but WITHOUT ANY WARRANTY; without even the implied warranty of
// MERCHANTABILITY or FITNESS for (A PARTICULAR PURPOSE. See the
// GNU General Public License for more details.
//
// You should have received a copy of the GNU General Public License
// along with fldigi. If not, see <http://www.gnu.org/licenses/>.
// ----------------------------------------------------------------------------
#include <malloc.h>
#include "rsid.h"
#include <math.h>
#include "fftw3.h"
#include "UZ7HOStuff.h"
#define M_PI 3.1415926f
#define true 1
#define false 0
#define TRUE 1
#define FALSE 0
#define WORD unsigned int
#define BYTE unsigned char
#define byte unsigned char
void SampleSink(int LR, short Sample);
void Flush();
void extSetOffset(int rxOffset);
void mon_rsid(int snd_ch, char * RSID);
extern int RSID_SABM[4];
extern int RSID_UI[4];
extern int RSID_SetModem[4];
struct RSIDs {
unsigned short rs;
trx_mode mode;
const char* name;
};
extern int SampleNo;
extern int Number;
int len;
int symlen;
#include "rsid_defs.cxx"
#define RSWINDOW 1
const int Squares[] = {
0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0,
0, 1, 2, 3, 4, 5, 6, 7, 8, 9,10,11,12,13,14,15,
0, 2, 4, 6, 8,10,12,14, 9,11,13,15, 1, 3, 5, 7,
0, 3, 6, 5,12,15,10, 9, 1, 2, 7, 4,13,14,11, 8,
0, 4, 8,12, 9,13, 1, 5,11,15, 3, 7, 2, 6,10,14,
0, 5,10,15,13, 8, 7, 2, 3, 6, 9,12,14,11, 4, 1,
0, 6,12,10, 1, 7,13,11, 2, 4,14, 8, 3, 5,15, 9,
0, 7,14, 9, 5, 2,11,12,10,13, 4, 3,15, 8, 1, 6,
0, 8, 9, 1,11, 3, 2,10,15, 7, 6,14, 4,12,13, 5,
0, 9,11, 2,15, 6, 4,13, 7,14,12, 5, 8, 1, 3,10,
0,10,13, 7, 3, 9,14, 4, 6,12,11, 1, 5,15, 8, 2,
0,11,15, 4, 7,12, 8, 3,14, 5, 1,10, 9, 2, 6,13,
0,12, 1,13, 2,14, 3,15, 4, 8, 5, 9, 6,10, 7,11,
0,13, 3,14, 6,11, 5, 8,12, 1,15, 2,10, 7, 9, 4,
0,14, 5,11,10, 4,15, 1,13, 3, 8, 6, 7, 9, 2,12,
0,15, 7, 8,14, 1, 9, 6, 5,10, 2,13,11, 4,12, 3
};
const int indices[] = {
2, 4, 8, 9, 11, 15, 7, 14, 5, 10, 13, 3
};
int rmode, rmode2;
void Encode(int code, unsigned char *rsid)
{
rsid[0] = code >> 8;
rsid[1] = (code >> 4) & 0x0f;
rsid[2] = code & 0x0f;
for (int i = 3; i < RSID_NSYMBOLS; i++)
rsid[i] = 0;
for (int i = 0; i < 12; i++) {
for (int j = RSID_NSYMBOLS - 1; j > 0; j--)
rsid[j] = rsid[j - 1] ^ Squares[(rsid[j] << 4) + indices[i]];
rsid[0] = Squares[(rsid[0] << 4) + indices[i]];
}
}
//=============================================================================
// transmit rsid code for current mode
//=============================================================================
float sampd;
short samps;
// Each symbol is transmitted using MFSK modulation.There are 16 possibilities of frequencies separated by
// 11025 / 1024 = 10.766 Hz. Each symbol transmission being done on only one frequency for a duration equal
// to 1024 / 11025 x 1000 = 92.88 ms.The entire RSID sequence of 15 symbols is transmitted in 15 x 1024 / 11025 = 1393 ms.
// The analysis is based on a Fast Fourier transform of 2048 points at 11025 samples / sec, regularly done at each
// semi - step of time(46.44 ms).
// For each semi - step of time(46.44 ms) and for each semi - step of frequency(5.38 Hz),the program attempts to detect
// an RSID extending for the last 1.393 seconds.So each second, about 8500 possible RSID
// (depending on the selected bandwidth) are tested
// But we are working at 12000 samples/sec so 92.88 mS = 1114.56 samples, so I think we run fft every 557.28 samples (46.44 ms)
// How do we get 5.28 Hz buckets at 12000? Can we run fft of length 2,272.727 (say 2273) length?
// Actually not sure we need to. We can interpolate freq and so long as we can get within a few Hz should be ok
// Spec says tone spacing ia 10.766 (11025 / 1024)
double toneinterval = RSID_SAMPLE_RATE / 1024;
// fftw library interface
static fftwf_complex * in = 0, *out;
static fftwf_plan p;
#define N 4096
short savedSamples[N + 1000]; // At least N + max input length (currently uses 512);
int savedSampLen = 0;
int firstBin = (300 * 2048) / 12000; // Search Lowest (300 Hz)
int lastBin = (3000 * 2048) / 12000;; // Seach Highest (3000 Hz)
double avmag; // Average magnitude over spectrum
int fft_buckets[RSID_NTIMES][RSID_FFT_SIZE]; // This seems to have last 30 sets of values
float aFFTAmpl[RSID_FFT_SIZE]; // Amplitude samples from fft
// Table of precalculated Reed Solomon symbols
unsigned char pCodes1[256][16];
unsigned char pCodes2[256][16];
int found1;
int found2;
double rsid_secondary_time_out;
int bPrevTimeSliceValid;
int iPrevDistance;
int iPrevBin;
int iPrevSymbol;
int fft_buckets[RSID_NTIMES][RSID_FFT_SIZE];
int bPrevTimeSliceValid2;
int iPrevDistance2;
int iPrevBin2;
int iPrevSymbol2;
int hamming_resolution = 2;
int needRSID[4] = { 0,0,0,0 }; // Request TX scheduler to send RSID
int needSetOffset[4] = { 0,0,0,0 };
void CalculateBuckets(const float *pSpectrum, int iBegin, int iEnd);
void Search();
void RSIDinitfft()
{
unsigned char * c;
in = (fftwf_complex*)fftwf_malloc(sizeof(fftwf_complex) * N);
out = (fftwf_complex*)fftwf_malloc(sizeof(fftwf_complex) * N);
p = fftwf_plan_dft_1d(N, in, out, FFTW_FORWARD, FFTW_MEASURE);
// Initialization of assigned mode/submode IDs.
for (int i = 0; i < rsid_ids_size1; i++)
{
c = &pCodes1[i][0];
Encode(rsid_ids_1[i].rs, c);
}
}
void reset()
{
iPrevDistance = iPrevDistance2 = 99;
bPrevTimeSliceValid = bPrevTimeSliceValid2 = false;
found1 = found2 = false;
rsid_secondary_time_out = 0;
}
// Compute fft of last 557 * 2 values and return bucket number of peak
static int dofft(short * inp, float * mags)
{
int i;
float mag;
float max = 0;
int maxindex = 0;
memset(in, 0, sizeof(fftwf_complex) * N);
avmag = 0;
for (i = 0; i < 512 * 2; i++)
{
// in[i][0] = inp[i] * 1.0f;
// Hamming Window
in[i][0] = inp[i];
in[i][0] = (float)((0.53836f - (0.46164f * cos(2 * M_PI * i / (float)(557.0 * 2.0 - 1)))) * inp[i]);
in[i][1] = 0;
}
fftwf_execute(p);
for (i = firstBin; i < lastBin; i++) // only need buckets up to 3000
{
// Convert Real/Imag to amplitude
mag = powf(out[i][0], 2);
mag += powf(out[i][1], 2);
mag = sqrtf(mag);
mags[i] = mag;
avmag += mag;
if (mag > max)
{
max = mag;
maxindex = i;
}
}
avmag /= (lastBin - firstBin);
return maxindex;
}
void RSIDProcessSamples(short * Samples, int nSamples)
{
// Add to saved samples, and if we have more than 557 run FFT and save remaining
// We process last 557 + new 557 + zero padding
// Trying with 512 @ 12000
// savedSampLen is number of shorts not bytes
if (in == 0) // Not initialised
return;
memcpy(&savedSamples[savedSampLen], Samples, nSamples * sizeof(short));
savedSampLen += nSamples;
if (savedSampLen >= 512 * 2) // Old + New
{
int peakBucket;
peakBucket = dofft(savedSamples, aFFTAmpl);
if (peakBucket > firstBin && peakBucket < lastBin)
{
// float freq;
// freq = peakBucket * 12000.0f / 2048;
// Debugprintf("%d %f %f %f", peakBucket, freq, aFFTAmpl[peakBucket], avmag);
}
savedSampLen -= 512;
memmove(savedSamples, &savedSamples[512 * sizeof(short)], savedSampLen * sizeof(short));
Search();
}
}
int HammingDistance(int iBucket, unsigned char *p2)
{
int dist = 0;
for (int i = 0, j = 0; i < RSID_NSYMBOLS; i++, j += 2)
{
if (fft_buckets[j][iBucket] != p2[i])
{
++dist;
if (dist > hamming_resolution)
break;
}
}
return dist;
}
int iDistanceMin = 1000; // infinity
int search_amp(int *bin_out, int *symbol_out)
{
int i, j;
int iDistance = 1000;
int iBin = -1;
int iSymbol = -1;
int tblsize;
iDistanceMin = 1000; // infinity
tblsize = rsid_ids_size1;
unsigned char *pc = 0;
for (i = 0; i < tblsize; i++)
{
pc = &pCodes1[i][0];
for (j = firstBin; j < lastBin - RSID_NTIMES; j++)
{
iDistance = HammingDistance(j, pc);
if (iDistance < iDistanceMin)
{
iDistanceMin = iDistance;
iSymbol = i;
iBin = j;
if (iDistanceMin == 0) break;
}
}
}
if (iDistanceMin <= hamming_resolution)
{
*symbol_out = iSymbol;
*bin_out = iBin;
return true;
}
return false;
}
void apply(int iBin, int iSymbol)
{
// Does something with the found id
const struct RSIDs *prsid = &rsid_ids_1[iSymbol];
int Freq = (int)(iBin + 15) * 12000.0f / 2048;
char Msg[128];
int Offset = Freq - rx_freq[0];
int i;
int nearest = -1, minOffset = 9999, absOffset;
// If there is more than one decoder with update from rsid set update the nearest
for (i = 0; i < 4; i++)
{
if (RSID_SetModem[i])
{
absOffset = abs(Freq - rx_freq[i]);
if (absOffset < minOffset)
{
// Nearer
nearest = i;
minOffset = absOffset;
}
}
}
// We don't run this unless at least one modem has RSID_SetModem set.
Offset = Freq - rx_freq[nearest];
sprintf(Msg, "RSID %s %d %d Nearest Modem %c Offset %d", prsid->name, iDistanceMin, Freq, nearest + 'A', Offset);
mon_rsid(0, Msg);
// Set Modem RX Offset to match received freq
chanOffset[nearest] = Offset;
needSetOffset[nearest] = 1; // Update GUI
}
void Search()
{
int symbol = -1;
int bin = -1;
// We have just calculated a new set of fft amplitude bins in aFFTAmpl
// we find peak bin, and store in fft_buckets array. This has 30 sets of 1024 buckets (though we only use first 512, as we limit search to 3 KHz)
// We move previous 29 entries to start of array and add new values on end so samples corresponding to first bit of rsid msg are at start
memmove(fft_buckets, &(fft_buckets[1][0]), (RSID_NTIMES - 1) * RSID_FFT_SIZE * sizeof(int));
memset(&(fft_buckets[RSID_NTIMES - 1][0]), 0, RSID_FFT_SIZE * sizeof(int));
// We process even then odd bins, using alternate bins to get resolution to 1/2 bin
CalculateBuckets(aFFTAmpl, firstBin, lastBin - RSID_NTIMES);
CalculateBuckets(aFFTAmpl, firstBin + 1, lastBin - RSID_NTIMES);
// Now have 30 sets of 512 bit valies (0-15). We look for a set of 15 that match an ID
found1 = search_amp(&bin, &symbol);
if (found1)
{
apply(bin, symbol);
reset();
}
}
void CalculateBuckets(const float *pSpectrum, int iBegin, int iEnd)
{
float Amp = 0.0f, AmpMax = 0.0f;
int iBucketMax = iBegin - 2;
int j;
// This searches odd and even pairs of amps, hence the += 2
for (int i = iBegin; i < iEnd; i += 2)
{
if (iBucketMax == i - 2)
{
// if max was first in grooup of 15 redo full search
AmpMax = pSpectrum[i];
iBucketMax = i;
for (j = i + 2; j < i + RSID_NTIMES + 2; j += 2)
{
Amp = pSpectrum[j];
if (Amp > AmpMax)
{
AmpMax = Amp;
iBucketMax = j;
}
}
}
else
{
// Max wasn't first, so must be in next 14, so we can just check if new last is > max
j = i + RSID_NTIMES;
Amp = pSpectrum[j];
if (Amp > AmpMax)
{
AmpMax = Amp;
iBucketMax = j;
}
}
fft_buckets[RSID_NTIMES - 1][i] = (iBucketMax - i) >> 1;
}
}
void sendRSID(int Chan, int dropTX)
{
unsigned char rsid[RSID_NSYMBOLS];
float sr;
int iTone;
float freq, phaseincr;
float fr;
float phase;
int Mode = speed[Chan];
int Freq = rx_freq[Chan];
rmode2 = 687;
rmode = 35 + Mode; // Packet 300 or 1200
Encode(rmode, rsid);
sr = 12000;
symlen = (size_t)floor(RSID_SYMLEN * sr);
SampleNo = 0;
SoundIsPlaying = TRUE;
RadioPTT(Chan, 1);
Number = 0;
// transmit 5 symbol periods of silence at beginning of rsid
for (int i = 0; i < 5 * symlen; i++)
SampleSink(0, 0);
// transmit sequence of 15 symbols (tones)
fr = 1.0f * Freq - (RSID_SAMPLE_RATE * 7 / 1024);
phase = 0.0f;
for (int i = 0; i < 15; i++)
{
iTone = rsid[i];
freq = fr + iTone * RSID_SAMPLE_RATE / 1024;
phaseincr = 2.0f * M_PI * freq / sr;
for (int j = 0; j < symlen; j++)
{
phase += phaseincr;
if (phase > 2.0 * M_PI) phase -= 2.0 * M_PI;
sampd = sinf(phase);
sampd = sampd * 32767.0f;
samps = (short)sampd;
SampleSink(0, samps);
}
}
// 5 symbol periods of silence at end of transmission
// and between RsID and the data signal
int nperiods = 5;
for (int i = 0; i < nperiods * symlen; i++)
SampleSink(modemtoSoundLR[Chan], 0);
tx_status[Chan] = TX_SILENCE; // Stop TX
Flush();
if (dropTX)
RadioPTT(Chan, 0);
}
// Experimental Busy Detect, based on ARDOP code
static int LastBusyCheck = 0;
static int BusyCount = 0;
static int BusyStatus = 0;
static int LastBusyStatus = 0;
static int BusyDet = 5; // DCD Threshold
int LastStart = 0;
int LastStop = 0;
extern int LastBusyOn;
extern int LastBusyOff;
static int LastBusy = FALSE;
extern float dblAvgStoNSlowNarrow;
extern float dblAvgStoNFastNarrow;
extern float dblAvgStoNSlowWide;
extern float dblAvgStoNFastWide;
int BusyOnCnt = 0; // used to filter Busy ON detections
int BusyOffCnt = 0; // used to filter Busy OFF detections
unsigned int LastBusyTrip = 0;
unsigned int PriorLastBusyTrip = 0;
unsigned int LastBusyClear = 0;
unsigned int LastTrip;
void SortSignals(float * dblMag, int intStartBin, int intStopBin, int intNumBins, float * dblAVGSignalPerBin, float * dblAVGBaselinePerBin);
void SortSignals2(float * dblMag, int intStartBin, int intStopBin, int intNumBins, float * dblAVGSignalPerBin, float * dblAVGBaselinePerBin);
static BOOL BusyDetect3(float * dblMag, int intStart, int intStop) // this only called while searching for leader ...once leader detected, no longer called.
{
// each bin is about 12000/2048 or 5.86 Hz
// First sort signals and look at highes signals:baseline ratio..
float dblAVGSignalPerBinNarrow, dblAVGSignalPerBinWide, dblAVGBaselineNarrow, dblAVGBaselineWide;
float dblSlowAlpha = 0.2f;
float dblAvgStoNNarrow = 0, dblAvgStoNWide = 0;
int intNarrow = 16; // 16 x 5.86 about 94 z
int intWide = ((intStop - intStart) * 2) / 3; //* 0.66);
int blnBusy = FALSE;
int BusyDet4th = BusyDet * BusyDet * BusyDet * BusyDet;
int BusyDet = 5;
unsigned int HoldMs = 5000;
// First sort signals and look at highest signals:baseline ratio..
// First narrow band (~94Hz)
SortSignals2(dblMag, intStart, intStop, intNarrow, &dblAVGSignalPerBinNarrow, &dblAVGBaselineNarrow);
if (LastStart == intStart && LastStop == intStop)
dblAvgStoNNarrow = (1 - dblSlowAlpha) * dblAvgStoNNarrow + dblSlowAlpha * dblAVGSignalPerBinNarrow / dblAVGBaselineNarrow;
else
{
// This initializes the Narrow average after a bandwidth change
dblAvgStoNNarrow = dblAVGSignalPerBinNarrow / dblAVGBaselineNarrow;
LastStart = intStart;
LastStop = intStop;
}
// Wide band (66% of current bandwidth)
SortSignals2(dblMag, intStart, intStop, intWide, &dblAVGSignalPerBinWide, &dblAVGBaselineWide);
if (LastStart == intStart && LastStop == intStop)
dblAvgStoNWide = (1 - dblSlowAlpha) * dblAvgStoNWide + dblSlowAlpha * dblAVGSignalPerBinWide / dblAVGBaselineWide;
else
{
// This initializes the Wide average after a bandwidth change
dblAvgStoNWide = dblAVGSignalPerBinWide / dblAVGBaselineWide;
LastStart = intStart;
LastStop = intStop;
}
// Preliminary calibration...future a function of bandwidth and BusyDet.
blnBusy = (dblAvgStoNNarrow > (3 + 0.008 * BusyDet4th)) || (dblAvgStoNWide > (5 + 0.016 * BusyDet4th));
if (BusyDet == 0)
blnBusy = FALSE; // 0 Disables check ?? Is this the best place to do this?
// WriteDebugLog(LOGDEBUG, "Busy %d Wide %f Narrow %f", blnBusy, dblAvgStoNWide, dblAvgStoNNarrow);
if (blnBusy)
{
// This requires multiple adjacent busy conditions to skip over one nuisance Busy trips.
// Busy must be present at least 3 consecutive times ( ~250 ms) to be reported
BusyOnCnt += 1;
BusyOffCnt = 0;
if (BusyOnCnt > 3)
LastTrip = Now;
}
else
{
BusyOffCnt += 1;
BusyOnCnt = 0;
}
if (LastBusy == 0 && BusyOnCnt >= 3)
{
PriorLastBusyTrip = LastBusyTrip; // save old dttLastBusyTrip for use in BUSYBLOCKING function
LastBusyTrip = Now;
LastBusy = TRUE;
}
else
{
if (LastBusy && (Now - LastTrip) > HoldMs && BusyOffCnt >= 3)
{
LastBusyClear = Now;
LastBusy = FALSE;
}
}
return LastBusy;
}
static void UpdateBusyDetector()
{
// Use applitude bins in aFFTAmpl
float dblMagAvg = 0;
int intTuneLineLow, intTuneLineHi;
int i;
int BusyFlag;
if (Now - LastBusyCheck < 100)
return;
LastBusyCheck = Now;
for (i = 52; i < 512; i++)
{
// starting at ~300 Hz to ~3000 Hz Which puts the center of the signal in the center of the window (~1500Hz)
dblMagAvg += aFFTAmpl[i];
}
intTuneLineLow = 52;
intTuneLineHi = 512;
BusyFlag = BusyDetect3(aFFTAmpl, intTuneLineLow, intTuneLineHi);
if (BusyFlag == 0)
{
if (BusyCount == 0)
BusyStatus = 0;
else
BusyCount--;
}
else
{
BusyStatus = 1;
BusyCount = 10; // Try delaying busy off a bit
}
if (BusyStatus && !LastBusyStatus)
{
Debugprintf("BUSY TRUE");
}
// stcStatus.Text = "True"
// queTNCStatus.Enqueue(stcStatus)
// 'Debug.WriteLine("BUSY TRUE @ " & Format(DateTime.UtcNow, "HH:mm:ss"))
else if (LastBusyStatus && !BusyStatus)
{
Debugprintf("BUSY FALSE");
}
LastBusyStatus = BusyStatus;
}