From da077c73f8ef929eb6e7616a723687e1cd93a808 Mon Sep 17 00:00:00 2001 From: Jonathan Naylor Date: Mon, 27 Jul 2020 10:38:07 +0100 Subject: [PATCH] Allow the FM network sample rate to be specified. --- Conf.cpp | 8 ++++ Conf.h | 2 + FMControl.cpp | 22 ++++------ FMControl.h | 10 ++--- FMNetwork.cpp | 115 ++++++++++++++++++++++++++++++++++++++++++-------- FMNetwork.h | 11 +++-- MMDVM.ini | 1 + MMDVMHost.cpp | 4 +- Version.h | 2 +- 9 files changed, 134 insertions(+), 41 deletions(-) diff --git a/Conf.cpp b/Conf.cpp index 380dcea..1ea5cc5 100644 --- a/Conf.cpp +++ b/Conf.cpp @@ -268,6 +268,7 @@ m_fmGatewayAddress(), m_fmGatewayPort(0U), m_fmLocalAddress(), m_fmLocalPort(0U), +m_fmSampleRate(8000U), m_fmNetworkModeHang(3U), m_fmNetworkDebug(false), m_ax25NetworkEnabled(false), @@ -933,6 +934,8 @@ bool CConf::read() m_fmGatewayAddress = value; else if (::strcmp(key, "GatewayPort") == 0) m_fmGatewayPort = (unsigned int)::atoi(value); + else if (::strcmp(key, "SampleRate") == 0) + m_fmSampleRate = (unsigned int)::atoi(value); else if (::strcmp(key, "ModeHang") == 0) m_fmNetworkModeHang = (unsigned int)::atoi(value); else if (::strcmp(key, "Debug") == 0) @@ -2054,6 +2057,11 @@ unsigned int CConf::getFMLocalPort() const return m_fmLocalPort; } +unsigned int CConf::getFMSampleRate() const +{ + return m_fmSampleRate; +} + unsigned int CConf::getFMNetworkModeHang() const { return m_fmNetworkModeHang; diff --git a/Conf.h b/Conf.h index 095915d..01ef9f5 100644 --- a/Conf.h +++ b/Conf.h @@ -279,6 +279,7 @@ public: unsigned int getFMGatewayPort() const; std::string getFMLocalAddress() const; unsigned int getFMLocalPort() const; + unsigned int getFMSampleRate() const; unsigned int getFMNetworkModeHang() const; bool getFMNetworkDebug() const; @@ -569,6 +570,7 @@ private: unsigned int m_fmGatewayPort; std::string m_fmLocalAddress; unsigned int m_fmLocalPort; + unsigned int m_fmSampleRate; unsigned int m_fmNetworkModeHang; bool m_fmNetworkDebug; diff --git a/FMControl.cpp b/FMControl.cpp index 8a8f133..c07c8d4 100644 --- a/FMControl.cpp +++ b/FMControl.cpp @@ -89,7 +89,7 @@ bool CFMControl::writeModem(const unsigned char* data, unsigned int length) unsigned int pack = 0U; unsigned char* packPointer = (unsigned char*)&pack; - unsigned short out[168U]; + float out[168U]; unsigned int nOut = 0U; short unpackedSamples[2U]; @@ -102,28 +102,24 @@ bool CFMControl::writeModem(const unsigned char* data, unsigned int length) unpackedSamples[0U] = 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) + for (unsigned char j = 0U; j < 2U; j++) { + // Convert to float (-1.0 to +1.0) float sampleFloat = float(unpackedSamples[j]) / 2048.0F; - //De-emphasise and remove CTCSS + // De-emphasise and remove CTCSS sampleFloat = m_deemphasis->filter(sampleFloat); - sampleFloat = m_filterStage3->filter(m_filterStage2->filter(m_filterStage1->filter(sampleFloat))); - - // 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); + out[nOut++] = m_filterStage3->filter(m_filterStage2->filter(m_filterStage1->filter(sampleFloat))); } } #if defined(DUMP_RF_AUDIO) FILE * audiofile = fopen("./audiodump.bin", "ab"); if(audiofile != NULL) { - fwrite(out, sizeof(unsigned short), nOut, audiofile); + fwrite(out, sizeof(float), nOut, audiofile); fclose(audiofile); } #endif - return m_network->writeData((unsigned char*)out, nOut * sizeof(unsigned short)); + return m_network->writeData(out, nOut); } return true; @@ -140,7 +136,7 @@ unsigned int CFMControl::readModem(unsigned char* data, unsigned int space) if (space > 252U) space = 252U; - unsigned short netData[168U];//modem can handle up to 168 samples at a time + unsigned short netData[168U]; // Modem can handle up to 168 samples at a time unsigned int length = m_network->read((unsigned char*)netData, 168U * sizeof(unsigned short)); length /= sizeof(unsigned short); if (length == 0U) @@ -150,7 +146,7 @@ unsigned int CFMControl::readModem(unsigned char* data, unsigned int space) unsigned char* packPointer = (unsigned char*)&pack; unsigned int nData = 0U; - for(unsigned int i = 0; i < length; i++) { + for (unsigned int i = 0; i < length; i++) { unsigned short netSample = SWAP_BYTES_16(netData[i]); // Convert the unsigned 16-bit data (+65535 - 0) to float (+1.0 - -1.0) diff --git a/FMControl.h b/FMControl.h index 1647010..ac2142e 100644 --- a/FMControl.h +++ b/FMControl.h @@ -45,11 +45,11 @@ private: CFMNetwork* m_network; bool m_enabled; CRingBuffer m_incomingRFAudio; - CIIRDirectForm1Filter * m_preemphasis; - CIIRDirectForm1Filter * m_deemphasis; - CIIRDirectForm1Filter * m_filterStage1; - CIIRDirectForm1Filter * m_filterStage2; - CIIRDirectForm1Filter * m_filterStage3; + CIIRDirectForm1Filter* m_preemphasis; + CIIRDirectForm1Filter* m_deemphasis; + CIIRDirectForm1Filter* m_filterStage1; + CIIRDirectForm1Filter* m_filterStage2; + CIIRDirectForm1Filter* m_filterStage3; }; #endif diff --git a/FMNetwork.cpp b/FMNetwork.cpp index 67d0f2e..afc2014 100644 --- a/FMNetwork.cpp +++ b/FMNetwork.cpp @@ -27,10 +27,11 @@ const unsigned int BUFFER_LENGTH = 500U; -CFMNetwork::CFMNetwork(const std::string& localAddress, unsigned int localPort, const std::string& gatewayAddress, unsigned int gatewayPort, bool debug) : +CFMNetwork::CFMNetwork(const std::string& localAddress, unsigned int localPort, const std::string& gatewayAddress, unsigned int gatewayPort, unsigned int sampleRate, bool debug) : m_socket(localAddress, localPort), m_address(), m_port(gatewayPort), +m_sampleRate(sampleRate), m_debug(debug), m_enabled(false), m_buffer(2000U, "FM Network"), @@ -38,12 +39,22 @@ m_pollTimer(1000U, 5U) { assert(gatewayPort > 0U); assert(!gatewayAddress.empty()); + assert(sampleRate > 0U); m_address = CUDPSocket::lookup(gatewayAddress); + + int error; + m_incoming = ::src_new(SRC_SINC_FASTEST, 1, &error); + m_outgoing = ::src_new(SRC_SINC_FASTEST, 1, &error); + + assert(m_incoming != NULL); + assert(m_outgoing != NULL); } CFMNetwork::~CFMNetwork() { + ::src_delete(m_incoming); + ::src_delete(m_outgoing); } bool CFMNetwork::open() @@ -58,23 +69,53 @@ bool CFMNetwork::open() return m_socket.open(); } -bool CFMNetwork::writeData(const unsigned char* data, unsigned int length) +bool CFMNetwork::writeData(const float* data, unsigned int nSamples) { + assert(m_outgoing != NULL); assert(data != NULL); + assert(nSamples > 0U); - unsigned char buffer[500U]; - ::memset(buffer, 0x00U, 500U); + float out[1000U]; + SRC_DATA src; + + if (m_sampleRate != 8000U) { + src.data_in = data; + src.data_out = out; + src.input_frames = nSamples; + src.output_frames = 1000; + src.end_of_input = 0; + src.src_ratio = double(m_sampleRate) / 8000.0; + + int ret = ::src_process(m_outgoing, &src); + if (ret != 0) { + LogError("Error up/downsampling of the output audio has an error - %s", src_strerror(ret)); + return false; + } + } else { + src.data_out = data; + src.output_frames_gen = nSamples; + } + + unsigned int length = 3U; + + unsigned char buffer[1500U]; + ::memset(buffer, 0x00U, 1500U); buffer[0U] = 'F'; buffer[1U] = 'M'; buffer[2U] = 'D'; - ::memcpy(buffer + 3U, data, length); + for (long i = 0L; i < src.output_frames_gen; i++) { + unsigned short val = (unsigned short)((src.data_out[i] + 1.0F) * 32767.0F + 0.5F); + + buffer[length++] = (val >> 8) & 0xFFU; + buffer[length++] = (val >> 0) & 0xFFU; + } if (m_debug) - CUtils::dump(1U, "FM Network Data Sent", buffer, length + 3U); + CUtils::dump(1U, "FM Network Data Sent", buffer, length); - return m_socket.write(buffer, length + 3U, m_address, m_port); + return m_socket.write(buffer, length, m_address, m_port); } bool CFMNetwork::writeEOT() @@ -131,29 +172,67 @@ void CFMNetwork::clock(unsigned int ms) m_buffer.addData(buffer + 3U, length - 3U); } -unsigned int CFMNetwork::read(unsigned char* data, unsigned int space) +unsigned int CFMNetwork::read(float* data, unsigned int nSamples) { + assert(m_incoming != NULL); assert(data != NULL); + assert(nSamples > 0U); - - unsigned int bytes = m_buffer.dataSize(); + unsigned int bytes = m_buffer.dataSize() / sizeof(unsigned short); if (bytes == 0U) return 0U; - if (bytes < space) - space = bytes; + if (bytes < nSamples) + nSamples = bytes; - //we store usignedshorts, therefore ensure we always return and even number of data - if(space > 0 && space % 2 != 0) - space--;//round down to multiple of 2 + unsigned char buffer[1500U]; + m_buffer.getData(buffer, nSamples * sizeof(unsigned short)); - m_buffer.getData(data, space); + SRC_DATA src; - return space; + if (m_sampleRate != 8000U) { + float in[750U]; + + unsigned int j = 0U; + for (unsigned int i = 0U; i < nSamples; i++) { + unsigned short val = ((buffer[j++] & 0xFFU) << 8) + ((buffer[j++] & 0xFFU) << 0); + in[i] = (float(val) - 32768.0F) / 32768.0F; + } + + src.data_in = in; + src.data_out = data; + src.input_frames = nSamples; + src.output_frames = 750; + src.end_of_input = 0; + src.src_ratio = 8000.0 / double(m_sampleRate); + + int ret = ::src_process(m_incoming, &src); + if (ret != 0) { + LogError("Error up/downsampling of the input audio has an error - %s", src_strerror(ret)); + return 0U; + } + + return src.output_frames_gen; + } else { + unsigned int j = 0U; + for (unsigned int i = 0U; i < nSamples; i++) { + unsigned short val = ((buffer[j++] & 0xFFU) << 8) + ((buffer[j++] & 0xFFU) << 0); + data[i] = (float(val) - 32768.0F) / 32768.0F; + } + + return nSamples; + } } void CFMNetwork::reset() { + assert(m_incoming != NULL); + assert(m_outgoing != NULL); + + m_buffer.clear(); + + ::src_reset(m_incoming); + ::src_reset(m_outgoing); } void CFMNetwork::close() @@ -168,7 +247,7 @@ void CFMNetwork::enable(bool enabled) if (enabled && !m_enabled) reset(); else if (!enabled && m_enabled) - m_buffer.clear(); + reset(); m_enabled = enabled; } diff --git a/FMNetwork.h b/FMNetwork.h index 453e776..6dcdaac 100644 --- a/FMNetwork.h +++ b/FMNetwork.h @@ -23,23 +23,25 @@ #include "UDPSocket.h" #include "Timer.h" +#include + #include #include class CFMNetwork { public: - CFMNetwork(const std::string& myAddress, unsigned int myPort, const std::string& gatewayAddress, unsigned int gatewayPort, bool debug); + CFMNetwork(const std::string& myAddress, unsigned int myPort, const std::string& gatewayAddress, unsigned int gatewayPort, unsigned int sampleRate, bool debug); ~CFMNetwork(); bool open(); void enable(bool enabled); - bool writeData(const unsigned char* data, unsigned int length); + bool writeData(const float* data, unsigned int nSamples); bool writeEOT(); - unsigned int read(unsigned char* data, unsigned int space); + unsigned int read(float* data, unsigned int nSamples); void reset(); @@ -51,10 +53,13 @@ private: CUDPSocket m_socket; in_addr m_address; unsigned int m_port; + unsigned int m_sampleRate; bool m_debug; bool m_enabled; CRingBuffer m_buffer; CTimer m_pollTimer; + SRC_STATE* m_incoming; + SRC_STATE* m_outgoing; bool writePoll(); }; diff --git a/MMDVM.ini b/MMDVM.ini index 911cb29..6c35909 100644 --- a/MMDVM.ini +++ b/MMDVM.ini @@ -250,6 +250,7 @@ LocalAddress=127.0.0.1 LocalPort=3810 GatewayAddress=127.0.0.1 GatewayPort=4810 +SampleRate=8000 # ModeHang=3 Debug=0 diff --git a/MMDVMHost.cpp b/MMDVMHost.cpp index 2a04430..7066ad1 100644 --- a/MMDVMHost.cpp +++ b/MMDVMHost.cpp @@ -1690,6 +1690,7 @@ bool CMMDVMHost::createFMNetwork() unsigned int gatewayPort = m_conf.getFMGatewayPort(); std::string localAddress = m_conf.getFMLocalAddress(); unsigned int localPort = m_conf.getFMLocalPort(); + unsigned int sampleRate = m_conf.getFMSampleRate(); m_fmNetModeHang = m_conf.getFMNetworkModeHang(); bool debug = m_conf.getFMNetworkDebug(); @@ -1698,9 +1699,10 @@ bool CMMDVMHost::createFMNetwork() LogInfo(" Gateway Port: %u", gatewayPort); LogInfo(" Local Address: %s", localAddress.c_str()); LogInfo(" Local Port: %u", localPort); + LogInfo(" Sample Rate: %u", sampleRate); LogInfo(" Mode Hang: %us", m_fmNetModeHang); - m_fmNetwork = new CFMNetwork(localAddress, localPort, gatewayAddress, gatewayPort, debug); + m_fmNetwork = new CFMNetwork(localAddress, localPort, gatewayAddress, gatewayPort, sampleRate, debug); bool ret = m_fmNetwork->open(); if (!ret) { diff --git a/Version.h b/Version.h index 874077c..d5b3f85 100644 --- a/Version.h +++ b/Version.h @@ -19,6 +19,6 @@ #if !defined(VERSION_H) #define VERSION_H -const char* VERSION = "20200713"; +const char* VERSION = "20200727"; #endif