mirror of
https://github.com/g4klx/MMDVMHost
synced 2025-12-21 23:45:49 +08:00
Merge branch 'FIR' into IAX
This commit is contained in:
Jonathan Naylor
168
FMControl.cpp
168
FMControl.cpp
@@ -30,10 +30,55 @@
|
||||
|
||||
const float DEEMPHASIS_GAIN_DB = 8.0F; // Audio gain adjustment
|
||||
const float PREEMPHASIS_GAIN_DB = 0.0F; // Audio gain adjustment
|
||||
const float FILTER_GAIN_DB = 2.0F; // Audio gain adjustment
|
||||
const unsigned int FM_MASK = 0x00000FFFU;
|
||||
|
||||
CFMControl::CFMControl(IFMNetwork* network, float txAudioGain, float rxAudioGain, bool preEmphasisOn, bool deEmphasisOn) :
|
||||
// Created using http://t-filter.engineerjs.com/
|
||||
const float FILTER_TAPS[] = {
|
||||
-0.05171164345032154F, 0.004577697825204152F, 0.0841254955090718F, 0.021290266836279378F, -0.004089760781267529F,
|
||||
0.017543929045823792F, -0.019757885035823444F, -0.007257327157430058F, -0.004449379743389856F, -0.02385756312026885F,
|
||||
0.0008185571872412343F, -0.013864145248825219F, -0.011210498318183482F, 0.005254565026658612F, -0.015043483504571332F,
|
||||
0.005461884596609861F, 0.002026322985797473F, -0.007542135237135551F, 0.015720810214077726F, -0.004651704877827266F,
|
||||
0.005558418419098179F, 0.013941762626730268F, -0.007509473824811953F, 0.016618272581132985F, 0.002487274459992476F,
|
||||
-0.0024802940122075103F, 0.017683063350944713F, -0.009994442394814056F, 0.007156447064599394F, 0.006433822485904772F
|
||||
-0.014292696730698431F, 0.013319129319180239F, -0.010408978940031273F, -0.007266470155557534F, 0.009230635462251885F,
|
||||
-0.021463578074814135F, 0.005194836976513505F, -0.004126873023205119F, -0.01808260377380932F, 0.01304137129976236F,
|
||||
-0.017532323849990875F, -0.001929317250481216F, 0.009395238582798674F, -0.01975948679774396F, 0.01622855958724977F,
|
||||
-0.0029621037585558234F, -0.006899065995755296F, 0.02321184132395859F, -0.013467585741796682F, 0.01371204158005827F,
|
||||
0.013692208003629535F, -0.012367114737680571F, 0.02722969001859293F, -0.005345784554487899F, 0.0010074007729973767F,
|
||||
0.022305163683623532F, -0.019636621576653458F, 0.016349411295640864F, 0.0003626724003974509F, -0.018539298657843845F,
|
||||
0.01994785867615676F, -0.024001641760916962F, -0.0034795731393629766F, 0.0060060611058407164F, -0.033081075064776075F,
|
||||
0.012754552578116456F, -0.01694452885708811F, -0.019221979726946815F, 0.016490911013530663F, -0.03092454436058619F,
|
||||
0.00861201477103357F, 0.004613884756593608F, -0.021987826895264795F, 0.030752871497932738F, -0.011775928674227762F,
|
||||
0.007473527853524001F, 0.03126561745903647F, -0.016067027177553667F, 0.0378218068192532F, 0.010312992900560435F,
|
||||
-0.0006238863579307288F, 0.04477891903198119F, -0.015623491538694835F, 0.02384438222815608F, 0.01750864066332353F,
|
||||
-0.026325530246751806F, 0.03302218423753518F, -0.03126149618245644F, -0.014849557775866738F, 0.005783253669755862F,
|
||||
-0.07047543753434074F, 0.004291583771423271F, -0.05850933604118403F, -0.06807719431962597F, 0.00009772317389628164F,
|
||||
-0.13523267568572162F, 0.0004875192337944561F, -0.07994164839657204F, -0.18719066454627675F, 0.5305904386356379F,
|
||||
0.5305904386356379F, -0.18719066454627675F, -0.07994164839657204F, 0.0004875192337944561F, -0.13523267568572162F,
|
||||
0.00009772317389628164F, -0.06807719431962597F, -0.05850933604118403F, 0.004291583771423271F, -0.07047543753434074F,
|
||||
0.005783253669755862F, -0.014849557775866738F, -0.03126149618245644F, 0.03302218423753518F, -0.026325530246751806F,
|
||||
0.01750864066332353F, 0.02384438222815608F, -0.015623491538694835F, 0.04477891903198119F, -0.0006238863579307288F,
|
||||
0.010312992900560435F, 0.0378218068192532F, -0.016067027177553667F, 0.03126561745903647F, 0.007473527853524001F,
|
||||
-0.011775928674227762F, 0.030752871497932738F, -0.021987826895264795F, 0.004613884756593608F, 0.00861201477103357F,
|
||||
-0.03092454436058619F, 0.016490911013530663F, -0.019221979726946815F, -0.01694452885708811F, 0.012754552578116456F,
|
||||
-0.033081075064776075F, 0.0060060611058407164F, -0.0034795731393629766F, -0.024001641760916962F, 0.01994785867615676F,
|
||||
-0.018539298657843845F, 0.0003626724003974509F, 0.016349411295640864F, -0.019636621576653458F, 0.022305163683623532F,
|
||||
0.0010074007729973767F, -0.005345784554487899F, 0.02722969001859293F, -0.012367114737680571F, 0.013692208003629535F,
|
||||
0.01371204158005827F, -0.013467585741796682F, 0.02321184132395859F, -0.006899065995755296F, -0.0029621037585558234F,
|
||||
0.01622855958724977F, -0.01975948679774396F, 0.009395238582798674F, -0.001929317250481216F, -0.017532323849990875F,
|
||||
0.01304137129976236F, -0.01808260377380932F, -0.004126873023205119F, 0.005194836976513505F, -0.021463578074814135F,
|
||||
0.009230635462251885F, -0.007266470155557534F, -0.010408978940031273F, 0.013319129319180239F, -0.014292696730698431F,
|
||||
0.006433822485904772F, 0.007156447064599394F, -0.009994442394814056F, 0.017683063350944713F, -0.0024802940122075103F,
|
||||
0.002487274459992476F, 0.016618272581132985F, -0.007509473824811953F, 0.013941762626730268F, 0.005558418419098179F,
|
||||
-0.004651704877827266F, 0.015720810214077726F, -0.007542135237135551F, 0.002026322985797473F, 0.005461884596609861F,
|
||||
-0.015043483504571332F, 0.005254565026658612F, -0.011210498318183482F, -0.013864145248825219F, 0.0008185571872412343F,
|
||||
-0.02385756312026885F, -0.004449379743389856F, -0.007257327157430058F, -0.019757885035823444F, 0.017543929045823792F,
|
||||
-0.004089760781267529F, 0.021290266836279378F, 0.0841254955090718F, 0.004577697825204152F, -0.05171164345032154F
|
||||
};
|
||||
|
||||
const unsigned int FILTER_TAPS_COUNT = 200U;
|
||||
|
||||
CFMControl::CFMControl(CFMNetwork* network, float txAudioGain, float rxAudioGain, bool preEmphasisOn, bool deEmphasisOn) :
|
||||
m_network(network),
|
||||
m_txAudioGain(txAudioGain),
|
||||
m_rxAudioGain(rxAudioGain),
|
||||
@@ -42,95 +87,72 @@ m_deEmphasisOn(deEmphasisOn),
|
||||
m_enabled(false),
|
||||
m_begin(true),
|
||||
m_incomingRFAudio(1600U, "Incoming RF FM Audio"),
|
||||
m_preEmphasis(NULL),
|
||||
m_deEmphasis(NULL),
|
||||
m_filterStage1(NULL),
|
||||
m_filterStage2(NULL),
|
||||
m_filterStage3(NULL)
|
||||
m_preEmphasis(8.315375384336983F, -7.03334621603483F, 0.0F, 1.0F, 0.282029168302153F, 0.0F, PREEMPHASIS_GAIN_DB),
|
||||
m_deEmphasis(0.07708787090460224F, 0.07708787090460224F, 0.0F, 1.0F, -0.8458242581907955F, 0.0F, DEEMPHASIS_GAIN_DB),
|
||||
m_filter(FILTER_TAPS, FILTER_TAPS_COUNT)
|
||||
{
|
||||
assert(txAudioGain > 0.0F);
|
||||
assert(rxAudioGain > 0.0F);
|
||||
|
||||
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);
|
||||
|
||||
// Chebyshev 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);
|
||||
assert(txAudioGain > 0.0F);
|
||||
assert(rxAudioGain > 0.0F);
|
||||
}
|
||||
|
||||
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)
|
||||
{
|
||||
assert(data != NULL);
|
||||
assert(length > 0U);
|
||||
assert(data != NULL);
|
||||
assert(length > 0U);
|
||||
|
||||
if (m_network == NULL)
|
||||
return true;
|
||||
if (m_network == NULL)
|
||||
return true;
|
||||
|
||||
if (data[0U] == TAG_HEADER)
|
||||
return true;
|
||||
if (data[0U] == TAG_HEADER)
|
||||
return true;
|
||||
|
||||
if (data[0U] == TAG_EOT) {
|
||||
m_begin = true;
|
||||
return m_network->writeEnd();
|
||||
}
|
||||
if (data[0U] == TAG_EOT)
|
||||
return m_network->writeEnd();
|
||||
|
||||
if (data[0U] != TAG_DATA)
|
||||
return false;
|
||||
if (data[0U] != TAG_DATA)
|
||||
return false;
|
||||
|
||||
if (m_begin) {
|
||||
m_begin = false;
|
||||
m_network->writeStart();
|
||||
}
|
||||
m_incomingRFAudio.addData(data + 1U, length - 1U);
|
||||
unsigned int bufferLength = m_incomingRFAudio.dataSize();
|
||||
if (bufferLength > 240U) // 160 samples 12-bit
|
||||
bufferLength = 240U; // 160 samples 12-bit
|
||||
|
||||
m_incomingRFAudio.addData(data + 1U, length - 1U);
|
||||
unsigned int bufferLength = m_incomingRFAudio.dataSize();
|
||||
if (bufferLength > 240U) // 160 samples 12-bit
|
||||
bufferLength = 240U; // 160 samples 12-bit
|
||||
if (bufferLength >= 3U) {
|
||||
bufferLength = bufferLength - bufferLength % 3U; // Round down to nearest multiple of 3
|
||||
unsigned char bufferData[240U]; // 160 samples 12-bit
|
||||
m_incomingRFAudio.getData(bufferData, bufferLength);
|
||||
|
||||
if (bufferLength >= 3U) {
|
||||
bufferLength = bufferLength - bufferLength % 3U; // Round down to nearest multiple of 3
|
||||
unsigned char bufferData[240U]; // 160 samples 12-bit
|
||||
m_incomingRFAudio.getData(bufferData, bufferLength);
|
||||
unsigned int pack = 0U;
|
||||
unsigned char* packPointer = (unsigned char*)&pack;
|
||||
float out[160U]; // 160 samples 12-bit
|
||||
unsigned int nOut = 0U;
|
||||
short unpackedSamples[2U];
|
||||
|
||||
unsigned int pack = 0U;
|
||||
unsigned char* packPointer = (unsigned char*)&pack;
|
||||
float out[160U]; // 160 samples 12-bit
|
||||
unsigned int nOut = 0U;
|
||||
short unpackedSamples[2U];
|
||||
for (unsigned int i = 0U; i < bufferLength; i += 3U) {
|
||||
// Extract unsigned 12 bit unsigned sample pairs packed into 3 bytes to 16 bit signed
|
||||
packPointer[0U] = bufferData[i + 0U];
|
||||
packPointer[1U] = bufferData[i + 1U];
|
||||
packPointer[2U] = bufferData[i + 2U];
|
||||
|
||||
for (unsigned int i = 0U; i < bufferLength; i += 3U) {
|
||||
// Extract unsigned 12 bit unsigned sample pairs packed into 3 bytes to 16 bit signed
|
||||
packPointer[0U] = bufferData[i + 0U];
|
||||
packPointer[1U] = bufferData[i + 1U];
|
||||
packPointer[2U] = bufferData[i + 2U];
|
||||
unpackedSamples[1U] = short(int(pack & FM_MASK) - 2048);
|
||||
unpackedSamples[0U] = short(int(pack >> 12 & FM_MASK) - 2048);
|
||||
|
||||
unpackedSamples[1U] = short(int(pack & FM_MASK) - 2048);
|
||||
unpackedSamples[0U] = short(int(pack >> 12 & FM_MASK) - 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]) * m_rxAudioGain) / 2048.0F;
|
||||
|
||||
// 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]) * m_rxAudioGain) / 2048.0F;
|
||||
// De-emphasise and remove CTCSS
|
||||
if (m_deEmphasisOn)
|
||||
sampleFloat = m_deEmphasis.filter(sampleFloat);
|
||||
|
||||
// De-emphasise and remove CTCSS
|
||||
if (m_deEmphasisOn)
|
||||
sampleFloat = m_deEmphasis->filter(sampleFloat);
|
||||
|
||||
out[nOut++] = m_filterStage3->filter(m_filterStage2->filter(m_filterStage1->filter(sampleFloat)));
|
||||
}
|
||||
}
|
||||
out[nOut++] = m_filter.filter(sampleFloat);
|
||||
}
|
||||
}
|
||||
|
||||
#if defined(DUMP_RF_AUDIO)
|
||||
FILE* audiofile = ::fopen("./audiodump.bin", "ab");
|
||||
@@ -168,9 +190,9 @@ unsigned int CFMControl::readModem(unsigned char* data, unsigned int space)
|
||||
for (unsigned int i = 0; i < length; i++) {
|
||||
float sampleFloat = netData[i] * m_txAudioGain;
|
||||
|
||||
// Pre-emphasis
|
||||
if (m_preEmphasisOn)
|
||||
sampleFloat = m_preEmphasis->filter(sampleFloat);
|
||||
// Pre-emphasis
|
||||
if (m_preEmphasisOn)
|
||||
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);
|
||||
|
||||
Reference in New Issue
Block a user