diff --git a/FMControl.cpp b/FMControl.cpp index 55477cd..13c4132 100644 --- a/FMControl.cpp +++ b/FMControl.cpp @@ -24,20 +24,40 @@ #include #endif -const float EMPHASIS_GAIN_DB = 0.0F; //Gain needs to be the same for pre an deeemphasis -const unsigned int FM_MASK = 0x00000FFFU; +#define SWAP_BYTES_16(a) (((a >> 8) & 0x00FFU) | ((a << 8) & 0xFF00U)) + +const float DEEMPHASIS_GAIN_DB = 0.0F; +const float PREEMPHASIS_GAIN_DB = 13.0F; +const float FILTER_GAIN_DB = 0.0F; +const unsigned int FM_MASK = 0x00000FFFU; CFMControl::CFMControl(CFMNetwork* network) : m_network(network), m_enabled(false), m_incomingRFAudio(1600U, "Incoming RF FM Audio"), -m_preemphasis(0.3889703155F, -0.32900055326F, 0.0F, 1.0F, 0.2820291817F, 0.0F, EMPHASIS_GAIN_DB), -m_deemphasis(1.0F, 0.2820291817F, 0.0F, 0.3889703155F, -0.32900055326F, 0.0F, EMPHASIS_GAIN_DB) +m_preemphasis (NULL), +m_deemphasis (NULL), +m_filterStage1(NULL), +m_filterStage2(NULL), +m_filterStage3(NULL) { + m_preemphasis = new CIIRDirectForm1Filter(8.315375384336983F,-7.03334621603483F,0.0F,1.0F,0.282029168302153F,0.0F, PREEMPHASIS_GAIN_DB); + m_deemphasis = new CIIRDirectForm1Filter(0.07708787090460224F,0.07708787090460224F,0.0F,1.0F,-0.8458242581907955F,0.0F, DEEMPHASIS_GAIN_DB); + + //cheby type 1 0.2dB cheby type 1 3rd order 300-2700Hz fs=8000 + m_filterStage1 = new CIIRDirectForm1Filter(0.29495028f, 0.0f, -0.29495028f, 1.0f, -0.61384624f, -0.057158668f, FILTER_GAIN_DB); + m_filterStage2 = new CIIRDirectForm1Filter(1.0f, 2.0f, 1.0f, 1.0f, 0.9946123f, 0.6050482f, FILTER_GAIN_DB); + m_filterStage3 = new CIIRDirectForm1Filter(1.0f, -2.0f, 1.0f, 1.0f, -1.8414584f, 0.8804949f, FILTER_GAIN_DB); } CFMControl::~CFMControl() { + delete m_preemphasis ; + delete m_deemphasis ; + + delete m_filterStage1; + delete m_filterStage2; + delete m_filterStage3; } bool CFMControl::writeModem(const unsigned char* data, unsigned int length) @@ -63,62 +83,47 @@ bool CFMControl::writeModem(const unsigned char* data, unsigned int length) bufferLength = 255U; if (bufferLength >= 3U) { -#if defined(DUMP_RF_AUDIO) - FILE* audiofile = ::fopen("./audiodump.bin", "ab"); -#endif bufferLength = bufferLength - bufferLength % 3U; //round down to nearest multiple of 3 unsigned char bufferData[255U]; m_incomingRFAudio.getData(bufferData, bufferLength); - - unsigned int nSamples = 0; - float samples[85U]; // 255 / 3; - // Unpack the serial data into float values. + unsigned int pack = 0U; + unsigned char* packPointer = (unsigned char*)&pack; + unsigned short out[168U]; // 84 * 2 + unsigned int nOut = 0U; + short unpackedSamples[2U]; + for (unsigned int i = 0U; i < bufferLength; i += 3U) { - short sample1 = 0U; - short sample2 = 0U; - - unsigned int pack = 0U; - unsigned char* packPointer = (unsigned char*)&pack; - + //extract unsigned 12 bit unsigned sample pairs packed into 3 bytes to 16 bit signed packPointer[0U] = bufferData[i]; packPointer[1U] = bufferData[i + 1U]; packPointer[2U] = bufferData[i + 2U]; + unpackedSamples[1U] = short(int(pack & FM_MASK) - 2048); + unpackedSamples[0U] = short(int(pack >> 12) - 2048); - //extract unsigned 12 bit samples to 16 bit signed - sample2 = short(int(pack & FM_MASK) - 2048); - sample1 = short(int(pack >> 12) - 2048); + //process unpacked sample pair + for(unsigned char j = 0U; j < 2U; j++) { + //Convert to float (-1.0 to +1.0) + float sampleFloat = float(unpackedSamples[j]) / 2048.0F; - // Convert from unsigned short (0 - +4095) to float (-1.0 - +1.0) - samples[nSamples++] = float(sample1) / 2048.0F; - samples[nSamples++] = float(sample2) / 2048.0F; - } + //De-emphasise and remove CTCSS + sampleFloat = m_deemphasis->filter(sampleFloat); + sampleFloat = m_filterStage3->filter(m_filterStage2->filter(m_filterStage1->filter(sampleFloat))); - //De-emphasise the data and any other processing needed (maybe a low-pass filter to remove the CTCSS) - for (unsigned int i = 0U; i < nSamples; i++) - samples[i] = m_deemphasis.filter(samples[i]); - -#if defined(DUMP_RF_AUDIO) - if (audiofile != NULL) - ::fwrite(samples, sizeof(float), nSamples, audiofile); -#endif - unsigned short out[170U]; // 85 * 2 - unsigned int nOut = 0U; - - // Repack the data (8-bit unsigned values containing unsigned 16-bit data) - for (unsigned int i = 0U; i < nSamples; i++) { - unsigned short sample = (unsigned short)((samples[i] + 1.0F) * 32767.0F + 0.5F); - out[nOut++] = (sample >> 8) & 0xFFU; - out[nOut++] = (sample >> 0) & 0xFFU; + // Repack the float data to 16 bit unsigned + unsigned short sampleUShort = (unsigned short)((sampleFloat + 1.0F) * 32767.0F + 0.5F); + out[nOut++] = SWAP_BYTES_16(sampleUShort); + } } #if defined(DUMP_RF_AUDIO) - if (audiofile != NULL) { - ::fclose(audiofile); - audiofile = NULL; + FILE * audiofile = fopen("./audiodump.bin", "ab"); + if(audiofile != NULL) { + fwrite(out, sizeof(unsigned short), nOut, audiofile); + fclose(audiofile); } #endif - return m_network->writeData((unsigned char*)out, nOut); + return m_network->writeData((unsigned char*)out, nOut * sizeof(unsigned short)); } return true; @@ -135,42 +140,41 @@ unsigned int CFMControl::readModem(unsigned char* data, unsigned int space) if (space > 252U) space = 252U; - unsigned char netData[168U];//84 * 2 modem can handle up to 84 samples (252 bytes) at a time - unsigned int length = m_network->read(netData, 168U); + unsigned short netData[84U];//modem can handle up to 84 samples (252 bytes) at a time + unsigned int length = m_network->read((unsigned char*)netData, 84U * sizeof(unsigned short)); + length /= sizeof(unsigned short); if (length == 0U) return 0U; - float samples[84U]; - unsigned int nSamples = 0U; - // Convert the unsigned 16-bit data (+65535 - 0) to float (+1.0 - -1.0) - for (unsigned int i = 0U; i < length; i += 2U) { - unsigned short sample = (netData[i + 0U] << 8) | netData[i + 1U]; - samples[nSamples++] = (float(sample) / 32767.0F) - 1.0F; - } - - // Pre-emphasise the data and other stuff. - for (unsigned int i = 0U; i < nSamples; i++) - samples[i] = m_preemphasis.filter(samples[i]); - - // Pack the floating point data (+1.0 to -1.0) to packed 12-bit samples (+2047 - -2048) unsigned int pack = 0U; unsigned char* packPointer = (unsigned char*)&pack; - unsigned int j = 0U; - unsigned int i = 0U; - for (; i < nSamples && j < space; i += 2U, j += 3U) { - unsigned short sample1 = (unsigned short)((samples[i] + 1.0F) * 2048.0F + 0.5F); - unsigned short sample2 = (unsigned short)((samples[i + 1] + 1.0F) * 2048.0F + 0.5F); + unsigned int nData = 0U; - pack = 0; - pack = ((unsigned int)sample1) << 12; - pack |= sample2; + for(unsigned int i = 0; i < length; i++) { + unsigned short netSample = SWAP_BYTES_16(netData[i]);//((netData[i] << 8) & 0xFF00U)| ((netData[i] >> 8) & 0x00FFU); + // Convert the unsigned 16-bit data (+65535 - 0) to float (+1.0 - -1.0) + float sampleFloat = (float(netSample) / 32768.0F) - 1.0F; - data[j] = packPointer[0U]; - data[j + 1U] = packPointer[1U]; - data[j + 2U] = packPointer[2U]; + //preemphasis + sampleFloat = m_preemphasis->filter(sampleFloat); + + // Convert float to 12-bit samples (0 to 4095) + unsigned int sample12bit = (unsigned int)((sampleFloat + 1.0F) * 2048.0F + 0.5F); + + // pack 2 samples onto 3 bytes + if((i & 1U) == 0) { + pack = 0U; + pack = sample12bit << 12; + } else { + pack |= sample12bit; + + data[nData++] = packPointer[0U]; + data[nData++] = packPointer[1U]; + data[nData++] = packPointer[2U]; + } } - return j;//return the number of bytes written + return nData; } void CFMControl::clock(unsigned int ms) diff --git a/FMControl.h b/FMControl.h index 9067d89..1647010 100644 --- a/FMControl.h +++ b/FMControl.h @@ -25,8 +25,8 @@ // Uncomment this to dump audio to a raw audio file // The file will be written in same folder as executable -// Toplay the file : aplay -f FLOAT_LE -c1 -r8000 -t raw audiodump.bin -//#define DUMP_RF_AUDIO +// Toplay the file : ffplay -autoexit -f u16be -ar 8000 audiodump.bin +// #define DUMP_RF_AUDIO class CFMControl { public: @@ -45,8 +45,11 @@ private: CFMNetwork* m_network; bool m_enabled; CRingBuffer m_incomingRFAudio; - CIIRDirectForm1Filter m_preemphasis; - CIIRDirectForm1Filter m_deemphasis; + CIIRDirectForm1Filter * m_preemphasis; + CIIRDirectForm1Filter * m_deemphasis; + CIIRDirectForm1Filter * m_filterStage1; + CIIRDirectForm1Filter * m_filterStage2; + CIIRDirectForm1Filter * m_filterStage3; }; #endif diff --git a/Modem.cpp b/Modem.cpp index cb2c18c..e4dcb79 100644 --- a/Modem.cpp +++ b/Modem.cpp @@ -160,7 +160,7 @@ m_rxNXDNData(1000U, "Modem RX NXDN"), m_txNXDNData(1000U, "Modem TX NXDN"), m_txPOCSAGData(1000U, "Modem TX POCSAG"), m_rxFMData(1000U, "Modem RX FM"), -m_txFMData(1000U, "Modem TX FM"), +m_txFMData(5000U, "Modem TX FM"), m_rxTransparentData(1000U, "Modem RX Transparent"), m_txTransparentData(1000U, "Modem TX Transparent"), m_sendTransparentDataFrameType(0U), diff --git a/SerialController.cpp b/SerialController.cpp index aacc6c6..970501a 100644 --- a/SerialController.cpp +++ b/SerialController.cpp @@ -304,6 +304,10 @@ bool CSerialController::open() ::cfsetospeed(&termios, B230400); ::cfsetispeed(&termios, B230400); break; + case 460800U: + ::cfsetospeed(&termios, B460800); + ::cfsetispeed(&termios, B460800); + break; default: LogError("Unsupported serial port speed - %u", m_speed); ::close(m_fd); diff --git a/Tools/DeEmphasis.py b/Tools/DeEmphasis.py new file mode 100644 index 0000000..44320c9 --- /dev/null +++ b/Tools/DeEmphasis.py @@ -0,0 +1,43 @@ +#based on https://github.com/gnuradio/gnuradio/blob/master/gr-analog/python/analog/fm_emph.py + +import math +import cmath +import numpy as np +import scipy.signal as signal +import pylab as pl + +tau = 750e-6 +fs = 8000 +fh = 2700 + +# Digital corner frequency +w_c = 1.0 / tau + +# Prewarped analog corner frequency +w_ca = 2.0 * fs * math.tan(w_c / (2.0 * fs)) + +# Resulting digital pole, zero, and gain term from the bilinear +# transformation of H(s) = w_ca / (s + w_ca) to +# H(z) = b0 (1 - z1 z^-1)/(1 - p1 z^-1) +k = -w_ca / (2.0 * fs) +z1 = -1.0 +p1 = (1.0 + k) / (1.0 - k) +b0 = -k / (1.0 - k) + +btaps = [ b0 * 1.0, b0 * -z1, 0 ] +ataps = [ 1.0, -p1, 0 ] + +# Since H(s = 0) = 1.0, then H(z = 1) = 1.0 and has 0 dB gain at DC + + +taps = np.concatenate((btaps, ataps), axis=0) +print("Taps") +print(*taps, "", sep=",", end="\n") + +f,h = signal.freqz(btaps,ataps, fs=fs) +pl.plot(f, 20*np.log10(np.abs(h))) +pl.xlabel('frequency/Hz') +pl.ylabel('gain/dB') +pl.ylim(top=0,bottom=-30) +pl.xlim(left=0, right=fh*2.5) +pl.show() \ No newline at end of file diff --git a/Tools/PreEmphasis.py b/Tools/PreEmphasis.py new file mode 100644 index 0000000..22c81f6 --- /dev/null +++ b/Tools/PreEmphasis.py @@ -0,0 +1,51 @@ +#based on https://github.com/gnuradio/gnuradio/blob/master/gr-analog/python/analog/fm_emph.py + +import math +import cmath +import numpy as np +import scipy.signal as signal +import pylab as pl + +tau = 750e-6 +fs = 8000 +fh = 2700 + +# Digital corner frequencies +w_cl = 1.0 / tau +w_ch = 2.0 * math.pi * fh + +# Prewarped analog corner frequencies +w_cla = 2.0 * fs * math.tan(w_cl / (2.0 * fs)) +w_cha = 2.0 * fs * math.tan(w_ch / (2.0 * fs)) + +# Resulting digital pole, zero, and gain term from the bilinear +# transformation of H(s) = (s + w_cla) / (s + w_cha) to +# H(z) = b0 (1 - z1 z^-1)/(1 - p1 z^-1) +kl = -w_cla / (2.0 * fs) +kh = -w_cha / (2.0 * fs) +z1 = (1.0 + kl) / (1.0 - kl) +p1 = (1.0 + kh) / (1.0 - kh) +b0 = (1.0 - kl) / (1.0 - kh) + +# Since H(s = infinity) = 1.0, then H(z = -1) = 1.0 and +# this filter has 0 dB gain at fs/2.0. +# That isn't what users are going to expect, so adjust with a +# gain, g, so that H(z = 1) = 1.0 for 0 dB gain at DC. +w_0dB = 2.0 * math.pi * 0.0 +g = abs(1.0 - p1 * cmath.rect(1.0, -w_0dB)) \ +/ (b0 * abs(1.0 - z1 * cmath.rect(1.0, -w_0dB))) + +btaps = [ g * b0 * 1.0, g * b0 * -z1, 0] +ataps = [ 1.0, -p1, 0] + +taps = np.concatenate((btaps, ataps), axis=0) +print("Taps") +print(*taps, "", sep=",", end="\n") + +f,h = signal.freqz(btaps,ataps, fs=fs) +pl.plot(f, 20*np.log10(np.abs(h))) +pl.xlabel('frequency/Hz') +pl.ylabel('gain/dB') +pl.ylim(top=30,bottom=0) +pl.xlim(left=0, right=fh*2.5) +pl.show() \ No newline at end of file