diff --git a/P252DMR/P252DMR.cpp b/P252DMR/P252DMR.cpp index 5303f77..36ae0a3 100644 --- a/P252DMR/P252DMR.cpp +++ b/P252DMR/P252DMR.cpp @@ -799,7 +799,7 @@ int CP252DMR::run() if (m_xlxReflectors != NULL) m_xlxReflectors->clock(ms); - if (ms < 5U) CThread::sleep(5U); + if (ms < 2U) CThread::sleep(2U); } m_p25Network->close(); diff --git a/USRP2M17/Conf.cpp b/USRP2M17/Conf.cpp new file mode 100644 index 0000000..38874d2 --- /dev/null +++ b/USRP2M17/Conf.cpp @@ -0,0 +1,233 @@ +/* + * Copyright (C) 2015,2016,2017 by Jonathan Naylor G4KLX + * Copyright (C) 2018 by Andy Uribe CA6JAU + * Copyright (C) 2021 by Doug McLain AD8DP + * + * This program 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 2 of the License, or + * (at your option) any later version. + * + * This program 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 this program; if not, write to the Free Software + * Foundation, Inc., 675 Mass Ave, Cambridge, MA 02139, USA. + */ + +#include "Conf.h" +#include "Log.h" + +#include +#include +#include +#include + +const int BUFFER_SIZE = 500; + +enum SECTION { + SECTION_NONE, + SECTION_M17_NETWORK, + SECTION_USRP_NETWORK, + SECTION_LOG +}; + +CConf::CConf(const std::string& file) : +m_file(file), +m_callsign(), +m_daemon(false), +m_usrpAddress(), +m_usrpDstPort(0U), +m_usrpLocalPort(0U), +m_usrpGainAdjDb(), +m_usrpDebug(false), +m_m17Name(), +m_m17Address(), +m_m17DstPort(0U), +m_m17LocalPort(0U), +m_m17GainAdjDb(), +m_m17Debug(false), +m_logDisplayLevel(0U), +m_logFileLevel(0U), +m_logFilePath(), +m_logFileRoot() +{ +} + +CConf::~CConf() +{ +} + +bool CConf::read() +{ + FILE* fp = ::fopen(m_file.c_str(), "rt"); + if (fp == NULL) { + ::fprintf(stderr, "Couldn't open the .ini file - %s\n", m_file.c_str()); + return false; + } + + SECTION section = SECTION_NONE; + + char buffer[BUFFER_SIZE]; + while (::fgets(buffer, BUFFER_SIZE, fp) != NULL) { + if (buffer[0U] == '#') + continue; + + if (buffer[0U] == '[') { + if (::strncmp(buffer, "[M17 Network]", 13U) == 0) + section = SECTION_M17_NETWORK; + else if (::strncmp(buffer, "[USRP Network]", 14U) == 0) + section = SECTION_USRP_NETWORK; + else if (::strncmp(buffer, "[Log]", 5U) == 0) + section = SECTION_LOG; + else + section = SECTION_NONE; + + continue; + } + + char* key = ::strtok(buffer, " \t=\r\n"); + if (key == NULL) + continue; + + char* value = ::strtok(NULL, "\r\n"); + if (value == NULL) + continue; + + // Remove quotes from the value + size_t len = ::strlen(value); + if (len > 1U && *value == '"' && value[len - 1U] == '"') { + value[len - 1U] = '\0'; + value++; + } + ::fprintf(stderr, "CConf key:val:section == %s:%s:%d\n", key, value, section); + if (section == SECTION_M17_NETWORK) { + if (::strcmp(key, "Callsign") == 0) + m_callsign = value; + else if (::strcmp(key, "LocalPort") == 0) + m_m17LocalPort = (unsigned int)::atoi(value); + else if (::strcmp(key, "Name") == 0) + m_m17Name = value; + else if (::strcmp(key, "Address") == 0) + m_m17Address = value; + else if (::strcmp(key, "DstPort") == 0) + m_m17DstPort = (uint32_t)::atoi(value); + else if (::strcmp(key, "GainAdjustdB") == 0) + m_m17GainAdjDb = value; + else if (::strcmp(key, "Debug") == 0) + m_m17Debug = ::atoi(value) == 1; + } else if (section == SECTION_USRP_NETWORK) { + if (::strcmp(key, "Address") == 0) + m_usrpAddress = value; + else if (::strcmp(key, "DstPort") == 0) + m_usrpDstPort = (uint32_t)::atoi(value); + else if (::strcmp(key, "LocalPort") == 0) + m_usrpLocalPort = (uint32_t)::atoi(value); + else if (::strcmp(key, "GainAdjustdB") == 0) + m_usrpGainAdjDb = value; + else if (::strcmp(key, "Debug") == 0) + m_usrpDebug = ::atoi(value) == 1; + } else if (section == SECTION_LOG) { + if (::strcmp(key, "FilePath") == 0) + m_logFilePath = value; + else if (::strcmp(key, "FileRoot") == 0) + m_logFileRoot = value; + else if (::strcmp(key, "FileLevel") == 0) + m_logFileLevel = (uint32_t)::atoi(value); + else if (::strcmp(key, "DisplayLevel") == 0) + m_logDisplayLevel = (uint32_t)::atoi(value); + } + } + + ::fclose(fp); + + return true; +} + +std::string CConf::getCallsign() const +{ + return m_callsign; +} + +std::string CConf::getM17Name() const +{ + return m_m17Name; +} + +std::string CConf::getM17Address() const +{ + return m_m17Address; +} + +uint16_t CConf::getM17DstPort() const +{ + return m_m17DstPort; +} + +uint16_t CConf::getM17LocalPort() const +{ + return m_m17LocalPort; +} + +std::string CConf::getM17GainAdjDb() const +{ + return m_m17GainAdjDb; +} + +bool CConf::getM17Debug() const +{ + return m_m17Debug; +} + +bool CConf::getDaemon() const +{ + return m_daemon; +} + +std::string CConf::getUSRPAddress() const +{ + return m_usrpAddress; +} + +uint16_t CConf::getUSRPDstPort() const +{ + return m_usrpDstPort; +} + +uint16_t CConf::getUSRPLocalPort() const +{ + return m_usrpLocalPort; +} + +std::string CConf::getUSRPGainAdjDb() const +{ + return m_usrpGainAdjDb; +} + +bool CConf::getUSRPDebug() const +{ + return m_usrpDebug; +} + +uint32_t CConf::getLogDisplayLevel() const +{ + return m_logDisplayLevel; +} + +uint32_t CConf::getLogFileLevel() const +{ + return m_logFileLevel; +} + +std::string CConf::getLogFilePath() const +{ + return m_logFilePath; +} + +std::string CConf::getLogFileRoot() const +{ + return m_logFileRoot; +} diff --git a/USRP2M17/Conf.h b/USRP2M17/Conf.h new file mode 100644 index 0000000..2c5d110 --- /dev/null +++ b/USRP2M17/Conf.h @@ -0,0 +1,82 @@ +/* + * Copyright (C) 2015,2016,2017 by Jonathan Naylor G4KLX + * Copyright (C) 2018 by Andy Uribe CA6JAU +* Copyright (C) 2021 by Doug McLain AD8DP + * + * This program 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 2 of the License, or + * (at your option) any later version. + * + * This program 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 this program; if not, write to the Free Software + * Foundation, Inc., 675 Mass Ave, Cambridge, MA 02139, USA. + */ + +#if !defined(CONF_H) +#define CONF_H + +#include +#include + +class CConf +{ +public: + CConf(const std::string& file); + ~CConf(); + + bool read(); + + // The M17 Network section + std::string getCallsign() const; + bool getDaemon() const; + std::string getM17Name() const; + std::string getM17Address() const; + uint16_t getM17DstPort() const; + uint16_t getM17LocalPort() const; + std::string getM17GainAdjDb() const; + bool getM17Debug() const; + + // The USRP Network section + std::string getUSRPAddress() const; + uint16_t getUSRPDstPort() const; + uint16_t getUSRPLocalPort() const; + std::string getUSRPGainAdjDb() const; + bool getUSRPDebug() const; + + // The Log section + uint32_t getLogDisplayLevel() const; + uint32_t getLogFileLevel() const; + std::string getLogFilePath() const; + std::string getLogFileRoot() const; + +private: + std::string m_file; + std::string m_callsign; + bool m_daemon; + + std::string m_usrpAddress; + uint16_t m_usrpDstPort; + uint16_t m_usrpLocalPort; + std::string m_usrpGainAdjDb; + bool m_usrpDebug; + + std::string m_m17Name; + std::string m_m17Address; + uint16_t m_m17DstPort; + uint16_t m_m17LocalPort; + std::string m_m17GainAdjDb; + bool m_m17Debug; + + uint32_t m_logDisplayLevel; + uint32_t m_logFileLevel; + std::string m_logFilePath; + std::string m_logFileRoot; +}; + +#endif diff --git a/USRP2M17/Log.cpp b/USRP2M17/Log.cpp new file mode 100644 index 0000000..fc37ebf --- /dev/null +++ b/USRP2M17/Log.cpp @@ -0,0 +1,136 @@ +/* + * Copyright (C) 2015,2016 by Jonathan Naylor G4KLX + * + * This program 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 2 of the License, or + * (at your option) any later version. + * + * This program 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 this program; if not, write to the Free Software + * Foundation, Inc., 675 Mass Ave, Cambridge, MA 02139, USA. + */ + +#include "Log.h" + +#if defined(_WIN32) || defined(_WIN64) +#include +#else +#include +#endif + +#include +#include +#include +#include +#include +#include + +static unsigned int m_fileLevel = 2U; +static std::string m_filePath; +static std::string m_fileRoot; + +static FILE* m_fpLog = NULL; + +static unsigned int m_displayLevel = 2U; + +static struct tm m_tm; + +static char LEVELS[] = " DMIWEF"; + +static bool LogOpen() +{ + if (m_fileLevel == 0U) + return true; + + time_t now; + ::time(&now); + + struct tm* tm = ::gmtime(&now); + + if (tm->tm_mday == m_tm.tm_mday && tm->tm_mon == m_tm.tm_mon && tm->tm_year == m_tm.tm_year) { + if (m_fpLog != NULL) + return true; + } else { + if (m_fpLog != NULL) + ::fclose(m_fpLog); + } + + char filename[100U]; +#if defined(_WIN32) || defined(_WIN64) + ::sprintf(filename, "%s\\%s-%04d-%02d-%02d.log", m_filePath.c_str(), m_fileRoot.c_str(), tm->tm_year + 1900, tm->tm_mon + 1, tm->tm_mday); +#else + ::sprintf(filename, "%s/%s-%04d-%02d-%02d.log", m_filePath.c_str(), m_fileRoot.c_str(), tm->tm_year + 1900, tm->tm_mon + 1, tm->tm_mday); +#endif + + m_fpLog = ::fopen(filename, "a+t"); + m_tm = *tm; + + return m_fpLog != NULL; +} + +bool LogInitialise(const std::string& filePath, const std::string& fileRoot, unsigned int fileLevel, unsigned int displayLevel) +{ + m_filePath = filePath; + m_fileRoot = fileRoot; + m_fileLevel = fileLevel; + m_displayLevel = displayLevel; + return ::LogOpen(); +} + +void LogFinalise() +{ + if (m_fpLog != NULL) + ::fclose(m_fpLog); +} + +void Log(unsigned int level, const char* fmt, ...) +{ + assert(fmt != NULL); + + char buffer[300U]; +#if defined(_WIN32) || defined(_WIN64) + SYSTEMTIME st; + ::GetSystemTime(&st); + + ::sprintf(buffer, "%c: %04u-%02u-%02u %02u:%02u:%02u.%03u ", LEVELS[level], st.wYear, st.wMonth, st.wDay, st.wHour, st.wMinute, st.wSecond, st.wMilliseconds); +#else + struct timeval now; + ::gettimeofday(&now, NULL); + + struct tm* tm = ::gmtime(&now.tv_sec); + + ::sprintf(buffer, "%c: %04d-%02d-%02d %02d:%02d:%02d.%03lu ", LEVELS[level], tm->tm_year + 1900, tm->tm_mon + 1, tm->tm_mday, tm->tm_hour, tm->tm_min, tm->tm_sec, now.tv_usec / 1000U); +#endif + + va_list vl; + va_start(vl, fmt); + + ::vsprintf(buffer + ::strlen(buffer), fmt, vl); + + va_end(vl); + + if (level >= m_fileLevel && m_fileLevel != 0U) { + bool ret = ::LogOpen(); + if (!ret) + return; + + ::fprintf(m_fpLog, "%s\n", buffer); + ::fflush(m_fpLog); + } + + if (level >= m_displayLevel && m_displayLevel != 0U) { + ::fprintf(stdout, "%s\n", buffer); + ::fflush(stdout); + } + + if (level == 6U) { // Fatal + ::fclose(m_fpLog); + exit(1); + } +} diff --git a/USRP2M17/Log.h b/USRP2M17/Log.h new file mode 100644 index 0000000..d671ef9 --- /dev/null +++ b/USRP2M17/Log.h @@ -0,0 +1,36 @@ +/* + * Copyright (C) 2015,2016 by Jonathan Naylor G4KLX + * + * This program 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 2 of the License, or + * (at your option) any later version. + * + * This program 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 this program; if not, write to the Free Software + * Foundation, Inc., 675 Mass Ave, Cambridge, MA 02139, USA. + */ + +#if !defined(LOG_H) +#define LOG_H + +#include + +#define LogDebug(fmt, ...) Log(1U, fmt, ##__VA_ARGS__) +#define LogMessage(fmt, ...) Log(2U, fmt, ##__VA_ARGS__) +#define LogInfo(fmt, ...) Log(3U, fmt, ##__VA_ARGS__) +#define LogWarning(fmt, ...) Log(4U, fmt, ##__VA_ARGS__) +#define LogError(fmt, ...) Log(5U, fmt, ##__VA_ARGS__) +#define LogFatal(fmt, ...) Log(6U, fmt, ##__VA_ARGS__) + +extern void Log(unsigned int level, const char* fmt, ...); + +extern bool LogInitialise(const std::string& filePath, const std::string& fileRoot, unsigned int fileLevel, unsigned int displayLevel); +extern void LogFinalise(); + +#endif diff --git a/USRP2M17/M17Network.cpp b/USRP2M17/M17Network.cpp new file mode 100644 index 0000000..5d6f0be --- /dev/null +++ b/USRP2M17/M17Network.cpp @@ -0,0 +1,129 @@ +/* + * Copyright (C) 2009-2014,2016 by Jonathan Naylor G4KLX + * Copyright (C) 2021 by Doug McLain AD8DP + * + * This program 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 2 of the License, or + * (at your option) any later version. + * + * This program 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 this program; if not, write to the Free Software + * Foundation, Inc., 675 Mass Ave, Cambridge, MA 02139, USA. + */ + +#include "M17Network.h" +#include "Utils.h" +#include "Log.h" + +#include +#include +#include + + +CM17Network::CM17Network(const std::string& address, uint16_t dstPort, uint16_t localPort, uint8_t* callsign, bool debug) : +m_address(), +m_port(dstPort), +m_socket(localPort), +m_debug(debug) +{ + memcpy(m_callsign, callsign, 6); + m_address = CUDPSocket::lookup(address); +} + +CM17Network::~CM17Network() +{ +} + +bool CM17Network::open() +{ + LogInfo("Opening M17 network connection"); + + return m_socket.open(); +} + +bool CM17Network::writeData(const unsigned char* data, unsigned int length) +{ + assert(data != NULL); + assert(length > 0U); + + if (m_debug) + CUtils::dump(1U, "M17 Network Data Sent", data, length); + + return m_socket.write(data, length, m_address, m_port); +} + +bool CM17Network::writePoll() +{ + unsigned char data[10U]; + + memcpy(data, "PONG", 4); + memcpy(data+4, m_callsign, 6); + + if (m_debug) + CUtils::dump(1U, "M17 Network Pong Sent", data, 10U); + + return m_socket.write(data, 10U, m_address, m_port); +} + +bool CM17Network::writeLink(char m) +{ + unsigned char data[11U]; + + memcpy(data, "CONN", 4); + memcpy(data+4, m_callsign, 6); + data[10U] = m; + if (m_debug) + CUtils::dump(1U, "M17 Network Link Sent", data, 11U); + + //LogInfo("writeLink add:port == %x, %x", m_address.s_addr, m_port); + return m_socket.write(data, 11U, m_address, m_port); +} + +bool CM17Network::writeUnlink() +{ + unsigned char data[10U]; + + memcpy(data, "DISC", 4); + memcpy(data+4, m_callsign, 6); + + if (m_debug) + CUtils::dump(1U, "M17 Network Unlink Sent", data, 10U); + + return m_socket.write(data, 10U, m_address, m_port); +} + +unsigned int CM17Network::readData(unsigned char* data, unsigned int length) +{ + assert(data != NULL); + assert(length > 0U); + + in_addr address; + unsigned int port; + int len = m_socket.read(data, length, address, port); + if (len <= 0) + return 0U; + + // Check if the data is for us + if (m_address.s_addr != address.s_addr || port != m_port) { + LogMessage("M17 packet received from an invalid source, %08X != %08X and/or %u != %u", m_address.s_addr, address.s_addr, m_port, port); + return 0U; + } + + if (m_debug) + CUtils::dump(1U, "M17 Network Data Received", data, len); + + return len; +} + +void CM17Network::close() +{ + m_socket.close(); + + LogInfo("Closing P25 network connection"); +} diff --git a/USRP2M17/M17Network.h b/USRP2M17/M17Network.h new file mode 100644 index 0000000..34ba0cc --- /dev/null +++ b/USRP2M17/M17Network.h @@ -0,0 +1,48 @@ +/* + * Copyright (C) 2009-2014,2016 by Jonathan Naylor G4KLX + * Copyright (C) 2021 by Doug McLain AD8DP + * + * This program 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 2 of the License, or + * (at your option) any later version. + * + * This program 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 this program; if not, write to the Free Software + * Foundation, Inc., 675 Mass Ave, Cambridge, MA 02139, USA. + */ + +#ifndef M17Network_H +#define M17Network_H + +#include "UDPSocket.h" + +#include +#include + +class CM17Network { +public: + CM17Network(const std::string& address, uint16_t dstPort, uint16_t localPort, uint8_t* callsign, bool debug); + ~CM17Network(); + + bool open(); + bool writeData(const unsigned char* data, unsigned int length); + unsigned int readData(unsigned char* data, unsigned int length); + bool writePoll(); + bool writeLink(char m); + bool writeUnlink(); + void close(); +private: + in_addr m_address; + uint16_t m_port; + CUDPSocket m_socket; + bool m_debug; + unsigned char m_callsign[6]; +}; + +#endif diff --git a/USRP2M17/Makefile b/USRP2M17/Makefile new file mode 100644 index 0000000..a68a90d --- /dev/null +++ b/USRP2M17/Makefile @@ -0,0 +1,23 @@ +CC ?= gcc +CXX ?= g++ +CFLAGS ?= -g -O3 -Wall -std=c++0x -pthread +LIBS = -lm -lpthread +LDFLAGS ?= -g + +OBJECTS = Conf.o Log.o M17Network.o ModeConv.o StopWatch.o Timer.o UDPSocket.o USRPNetwork.o Utils.o \ + codec2/codebooks.o codec2/kiss_fft.o codec2/lpc.o codec2/nlp.o codec2/pack.o codec2/qbase.o codec2/quantise.o codec2/codec2.o USRP2M17.o + +all: USRP2M17 + +USRP2M17: $(OBJECTS) + $(CXX) $(OBJECTS) $(CFLAGS) $(LIBS) -o USRP2M17 + +%.o: %.cpp + $(CXX) $(CFLAGS) -c -o $@ $< + +install: + install -m 755 USRP2M17 /usr/local/bin/ + +clean: + $(RM) USRP2M17 *.o *.d *.bak codec2/*.o *~ + diff --git a/USRP2M17/ModeConv.cpp b/USRP2M17/ModeConv.cpp new file mode 100644 index 0000000..ab68a23 --- /dev/null +++ b/USRP2M17/ModeConv.cpp @@ -0,0 +1,166 @@ +/* + * Copyright (C) 2010,2014,2016,2018 by Jonathan Naylor G4KLX + * Copyright (C) 2016 Mathias Weyland, HB9FRV + * Copyright (C) 2018 by Andy Uribe CA6JAU + * Copyright (C) 2021 by Doug McLain AD8DP + * + * This program 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 2 of the License, or + * (at your option) any later version. + * + * This program 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 this program; if not, write to the Free Software + * Foundation, Inc., 675 Mass Ave, Cambridge, MA 02139, USA. + */ + +#include "ModeConv.h" +#include "Utils.h" +#include "Log.h" + +#include +#include + +CModeConv::CModeConv() : +m_m17N(0U), +m_usrpN(0U), +m_M17(5000U, "USRP2M17"), +m_USRP(5000U, "M172USRP"), +m_m17GainMultiplier(1), +m_m17Attenuate(false), +m_usrpGainMultiplier(3), +m_usrpAttenuate(true) +{ + m_c2 = new CCodec2(true); +} + +CModeConv::~CModeConv() +{ +} + +void CModeConv::setUSRPGainAdjDb(std::string dbstring) +{ + float db = std::stof(dbstring); + + float ratio = powf(10.0, (db/10.0)); + if(db < 0){ + ratio = 1/ratio; + m_usrpAttenuate = true; + } + m_usrpGainMultiplier = (uint16_t)roundf(ratio); +} + +void CModeConv::setM17GainAdjDb(std::string dbstring) +{ + float db = std::stof(dbstring); + + float ratio = powf(10.0, (db/10.0)); + if(db < 0){ + ratio = 1/ratio; + m_m17Attenuate = true; + } + m_m17GainMultiplier = (uint16_t)roundf(ratio); +} + +void CModeConv::putUSRPHeader() +{ + const uint8_t quiet[] = { 0x00u, 0x01u, 0x43u, 0x09u, 0xe4u, 0x9cu, 0x08u, 0x21u }; + + m_M17.addData(&TAG_HEADER, 1U); + m_M17.addData(quiet, 8U); + m_m17N += 1U; +} + +void CModeConv::putUSRPEOT() +{ + const uint8_t quiet[] = { 0x00u, 0x01u, 0x43u, 0x09u, 0xe4u, 0x9cu, 0x08u, 0x21u }; + + if((m_m17N % 2) == 0){ + m_M17.addData(&TAG_DATA, 1U); + m_M17.addData(quiet, 8U); + m_m17N += 1U; + } + + m_M17.addData(&TAG_EOT, 1U); + m_M17.addData(quiet, 8U); + m_m17N += 1U; +} + +void CModeConv::putUSRP(int16_t* data) +{ + uint8_t codec2[8U]; + + ::memset(codec2, 0, sizeof(codec2)); + + int16_t audio_adjusted[160U]; + + for(int i = 0; i < 160; ++i){ + audio_adjusted[i] = m_usrpAttenuate ? data[i] / m_usrpGainMultiplier : data[i] * m_usrpGainMultiplier; + } + + m_c2->codec2_encode(codec2, audio_adjusted); + m_M17.addData(&TAG_DATA, 1U); + m_M17.addData(codec2, 8U); + m_m17N += 1U; +} + +void CModeConv::putM17(uint8_t* data) +{ + int16_t audio[160U]; + int16_t audio_adjusted[160U]; + uint8_t codec2[8U]; + + ::memset(audio, 0, sizeof(audio)); + ::memcpy(codec2, &data[36], 8); + + m_c2->codec2_decode(audio, codec2); + + for(int i = 0; i < 160; ++i){ + audio_adjusted[i] = m_m17Attenuate ? audio[i] / m_m17GainMultiplier : audio[i] * m_m17GainMultiplier; + } + + m_USRP.addData(audio_adjusted, 160U); + m_usrpN += 1U; + + ::memcpy(codec2, &data[44], 8); + m_c2->codec2_decode(audio, codec2); + for(int i = 0; i < 160; ++i){ + audio_adjusted[i] = m_m17Attenuate ? audio[i] / m_m17GainMultiplier : audio[i] * m_m17GainMultiplier; + } + + m_USRP.addData(audio_adjusted, 160U); + m_usrpN += 1U; +} + +bool CModeConv::getUSRP(int16_t* data) +{ + if(m_usrpN){ + --m_usrpN; + return m_USRP.getData(data, 160U); + } + else{ + return false; + } +} + +uint32_t CModeConv::getM17(uint8_t* data) +{ + uint8_t tag[2U]; + + tag[0U] = TAG_NODATA; + tag[1U] = TAG_NODATA; + + if (m_m17N >= 2U) { + m_M17.getData(tag, 1U); + m_M17.getData(data, 8U); + m_M17.getData(tag+1, 1U); + m_M17.getData(data+8, 8U); + m_m17N -= 2U; + } + return (tag[1U] == TAG_EOT) ? tag[1U] : tag[0]; +} diff --git a/USRP2M17/ModeConv.h b/USRP2M17/ModeConv.h new file mode 100644 index 0000000..254ca94 --- /dev/null +++ b/USRP2M17/ModeConv.h @@ -0,0 +1,58 @@ +/* + * Copyright (C) 2010,2014,2016,2018 by Jonathan Naylor G4KLX + * Copyright (C) 2018 by Andy Uribe CA6JAU + * Copyright (C) 2021 by Doug McLain AD8DP + * + * This program 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 2 of the License, or + * (at your option) any later version. + * + * This program 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 this program; if not, write to the Free Software + * Foundation, Inc., 675 Mass Ave, Cambridge, MA 02139, USA. + */ + +#include "RingBuffer.h" +#include "codec2/codec2.h" + +const unsigned char TAG_HEADER = 0x00U; +const unsigned char TAG_DATA = 0x01U; +const unsigned char TAG_LOST = 0x02U; +const unsigned char TAG_EOT = 0x03U; +const unsigned char TAG_NODATA = 0x04U; + +#if !defined(MODECONV_H) +#define MODECONV_H + +class CModeConv { +public: + CModeConv(); + ~CModeConv(); + + void setUSRPGainAdjDb(std::string dbstring); + void setM17GainAdjDb(std::string dbstring); + void putUSRP(int16_t* data); + void putUSRPHeader(); + void putUSRPEOT(); + void putM17(uint8_t* data); + uint32_t getM17(uint8_t* data); + bool getUSRP(int16_t* data); +private: + uint32_t m_m17N; + uint32_t m_usrpN; + CRingBuffer m_M17; + CRingBuffer m_USRP; + CCodec2 *m_c2; + uint16_t m_m17GainMultiplier; + bool m_m17Attenuate; + uint16_t m_usrpGainMultiplier; + bool m_usrpAttenuate; +}; + +#endif diff --git a/USRP2M17/README.md b/USRP2M17/README.md new file mode 100644 index 0000000..3a306f5 --- /dev/null +++ b/USRP2M17/README.md @@ -0,0 +1,9 @@ +# Description + +This is the source code of USRP2M17, which converts USRP PCM audio and M17 digital mode, based on Jonathan G4KLX's [MMDVM](https://github.com/g4klx) software. Typical uses are connecting M17 reflectors to AllStar nodes and can be used with MMDVM modems in FM mode as stand alone radios. + +# Configuration +M17 and USRP both have a configuration value 'GainAdjustDB". Thru trial and error I have found the best balance of audio levels which are set as the defaults in the provided USRP2M17.ini file. For AllStar connections, USRP address and ports are the values defined in your USRP channel based node. + +# Building +run 'make' from the source directory. diff --git a/USRP2M17/RingBuffer.h b/USRP2M17/RingBuffer.h new file mode 100644 index 0000000..707de1c --- /dev/null +++ b/USRP2M17/RingBuffer.h @@ -0,0 +1,154 @@ +/* + * Copyright (C) 2006-2009,2012,2013,2015,2016 by Jonathan Naylor G4KLX + * + * This program 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 2 of the License, or + * (at your option) any later version. + * + * This program 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 this program; if not, write to the Free Software + * Foundation, Inc., 675 Mass Ave, Cambridge, MA 02139, USA. + */ + +#ifndef RingBuffer_H +#define RingBuffer_H + +#include "Log.h" + +#include +#include +#include + +template class CRingBuffer { +public: + CRingBuffer(unsigned int length, const char* name) : + m_length(length), + m_name(name), + m_buffer(NULL), + m_iPtr(0U), + m_oPtr(0U) + { + assert(length > 0U); + assert(name != NULL); + + m_buffer = new T[length]; + + ::memset(m_buffer, 0x00, m_length * sizeof(T)); + } + + ~CRingBuffer() + { + delete[] m_buffer; + } + + bool addData(const T* buffer, unsigned int nSamples) + { + if (nSamples >= freeSpace()) { + LogError("%s buffer overflow, clearing the buffer. (%u >= %u)", m_name, nSamples, freeSpace()); + clear(); + return false; + } + + for (unsigned int i = 0U; i < nSamples; i++) { + m_buffer[m_iPtr++] = buffer[i]; + + if (m_iPtr == m_length) + m_iPtr = 0U; + } + + return true; + } + + bool getData(T* buffer, unsigned int nSamples) + { + if (dataSize() < nSamples) { + LogError("**** Underflow in %s ring buffer, %u < %u", m_name, dataSize(), nSamples); + return false; + } + + for (unsigned int i = 0U; i < nSamples; i++) { + buffer[i] = m_buffer[m_oPtr++]; + + if (m_oPtr == m_length) + m_oPtr = 0U; + } + + return true; + } + + bool peek(T* buffer, unsigned int nSamples) + { + if (dataSize() < nSamples) { + LogError("**** Underflow peek in %s ring buffer, %u < %u", m_name, dataSize(), nSamples); + return false; + } + + unsigned int ptr = m_oPtr; + for (unsigned int i = 0U; i < nSamples; i++) { + buffer[i] = m_buffer[ptr++]; + + if (ptr == m_length) + ptr = 0U; + } + + return true; + } + + void clear() + { + m_iPtr = 0U; + m_oPtr = 0U; + + ::memset(m_buffer, 0x00, m_length * sizeof(T)); + } + + unsigned int freeSpace() const + { + unsigned int len = m_length; + + if (m_oPtr > m_iPtr) + len = m_oPtr - m_iPtr; + else if (m_iPtr > m_oPtr) + len = m_length - (m_iPtr - m_oPtr); + + if (len > m_length) + len = 0U; + + return len; + } + + unsigned int dataSize() const + { + return m_length - freeSpace(); + } + + bool hasSpace(unsigned int length) const + { + return freeSpace() > length; + } + + bool hasData() const + { + return m_oPtr != m_iPtr; + } + + bool isEmpty() const + { + return m_oPtr == m_iPtr; + } + +private: + unsigned int m_length; + const char* m_name; + T* m_buffer; + unsigned int m_iPtr; + unsigned int m_oPtr; +}; + +#endif diff --git a/USRP2M17/StopWatch.cpp b/USRP2M17/StopWatch.cpp new file mode 100644 index 0000000..481241b --- /dev/null +++ b/USRP2M17/StopWatch.cpp @@ -0,0 +1,105 @@ +/* + * Copyright (C) 2015,2016,2018 by Jonathan Naylor G4KLX + * + * This program 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 2 of the License, or + * (at your option) any later version. + * + * This program 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 this program; if not, write to the Free Software + * Foundation, Inc., 675 Mass Ave, Cambridge, MA 02139, USA. + */ + +#include "StopWatch.h" + +#if defined(_WIN32) || defined(_WIN64) + +CStopWatch::CStopWatch() : +m_frequencyS(), +m_frequencyMS(), +m_start() +{ + ::QueryPerformanceFrequency(&m_frequencyS); + + m_frequencyMS.QuadPart = m_frequencyS.QuadPart / 1000ULL; +} + +CStopWatch::~CStopWatch() +{ +} + +unsigned long long CStopWatch::time() const +{ + LARGE_INTEGER now; + ::QueryPerformanceCounter(&now); + + return (unsigned long long)(now.QuadPart / m_frequencyMS.QuadPart); +} + +unsigned long long CStopWatch::start() +{ + ::QueryPerformanceCounter(&m_start); + + return (unsigned long long)(m_start.QuadPart / m_frequencyS.QuadPart); +} + +unsigned int CStopWatch::elapsed() +{ + LARGE_INTEGER now; + ::QueryPerformanceCounter(&now); + + LARGE_INTEGER temp; + temp.QuadPart = (now.QuadPart - m_start.QuadPart) * 1000; + + return (unsigned int)(temp.QuadPart / m_frequencyS.QuadPart); +} + +#else + +#include +#include + +CStopWatch::CStopWatch() : +m_startMS(0ULL) +{ +} + +CStopWatch::~CStopWatch() +{ +} + +unsigned long long CStopWatch::time() const +{ + struct timeval now; + ::gettimeofday(&now, NULL); + + return now.tv_sec * 1000ULL + now.tv_usec / 1000ULL; +} + +unsigned long long CStopWatch::start() +{ + struct timespec now; + ::clock_gettime(CLOCK_MONOTONIC, &now); + + m_startMS = now.tv_sec * 1000ULL + now.tv_nsec / 1000000ULL; + + return m_startMS; +} + +unsigned int CStopWatch::elapsed() +{ + struct timespec now; + ::clock_gettime(CLOCK_MONOTONIC, &now); + + unsigned long long nowMS = now.tv_sec * 1000ULL + now.tv_nsec / 1000000ULL; + + return nowMS - m_startMS; +} + +#endif diff --git a/USRP2M17/StopWatch.h b/USRP2M17/StopWatch.h new file mode 100644 index 0000000..3f8fa19 --- /dev/null +++ b/USRP2M17/StopWatch.h @@ -0,0 +1,49 @@ +/* + * Copyright (C) 2015,2016,2018 by Jonathan Naylor G4KLX + * + * This program 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 2 of the License, or + * (at your option) any later version. + * + * This program 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 this program; if not, write to the Free Software + * Foundation, Inc., 675 Mass Ave, Cambridge, MA 02139, USA. + */ + +#if !defined(STOPWATCH_H) +#define STOPWATCH_H + +#if defined(_WIN32) || defined(_WIN64) +#include +#else +#include +#endif + +class CStopWatch +{ +public: + CStopWatch(); + ~CStopWatch(); + + unsigned long long time() const; + + unsigned long long start(); + unsigned int elapsed(); + +private: +#if defined(_WIN32) || defined(_WIN64) + LARGE_INTEGER m_frequencyS; + LARGE_INTEGER m_frequencyMS; + LARGE_INTEGER m_start; +#else + unsigned long long m_startMS; +#endif +}; + +#endif diff --git a/USRP2M17/Timer.cpp b/USRP2M17/Timer.cpp new file mode 100644 index 0000000..53956e4 --- /dev/null +++ b/USRP2M17/Timer.cpp @@ -0,0 +1,68 @@ +/* + * Copyright (C) 2009,2010,2015 by Jonathan Naylor G4KLX + * + * This program 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 2 of the License, or + * (at your option) any later version. + * + * This program 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 this program; if not, write to the Free Software + * Foundation, Inc., 675 Mass Ave, Cambridge, MA 02139, USA. + */ + +#include "Timer.h" + +#include +#include + +CTimer::CTimer(unsigned int ticksPerSec, unsigned int secs, unsigned int msecs) : +m_ticksPerSec(ticksPerSec), +m_timeout(0U), +m_timer(0U) +{ + assert(ticksPerSec > 0U); + + if (secs > 0U || msecs > 0U) { + // m_timeout = ((secs * 1000U + msecs) * m_ticksPerSec) / 1000U + 1U; + unsigned long long temp = (secs * 1000ULL + msecs) * m_ticksPerSec; + m_timeout = (unsigned int)(temp / 1000ULL + 1ULL); + } +} + +CTimer::~CTimer() +{ +} + +void CTimer::setTimeout(unsigned int secs, unsigned int msecs) +{ + if (secs > 0U || msecs > 0U) { + // m_timeout = ((secs * 1000U + msecs) * m_ticksPerSec) / 1000U + 1U; + unsigned long long temp = (secs * 1000ULL + msecs) * m_ticksPerSec; + m_timeout = (unsigned int)(temp / 1000ULL + 1ULL); + } else { + m_timeout = 0U; + m_timer = 0U; + } +} + +unsigned int CTimer::getTimeout() const +{ + if (m_timeout == 0U) + return 0U; + + return (m_timeout - 1U) / m_ticksPerSec; +} + +unsigned int CTimer::getTimer() const +{ + if (m_timer == 0U) + return 0U; + + return (m_timer - 1U) / m_ticksPerSec; +} diff --git a/USRP2M17/Timer.h b/USRP2M17/Timer.h new file mode 100644 index 0000000..87d68f5 --- /dev/null +++ b/USRP2M17/Timer.h @@ -0,0 +1,89 @@ +/* + * Copyright (C) 2009,2010,2011,2014 by Jonathan Naylor G4KLX + * + * This program 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 2 of the License, or + * (at your option) any later version. + * + * This program 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 this program; if not, write to the Free Software + * Foundation, Inc., 675 Mass Ave, Cambridge, MA 02139, USA. + */ + +#ifndef Timer_H +#define Timer_H + +class CTimer { +public: + CTimer(unsigned int ticksPerSec, unsigned int secs = 0U, unsigned int msecs = 0U); + ~CTimer(); + + void setTimeout(unsigned int secs, unsigned int msecs = 0U); + + unsigned int getTimeout() const; + unsigned int getTimer() const; + + unsigned int getRemaining() + { + if (m_timeout == 0U || m_timer == 0U) + return 0U; + + if (m_timer >= m_timeout) + return 0U; + + return (m_timeout - m_timer) / m_ticksPerSec; + } + + bool isRunning() + { + return m_timer > 0U; + } + + void start(unsigned int secs, unsigned int msecs = 0U) + { + setTimeout(secs, msecs); + + start(); + } + + void start() + { + if (m_timeout > 0U) + m_timer = 1U; + } + + void stop() + { + m_timer = 0U; + } + + bool hasExpired() + { + if (m_timeout == 0U || m_timer == 0U) + return false; + + if (m_timer >= m_timeout) + return true; + + return false; + } + + void clock(unsigned int ticks = 1U) + { + if (m_timer > 0U && m_timeout > 0U) + m_timer += ticks; + } + +private: + unsigned int m_ticksPerSec; + unsigned int m_timeout; + unsigned int m_timer; +}; + +#endif diff --git a/USRP2M17/UDPSocket.cpp b/USRP2M17/UDPSocket.cpp new file mode 100644 index 0000000..8d88c62 --- /dev/null +++ b/USRP2M17/UDPSocket.cpp @@ -0,0 +1,212 @@ +/* + * Copyright (C) 2006-2016 by Jonathan Naylor G4KLX + * + * This program 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 2 of the License, or + * (at your option) any later version. + * + * This program 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 this program; if not, write to the Free Software + * Foundation, Inc., 675 Mass Ave, Cambridge, MA 02139, USA. + */ + +#include "UDPSocket.h" +#include "Log.h" +#include +#include +#include + + +CUDPSocket::CUDPSocket(const std::string& address, unsigned int port) : +m_address(address), +m_port(port), +m_fd(-1) +{ + assert(!address.empty()); +} + +CUDPSocket::CUDPSocket(unsigned int port) : +m_address(), +m_port(port), +m_fd(-1) +{ + +} + +CUDPSocket::~CUDPSocket() +{ + +} + +in_addr CUDPSocket::lookup(const std::string& hostname) +{ + in_addr addr; + + in_addr_t address = ::inet_addr(hostname.c_str()); + if (address != in_addr_t(-1)) { + addr.s_addr = address; + LogInfo("inet_addr() returns %x", addr.s_addr); + return addr; + } + + struct hostent* hp = ::gethostbyname(hostname.c_str()); + if (hp != NULL) { + ::memcpy(&addr, hp->h_addr_list[0], sizeof(struct in_addr)); + LogInfo("gethostbyname() returns %x", addr.s_addr); + return addr; + } + + LogError("Cannot find address for host %s", hostname.c_str()); + + addr.s_addr = INADDR_NONE; + return addr; +} + +bool CUDPSocket::open() +{ + m_fd = ::socket(AF_INET, SOCK_DGRAM, 0); + if (m_fd < 0) { + + LogError("Cannot create the UDP socket, err: %d", errno); + return false; + } + + if (m_port > 0U) { + sockaddr_in addr; + ::memset(&addr, 0x00, sizeof(sockaddr_in)); + addr.sin_family = AF_INET; + addr.sin_port = htons(m_port); + addr.sin_addr.s_addr = htonl(INADDR_ANY); + + if (!m_address.empty()) { + + addr.sin_addr.s_addr = ::inet_addr(m_address.c_str()); + + if (addr.sin_addr.s_addr == INADDR_NONE) { + LogError("The local address is invalid - %s", m_address.c_str()); + return false; + } + } + + int reuse = 1; + if (::setsockopt(m_fd, SOL_SOCKET, SO_REUSEADDR, (char *)&reuse, sizeof(reuse)) == -1) { + + LogError("Cannot set the UDP socket option, err: %d", errno); + + return false; + } + + if (::bind(m_fd, (sockaddr*)&addr, sizeof(sockaddr_in)) == -1) { + + LogError("Cannot bind the UDP address, err: %d", errno); + + return false; + } + } + + return true; +} + +int CUDPSocket::read(unsigned char* buffer, unsigned int length, in_addr& address, unsigned int& port) +{ + assert(buffer != NULL); + assert(length > 0U); + + // Check that the readfrom() won't block + fd_set readFds; + FD_ZERO(&readFds); +#if defined(_WIN32) || defined(_WIN64) + FD_SET((unsigned int)m_fd, &readFds); +#else + FD_SET(m_fd, &readFds); +#endif + + // Return immediately + timeval tv; + tv.tv_sec = 0L; + tv.tv_usec = 0L; + + int ret = ::select(m_fd + 1, &readFds, NULL, NULL, &tv); + if (ret < 0) { +#if defined(_WIN32) || defined(_WIN64) + LogError("Error returned from UDP select, err: %lu", ::GetLastError()); +#else + LogError("Error returned from UDP select, err: %d", errno); +#endif + return -1; + } + + if (ret == 0) + return 0; + + sockaddr_in addr; +#if defined(_WIN32) || defined(_WIN64) + int size = sizeof(sockaddr_in); +#else + socklen_t size = sizeof(sockaddr_in); +#endif + +#if defined(_WIN32) || defined(_WIN64) + int len = ::recvfrom(m_fd, (char*)buffer, length, 0, (sockaddr *)&addr, &size); +#else + ssize_t len = ::recvfrom(m_fd, (char*)buffer, length, 0, (sockaddr *)&addr, &size); +#endif + if (len <= 0) { +#if defined(_WIN32) || defined(_WIN64) + LogError("Error returned from recvfrom, err: %lu", ::GetLastError()); +#else + LogError("Error returned from recvfrom, err: %d", errno); +#endif + return -1; + } + + address = addr.sin_addr; + port = ntohs(addr.sin_port); + + return len; +} + +bool CUDPSocket::write(const unsigned char* buffer, unsigned int length, const in_addr& address, unsigned int port) +{ + assert(buffer != NULL); + assert(length > 0U); + + sockaddr_in addr; + ::memset(&addr, 0x00, sizeof(sockaddr_in)); + + addr.sin_family = AF_INET; + addr.sin_addr = address; + addr.sin_port = htons(port); + + + ssize_t ret = ::sendto(m_fd, (char *)buffer, length, 0, (sockaddr *)&addr, sizeof(sockaddr_in)); + + if (ret < 0) { + + LogError("Error returned from sendto, err: %d", errno); + + return false; + } + + + if (ret != ssize_t(length)) + return false; + + + return true; +} + +void CUDPSocket::close() +{ +#if defined(_WIN32) || defined(_WIN64) + ::closesocket(m_fd); +#else + ::close(m_fd); +#endif +} diff --git a/USRP2M17/UDPSocket.h b/USRP2M17/UDPSocket.h new file mode 100644 index 0000000..c68c11e --- /dev/null +++ b/USRP2M17/UDPSocket.h @@ -0,0 +1,54 @@ +/* + * Copyright (C) 2009-2011,2013,2015,2016 by Jonathan Naylor G4KLX + * + * This program 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 2 of the License, or + * (at your option) any later version. + * + * This program 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 this program; if not, write to the Free Software + * Foundation, Inc., 675 Mass Ave, Cambridge, MA 02139, USA. + */ + +#ifndef UDPSocket_H +#define UDPSocket_H + +#include + +#include +#include +#include +#include +#include +#include +#include +#include + +class CUDPSocket { +public: + CUDPSocket(const std::string& address, unsigned int port = 0U); + CUDPSocket(unsigned int port = 0U); + ~CUDPSocket(); + + bool open(); + + int read(unsigned char* buffer, unsigned int length, in_addr& address, unsigned int& port); + bool write(const unsigned char* buffer, unsigned int length, const in_addr& address, unsigned int port); + + void close(); + + static in_addr lookup(const std::string& hostName); + +private: + std::string m_address; + unsigned short m_port; + int m_fd; +}; + +#endif diff --git a/USRP2M17/USRP2M17.cpp b/USRP2M17/USRP2M17.cpp new file mode 100644 index 0000000..950343a --- /dev/null +++ b/USRP2M17/USRP2M17.cpp @@ -0,0 +1,438 @@ +/* +* Copyright (C) 2021 by Doug McLain AD8DP +* +* This program 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 2 of the License, or +* (at your option) any later version. +* +* This program 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 this program; if not, write to the Free Software +* Foundation, Inc., 675 Mass Ave, Cambridge, MA 02139, USA. +*/ + +#include "USRP2M17.h" +#include +#include +#include +#include +#include + +#define USRP_FRAME_PER 15U +#define M17_FRAME_PER 35U + +const char* DEFAULT_INI_FILE = "/etc/USRP2M17.ini"; + +const char* HEADER1 = "This software is for use on amateur radio networks only,"; +const char* HEADER2 = "it is to be used for educational purposes only. Its use on"; +const char* HEADER3 = "commercial networks is strictly prohibited."; +const char* HEADER4 = "Copyright(C) 2021 by AD8DP"; + +#define M17CHARACTERS " ABCDEFGHIJKLMNOPQRSTUVWXYZ0123456789-/." + +#include +#include +#include +#include +#include +#include +#include + +static bool m_killed = false; + +void sig_handler(int signo) +{ + if (signo == SIGTERM) { + m_killed = true; + ::fprintf(stdout, "Received SIGTERM\n"); + } +} + +void encode_callsign(uint8_t *callsign) +{ + const std::string m17_alphabet(M17CHARACTERS); + char cs[10]; + memset(cs, 0, sizeof(cs)); + memcpy(cs, callsign, strlen((char *)callsign)); + uint64_t encoded = 0; + for(int i = std::strlen((char *)callsign)-1; i >= 0; i--) { + auto pos = m17_alphabet.find(cs[i]); + if (pos == std::string::npos) { + pos = 0; + } + encoded *= 40; + encoded += pos; + } + for (int i=0; i<6; i++) { + callsign[i] = (encoded >> (8*(5-i)) & 0xFFU); + } +} + +void decode_callsign(uint8_t *callsign) +{ + const std::string m17_alphabet(M17CHARACTERS); + uint8_t code[6]; + uint64_t coded = callsign[0]; + for (int i=1; i<6; i++) + coded = (coded << 8) | callsign[i]; + if (coded > 0xee6b27ffffffu) { + //std::cerr << "Callsign code is too large, 0x" << std::hex << coded << std::endl; + return; + } + memcpy(code, callsign, 6); + memset(callsign, 0, 10); + int i = 0; + while (coded) { + callsign[i++] = m17_alphabet[coded % 40]; + coded /= 40; + } +} + +int main(int argc, char** argv) +{ + const char* iniFile = DEFAULT_INI_FILE; + if (argc > 1) { + for (int currentArg = 1; currentArg < argc; ++currentArg) { + std::string arg = argv[currentArg]; + if ((arg == "-v") || (arg == "--version")) { + ::fprintf(stdout, "USRP2M17 version %s\n", VERSION); + return 0; + } else if (arg.substr(0, 1) == "-") { + ::fprintf(stderr, "Usage: USRP2M17 [-v|--version] [filename]\n"); + return 1; + } else { + iniFile = argv[currentArg]; + } + } + } + + // Capture SIGTERM to finish gracelessly + if (signal(SIGTERM, sig_handler) == SIG_ERR) + ::fprintf(stdout, "Can't catch SIGTERM\n"); + + CUSRP2M17* gateway = new CUSRP2M17(std::string(iniFile)); + + int ret = gateway->run(); + + delete gateway; + + return ret; +} + +CUSRP2M17::CUSRP2M17(const std::string& configFile) : +m_callsign(), +m_m17Ref(), +m_conf(configFile), +m_usrpNetwork(NULL), +m_m17Network(NULL), +m_conv(), +m_m17Src(), +m_m17Dst(), +m_m17Frame(NULL), +m_m17Frames(0U), +m_usrpFrame(NULL), +m_usrpFrames(0U) +{ + m_m17Frame = new uint8_t[100U]; + m_usrpFrame = new uint8_t[400U]; + + ::memset(m_m17Frame, 0U, 100U); + ::memset(m_usrpFrame, 0U, 400U); +} + +CUSRP2M17::~CUSRP2M17() +{ + delete[] m_m17Frame; + delete[] m_usrpFrame; +} + +int CUSRP2M17::run() +{ + bool ret = m_conf.read(); + if (!ret) { + ::fprintf(stderr, "USRP2M17: cannot read the .ini file\n"); + return 1; + } + + //setlocale(LC_ALL, "C"); + + uint32_t logDisplayLevel = m_conf.getLogDisplayLevel(); + + if(m_conf.getDaemon()) + logDisplayLevel = 0U; + + bool m_daemon = m_conf.getDaemon(); + if (m_daemon) { + // Create new process + pid_t pid = ::fork(); + if (pid == -1) { + ::fprintf(stderr, "Couldn't fork() , exiting\n"); + return -1; + } else if (pid != 0) + exit(EXIT_SUCCESS); + + // Create new session and process group + if (::setsid() == -1) { + ::fprintf(stderr, "Couldn't setsid(), exiting\n"); + return -1; + } + + // Set the working directory to the root directory + if (::chdir("/") == -1) { + ::fprintf(stderr, "Couldn't cd /, exiting\n"); + return -1; + } + + // If we are currently root... + if (getuid() == 0) { + struct passwd* user = ::getpwnam("mmdvm"); + if (user == NULL) { + ::fprintf(stderr, "Could not get the mmdvm user, exiting\n"); + return -1; + } + + uid_t mmdvm_uid = user->pw_uid; + gid_t mmdvm_gid = user->pw_gid; + + // Set user and group ID's to mmdvm:mmdvm + if (setgid(mmdvm_gid) != 0) { + ::fprintf(stderr, "Could not set mmdvm GID, exiting\n"); + return -1; + } + + if (setuid(mmdvm_uid) != 0) { + ::fprintf(stderr, "Could not set mmdvm UID, exiting\n"); + return -1; + } + + // Double check it worked (AKA Paranoia) + if (setuid(0) != -1) { + ::fprintf(stderr, "It's possible to regain root - something is wrong!, exiting\n"); + return -1; + } + } + } + + ret = ::LogInitialise(m_conf.getLogFilePath(), m_conf.getLogFileRoot(), m_conf.getLogFileLevel(), logDisplayLevel); + if (!ret) { + ::fprintf(stderr, "USRP2M17: unable to open the log file\n"); + return 1; + } + + if (m_daemon) { + ::close(STDIN_FILENO); + ::close(STDOUT_FILENO); + ::close(STDERR_FILENO); + } + + LogInfo(HEADER1); + LogInfo(HEADER2); + LogInfo(HEADER3); + LogInfo(HEADER4); + + m_callsign = m_conf.getCallsign(); + m_m17Ref = m_conf.getM17Name(); + char module = m_m17Ref.c_str()[m_m17Ref.find(' ')+1]; + + std::string m17_address = m_conf.getM17Address(); + uint16_t m17_dstPort = m_conf.getM17DstPort(); + uint16_t m17_localPort = m_conf.getM17LocalPort(); + bool m17_debug = m_conf.getM17Debug(); + + m_conv.setM17GainAdjDb(m_conf.getM17GainAdjDb()); + + uint16_t streamid = 0; + uint8_t m17_src[10]; + uint8_t m17_dst[10]; + + memcpy(m17_src, m_callsign.c_str(), 9); + m17_src[9] = 0x00; + encode_callsign(m17_src); + + m_m17Network = new CM17Network(m17_address, m17_dstPort, m17_localPort, m17_src, m17_debug); + + ret = m_m17Network->open(); + if (!ret) { + ::LogError("Cannot open the M17 network port"); + ::LogFinalise(); + return 1; + } + + std::string usrp_address = m_conf.getUSRPAddress(); + uint16_t usrp_dstPort = m_conf.getUSRPDstPort(); + uint16_t usrp_localPort = m_conf.getUSRPLocalPort(); + bool usrp_debug = m_conf.getUSRPDebug(); + + m_conv.setUSRPGainAdjDb(m_conf.getUSRPGainAdjDb()); + + m_usrpNetwork = new CUSRPNetwork(usrp_address, usrp_dstPort, usrp_localPort, usrp_debug); + + ret = m_usrpNetwork->open(); + if (!ret) { + ::LogError("Cannot open the USRP network port"); + ::LogFinalise(); + return 1; + } + + CTimer networkWatchdog(100U, 0U, 1500U); + CTimer pollTimer(1000U, 8U); + CStopWatch stopWatch; + CStopWatch m17Watch; + CStopWatch usrpWatch; + + pollTimer.start(); + stopWatch.start(); + m17Watch.start(); + usrpWatch.start(); + + uint16_t m17_cnt = 0; + uint32_t usrp_cnt = 0; + + m_m17Network->writeLink(module); + + LogMessage("Starting USRP2M17-%s", VERSION); + + for (; m_killed == 0;) { + uint8_t buffer[2000U]; + memset(buffer, 0, sizeof(buffer)); + + uint32_t ms = stopWatch.elapsed(); + + if (m17Watch.elapsed() > M17_FRAME_PER) { + uint32_t m17FrameType = m_conv.getM17(m_m17Frame); + + if(m17FrameType == TAG_HEADER) { + m17_cnt = 0U; + m17Watch.start(); + + streamid = static_cast((::rand() & 0xFFFF)); + memcpy(m17_dst, m_m17Ref.c_str(), m_m17Ref.size()); + m17_dst[9] = 0x00; + encode_callsign(m17_dst); + + memcpy(buffer, "M17 ", 4); + memcpy(buffer+4, &streamid, 2); + memcpy(buffer+6, m17_dst, 6); + memcpy(buffer+12, m17_src, 6); + buffer[19] = 0x05; + memcpy(buffer+36, m_m17Frame, 16); + m_m17Network->writeData(buffer, 54U); + } + else if(m17FrameType == TAG_EOT) { + m17_cnt |= 0x8000; + memcpy(buffer, "M17 ", 4); + memcpy(buffer+4, &streamid, 2); + memcpy(buffer+6, m17_dst, 6); + memcpy(buffer+12, m17_src, 6); + buffer[19] = 0x05; + buffer[34] = m17_cnt >> 8; + buffer[35] = m17_cnt & 0xff; + memcpy(buffer+36, m_m17Frame, 16); + m_m17Network->writeData(buffer, 54U); + m17Watch.start(); + } + else if(m17FrameType == TAG_DATA) { + //CUtils::dump(1U, "M17 Data", m_p25Frame, 11U); + m17_cnt++; + memcpy(buffer, "M17 ", 4); + memcpy(buffer+4, &streamid, 2); + memcpy(buffer+6, m17_dst, 6); + memcpy(buffer+12, m17_src, 6); + buffer[19] = 0x05; + buffer[34] = m17_cnt >> 8; + buffer[35] = m17_cnt & 0xff; + memcpy(buffer+36, m_m17Frame, 16); + m_m17Network->writeData(buffer, 54U); + m17Watch.start(); + } + } + + while (m_m17Network->readData(m_m17Frame, 54U) > 0U) { + //CUtils::dump(1U, "M17 Data", m_p25Frame, 22U); + if (!memcmp(m_m17Frame, "M17 ", 4)) { + if (m_m17Frame[34] == 0 && m_m17Frame[35] == 0) { + m_m17Frames = 0; + } + else if (m_m17Frame[34U] & 0x80U) { + LogMessage("M17 received end of voice transmission, %.1f seconds", float(m_m17Frames) / 25.0F); + } + else{ + m_conv.putM17(m_m17Frame); + } + uint8_t cs[10]; + memcpy(cs, m_m17Frame+12, 6); + decode_callsign(cs); + std::string css((char *)cs); + css = css.substr(0, css.find(' ')); + + m_m17Frames++; + } + } + + if (usrpWatch.elapsed() > USRP_FRAME_PER) { + int16_t pcm[160]; + if(m_conv.getUSRP(pcm)){ + //CUtils::dump(1U, "USRP data:", m_usrpFrame, 33U); + const uint32_t cnt = htonl(usrp_cnt); + memcpy(m_usrpFrame, "USRP", 4); + memset(m_usrpFrame+4, 0, 28); + memcpy(m_usrpFrame+4, &cnt, 4); + m_usrpFrame[15] = 1; + + for(int i = 0; i < 320; i+=2){ + m_usrpFrame[32+i] = pcm[(i/2)] & 0xff; + m_usrpFrame[32+i+1] = pcm[(i/2)] >> 8; + } + + m_usrpNetwork->writeData(m_usrpFrame, 352); + usrp_cnt++; + usrpWatch.start(); + } + } + uint32_t len = 0; + while ( (len = m_usrpNetwork->readData(m_usrpFrame, 400)) ) { + if((len == 32) && !memcmp(m_usrpFrame, "USRP", 4)) { + LogMessage("USRP received end of voice transmission, %.1f seconds", float(m_usrpFrames) / 50.0F); + m_conv.putUSRPEOT(); + m_usrpFrames = 0U; + } + + if( (!memcmp(m_usrpFrame, "USRP", 4)) && len == 352) { + if(!m_usrpFrames){ + m_conv.putUSRPHeader(); + LogMessage("USRP first frame received"); + } + int16_t pcm[160]; + for(int i = 0; i < 160; ++i){ + pcm[i] = (m_usrpFrame[32+(i*2)+1] << 8) | m_usrpFrame[32+(i*2)]; + } + m_conv.putUSRP(pcm); + m_usrpFrames++; + } + } + + stopWatch.start(); + pollTimer.clock(ms); + + if (pollTimer.isRunning() && pollTimer.hasExpired()) { + m_m17Network->writePoll(); + pollTimer.start(); + } + + if (ms < 5U) ::usleep(5U * 1000U); + } + + m_m17Network->close(); + m_usrpNetwork->close(); + delete m_usrpNetwork; + delete m_m17Network; + + ::LogFinalise(); + + return 0; +} diff --git a/USRP2M17/USRP2M17.h b/USRP2M17/USRP2M17.h new file mode 100644 index 0000000..5cbe2ec --- /dev/null +++ b/USRP2M17/USRP2M17.h @@ -0,0 +1,58 @@ +/* +* Copyright (C) 2021 by Doug McLain AD8DP +* +* This program 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 2 of the License, or +* (at your option) any later version. +* +* This program 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 this program; if not, write to the Free Software +* Foundation, Inc., 675 Mass Ave, Cambridge, MA 02139, USA. +*/ + +#if !defined(USRP2M17_H) +#define USRP2M17_H + +#include "ModeConv.h" +#include "USRPNetwork.h" +#include "M17Network.h" +#include "UDPSocket.h" +#include "StopWatch.h" +#include "Version.h" +#include "Timer.h" +#include "Utils.h" +#include "Conf.h" +#include "Log.h" + +#include + +class CUSRP2M17 +{ +public: + CUSRP2M17(const std::string& configFile); + ~CUSRP2M17(); + + int run(); + +private: + std::string m_callsign; + std::string m_m17Ref; + CConf m_conf; + CUSRPNetwork* m_usrpNetwork; + CM17Network* m_m17Network; + CModeConv m_conv; + std::string m_m17Src; + std::string m_m17Dst; + uint8_t* m_m17Frame; + uint32_t m_m17Frames; + uint8_t* m_usrpFrame; + uint32_t m_usrpFrames; +}; + +#endif diff --git a/USRP2M17/USRP2M17.ini b/USRP2M17/USRP2M17.ini new file mode 100644 index 0000000..82c09e5 --- /dev/null +++ b/USRP2M17/USRP2M17.ini @@ -0,0 +1,24 @@ +[M17 Network] +Callsign=AD8DP D +Address=51.81.119.111 +Name=M17-M17 C +LocalPort=32010 +DstPort=17000 +GainAdjustdB=3 +Daemon=0 +Debug=1 + +[USRP Network] +Address=127.0.0.1 +DstPort=32001 +LocalPort=34001 +GainAdjustdB=-6 +Debug=1 + +[Log] +# Logging levels, 0=No logging +DisplayLevel=1 +FileLevel=1 +FilePath=. +FileRoot=USRP2M17 + diff --git a/USRP2M17/USRPNetwork.cpp b/USRP2M17/USRPNetwork.cpp new file mode 100644 index 0000000..dd3cdb9 --- /dev/null +++ b/USRP2M17/USRPNetwork.cpp @@ -0,0 +1,78 @@ +/* + * Copyright (C) 2015,2016,2017 by Jonathan Naylor G4KLX + * Copyright (C) 2021 by Doug McLain AD8DP + * + * This program 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 2 of the License, or + * (at your option) any later version. + * + * This program 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 this program; if not, write to the Free Software + * Foundation, Inc., 675 Mass Ave, Cambridge, MA 02139, USA. + */ + +#include "USRPNetwork.h" +#include "StopWatch.h" +#include "Utils.h" +#include "Log.h" + +#include + +CUSRPNetwork::CUSRPNetwork(const std::string& address, uint16_t dstPort, uint16_t localPort, bool debug) : +m_address(), +m_port(dstPort), +m_socket(localPort), +m_debug(debug) +{ + m_address = CUDPSocket::lookup(address); + CStopWatch stopWatch; +} + +CUSRPNetwork::~CUSRPNetwork() +{ +} + +bool CUSRPNetwork::open() +{ + LogMessage("USRP Network, Opening"); + return m_socket.open(); +} + +void CUSRPNetwork::close() +{ + m_socket.close(); +} + +uint32_t CUSRPNetwork::readData(uint8_t* data, uint32_t length) +{ + in_addr address; + unsigned int port; + int len = m_socket.read(data, length, address, port); + if (len <= 0) + return 0U; + + // Check if the data is for us + if (m_address.s_addr != address.s_addr || port != m_port) { + LogMessage("USRP packet received from an invalid source, %08X != %08X and/or %u != %u", m_address.s_addr, address.s_addr, m_port, port); + return 0U; + } + + if (m_debug) + CUtils::dump(1U, "USRP Network Data Received", data, len); + + return len; +} + +bool CUSRPNetwork::writeData(const uint8_t* data, uint32_t length) +{ + if (m_debug) + CUtils::dump(1U, "USRP Network Data Sent", data, length); + + return m_socket.write(data, length, m_address, m_port); +} diff --git a/USRP2M17/USRPNetwork.h b/USRP2M17/USRPNetwork.h new file mode 100644 index 0000000..01aad89 --- /dev/null +++ b/USRP2M17/USRPNetwork.h @@ -0,0 +1,47 @@ +/* + * Copyright (C) 2015,2016,2017 by Jonathan Naylor G4KLX + * Copyright (C) 2021 by Doug McLain AD8DP + * + * This program 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 2 of the License, or + * (at your option) any later version. + * + * This program 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 this program; if not, write to the Free Software + * Foundation, Inc., 675 Mass Ave, Cambridge, MA 02139, USA. + */ + +#if !defined(USRPNetwork_H) +#define USRPNetwork_H + +#include "UDPSocket.h" +#include "Timer.h" + +#include +#include + +class CUSRPNetwork +{ +public: + CUSRPNetwork(const std::string& address, uint16_t dstPort, uint16_t localPort, bool debug); + ~CUSRPNetwork(); + + uint32_t getConfig(uint8_t* config) const; + bool open(); + bool writeData(const uint8_t* data, uint32_t length); + uint32_t readData(uint8_t* data, uint32_t length); + void close(); +private: + in_addr m_address; + uint16_t m_port; + CUDPSocket m_socket; + bool m_debug; +}; + +#endif diff --git a/USRP2M17/Utils.cpp b/USRP2M17/Utils.cpp new file mode 100644 index 0000000..49ded13 --- /dev/null +++ b/USRP2M17/Utils.cpp @@ -0,0 +1,146 @@ +/* + * Copyright (C) 2009,2014,2015,2016 Jonathan Naylor, G4KLX + * + * This program 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; version 2 of the License. + * + * This program 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. + */ + +#include "Utils.h" +#include "Log.h" + +#include +#include + +void CUtils::dump(const std::string& title, const unsigned char* data, unsigned int length) +{ + assert(data != NULL); + + dump(2U, title, data, length); +} + +void CUtils::dump(int level, const std::string& title, const unsigned char* data, unsigned int length) +{ + assert(data != NULL); + + ::Log(level, "%s", title.c_str()); + + unsigned int offset = 0U; + + while (length > 0U) { + std::string output; + + unsigned int bytes = (length > 16U) ? 16U : length; + + for (unsigned i = 0U; i < bytes; i++) { + char temp[10U]; + ::sprintf(temp, "%02X ", data[offset + i]); + output += temp; + } + + for (unsigned int i = bytes; i < 16U; i++) + output += " "; + + output += " *"; + + for (unsigned i = 0U; i < bytes; i++) { + unsigned char c = data[offset + i]; + + if (::isprint(c)) + output += c; + else + output += '.'; + } + + output += '*'; + + ::Log(level, "%04X: %s", offset, output.c_str()); + + offset += 16U; + + if (length >= 16U) + length -= 16U; + else + length = 0U; + } +} + +void CUtils::dump(const std::string& title, const bool* bits, unsigned int length) +{ + assert(bits != NULL); + + dump(2U, title, bits, length); +} + +void CUtils::dump(int level, const std::string& title, const bool* bits, unsigned int length) +{ + assert(bits != NULL); + + unsigned char bytes[100U]; + unsigned int nBytes = 0U; + for (unsigned int n = 0U; n < length; n += 8U, nBytes++) + bitsToByteBE(bits + n, bytes[nBytes]); + + dump(level, title, bytes, nBytes); +} + +void CUtils::byteToBitsBE(unsigned char byte, bool* bits) +{ + assert(bits != NULL); + + bits[0U] = (byte & 0x80U) == 0x80U; + bits[1U] = (byte & 0x40U) == 0x40U; + bits[2U] = (byte & 0x20U) == 0x20U; + bits[3U] = (byte & 0x10U) == 0x10U; + bits[4U] = (byte & 0x08U) == 0x08U; + bits[5U] = (byte & 0x04U) == 0x04U; + bits[6U] = (byte & 0x02U) == 0x02U; + bits[7U] = (byte & 0x01U) == 0x01U; +} + +void CUtils::byteToBitsLE(unsigned char byte, bool* bits) +{ + assert(bits != NULL); + + bits[0U] = (byte & 0x01U) == 0x01U; + bits[1U] = (byte & 0x02U) == 0x02U; + bits[2U] = (byte & 0x04U) == 0x04U; + bits[3U] = (byte & 0x08U) == 0x08U; + bits[4U] = (byte & 0x10U) == 0x10U; + bits[5U] = (byte & 0x20U) == 0x20U; + bits[6U] = (byte & 0x40U) == 0x40U; + bits[7U] = (byte & 0x80U) == 0x80U; +} + +void CUtils::bitsToByteBE(const bool* bits, unsigned char& byte) +{ + assert(bits != NULL); + + byte = bits[0U] ? 0x80U : 0x00U; + byte |= bits[1U] ? 0x40U : 0x00U; + byte |= bits[2U] ? 0x20U : 0x00U; + byte |= bits[3U] ? 0x10U : 0x00U; + byte |= bits[4U] ? 0x08U : 0x00U; + byte |= bits[5U] ? 0x04U : 0x00U; + byte |= bits[6U] ? 0x02U : 0x00U; + byte |= bits[7U] ? 0x01U : 0x00U; +} + +void CUtils::bitsToByteLE(const bool* bits, unsigned char& byte) +{ + assert(bits != NULL); + + byte = bits[0U] ? 0x01U : 0x00U; + byte |= bits[1U] ? 0x02U : 0x00U; + byte |= bits[2U] ? 0x04U : 0x00U; + byte |= bits[3U] ? 0x08U : 0x00U; + byte |= bits[4U] ? 0x10U : 0x00U; + byte |= bits[5U] ? 0x20U : 0x00U; + byte |= bits[6U] ? 0x40U : 0x00U; + byte |= bits[7U] ? 0x80U : 0x00U; +} diff --git a/USRP2M17/Utils.h b/USRP2M17/Utils.h new file mode 100644 index 0000000..ade28c0 --- /dev/null +++ b/USRP2M17/Utils.h @@ -0,0 +1,36 @@ +/* + * Copyright (C) 2009,2014,2015 by Jonathan Naylor, G4KLX + * + * This program 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; version 2 of the License. + * + * This program 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. + */ + +#ifndef Utils_H +#define Utils_H + +#include + +class CUtils { +public: + static void dump(const std::string& title, const unsigned char* data, unsigned int length); + static void dump(int level, const std::string& title, const unsigned char* data, unsigned int length); + + static void dump(const std::string& title, const bool* bits, unsigned int length); + static void dump(int level, const std::string& title, const bool* bits, unsigned int length); + + static void byteToBitsBE(unsigned char byte, bool* bits); + static void byteToBitsLE(unsigned char byte, bool* bits); + + static void bitsToByteBE(const bool* bits, unsigned char& byte); + static void bitsToByteLE(const bool* bits, unsigned char& byte); + +private: +}; + +#endif diff --git a/USRP2M17/Version.h b/USRP2M17/Version.h new file mode 100644 index 0000000..24c519b --- /dev/null +++ b/USRP2M17/Version.h @@ -0,0 +1,25 @@ +/* + * Copyright (C) 2015,2016,2017 by Jonathan Naylor G4KLX + * Copyright (C) 2018 by Andy Uribe CA6JAU + * + * This program 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 2 of the License, or + * (at your option) any later version. + * + * This program 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 this program; if not, write to the Free Software + * Foundation, Inc., 675 Mass Ave, Cambridge, MA 02139, USA. + */ + +#if !defined(VERSION_H) +#define VERSION_H + +const char* VERSION = "20210406"; + +#endif diff --git a/USRP2M17/codec2/codebooks.cpp b/USRP2M17/codec2/codebooks.cpp new file mode 100644 index 0000000..bf319ce --- /dev/null +++ b/USRP2M17/codec2/codebooks.cpp @@ -0,0 +1,964 @@ +/* + * This intermediary file and the files that used to create it are under + * The LGPL. See the file COPYING. + */ + +#include "defines.h" + +/* codebook/lsp1.txt */ +static float codes00[] = +{ + 225, + 250, + 275, + 300, + 325, + 350, + 375, + 400, + 425, + 450, + 475, + 500, + 525, + 550, + 575, + 600 +}; +/* codebook/lsp2.txt */ +static float codes01[] = +{ + 325, + 350, + 375, + 400, + 425, + 450, + 475, + 500, + 525, + 550, + 575, + 600, + 625, + 650, + 675, + 700 +}; +/* codebook/lsp3.txt */ +static float codes02[] = +{ + 500, + 550, + 600, + 650, + 700, + 750, + 800, + 850, + 900, + 950, + 1000, + 1050, + 1100, + 1150, + 1200, + 1250 +}; +/* codebook/lsp4.txt */ +static float codes03[] = +{ + 700, + 800, + 900, + 1000, + 1100, + 1200, + 1300, + 1400, + 1500, + 1600, + 1700, + 1800, + 1900, + 2000, + 2100, + 2200 +}; +/* codebook/lsp5.txt */ +static float codes04[] = +{ + 950, + 1050, + 1150, + 1250, + 1350, + 1450, + 1550, + 1650, + 1750, + 1850, + 1950, + 2050, + 2150, + 2250, + 2350, + 2450 +}; +/* codebook/lsp6.txt */ +static float codes05[] = +{ + 1100, + 1200, + 1300, + 1400, + 1500, + 1600, + 1700, + 1800, + 1900, + 2000, + 2100, + 2200, + 2300, + 2400, + 2500, + 2600 +}; +/* codebook/lsp7.txt */ +static float codes06[] = +{ + 1500, + 1600, + 1700, + 1800, + 1900, + 2000, + 2100, + 2200, + 2300, + 2400, + 2500, + 2600, + 2700, + 2800, + 2900, + 3000 +}; +/* codebook/lsp8.txt */ +static float codes07[] = +{ + 2300, + 2400, + 2500, + 2600, + 2700, + 2800, + 2900, + 3000 +}; +/* codebook/lsp9.txt */ +static float codes08[] = +{ + 2500, + 2600, + 2700, + 2800, + 2900, + 3000, + 3100, + 3200 +}; +/* codebook/lsp10.txt */ +static float codes09[] = +{ + 2900, + 3100, + 3300, + 3500 +}; + +const struct lsp_codebook lsp_cb[] = +{ + /* codebook/lsp1.txt */ + { + 1, + 4, + 16, + codes00 + }, + /* codebook/lsp2.txt */ + { + 1, + 4, + 16, + codes01 + }, + /* codebook/lsp3.txt */ + { + 1, + 4, + 16, + codes02 + }, + /* codebook/lsp4.txt */ + { + 1, + 4, + 16, + codes03 + }, + /* codebook/lsp5.txt */ + { + 1, + 4, + 16, + codes04 + }, + /* codebook/lsp6.txt */ + { + 1, + 4, + 16, + codes05 + }, + /* codebook/lsp7.txt */ + { + 1, + 4, + 16, + codes06 + }, + /* codebook/lsp8.txt */ + { + 1, + 3, + 8, + codes07 + }, + /* codebook/lsp9.txt */ + { + 1, + 3, + 8, + codes08 + }, + /* codebook/lsp10.txt */ + { + 1, + 2, + 4, + codes09 + }, + { 0, 0, 0, 0 } +}; + +/* codebook/dlsp1.txt */ +static float codes10[] = +{ + 25, + 50, + 75, + 100, + 125, + 150, + 175, + 200, + 225, + 250, + 275, + 300, + 325, + 350, + 375, + 400, + 425, + 450, + 475, + 500, + 525, + 550, + 575, + 600, + 625, + 650, + 675, + 700, + 725, + 750, + 775, + 800 +}; +/* codebook/dlsp2.txt */ +static float codes11[] = +{ + 25, + 50, + 75, + 100, + 125, + 150, + 175, + 200, + 225, + 250, + 275, + 300, + 325, + 350, + 375, + 400, + 425, + 450, + 475, + 500, + 525, + 550, + 575, + 600, + 625, + 650, + 675, + 700, + 725, + 750, + 775, + 800 +}; +/* codebook/dlsp3.txt */ +static float codes12[] = +{ + 25, + 50, + 75, + 100, + 125, + 150, + 175, + 200, + 225, + 250, + 275, + 300, + 325, + 350, + 375, + 400, + 425, + 450, + 475, + 500, + 525, + 550, + 575, + 600, + 625, + 650, + 675, + 700, + 725, + 750, + 775, + 800 +}; +/* codebook/dlsp4.txt */ +static float codes13[] = +{ + 25, + 50, + 75, + 100, + 125, + 150, + 175, + 200, + 250, + 300, + 350, + 400, + 450, + 500, + 550, + 600, + 650, + 700, + 750, + 800, + 850, + 900, + 950, + 1000, + 1050, + 1100, + 1150, + 1200, + 1250, + 1300, + 1350, + 1400 +}; +/* codebook/dlsp5.txt */ +static float codes14[] = +{ + 25, + 50, + 75, + 100, + 125, + 150, + 175, + 200, + 250, + 300, + 350, + 400, + 450, + 500, + 550, + 600, + 650, + 700, + 750, + 800, + 850, + 900, + 950, + 1000, + 1050, + 1100, + 1150, + 1200, + 1250, + 1300, + 1350, + 1400 +}; +/* codebook/dlsp6.txt */ +static float codes15[] = +{ + 25, + 50, + 75, + 100, + 125, + 150, + 175, + 200, + 250, + 300, + 350, + 400, + 450, + 500, + 550, + 600, + 650, + 700, + 750, + 800, + 850, + 900, + 950, + 1000, + 1050, + 1100, + 1150, + 1200, + 1250, + 1300, + 1350, + 1400 +}; +/* codebook/dlsp7.txt */ +static float codes16[] = +{ + 25, + 50, + 75, + 100, + 125, + 150, + 175, + 200, + 225, + 250, + 275, + 300, + 325, + 350, + 375, + 400, + 425, + 450, + 475, + 500, + 525, + 550, + 575, + 600, + 625, + 650, + 675, + 700, + 725, + 750, + 775, + 800 +}; +/* codebook/dlsp8.txt */ +static float codes17[] = +{ + 25, + 50, + 75, + 100, + 125, + 150, + 175, + 200, + 225, + 250, + 275, + 300, + 325, + 350, + 375, + 400, + 425, + 450, + 475, + 500, + 525, + 550, + 575, + 600, + 625, + 650, + 675, + 700, + 725, + 750, + 775, + 800 +}; +/* codebook/dlsp9.txt */ +static float codes18[] = +{ + 25, + 50, + 75, + 100, + 125, + 150, + 175, + 200, + 225, + 250, + 275, + 300, + 325, + 350, + 375, + 400, + 425, + 450, + 475, + 500, + 525, + 550, + 575, + 600, + 625, + 650, + 675, + 700, + 725, + 750, + 775, + 800 +}; +/* codebook/dlsp10.txt */ +static float codes19[] = +{ + 25, + 50, + 75, + 100, + 125, + 150, + 175, + 200, + 225, + 250, + 275, + 300, + 325, + 350, + 375, + 400, + 425, + 450, + 475, + 500, + 525, + 550, + 575, + 600, + 625, + 650, + 675, + 700, + 725, + 750, + 775, + 800 +}; + +const struct lsp_codebook lsp_cbd[] = +{ + /* codebook/dlsp1.txt */ + { + 1, + 5, + 32, + codes10 + }, + /* codebook/dlsp2.txt */ + { + 1, + 5, + 32, + codes11 + }, + /* codebook/dlsp3.txt */ + { + 1, + 5, + 32, + codes12 + }, + /* codebook/dlsp4.txt */ + { + 1, + 5, + 32, + codes13 + }, + /* codebook/dlsp5.txt */ + { + 1, + 5, + 32, + codes14 + }, + /* codebook/dlsp6.txt */ + { + 1, + 5, + 32, + codes15 + }, + /* codebook/dlsp7.txt */ + { + 1, + 5, + 32, + codes16 + }, + /* codebook/dlsp8.txt */ + { + 1, + 5, + 32, + codes17 + }, + /* codebook/dlsp9.txt */ + { + 1, + 5, + 32, + codes18 + }, + /* codebook/dlsp10.txt */ + { + 1, + 5, + 32, + codes19 + }, + { 0, 0, 0, 0 } +}; + + +/* codebook/gecb.txt */ +static float codes30[] = +{ + 2.71, 12.0184, + 0.04675, -2.73881, + 0.120993, 8.38895, + -1.58028, -0.892307, + 1.19307, -1.91561, + 0.187101, -3.27679, + 0.332251, -7.66455, + -1.47944, 31.2461, + 1.52761, 27.7095, + -0.524379, 5.25012, + 0.55333, 7.4388, + -0.843451, -1.95299, + 2.26389, 8.61029, + 0.143143, 2.36549, + 0.616506, 1.28427, + -1.71133, 22.0967, + 1.00813, 17.3965, + -0.106718, 1.41891, + -0.136246, 14.2736, + -1.70909, -20.5319, + 1.65787, -3.39107, + 0.138049, -4.95785, + 0.536729, -1.94375, + 0.196307, 36.8519, + 1.27248, 22.5565, + -0.670219, -1.90604, + 0.382092, 6.40113, + -0.756911, -4.90102, + 1.82931, 4.6138, + 0.318794, 0.73683, + 0.612815, -2.07505, + -0.410151, 24.7871, + 1.77602, 13.1909, + 0.106457, -0.104492, + 0.192206, 10.1838, + -1.82442, -7.71565, + 0.931346, 4.34835, + 0.308813, -4.086, + 0.397143, -11.8089, + -0.048715, 41.2273, + 0.877342, 35.8503, + -0.759794, 0.476634, + 0.978593, 7.67467, + -1.19506, 3.03883, + 2.63989, -3.41106, + 0.191127, 3.60351, + 0.402932, 1.0843, + -2.15202, 18.1076, + 1.5468, 8.32271, + -0.143089, -4.07592, + -0.150142, 5.86674, + -1.40844, -3.2507, + 1.56615, -10.4132, + 0.178171, -10.2267, + 0.362164, -0.028556, + -0.070125, 24.3907, + 0.594752, 17.4828, + -0.28698, -6.90407, + 0.464818, 10.2055, + -1.00684, -14.3572, + 2.32957, -3.69161, + 0.335745, 2.40714, + 1.01966, -3.15565, + -1.25945, 7.9919, + 2.38369, 19.6806, + -0.094947, -2.41374, + 0.20933, 6.66477, + -2.22103, 1.37986, + 1.29239, 2.04633, + 0.243626, -0.890741, + 0.428773, -7.19366, + -1.11374, 41.3414, + 2.6098, 31.1405, + -0.446468, 2.53419, + 0.490104, 4.62757, + -1.11723, -3.24174, + 1.79156, 8.41493, + 0.156012, 0.183336, + 0.532447, 3.15455, + -0.764484, 18.514, + 0.952395, 11.7713, + -0.332567, 0.346987, + 0.202165, 14.7168, + -2.12924, -15.559, + 1.35358, -1.92679, + -0.010963, -16.3364, + 0.399053, -2.79057, + 0.750657, 31.1483, + 0.655743, 24.4819, + -0.45321, -0.735879, + 0.2869, 6.5467, + -0.715673, -12.3578, + 1.54849, 3.87217, + 0.271874, 0.802339, + 0.502073, -4.85485, + -0.497037, 17.7619, + 1.19116, 13.9544, + 0.01563, 1.33157, + 0.341867, 8.93537, + -2.31601, -5.39506, + 0.75861, 1.9645, + 0.24132, -3.23769, + 0.267151, -11.2344, + -0.273126, 32.6248, + 1.75352, 40.432, + -0.784011, 3.04576, + 0.705987, 5.66118, + -1.3864, 1.35356, + 2.37646, 1.67485, + 0.242973, 4.73218, + 0.491227, 0.354061, + -1.60676, 8.65895, + 1.16711, 5.9871, + -0.137601, -12.0417, + -0.251375, 10.3972, + -1.43151, -8.90411, + 0.98828, -13.209, + 0.261484, -6.35497, + 0.395932, -0.702529, + 0.283704, 26.8996, + 0.420959, 15.4418, + -0.355804, -13.7278, + 0.527372, 12.3985, + -1.16956, -15.9985, + 1.90669, -5.81605, + 0.354492, 3.85157, + 0.82576, -4.16264, + -0.49019, 13.0572, + 2.25577, 13.5264, + -0.004956, -3.23713, + 0.026709, 7.86645, + -1.81037, -0.451183, + 1.08383, -0.18362, + 0.135836, -2.26658, + 0.375812, -5.51225, + -1.96644, 38.6829, + 1.97799, 24.5655, + -0.704656, 6.35881, + 0.480786, 7.05175, + -0.976417, -2.42273, + 2.50215, 6.75935, + 0.083588, 3.2588, + 0.543629, 0.910013, + -1.23196, 23.0915, + 0.785492, 14.807, + -0.213554, 1.688, + 0.004748, 18.1718, + -1.54719, -16.1168, + 1.50104, -3.28114, + 0.080133, -4.63472, + 0.476592, -2.18093, + 0.44247, 40.304, + 1.07277, 27.592, + -0.594738, -4.16681, + 0.42248, 7.61609, + -0.927521, -7.27441, + 1.99162, 1.29636, + 0.291307, 2.39878, + 0.721081, -1.95062, + -0.804256, 24.9295, + 1.64839, 19.1197, + 0.060852, -0.590639, + 0.266085, 9.10325, + -1.9574, -2.88461, + 1.11693, 2.6724, + 0.35458, -2.74854, + 0.330733, -14.1561, + -0.527851, 39.5756, + 0.991152, 43.195, + -0.589619, 1.26919, + 0.787401, 8.73071, + -1.0138, 1.02507, + 2.8254, 1.89538, + 0.24089, 2.74557, + 0.427195, 2.54446, + -1.95311, 12.244, + 1.44862, 12.0607, + -0.210492, -3.37906, + -0.056713, 10.204, + -1.65237, -5.10274, + 1.29475, -12.2708, + 0.111608, -8.67592, + 0.326634, -1.16763, + 0.021781, 31.1258, + 0.455335, 21.4684, + -0.37544, -3.37121, + 0.39362, 11.302, + -0.851456, -19.4149, + 2.10703, -2.22886, + 0.373233, 1.92406, + 0.884438, -1.72058, + -0.975127, 9.84013, + 2.0033, 17.3954, + -0.036915, -1.11137, + 0.148456, 5.39997, + -1.91441, 4.77382, + 1.44791, 0.537122, + 0.194979, -1.03818, + 0.495771, -9.95502, + -1.05899, 32.9471, + 2.01122, 32.4544, + -0.30965, 4.71911, + 0.436082, 4.63552, + -1.23711, -1.25428, + 2.02274, 9.42834, + 0.190342, 1.46077, + 0.479017, 2.48479, + -1.07848, 16.2217, + 1.20764, 9.65421, + -0.258087, -1.67236, + 0.071852, 13.416, + -1.87723, -16.072, + 1.28957, -4.87118, + 0.067713, -13.4427, + 0.435551, -4.1655, + 0.46614, 30.5895, + 0.904895, 21.598, + -0.518369, -2.53205, + 0.337363, 5.63726, + -0.554975, -17.4005, + 1.69188, 1.14574, + 0.227934, 0.889297, + 0.587303, -5.72973, + -0.262133, 18.6666, + 1.39505, 17.0029, + -0.01909, 4.30838, + 0.304235, 12.6699, + -2.07406, -6.46084, + 0.920546, 1.21296, + 0.284927, -1.78547, + 0.209724, -16.024, + -0.636067, 31.5768, + 1.34989, 34.6775, + -0.971625, 5.30086, + 0.590249, 4.44971, + -1.56787, 3.60239, + 2.1455, 4.51666, + 0.296022, 4.12017, + 0.445299, 0.868772, + -1.44193, 14.1284, + 1.35575, 6.0074, + -0.012814, -7.49657, + -0.43, 8.50012, + -1.20469, -7.11326, + 1.10102, -6.83682, + 0.196463, -6.234, + 0.436747, -1.12979, + 0.141052, 22.8549, + 0.290821, 18.8114, + -0.529536, -7.73251, + 0.63428, 10.7898, + -1.33472, -20.3258, + 1.81564, -1.90332, + 0.394778, 3.79758, + 0.732682, -8.18382, + -0.741244, 11.7683 +}; + +const struct lsp_codebook ge_cb[] = +{ + /* codebook/gecb.txt */ + { + 2, + 8, + 256, + codes30 + }, + { 0, 0, 0, 0 } +}; diff --git a/USRP2M17/codec2/codec2.cpp b/USRP2M17/codec2/codec2.cpp new file mode 100644 index 0000000..9418633 --- /dev/null +++ b/USRP2M17/codec2/codec2.cpp @@ -0,0 +1,1745 @@ +/*---------------------------------------------------------------------------*\ + + FILE........: codec2.c + AUTHOR......: David Rowe + DATE CREATED: 21/8/2010 + + Codec2 fully quantised encoder and decoder functions. If you want use + codec2, the codec2_xxx functions are for you. + +\*---------------------------------------------------------------------------*/ + +/* + Copyright (C) 2010 David Rowe + + All rights reserved. + + This program is free software; you can redistribute it and/or modify + it under the terms of the GNU Lesser General Public License version 2.1, as + published by the free Software Foundation. This program 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 Lesser General Public License + along with this program; if not, see . +*/ + +#include +#include +#include +#include +#include +#include + +#include "nlp.h" +#include "lpc.h" +#include "quantise.h" +#include "codec2.h" +#include "codec2_internal.h" + +#define HPF_BETA 0.125 +#define BPF_N 101 + +CKissFFT kiss; + +/*---------------------------------------------------------------------------* \ + + FUNCTION HEADERS + +\*---------------------------------------------------------------------------*/ + + + + +/*---------------------------------------------------------------------------*\ + + FUNCTIONS + +\*---------------------------------------------------------------------------*/ + +/*---------------------------------------------------------------------------*\ + + FUNCTION....: codec2_create + AUTHOR......: David Rowe + DATE CREATED: 21/8/2010 + + Create and initialise an instance of the codec. Returns a pointer + to the codec states or NULL on failure. One set of states is + sufficient for a full duuplex codec (i.e. an encoder and decoder). + You don't need separate states for encoders and decoders. See + c2enc.c and c2dec.c for examples. + +\*---------------------------------------------------------------------------*/ + +CCodec2::CCodec2(bool is_3200) +{ + c2.mode = is_3200 ? 3200 : 1600; + + /* store constants in a few places for convenience */ + + c2.c2const = c2const_create(8000, N_S); + c2.Fs = c2.c2const.Fs; + int n_samp = c2.n_samp = c2.c2const.n_samp; + int m_pitch = c2.m_pitch = c2.c2const.m_pitch; + + c2.Pn.resize(2*n_samp); + c2.Sn_.resize(2*n_samp); + c2.w.resize(m_pitch); + c2.Sn.resize(m_pitch); + + for(int i=0; i Aw[FFT_ENC]; + + /* only need to zero these out due to (unused) snr calculation */ + + for(i=0; i<2; i++) + for(j=1; j<=MAX_AMP; j++) + model[i].A[j] = 0.0; + + /* unpack bits from channel ------------------------------------*/ + + /* this will partially fill the model params for the 2 x 10ms + frames */ + + model[0].voiced = qt.unpack(bits, &nbit, 1); + model[1].voiced = qt.unpack(bits, &nbit, 1); + + Wo_index = qt.unpack(bits, &nbit, WO_BITS); + model[1].Wo = qt.decode_Wo(&c2.c2const, Wo_index, WO_BITS); + model[1].L = PI/model[1].Wo; + + e_index = qt.unpack(bits, &nbit, E_BITS); + e[1] = qt.decode_energy(e_index, E_BITS); + + for(i=0; i Aw[FFT_ENC]; + + /* only need to zero these out due to (unused) snr calculation */ + + for(i=0; i<4; i++) + for(j=1; j<=MAX_AMP; j++) + model[i].A[j] = 0.0; + + /* unpack bits from channel ------------------------------------*/ + + /* this will partially fill the model params for the 4 x 10ms + frames */ + + model[0].voiced = qt.unpack(bits, &nbit, 1); + + model[1].voiced = qt.unpack(bits, &nbit, 1); + Wo_index = qt.unpack(bits, &nbit, WO_BITS); + model[1].Wo = qt.decode_Wo(&c2.c2const, Wo_index, WO_BITS); + model[1].L = PI/model[1].Wo; + + e_index = qt.unpack(bits, &nbit, E_BITS); + e[1] = qt.decode_energy(e_index, E_BITS); + + model[2].voiced = qt.unpack(bits, &nbit, 1); + + model[3].voiced = qt.unpack(bits, &nbit, 1); + Wo_index = qt.unpack(bits, &nbit, WO_BITS); + model[3].Wo = qt.decode_Wo(&c2.c2const, Wo_index, WO_BITS); + model[3].L = PI/model[3].Wo; + + e_index = qt.unpack(bits, &nbit, E_BITS); + e[3] = qt.decode_energy(e_index, E_BITS); + + for(i=0; i Aw[], float gain) +{ + int i; + + /* LPC based phase synthesis */ + std::complex H[MAX_AMP+1]; + sample_phase(model, H, Aw); + phase_synth_zero_order(c2.n_samp, model, &c2.ex_phase, H); + + postfilter(model, &c2.bg_est); + synthesise(c2.n_samp, &(c2.fftr_inv_cfg), c2.Sn_.data(), model, c2.Pn.data(), 1); + + for(i=0; i 32767.0) + speech[i] = 32767; + else if (c2.Sn_[i] < -32767.0) + speech[i] = -32767; + else + speech[i] = c2.Sn_[i]; + } + +} + + +/*---------------------------------------------------------------------------* \ + + FUNCTION....: analyse_one_frame() + AUTHOR......: David Rowe + DATE CREATED: 23/8/2010 + + Extract sinusoidal model parameters from 80 speech samples (10ms of + speech). + +\*---------------------------------------------------------------------------*/ + +void CCodec2::analyse_one_frame(MODEL *model, const short *speech) +{ + std::complex Sw[FFT_ENC]; + float pitch; + int i; + int n_samp = c2.n_samp; + int m_pitch = c2.m_pitch; + + /* Read input speech */ + + for(i=0; iWo = TWO_PI/pitch; + model->L = PI/model->Wo; + + /* estimate model parameters */ + two_stage_pitch_refinement(&c2.c2const, model, Sw); + + /* estimate phases when doing ML experiments */ + estimate_amplitudes(model, Sw, 0); + est_voicing_mbe(&c2.c2const, model, Sw, c2.W); +} + + +/*---------------------------------------------------------------------------* \ + + FUNCTION....: ear_protection() + AUTHOR......: David Rowe + DATE CREATED: Nov 7 2012 + + Limits output level to protect ears when there are bit errors or the input + is overdriven. This doesn't correct or mask bit errors, just reduces the + worst of their damage. + +\*---------------------------------------------------------------------------*/ + +void CCodec2::ear_protection(float in_out[], int n) +{ + float max_sample, over, gain; + int i; + + /* find maximum sample in frame */ + + max_sample = 0.0; + for(i=0; i max_sample) + max_sample = in_out[i]; + + /* determine how far above set point */ + + over = max_sample/30000.0; + + /* If we are x dB over set point we reduce level by 2x dB, this + attenuates major excursions in amplitude (likely to be caused + by bit errors) more than smaller ones */ + + if (over > 1.0) + { + gain = 1.0/(over*over); + for(i=0; i H[], + std::complex A[] /* LPC analysis filter in freq domain */ +) +{ + int m, b; + float r; + + r = TWO_PI/(FFT_ENC); + + /* Sample phase at harmonics */ + + for(m=1; m<=model->L; m++) + { + b = (int)(m*model->Wo/r + 0.5); + H[m] = std::conj(A[b]); + } +} + + +/*---------------------------------------------------------------------------*\ + + phase_synth_zero_order() + + Synthesises phases based on SNR and a rule based approach. No phase + parameters are required apart from the SNR (which can be reduced to a + 1 bit V/UV decision per frame). + + The phase of each harmonic is modelled as the phase of a synthesis + filter excited by an impulse. In many Codec 2 modes the synthesis + filter is a LPC filter. Unlike the first order model the position + of the impulse is not transmitted, so we create an excitation pulse + train using a rule based approach. + + Consider a pulse train with a pulse starting time n=0, with pulses + repeated at a rate of Wo, the fundamental frequency. A pulse train + in the time domain is equivalent to harmonics in the frequency + domain. We can make an excitation pulse train using a sum of + sinsusoids: + + for(m=1; m<=L; m++) + ex[n] = cos(m*Wo*n) + + Note: the Octave script ../octave/phase.m is an example of this if + you would like to try making a pulse train. + + The phase of each excitation harmonic is: + + arg(E[m]) = mWo + + where E[m] are the complex excitation (freq domain) samples, + arg(x), just returns the phase of a complex sample x. + + As we don't transmit the pulse position for this model, we need to + synthesise it. Now the excitation pulses occur at a rate of Wo. + This means the phase of the first harmonic advances by N_SAMP samples + over a synthesis frame of N_SAMP samples. For example if Wo is pi/20 + (200 Hz), then over a 10ms frame (N_SAMP=80 samples), the phase of the + first harmonic would advance (pi/20)*80 = 4*pi or two complete + cycles. + + We generate the excitation phase of the fundamental (first + harmonic): + + arg[E[1]] = Wo*N_SAMP; + + We then relate the phase of the m-th excitation harmonic to the + phase of the fundamental as: + + arg(E[m]) = m*arg(E[1]) + + This E[m] then gets passed through the LPC synthesis filter to + determine the final harmonic phase. + + Comparing to speech synthesised using original phases: + + - Through headphones speech synthesised with this model is not as + good. Through a loudspeaker it is very close to original phases. + + - If there are voicing errors, the speech can sound clicky or + staticy. If V speech is mistakenly declared UV, this model tends to + synthesise impulses or clicks, as there is usually very little shift or + dispersion through the LPC synthesis filter. + + - When combined with LPC amplitude modelling there is an additional + drop in quality. I am not sure why, theory is interformant energy + is raised making any phase errors more obvious. + + NOTES: + + 1/ This synthesis model is effectively the same as a simple LPC-10 + vocoders, and yet sounds much better. Why? Conventional wisdom + (AMBE, MELP) says mixed voicing is required for high quality + speech. + + 2/ I am pretty sure the Lincoln Lab sinusoidal coding guys (like xMBE + also from MIT) first described this zero phase model, I need to look + up the paper. + + 3/ Note that this approach could cause some discontinuities in + the phase at the edge of synthesis frames, as no attempt is made + to make sure that the phase tracks are continuous (the excitation + phases are continuous, but not the final phases after filtering + by the LPC spectra). Technically this is a bad thing. However + this may actually be a good thing, disturbing the phase tracks a + bit. More research needed, e.g. test a synthesis model that adds + a small delta-W to make phase tracks line up for voiced + harmonics. + +\*---------------------------------------------------------------------------*/ + +void CCodec2::phase_synth_zero_order( + int n_samp, + MODEL *model, + float *ex_phase, /* excitation phase of fundamental */ + std::complex H[] /* L synthesis filter freq domain samples */ + +) +{ + int m; + float new_phi; + std::complex Ex[MAX_AMP+1]; /* excitation samples */ + std::complex A_[MAX_AMP+1]; /* synthesised harmonic samples */ + + /* + Update excitation fundamental phase track, this sets the position + of each pitch pulse during voiced speech. After much experiment + I found that using just this frame's Wo improved quality for UV + sounds compared to interpolating two frames Wo like this: + + ex_phase[0] += (*prev_Wo+model->Wo)*N_SAMP/2; + */ + + ex_phase[0] += (model->Wo)*n_samp; + ex_phase[0] -= TWO_PI*floorf(ex_phase[0]/TWO_PI + 0.5); + + for(m=1; m<=model->L; m++) + { + + /* generate excitation */ + + if (model->voiced) + { + Ex[m] = std::polar(1.0f, ex_phase[0] * m); + } + else + { + + /* When a few samples were tested I found that LPC filter + phase is not needed in the unvoiced case, but no harm in + keeping it. + */ + float phi = TWO_PI*(float)codec2_rand()/CODEC2_RAND_MAX; + Ex[m] = std::polar(1.0f, phi); + } + + /* filter using LPC filter */ + + A_[m].real(H[m].real() * Ex[m].real() - H[m].imag() * Ex[m].imag()); + A_[m].imag(H[m].imag() * Ex[m].real() + H[m].real() * Ex[m].imag()); + + /* modify sinusoidal phase */ + + new_phi = atan2f(A_[m].imag(), A_[m].real()+1E-12); + model->phi[m] = new_phi; + } + +} + +/*---------------------------------------------------------------------------*\ + + postfilter() + + The post filter is designed to help with speech corrupted by + background noise. The zero phase model tends to make speech with + background noise sound "clicky". With high levels of background + noise the low level inter-formant parts of the spectrum will contain + noise rather than speech harmonics, so modelling them as voiced + (i.e. a continuous, non-random phase track) is inaccurate. + + Some codecs (like MBE) have a mixed voicing model that breaks the + spectrum into voiced and unvoiced regions. Several bits/frame + (5-12) are required to transmit the frequency selective voicing + information. Mixed excitation also requires accurate voicing + estimation (parameter estimators always break occasionally under + exceptional conditions). + + In our case we use a post filter approach which requires no + additional bits to be transmitted. The decoder measures the average + level of the background noise during unvoiced frames. If a harmonic + is less than this level it is made unvoiced by randomising it's + phases. + + This idea is rather experimental. Some potential problems that may + happen: + + 1/ If someone says "aaaaaaaahhhhhhhhh" will background estimator track + up to speech level? This would be a bad thing. + + 2/ If background noise suddenly dissapears from the source speech does + estimate drop quickly? What is noise suddenly re-appears? + + 3/ Background noise with a non-flat sepctrum. Current algorithm just + comsiders spectrum as a whole, but this could be broken up into + bands, each with their own estimator. + + 4/ Males and females with the same level of background noise. Check + performance the same. Changing Wo affects width of each band, may + affect bg energy estimates. + + 5/ Not sure what happens during long periods of voiced speech + e.g. "sshhhhhhh" + +\*---------------------------------------------------------------------------*/ + +#define BG_THRESH 40.0 // only consider low levels signals for bg_est +#define BG_BETA 0.1 // averaging filter constant +#define BG_MARGIN 6.0 // harmonics this far above BG noise are + // randomised. Helped make bg noise less + // spikey (impulsive) for mmt1, but speech was + // perhaps a little rougher. + +void CCodec2::postfilter( MODEL *model, float *bg_est ) +{ + int m, uv; + float e, thresh; + + /* determine average energy across spectrum */ + + e = 1E-12; + for(m=1; m<=model->L; m++) + e += model->A[m]*model->A[m]; + + assert(e > 0.0); + e = 10.0*log10f(e/model->L); + + /* If beneath threhold, update bg estimate. The idea + of the threshold is to prevent updating during high level + speech. */ + + if ((e < BG_THRESH) && !model->voiced) + *bg_est = *bg_est*(1.0 - BG_BETA) + e*BG_BETA; + + /* now mess with phases during voiced frames to make any harmonics + less then our background estimate unvoiced. + */ + + uv = 0; + thresh = exp10f((*bg_est + BG_MARGIN)/20.0); + if (model->voiced) + for(m=1; m<=model->L; m++) + if (model->A[m] < thresh) + { + model->phi[m] = (TWO_PI/CODEC2_RAND_MAX)*(float)codec2_rand(); + uv++; + } +} + +C2CONST CCodec2::c2const_create(int Fs, float framelength_s) +{ + C2CONST c2const; + + assert((Fs == 8000) || (Fs = 16000)); + c2const.Fs = Fs; + c2const.n_samp = round(Fs*framelength_s); + c2const.max_amp = floor(Fs*P_MAX_S/2); + c2const.p_min = floor(Fs*P_MIN_S); + c2const.p_max = floor(Fs*P_MAX_S); + c2const.m_pitch = floor(Fs*M_PITCH_S); + c2const.Wo_min = TWO_PI/c2const.p_max; + c2const.Wo_max = TWO_PI/c2const.p_min; + + if (Fs == 8000) + { + c2const.nw = 279; + } + else + { + c2const.nw = 511; /* actually a bit shorter in time but lets us maintain constant FFT size */ + } + + c2const.tw = Fs*TW_S; + + /* + fprintf(stderr, "max_amp: %d m_pitch: %d\n", c2const.n_samp, c2const.m_pitch); + fprintf(stderr, "p_min: %d p_max: %d\n", c2const.p_min, c2const.p_max); + fprintf(stderr, "Wo_min: %f Wo_max: %f\n", c2const.Wo_min, c2const.Wo_max); + fprintf(stderr, "nw: %d tw: %d\n", c2const.nw, c2const.tw); + */ + + return c2const; +} + +/*---------------------------------------------------------------------------*\ + + FUNCTION....: make_analysis_window + AUTHOR......: David Rowe + DATE CREATED: 11/5/94 + + Init function that generates the time domain analysis window and it's DFT. + +\*---------------------------------------------------------------------------*/ + +void CCodec2::make_analysis_window(C2CONST *c2const, FFT_STATE *fft_fwd_cfg, float w[], float W[]) +{ + float m; + std::complex wshift[FFT_ENC]; + int i,j; + int m_pitch = c2const->m_pitch; + int nw = c2const->nw; + + /* + Generate Hamming window centered on M-sample pitch analysis window + + 0 M/2 M-1 + |-------------|-------------| + |-------|-------| + nw samples + + All our analysis/synthsis is centred on the M/2 sample. + */ + + m = 0.0; + for(i=0; i temp[FFT_ENC]; + + for(i=0; i(0.0f, 0.0f); + } + for(i=0; i Sw[], float Sn[], float w[]) +{ + int i; + int m_pitch = c2const->m_pitch; + int nw = c2const->nw; + + for(i=0; i(0.0f, 0.0f); + } + + /* Centre analysis window on time axis, we need to arrange input + to FFT this way to make FFT phases correct */ + + /* move 2nd half to start of FFT input vector */ + + for(i=0; i Sw[]) +{ + float pmin,pmax,pstep; /* pitch refinment minimum, maximum and step */ + + /* Coarse refinement */ + + pmax = TWO_PI/model->Wo + 5; + pmin = TWO_PI/model->Wo - 5; + pstep = 1.0; + hs_pitch_refinement(model, Sw, pmin, pmax, pstep); + + /* Fine refinement */ + + pmax = TWO_PI/model->Wo + 1; + pmin = TWO_PI/model->Wo - 1; + pstep = 0.25; + hs_pitch_refinement(model,Sw,pmin,pmax,pstep); + + /* Limit range */ + + if (model->Wo < TWO_PI/c2const->p_max) + model->Wo = TWO_PI/c2const->p_max; + if (model->Wo > TWO_PI/c2const->p_min) + model->Wo = TWO_PI/c2const->p_min; + + model->L = floorf(PI/model->Wo); + + /* trap occasional round off issues with floorf() */ + if (model->Wo*model->L >= 0.95*PI) + { + model->L--; + } + assert(model->Wo*model->L < PI); +} + +/*---------------------------------------------------------------------------*\ + + FUNCTION....: hs_pitch_refinement + AUTHOR......: David Rowe + DATE CREATED: 27/5/94 + + Harmonic sum pitch refinement function. + + pmin pitch search range minimum + pmax pitch search range maximum + step pitch search step size + model current pitch estimate in model.Wo + + model refined pitch estimate in model.Wo + +\*---------------------------------------------------------------------------*/ + +void CCodec2::hs_pitch_refinement(MODEL *model, std::complex Sw[], float pmin, float pmax, float pstep) +{ + int m; /* loop variable */ + int b; /* bin for current harmonic centre */ + float E; /* energy for current pitch*/ + float Wo; /* current "test" fundamental freq. */ + float Wom; /* Wo that maximises E */ + float Em; /* mamimum energy */ + float r, one_on_r; /* number of rads/bin */ + float p; /* current pitch */ + + /* Initialisation */ + + model->L = PI/model->Wo; /* use initial pitch est. for L */ + Wom = model->Wo; + Em = 0.0; + r = TWO_PI/FFT_ENC; + one_on_r = 1.0/r; + + /* Determine harmonic sum for a range of Wo values */ + + for(p=pmin; p<=pmax; p+=pstep) + { + E = 0.0; + Wo = TWO_PI/p; + + /* Sum harmonic magnitudes */ + for(m=1; m<=model->L; m++) + { + b = (int)(m*Wo*one_on_r + 0.5); + E += Sw[b].real() * Sw[b].real() + Sw[b].imag() * Sw[b].imag(); + } + /* Compare to see if this is a maximum */ + + if (E > Em) + { + Em = E; + Wom = Wo; + } + } + + model->Wo = Wom; +} + +/*---------------------------------------------------------------------------*\ + + FUNCTION....: estimate_amplitudes + AUTHOR......: David Rowe + DATE CREATED: 27/5/94 + + Estimates the complex amplitudes of the harmonics. + +\*---------------------------------------------------------------------------*/ + +void CCodec2::estimate_amplitudes(MODEL *model, std::complex Sw[], int est_phase) +{ + int i,m; /* loop variables */ + int am,bm; /* bounds of current harmonic */ + float den; /* denominator of amplitude expression */ + + float r = TWO_PI/FFT_ENC; + float one_on_r = 1.0/r; + + for(m=1; m<=model->L; m++) + { + /* Estimate ampltude of harmonic */ + + den = 0.0; + am = (int)((m - 0.5)*model->Wo*one_on_r + 0.5); + bm = (int)((m + 0.5)*model->Wo*one_on_r + 0.5); + + for(i=am; iA[m] = sqrtf(den); + + if (est_phase) + { + int b = (int)(m*model->Wo/r + 0.5); /* DFT bin of centre of current harmonic */ + + /* Estimate phase of harmonic, this is expensive in CPU for + embedded devicesso we make it an option */ + + model->phi[m] = atan2f(Sw[b].imag(), Sw[b].real()); + } + } +} + +/*---------------------------------------------------------------------------*\ + + est_voicing_mbe() + + Returns the error of the MBE cost function for a fiven F0. + + Note: I think a lot of the operations below can be simplified as + W[].imag = 0 and has been normalised such that den always equals 1. + +\*---------------------------------------------------------------------------*/ + +float CCodec2::est_voicing_mbe( C2CONST *c2const, MODEL *model, std::complex Sw[], float W[]) +{ + int l,al,bl,m; /* loop variables */ + std::complex Am; /* amplitude sample for this band */ + int offset; /* centers Hw[] about current harmonic */ + float den; /* denominator of Am expression */ + float error; /* accumulated error between original and synthesised */ + float Wo; + float sig, snr; + float elow, ehigh, eratio; + float sixty; + std::complex Ew(0, 0); + + int l_1000hz = model->L*1000.0/(c2const->Fs/2); + sig = 1E-4; + for(l=1; l<=l_1000hz; l++) + { + sig += model->A[l]*model->A[l]; + } + + Wo = model->Wo; + error = 1E-4; + + /* Just test across the harmonics in the first 1000 Hz */ + + for(l=1; l<=l_1000hz; l++) + { + Am = std::complex(0.0f, 0.0f); + den = 0.0; + al = ceilf((l - 0.5)*Wo*FFT_ENC/TWO_PI); + bl = ceilf((l + 0.5)*Wo*FFT_ENC/TWO_PI); + + /* Estimate amplitude of harmonic assuming harmonic is totally voiced */ + + offset = FFT_ENC/2 - l*Wo*FFT_ENC/TWO_PI + 0.5; + for(m=al; m V_THRESH) + model->voiced = 1; + else + model->voiced = 0; + + /* post processing, helps clean up some voicing errors ------------------*/ + + /* + Determine the ratio of low freqency to high frequency energy, + voiced speech tends to be dominated by low frequency energy, + unvoiced by high frequency. This measure can be used to + determine if we have made any gross errors. + */ + + int l_2000hz = model->L*2000.0/(c2const->Fs/2); + int l_4000hz = model->L*4000.0/(c2const->Fs/2); + elow = ehigh = 1E-4; + for(l=1; l<=l_2000hz; l++) + { + elow += model->A[l]*model->A[l]; + } + for(l=l_2000hz; l<=l_4000hz; l++) + { + ehigh += model->A[l]*model->A[l]; + } + eratio = 10.0*log10f(elow/ehigh); + + /* Look for Type 1 errors, strongly V speech that has been + accidentally declared UV */ + + if (model->voiced == 0) + if (eratio > 10.0) + model->voiced = 1; + + /* Look for Type 2 errors, strongly UV speech that has been + accidentally declared V */ + + if (model->voiced == 1) + { + if (eratio < -10.0) + model->voiced = 0; + + /* A common source of Type 2 errors is the pitch estimator + gives a low (50Hz) estimate for UV speech, which gives a + good match with noise due to the close harmoonic spacing. + These errors are much more common than people with 50Hz3 + pitch, so we have just a small eratio threshold. */ + + sixty = 60.0*TWO_PI/c2const->Fs; + if ((eratio < -4.0) && (model->Wo <= sixty)) + model->voiced = 0; + } + //printf(" v: %d snr: %f eratio: %3.2f %f\n",model->voiced,snr,eratio,dF0); + + return snr; +} + +/*---------------------------------------------------------------------------*\ + + FUNCTION....: make_synthesis_window + AUTHOR......: David Rowe + DATE CREATED: 11/5/94 + + Init function that generates the trapezoidal (Parzen) sythesis window. + +\*---------------------------------------------------------------------------*/ + +void CCodec2::make_synthesis_window(C2CONST *c2const, float Pn[]) +{ + int i; + float win; + int n_samp = c2const->n_samp; + int tw = c2const->tw; + + /* Generate Parzen window in time domain */ + + win = 0.0; + for(i=0; i Sw_[FFT_DEC/2+1]; /* DFT of synthesised signal */ + float sw_[FFT_DEC]; /* synthesised signal */ + + if (shift) + { + /* Update memories */ + for(i=0; iL; l++) + { + b = (int)(l*model->Wo*FFT_DEC/TWO_PI + 0.5); + if (b > ((FFT_DEC/2)-1)) + { + b = (FFT_DEC/2)-1; + } + Sw_[b] = std::polar(model->A[l], model->phi[l]); + } + + /* Perform inverse DFT */ + + kiss.fftri(*fftr_inv_cfg, Sw_,sw_); + + /* Overlap add to previous samples */ + + for(i=0; ivoiced && !prev->voiced && !next->voiced) + { + interp->voiced = 0; + } + + /* Wo depends on voicing of this and adjacent frames */ + + if (interp->voiced) + { + if (prev->voiced && next->voiced) + interp->Wo = (1.0 - weight)*prev->Wo + weight*next->Wo; + if (!prev->voiced && next->voiced) + interp->Wo = next->Wo; + if (prev->voiced && !next->voiced) + interp->Wo = prev->Wo; + } + else + { + interp->Wo = Wo_min; + } + interp->L = PI/interp->Wo; +} + +/*---------------------------------------------------------------------------*\ + + FUNCTION....: interp_energy() + AUTHOR......: David Rowe + DATE CREATED: 22 May 2012 + + Interpolates centre 10ms sample of energy given two samples 20ms + apart. + +\*---------------------------------------------------------------------------*/ + +float CCodec2::interp_energy(float prev_e, float next_e) +{ + //return powf(10.0, (log10f(prev_e) + log10f(next_e))/2.0); + return sqrtf(prev_e * next_e); //looks better is math. identical and faster math +} + +/*---------------------------------------------------------------------------*\ + + FUNCTION....: interpolate_lsp_ver2() + AUTHOR......: David Rowe + DATE CREATED: 22 May 2012 + + Weighted interpolation of LSPs. + +\*---------------------------------------------------------------------------*/ + +void CCodec2::interpolate_lsp_ver2(float interp[], float prev[], float next[], float weight, int order) +{ + int i; + + for(i=0; i. +*/ + +#ifndef __CODEC2__ +#define __CODEC2__ + +#include + +#include "codec2_internal.h" +#include "defines.h" +#include "kiss_fft.h" +#include "nlp.h" +#include "quantise.h" + +#define CODEC2_MODE_3200 0 +#define CODEC2_MODE_1600 2 + +#ifndef CODEC2_MODE_EN_DEFAULT +#define CODEC2_MODE_EN_DEFAULT 1 +#endif + +#define CODEC2_RAND_MAX 32767 + +class CCodec2 +{ +public: + CCodec2(bool is_3200); + ~CCodec2(); + void codec2_encode(unsigned char *bits, const short *speech_in); + void codec2_decode(short *speech_out, const unsigned char *bits); + void codec2_set_mode(bool); + bool codec2_get_mode() {return (c2.mode == 3200); }; + int codec2_samples_per_frame(); + int codec2_bits_per_frame(); + +private: + // merged from other files + void sample_phase(MODEL *model, std::complex filter_phase[], std::complex A[]); + void phase_synth_zero_order(int n_samp, MODEL *model, float *ex_phase, std::complex filter_phase[]); + void postfilter(MODEL *model, float *bg_est); + + C2CONST c2const_create(int Fs, float framelength_ms); + + void make_analysis_window(C2CONST *c2const, FFT_STATE *fft_fwd_cfg, float w[], float W[]); + void dft_speech(C2CONST *c2const, FFT_STATE &fft_fwd_cfg, std::complex Sw[], float Sn[], float w[]); + void two_stage_pitch_refinement(C2CONST *c2const, MODEL *model, std::complex Sw[]); + void estimate_amplitudes(MODEL *model, std::complex Sw[], int est_phase); + float est_voicing_mbe(C2CONST *c2const, MODEL *model, std::complex Sw[], float W[]); + void make_synthesis_window(C2CONST *c2const, float Pn[]); + void synthesise(int n_samp, FFTR_STATE *fftr_inv_cfg, float Sn_[], MODEL *model, float Pn[], int shift); + int codec2_rand(void); + void hs_pitch_refinement(MODEL *model, std::complex Sw[], float pmin, float pmax, float pstep); + + void interp_Wo(MODEL *interp, MODEL *prev, MODEL *next, float Wo_min); + void interp_Wo2(MODEL *interp, MODEL *prev, MODEL *next, float weight, float Wo_min); + float interp_energy(float prev, float next); + void interpolate_lsp_ver2(float interp[], float prev[], float next[], float weight, int order); + + void analyse_one_frame(MODEL *model, const short *speech); + void synthesise_one_frame(short speech[], MODEL *model, std::complex Aw[], float gain); + void codec2_encode_3200(unsigned char *bits, const short *speech); + void codec2_encode_1600(unsigned char *bits, const short *speech); + void codec2_decode_3200(short *speech, const unsigned char *bits); + void codec2_decode_1600(short *speech, const unsigned char *bits); + void ear_protection(float in_out[], int n); + void lsp_to_lpc(float *freq, float *ak, int lpcrdr); + + void (CCodec2::*encode)(unsigned char *bits, const short *speech); + void (CCodec2::*decode)(short *speech, const unsigned char *bits); + Cnlp nlp; + CQuantize qt; + CODEC2 c2; +}; + +#endif diff --git a/USRP2M17/codec2/codec2_internal.h b/USRP2M17/codec2/codec2_internal.h new file mode 100644 index 0000000..1b037e1 --- /dev/null +++ b/USRP2M17/codec2/codec2_internal.h @@ -0,0 +1,67 @@ +/*---------------------------------------------------------------------------*\ + + FILE........: codec2_internal.h + AUTHOR......: David Rowe + DATE CREATED: April 16 2012 + + Header file for Codec2 internal states, exposed via this header + file to assist in testing. + +\*---------------------------------------------------------------------------*/ + +/* + Copyright (C) 2012 David Rowe + + All rights reserved. + + This program is free software; you can redistribute it and/or modify + it under the terms of the GNU Lesser General Public License version 2.1, as + published by the Free Software Foundation. This program 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 Lesser General Public License + along with this program; if not, see . +*/ + +#ifndef __CODEC2_INTERNAL__ +#define __CODEC2_INTERNAL__ + +#include "kiss_fft.h" + +using CODEC2 = struct codec2_tag { + int mode; + int Fs; + int n_samp; + int m_pitch; + int gray; /* non-zero for gray encoding */ + int lpc_pf; /* LPC post filter on */ + int bass_boost; /* LPC post filter bass boost */ + int smoothing; /* enable smoothing for channels with errors */ + float ex_phase; /* excitation model phase track */ + float bg_est; /* background noise estimate for post filter */ + float prev_f0_enc; /* previous frame's f0 estimate */ + float prev_e_dec; /* previous frame's LPC energy */ + float beta; /* LPC post filter parameters */ + float gamma; + float xq_enc[2]; /* joint pitch and energy VQ states */ + float xq_dec[2]; + float W[FFT_ENC]; /* DFT of w[] */ + float hpf_states[2]; /* high pass filter states */ + float prev_lsps_dec[LPC_ORD]; /* previous frame's LSPs */ + float *softdec; /* optional soft decn bits from demod */ + MODEL prev_model_dec; /* previous frame's model parameters */ + C2CONST c2const; + FFT_STATE fft_fwd_cfg; /* forward FFT config */ + FFTR_STATE fftr_fwd_cfg; /* forward real FFT config */ + FFTR_STATE fftr_inv_cfg; /* inverse FFT config */ + std::vector w; /* [m_pitch] time domain hamming window */ + std::vector Pn; /* [2*n_samp] trapezoidal synthesis window */ + std::vector Sn; /* [m_pitch] input speech */ + std::vector Sn_; /* [2*n_samp] synthesised output speech */ + std::vector bpf_buf; /* buffer for band pass filter */ +}; + +#endif diff --git a/USRP2M17/codec2/defines.h b/USRP2M17/codec2/defines.h new file mode 100644 index 0000000..f201313 --- /dev/null +++ b/USRP2M17/codec2/defines.h @@ -0,0 +1,127 @@ +/*---------------------------------------------------------------------------*\ + + FILE........: defines.h + AUTHOR......: David Rowe + DATE CREATED: 23/4/93 + + Defines and structures used throughout the codec. + +\*---------------------------------------------------------------------------*/ + +/* + Copyright (C) 2009 David Rowe + + All rights reserved. + + This program is free software; you can redistribute it and/or modify + it under the terms of the GNU Lesser General Public License version 2.1, as + published by the Free Software Foundation. This program 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 Lesser General Public License + along with this program; if not, see . +*/ + +#ifndef __DEFINES__ +#define __DEFINES__ + +#include +#include + +/*---------------------------------------------------------------------------*\ + + DEFINES + +\*---------------------------------------------------------------------------*/ + +/* General defines */ + +#define N_S 0.01 /* internal proc frame length in secs */ +#define TW_S 0.005 /* trapezoidal synth window overlap */ +#define MAX_AMP 160 /* maximum number of harmonics */ +#ifndef PI +#define PI 3.141592654 /* mathematical constant */ +#endif +#define TWO_PI 6.283185307 /* mathematical constant */ +#define MAX_STR 2048 /* maximum string size */ + +#define FFT_ENC 512 /* size of FFT used for encoder */ +#define FFT_DEC 512 /* size of FFT used in decoder */ +#define V_THRESH 6.0 /* voicing threshold in dB */ +#define LPC_ORD 10 /* LPC order */ +#define LPC_ORD_LOW 6 /* LPC order for lower rates */ + +/* Pitch estimation defines */ + +#define M_PITCH_S 0.0400 /* pitch analysis window in s */ +#define P_MIN_S 0.0025 /* minimum pitch period in s */ +#define P_MAX_S 0.0200 /* maximum pitch period in s */ +#define MAXFACTORS 32 // e.g. an fft of length 128 has 4 factors + // as far as kissfft is concerned 4*4*4*2 + +/*---------------------------------------------------------------------------*\ + + TYPEDEFS + +\*---------------------------------------------------------------------------*/ + +/* Structure to hold constants calculated at run time based on sample rate */ + +using C2CONST = struct c2const_tag +{ + int Fs; /* sample rate of this instance */ + int n_samp; /* number of samples per 10ms frame at Fs */ + int max_amp; /* maximum number of harmonics */ + int m_pitch; /* pitch estimation window size in samples */ + int p_min; /* minimum pitch period in samples */ + int p_max; /* maximum pitch period in samples */ + float Wo_min; + float Wo_max; + int nw; /* analysis window size in samples */ + int tw; /* trapezoidal synthesis window overlap */ +}; + +/* Structure to hold model parameters for one frame */ + +using MODEL = struct model_tag +{ + float Wo; /* fundamental frequency estimate in radians */ + int L; /* number of harmonics */ + float A[MAX_AMP+1]; /* amplitiude of each harmonic */ + float phi[MAX_AMP+1]; /* phase of each harmonic */ + int voiced; /* non-zero if this frame is voiced */ +}; + +/* describes each codebook */ + +struct lsp_codebook +{ + int k; /* dimension of vector */ + int log2m; /* number of bits in m */ + int m; /* elements in codebook */ + float *cb; /* The elements */ +}; + +using FFT_STATE = struct fft_state_tag +{ + int nfft; + bool inverse; + int factors[2*MAXFACTORS]; + std::vector> twiddles; +}; + +using FFTR_STATE = struct fftr_state_tag +{ + FFT_STATE substate; + std::vector> tmpbuf; + std::vector> super_twiddles; +}; + +extern const struct lsp_codebook lsp_cb[]; +extern const struct lsp_codebook lsp_cbd[]; +extern const struct lsp_codebook ge_cb[]; + +#endif diff --git a/USRP2M17/codec2/kiss_fft.cpp b/USRP2M17/codec2/kiss_fft.cpp new file mode 100644 index 0000000..cefb846 --- /dev/null +++ b/USRP2M17/codec2/kiss_fft.cpp @@ -0,0 +1,435 @@ +/* +Copyright (c) 2003-2010, Mark Borgerding + +All rights reserved. + +Redistribution and use in source and binary forms, with or without modification, are permitted provided that the following conditions are met: + + * Redistributions of source code must retain the above copyright notice, this list of conditions and the following disclaimer. + * Redistributions in binary form must reproduce the above copyright notice, this list of conditions and the following disclaimer in the documentation and/or other materials provided with the distribution. + * Neither the author nor the names of any contributors may be used to endorse or promote products derived from this software without specific prior written permission. + +THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE. +*/ + +#include +#include + +#include "defines.h" +#include "kiss_fft.h" + +void CKissFFT::kf_bfly2(std::complex *Fout, const size_t fstride, FFT_STATE &st, int m) +{ + std::complex *Fout2; + std::complex *tw1 = st.twiddles.data(); + std::complex t; + Fout2 = Fout + m; + do + { + t = *Fout2 * *tw1; + tw1 += fstride; + *Fout2 = *Fout - t; + *Fout += t; + ++Fout2; + ++Fout; + } + while (--m); +} + +void CKissFFT::kf_bfly3(std::complex * Fout, const size_t fstride, FFT_STATE &st, int m) +{ + const size_t m2 = 2 * m; + std::complex *tw1,*tw2; + std::complex scratch[5]; + std::complex epi3; + epi3 = st.twiddles[fstride*m]; + + tw1 = tw2 = st.twiddles.data(); + + do + { + scratch[1] = Fout[m] * *tw1; + scratch[2] = Fout[m2] * *tw2; + + scratch[3] = scratch[1] + scratch[2]; + scratch[0] = scratch[1] - scratch[2]; + tw1 += fstride; + tw2 += fstride*2; + + Fout[m] = *Fout - (0.5f * scratch[3]); + + scratch[0] *= epi3.imag(); + + *Fout += scratch[3]; + + Fout[m2].real(Fout[m].real() + scratch[0].imag()); + Fout[m2].imag(Fout[m].imag() - scratch[0].real()); + + Fout[m].real(Fout[m].real() - scratch[0].imag()); + Fout[m].imag(Fout[m].imag() + scratch[0].real()); + + ++Fout; + } + while(--m); +} + +void CKissFFT::kf_bfly4(std::complex *Fout, const size_t fstride, FFT_STATE &st, int m) +{ + std::complex *tw1,*tw2,*tw3; + std::complex scratch[6]; + int k = m; + const int m2 = 2 * m; + const int m3 = 3 * m; + + + tw3 = tw2 = tw1 = st.twiddles.data(); + + do + { + scratch[0] = Fout[m] * *tw1; + scratch[1] = Fout[m2] * *tw2; + scratch[2] = Fout[m3] * *tw3; + + scratch[5] = *Fout - scratch[1]; + *Fout += scratch[1]; + scratch[3] = scratch[0] + scratch[2]; + scratch[4] = scratch[0] - scratch[2]; + Fout[m2] = *Fout - scratch[3]; + tw1 += fstride; + tw2 += fstride*2; + tw3 += fstride*3; + *Fout += scratch[3]; + + if(st.inverse) + { + Fout[m].real(scratch[5].real() - scratch[4].imag()); + Fout[m].imag(scratch[5].imag() + scratch[4].real()); + Fout[m3].real(scratch[5].real() + scratch[4].imag()); + Fout[m3].imag(scratch[5].imag() - scratch[4].real()); + } + else + { + Fout[m].real(scratch[5].real() + scratch[4].imag()); + Fout[m].imag(scratch[5].imag() - scratch[4].real()); + Fout[m3].real(scratch[5].real() - scratch[4].imag()); + Fout[m3].imag(scratch[5].imag() + scratch[4].real()); + } + ++Fout; + } + while(--k); +} + +void CKissFFT::kf_bfly5(std::complex * Fout, const size_t fstride, FFT_STATE &st, int m) +{ + std::complex scratch[13]; + std::complex *twiddles = st.twiddles.data(); + auto ya = twiddles[fstride*m]; + auto yb = twiddles[fstride*2*m]; + + auto Fout0 = Fout; + auto Fout1 = Fout0 + m; + auto Fout2 = Fout0 + 2 * m; + auto Fout3 = Fout0 + 3 * m; + auto Fout4 = Fout0 + 4 * m; + + auto tw = st.twiddles.data(); + for (int u=0; u *Fout, const size_t fstride, FFT_STATE &st, int m, int p) +{ + auto twiddles = st.twiddles.data(); + std::complex t; + int Norig = st.nfft; + + std::vector> scratch(p); + + for (int u=0; u= Norig) twidx-=Norig; + t = scratch[q] * twiddles[twidx]; + Fout[k] += t; + } + k += m; + } + } + scratch.clear(); +} + +void CKissFFT::kf_work(std::complex *Fout, const std::complex *f, const size_t fstride, int in_stride, int *factors, FFT_STATE &st) +{ + auto Fout_beg = Fout; + const int p = *factors++; /* the radix */ + const int m = *factors++; /* stage's fft length/p */ + const std::complex *Fout_end = Fout + p*m; + + if (m==1) + { + do + { + *Fout = *f; + f += fstride*in_stride; + } + while( ++Fout != Fout_end ); + } + else + { + do + { + // recursive call: + // DFT of size m*p performed by doing + // p instances of smaller DFTs of size m, + // each one takes a decimated version of the input + kf_work( Fout, f, fstride*p, in_stride, factors, st); + f += fstride*in_stride; + } + while( (Fout += m) != Fout_end ); + } + + Fout=Fout_beg; + + // recombine the p smaller DFTs + switch (p) + { + case 2: + kf_bfly2(Fout,fstride,st,m); + break; + case 3: + kf_bfly3(Fout,fstride,st,m); + break; + case 4: + kf_bfly4(Fout,fstride,st,m); + break; + case 5: + kf_bfly5(Fout,fstride,st,m); + break; + default: + kf_bfly_generic(Fout,fstride,st,m,p); + break; + } +} + +/* facbuf is populated by p1,m1,p2,m2, ... + where + p[i] * m[i] = m[i-1] + m0 = n */ +void CKissFFT::kf_factor(int n,int * facbuf) +{ + int p=4; + double floor_sqrt; + floor_sqrt = floorf( sqrtf((double)n) ); + + /*factor out powers of 4, powers of 2, then any remaining primes */ + do + { + while (n % p) + { + switch (p) + { + case 4: + p = 2; + break; + case 2: + p = 3; + break; + default: + p += 2; + break; + } + if (p > floor_sqrt) + p = n; /* no more factors, skip to end */ + } + n /= p; + *facbuf++ = p; + *facbuf++ = n; + } + while (n > 1); +} + +void CKissFFT::fft_alloc(FFT_STATE &state, const int nfft, bool inverse_fft) +{ + state.twiddles.resize(nfft); + + state.nfft = nfft; + state.inverse = inverse_fft; + + for (int i=0; i *fin, std::complex *fout, int in_stride) +{ + if (fin == fout) + { + //NOTE: this is not really an in-place FFT algorithm. + //It just performs an out-of-place FFT into a temp buffer + std::vector> tmpbuf(st.nfft); + kf_work(tmpbuf.data(), fin, true, in_stride, st.factors, st); + memcpy(fout, tmpbuf.data(), sizeof(std::complex)*st.nfft); + tmpbuf.clear(); + } + else + { + kf_work(fout, fin, 1, in_stride, st.factors, st); + } +} + +void CKissFFT::fft(FFT_STATE &cfg, const std::complex *fin, std::complex *fout) +{ + fft_stride(cfg, fin, fout, 1); +} + +int CKissFFT::fft_next_fast_size(int n) +{ + while(1) + { + int m = n; + while ( (m % 2) == 0 ) m /= 2; + while ( (m % 3) == 0 ) m /= 3; + while ( (m % 5) == 0 ) m /= 5; + if (m <= 1) + break; /* n is completely factorable by twos, threes, and fives */ + n++; + } + return n; +} + +void CKissFFT::fftr_alloc(FFTR_STATE &st, int nfft, const bool inverse_fft) +{ + nfft >>= 1; + + fft_alloc(st.substate, nfft, inverse_fft); + st.tmpbuf.resize(nfft); + st.super_twiddles.resize(nfft); + + for (int i=0; i *freqdata) +{ + assert(st.substate.inverse == false); + + auto ncfft = st.substate.nfft; + + /*perform the parallel fft of two real signals packed in real,imag*/ + fft( st.substate, (const std::complex*)timedata, st.tmpbuf.data()); + /* The real part of the DC element of the frequency spectrum in st->tmpbuf + * contains the sum of the even-numbered elements of the input time sequence + * The imag part is the sum of the odd-numbered elements + * + * The sum of tdc.r and tdc.i is the sum of the input time sequence. + * yielding DC of input time sequence + * The difference of tdc.r - tdc.i is the sum of the input (dot product) [1,-1,1,-1... + * yielding Nyquist bin of input time sequence + */ + + auto tdc = st.tmpbuf[0]; + freqdata[0].real(tdc.real() + tdc.imag()); + freqdata[ncfft].real(tdc.real() - tdc.imag()); + freqdata[ncfft].imag(0.f); + freqdata[0].imag(0.f); + + for (int k=1; k <= ncfft/2; ++k) + { + auto fpk = st.tmpbuf[k]; + auto fpnk = std::conj(st.tmpbuf[ncfft-k]); + + auto f1k = fpk + fpnk; + auto f2k = fpk - fpnk; + auto tw = f2k * st.super_twiddles[k-1]; + + freqdata[k] = 0.5f * (f1k + tw); + freqdata[ncfft-k].real(0.5f * (f1k.real() - tw.real())); + freqdata[ncfft-k].imag(0.5f * (tw.imag() - f1k.imag())); + } +} + +void CKissFFT::fftri(FFTR_STATE &st, const std::complex *freqdata, float *timedata) +{ + assert(st.substate.inverse == true); + + auto ncfft = st.substate.nfft; + + st.tmpbuf[0].real(freqdata[0].real() + freqdata[ncfft].real()); + st.tmpbuf[0].imag(freqdata[0].real() - freqdata[ncfft].real()); + + for (int k=1; k <= ncfft/2; ++k) + { + auto fk = freqdata[k]; + auto fnkc = std::conj(freqdata[ncfft - k]); + + auto fek = fk + fnkc; + auto tmp = fk - fnkc; + auto fok = tmp * st.super_twiddles[k-1]; + st.tmpbuf[k] = fek + fok; + st.tmpbuf[ncfft - k] = std::conj(fek - fok); + } + fft (st.substate, st.tmpbuf.data(), (std::complex *)timedata); +} diff --git a/USRP2M17/codec2/kiss_fft.h b/USRP2M17/codec2/kiss_fft.h new file mode 100644 index 0000000..7626617 --- /dev/null +++ b/USRP2M17/codec2/kiss_fft.h @@ -0,0 +1,35 @@ +#ifndef KISS_FFT_H +#define KISS_FFT_H + +#include + +#include +#include +#include +#include + +#include "defines.h" + +/* for real ffts, we need an even size */ +#define kiss_fftr_next_fast_size_real(n) (kiss_fft_next_fast_size( ((n)+1) >> 1) << 1 ) + +class CKissFFT +{ +public: + void fft_alloc(FFT_STATE &state, const int nfft, const bool inverse_fft); + void fft(FFT_STATE &cfg, const std::complex *fin, std::complex *fout); + void fft_stride(FFT_STATE &cfg, const std::complex *fin, std::complex *fout, int fin_stride); + int fft_next_fast_size(int n); + void fftr_alloc(FFTR_STATE &state, int nfft, const bool inverse_fft); + void fftr(FFTR_STATE &cfg,const float *timedata,std::complex *freqdata); + void fftri(FFTR_STATE &cfg,const std::complex *freqdata,float *timedata); +private: + void kf_bfly2(std::complex *Fout, const size_t fstride, FFT_STATE &st, int m); + void kf_bfly3(std::complex *Fout, const size_t fstride, FFT_STATE &st, int m); + void kf_bfly4(std::complex *Fout, const size_t fstride, FFT_STATE &st, int m); + void kf_bfly5(std::complex *Fout, const size_t fstride, FFT_STATE &st, int m); + void kf_bfly_generic(std::complex *Fout, const size_t fstride, FFT_STATE &st, int m, int p); + void kf_work(std::complex *Fout, const std::complex *f, const size_t fstride, int in_stride, int *factors, FFT_STATE &st); + void kf_factor(int n, int *facbuf); +}; +#endif diff --git a/USRP2M17/codec2/lpc.cpp b/USRP2M17/codec2/lpc.cpp new file mode 100644 index 0000000..10f2c09 --- /dev/null +++ b/USRP2M17/codec2/lpc.cpp @@ -0,0 +1,311 @@ +/*---------------------------------------------------------------------------*\ + + FILE........: lpc.c + AUTHOR......: David Rowe + DATE CREATED: 30 Sep 1990 (!) + + Linear Prediction functions written in C. + +\*---------------------------------------------------------------------------*/ + +/* + Copyright (C) 2009-2012 David Rowe + + All rights reserved. + + This program is free software; you can redistribute it and/or modify + it under the terms of the GNU Lesser General Public License version 2.1, as + published by the Free Software Foundation. This program 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 Lesser General Public License + along with this program; if not, see . +*/ + +#define LPC_MAX_N 512 /* maximum no. of samples in frame */ +#define PI 3.141592654 /* mathematical constant */ + +#define ALPHA 1.0 +#define BETA 0.94 + +#include +#include +#include "defines.h" +#include "lpc.h" + +/*---------------------------------------------------------------------------*\ + + pre_emp() + + Pre-emphasise (high pass filter with zero close to 0 Hz) a frame of + speech samples. Helps reduce dynamic range of LPC spectrum, giving + greater weight and hense a better match to low energy formants. + + Should be balanced by de-emphasis of the output speech. + +\*---------------------------------------------------------------------------*/ + +void Clpc::pre_emp( + float Sn_pre[], /* output frame of speech samples */ + float Sn[], /* input frame of speech samples */ + float *mem, /* Sn[-1]single sample memory */ + int Nsam /* number of speech samples to use */ +) +{ + int i; + + for(i=0; i 1.0) + k = 0.0; + + a[i][i] = k; + + for(j=1; j<=i-1; j++) + a[i][j] = a[i-1][j] + k*a[i-1][i-j]; /* Equation 38c, Makhoul */ + + e *= (1-k*k); /* Equation 38d, Makhoul */ + } + + for(i=1; i<=order; i++) + lpcs[i] = a[order][i]; + lpcs[0] = 1.0; +} + +/*---------------------------------------------------------------------------*\ + + inverse_filter() + + Inverse Filter, A(z). Produces an array of residual samples from an array + of input samples and linear prediction coefficients. + + The filter memory is stored in the first order samples of the input array. + +\*---------------------------------------------------------------------------*/ + +void Clpc::inverse_filter( + float Sn[], /* Nsam input samples */ + float a[], /* LPCs for this frame of samples */ + int Nsam, /* number of samples */ + float res[], /* Nsam residual samples */ + int order /* order of LPC */ +) +{ + int i,j; /* loop variables */ + + for(i=0; i. +*/ + +#ifndef __LPC__ +#define __LPC__ + +#define LPC_MAX_ORDER 20 + +class Clpc { +public: + void autocorrelate(float Sn[], float Rn[], int Nsam, int order); + void levinson_durbin(float R[], float lpcs[], int order); +private: + void pre_emp(float Sn_pre[], float Sn[], float *mem, int Nsam); + void de_emp(float Sn_se[], float Sn[], float *mem, int Nsam); + void hanning_window(float Sn[], float Wn[], int Nsam); + void inverse_filter(float Sn[], float a[], int Nsam, float res[], int order); + void synthesis_filter(float res[], float a[], int Nsam, int order, float Sn_[]); + void find_aks(float Sn[], float a[], int Nsam, int order, float *E); + void weight(float ak[], float gamma, int order, float akw[]); +}; + +#endif diff --git a/USRP2M17/codec2/nlp.cpp b/USRP2M17/codec2/nlp.cpp new file mode 100644 index 0000000..753e96b --- /dev/null +++ b/USRP2M17/codec2/nlp.cpp @@ -0,0 +1,520 @@ +/*---------------------------------------------------------------------------*\ + + FILE........: nlp.c + AUTHOR......: David Rowe + DATE CREATED: 23/3/93 + + Non Linear Pitch (NLP) estimation functions. + +\*---------------------------------------------------------------------------*/ + +/* + Copyright (C) 2009 David Rowe + + All rights reserved. + + This program is free software; you can redistribute it and/or modify + it under the terms of the GNU Lesser General Public License version 2.1, as + published by the Free Software Foundation. This program 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 Lesser General Public License + along with this program; if not, see . +*/ + +#include +#include +#include + +#include "defines.h" +#include "nlp.h" +#include "kiss_fft.h" + +extern CKissFFT kiss; + +/*---------------------------------------------------------------------------*\ + + GLOBALS + +\*---------------------------------------------------------------------------*/ + +/* 48 tap 600Hz low pass FIR filter coefficients */ + +static const float nlp_fir[] = +{ + -1.0818124e-03, + -1.1008344e-03, + -9.2768838e-04, + -4.2289438e-04, + 5.5034190e-04, + 2.0029849e-03, + 3.7058509e-03, + 5.1449415e-03, + 5.5924666e-03, + 4.3036754e-03, + 8.0284511e-04, + -4.8204610e-03, + -1.1705810e-02, + -1.8199275e-02, + -2.2065282e-02, + -2.0920610e-02, + -1.2808831e-02, + 3.2204775e-03, + 2.6683811e-02, + 5.5520624e-02, + 8.6305944e-02, + 1.1480192e-01, + 1.3674206e-01, + 1.4867556e-01, + 1.4867556e-01, + 1.3674206e-01, + 1.1480192e-01, + 8.6305944e-02, + 5.5520624e-02, + 2.6683811e-02, + 3.2204775e-03, + -1.2808831e-02, + -2.0920610e-02, + -2.2065282e-02, + -1.8199275e-02, + -1.1705810e-02, + -4.8204610e-03, + 8.0284511e-04, + 4.3036754e-03, + 5.5924666e-03, + 5.1449415e-03, + 3.7058509e-03, + 2.0029849e-03, + 5.5034190e-04, + -4.2289438e-04, + -9.2768838e-04, + -1.1008344e-03, + -1.0818124e-03 +}; + +static const float fdmdv_os_filter[]= { + -0.0008215855034550382, + -0.0007833023901802921, + 0.001075563790768233, + 0.001199092367787555, + -0.001765309502928316, + -0.002055372115328064, + 0.002986877604154257, + 0.003462567920638414, + -0.004856570111126334, + -0.005563143845031497, + 0.007533613299748122, + 0.008563932468880897, + -0.01126857129039911, + -0.01280782411693687, + 0.01651443896361847, + 0.01894875110322284, + -0.02421604439474981, + -0.02845107338464062, + 0.03672973563400258, + 0.04542046150312214, + -0.06189165826716491, + -0.08721876380763803, + 0.1496157094199961, + 0.4497962274137046, + 0.4497962274137046, + 0.1496157094199961, + -0.08721876380763803, + -0.0618916582671649, + 0.04542046150312216, + 0.03672973563400257, + -0.02845107338464062, + -0.02421604439474984, + 0.01894875110322284, + 0.01651443896361848, + -0.01280782411693687, + -0.0112685712903991, + 0.008563932468880899, + 0.007533613299748123, + -0.005563143845031501, + -0.004856570111126346, + 0.003462567920638419, + 0.002986877604154259, + -0.002055372115328063, + -0.001765309502928318, + 0.001199092367787557, + 0.001075563790768233, + -0.0007833023901802925, + -0.0008215855034550383 +}; + +/*---------------------------------------------------------------------------*\ + + nlp_create() + + Initialisation function for NLP pitch estimator. + +\*---------------------------------------------------------------------------*/ + +void Cnlp::nlp_create(C2CONST *c2const) +{ + int i; + int m = c2const->m_pitch; + int Fs = c2const->Fs; + + assert((Fs == 8000) || (Fs == 16000)); + snlp.Fs = Fs; + + snlp.m = m; + + /* if running at 16kHz allocate storage for decimating filter memory */ + + if (Fs == 16000) + { + snlp.Sn16k.resize(FDMDV_OS_TAPS_16K + c2const->n_samp); + for(i=0; i Sw[], /* Freq domain version of Sn[] */ +// float W[], /* Freq domain window */ + float *prev_f0 /* previous pitch f0 in Hz, memory for pitch tracking */ +) +{ + float notch; /* current notch filter output */ + std::complex Fw[PE_FFT_SIZE]; /* DFT of squared signal (input/output) */ + float gmax; + int gmax_bin; + int m, i, j; + float best_f0; + + m = snlp.m; + + /* Square, notch filter at DC, and LP filter vector */ + + /* If running at 16 kHz decimate to 8 kHz, as NLP ws designed for + Fs = 8kHz. The decimating filter introduces about 3ms of delay, + that shouldn't be a problem as pitch changes slowly. */ + + if (snlp.Fs == 8000) + { + /* Square latest input samples */ + + for(i=m-n; i gmax) + { + gmax = Fw[i].real(); + gmax_bin = i; + } + } + + best_f0 = post_process_sub_multiples(Fw, pmax, gmax, gmax_bin, prev_f0); + + /* Shift samples in buffer to make room for new samples */ + + for(i=0; i Fw[], int pmax, float gmax, int gmax_bin, float *prev_f0) +{ + int min_bin, cmax_bin; + int mult; + float thresh, best_f0; + int b, bmin, bmax, lmax_bin; + float lmax; + int prev_f0_bin; + + /* post process estimate by searching submultiples */ + + mult = 2; + min_bin = PE_FFT_SIZE*DEC/pmax; + cmax_bin = gmax_bin; + prev_f0_bin = *prev_f0*(PE_FFT_SIZE*DEC)/SAMPLE_RATE; + + while(gmax_bin/mult >= min_bin) + { + + b = gmax_bin/mult; /* determine search interval */ + bmin = 0.8*b; + bmax = 1.2*b; + if (bmin < min_bin) + bmin = min_bin; + + /* lower threshold to favour previous frames pitch estimate, + this is a form of pitch tracking */ + + if ((prev_f0_bin > bmin) && (prev_f0_bin < bmax)) + thresh = CNLP*0.5*gmax; + else + thresh = CNLP*gmax; + + lmax = 0; + lmax_bin = bmin; + for (b=bmin; b<=bmax; b++) /* look for maximum in interval */ + if (Fw[b].real() > lmax) + { + lmax = Fw[b].real(); + lmax_bin = b; + } + + if (lmax > thresh) + if ((lmax > Fw[lmax_bin-1].real()) && (lmax > Fw[lmax_bin+1].real())) + { + cmax_bin = lmax_bin; + } + + mult++; + } + + best_f0 = (float)cmax_bin*SAMPLE_RATE/(PE_FFT_SIZE*DEC); + + return best_f0; +} + +/*---------------------------------------------------------------------------*\ + + FUNCTION....: fdmdv_16_to_8() + AUTHOR......: David Rowe + DATE CREATED: 9 May 2012 + + Changes the sample rate of a signal from 16 to 8 kHz. + + n is the number of samples at the 8 kHz rate, there are FDMDV_OS*n + samples at the 48 kHz rate. As above however a memory of + FDMDV_OS_TAPS samples is reqd for in16k[] (see t16_8.c unit test as example). + + Low pass filter the 16 kHz signal at 4 kHz using the same filter as + the upsampler, then just output every FDMDV_OS-th filtered sample. + + Note: this function copied from fdmdv.c, included in nlp.c as a convenience + to avoid linking with another source file. + +\*---------------------------------------------------------------------------*/ + +void Cnlp::fdmdv_16_to_8(float out8k[], float in16k[], int n) +{ + float acc; + int i,j,k; + + for(i=0, k=0; k *inout) +{ + std::complex in[512]; + // decide whether to use the local stack based buffer for in + // or to allow kiss_fft to allocate RAM + // second part is just to play safe since first method + // is much faster and uses less RAM + if (cfg.nfft <= 512) + { + memcpy(in, inout, cfg.nfft*sizeof(std::complex)); + kiss.fft(cfg, in, inout); + } + else + { + kiss.fft(cfg, inout, inout); + } +} diff --git a/USRP2M17/codec2/nlp.h b/USRP2M17/codec2/nlp.h new file mode 100644 index 0000000..f480b24 --- /dev/null +++ b/USRP2M17/codec2/nlp.h @@ -0,0 +1,87 @@ +/*---------------------------------------------------------------------------*\ + + FILE........: nlp.c + AUTHOR......: David Rowe + DATE CREATED: 23/3/93 + + Non Linear Pitch (NLP) estimation functions. + +\*---------------------------------------------------------------------------*/ + +/* + Copyright (C) 2009 David Rowe + + All rights reserved. + + This program is free software; you can redistribute it and/or modify + it under the terms of the GNU Lesser General Public License version 2.1, as + published by the Free Software Foundation. This program 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 Lesser General Public License + along with this program; if not, see . +*/ + +#ifndef __NLP__ +#define __NLP__ + +#include +#include + +#include "defines.h" + +/*---------------------------------------------------------------------------*\ + + DEFINES + +\*---------------------------------------------------------------------------*/ + +#define PMAX_M 320 /* maximum NLP analysis window size */ +#define COEFF 0.95 /* notch filter parameter */ +#define PE_FFT_SIZE 512 /* DFT size for pitch estimation */ +#define DEC 5 /* decimation factor */ +#define SAMPLE_RATE 8000 +#define PI 3.141592654 /* mathematical constant */ +//#define T 0.1 /* threshold for local minima candidate */ +#define F0_MAX 500 +#define CNLP 0.3 /* post processor constant */ +#define NLP_NTAP 48 /* Decimation LPF order */ + +/* 8 to 16 kHz sample rate conversion */ + +#define FDMDV_OS 2 /* oversampling rate */ +#define FDMDV_OS_TAPS_16K 48 /* number of OS filter taps at 16kHz */ +#define FDMDV_OS_TAPS_8K (FDMDV_OS_TAPS_16K/FDMDV_OS) /* number of OS filter taps at 8kHz */ + + +using NLP = struct nlp_tag +{ + int Fs; /* sample rate in Hz */ + int m; + float w[PMAX_M/DEC]; /* DFT window */ + float sq[PMAX_M]; /* squared speech samples */ + float mem_x,mem_y; /* memory for notch filter */ + float mem_fir[NLP_NTAP]; /* decimation FIR filter memory */ + FFT_STATE fft_cfg; /* kiss FFT config */ + std::vector Sn16k; /* Fs=16kHz input speech vector */ +}; + + +class Cnlp { +public: + void nlp_create(C2CONST *c2const); + void nlp_destroy(); + float nlp(float Sn[], int n, float *pitch_samples, float *prev_f0); + void codec2_fft_inplace(FFT_STATE &cfg, std::complex *inout); + +private: + float post_process_sub_multiples(std::complex Fw[], int pmax, float gmax, int gmax_bin, float *prev_f0); + void fdmdv_16_to_8(float out8k[], float in16k[], int n); + + NLP snlp; +}; + +#endif diff --git a/USRP2M17/codec2/pack.cpp b/USRP2M17/codec2/pack.cpp new file mode 100644 index 0000000..8382f95 --- /dev/null +++ b/USRP2M17/codec2/pack.cpp @@ -0,0 +1,139 @@ +/* + Copyright (C) 2010 Perens LLC + + All rights reserved. + + This program is free software; you can redistribute it and/or modify + it under the terms of the GNU Lesser General Public License version 2.1, as + published by the Free Software Foundation. This program 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 Lesser General Public License + along with this program; if not, see . +*/ + +#include "defines.h" +#include "quantise.h" +#include + +/* Compile-time constants */ +/* Size of unsigned char in bits. Assumes 8 bits-per-char. */ +static const unsigned int WordSize = 8; + +/* Mask to pick the bit component out of bitIndex. */ +static const unsigned int IndexMask = 0x7; + +/* Used to pick the word component out of bitIndex. */ +static const unsigned int ShiftRight = 3; + +/** Pack a bit field into a bit string, encoding the field in Gray code. + * + * The output is an array of unsigned char data. The fields are efficiently + * packed into the bit string. The Gray coding is a naive attempt to reduce + * the effect of single-bit errors, we expect to do a better job as the + * codec develops. + * + * This code would be simpler if it just set one bit at a time in the string, + * but would hit the same cache line more often. I'm not sure the complexity + * gains us anything here. + * + * Although field is currently of int type rather than unsigned for + * compatibility with the rest of the code, indices are always expected to + * be >= 0. + */ +void CQuantize::pack( + unsigned char *bitArray, /* The output bit string. */ + unsigned int *bitIndex, /* Index into the string in BITS, not bytes.*/ + int field, /* The bit field to be packed. */ + unsigned int fieldWidth /* Width of the field in BITS, not bytes. */ +) +{ + pack_natural_or_gray(bitArray, bitIndex, field, fieldWidth, 1); +} + +void CQuantize::pack_natural_or_gray( + unsigned char *bitArray, /* The output bit string. */ + unsigned int *bitIndex, /* Index into the string in BITS, not bytes.*/ + int field, /* The bit field to be packed. */ + unsigned int fieldWidth, /* Width of the field in BITS, not bytes. */ + unsigned int gray /* non-zero for gray coding */ +) +{ + if (gray) + { + /* Convert the field to Gray code */ + field = (field >> 1) ^ field; + } + + do + { + unsigned int bI = *bitIndex; + unsigned int bitsLeft = WordSize - (bI & IndexMask); + unsigned int sliceWidth = bitsLeft < fieldWidth ? bitsLeft : fieldWidth; + unsigned int wordIndex = bI >> ShiftRight; + + bitArray[wordIndex] |= ((unsigned char)((field >> (fieldWidth - sliceWidth)) << (bitsLeft - sliceWidth))); + + *bitIndex = bI + sliceWidth; + fieldWidth -= sliceWidth; + } + while ( fieldWidth != 0 ); +} + +/** Unpack a field from a bit string, converting from Gray code to binary. + * + */ +int CQuantize::unpack( + const unsigned char *bitArray, /* The input bit string. */ + unsigned int *bitIndex, /* Index into the string in BITS, not bytes.*/ + unsigned int fieldWidth/* Width of the field in BITS, not bytes. */ +) +{ + return unpack_natural_or_gray(bitArray, bitIndex, fieldWidth, 1); +} + +/** Unpack a field from a bit string, to binary, optionally using + * natural or Gray code. + * + */ +int CQuantize::unpack_natural_or_gray( + const unsigned char *bitArray, /* The input bit string. */ + unsigned int *bitIndex, /* Index into the string in BITS, not bytes.*/ + unsigned int fieldWidth,/* Width of the field in BITS, not bytes. */ + unsigned int gray /* non-zero for Gray coding */ +) +{ + unsigned int field = 0; + unsigned int t; + + do + { + unsigned int bI = *bitIndex; + unsigned int bitsLeft = WordSize - (bI & IndexMask); + unsigned int sliceWidth = bitsLeft < fieldWidth ? bitsLeft : fieldWidth; + + field |= (((bitArray[bI >> ShiftRight] >> (bitsLeft - sliceWidth)) & ((1 << sliceWidth) - 1)) << (fieldWidth - sliceWidth)); + + *bitIndex = bI + sliceWidth; + fieldWidth -= sliceWidth; + } + while ( fieldWidth != 0 ); + + if (gray) + { + /* Convert from Gray code to binary. Works for maximum 8-bit fields. */ + t = field ^ (field >> 8); + t ^= (t >> 4); + t ^= (t >> 2); + t ^= (t >> 1); + } + else + { + t = field; + } + + return t; +} diff --git a/USRP2M17/codec2/qbase.cpp b/USRP2M17/codec2/qbase.cpp new file mode 100644 index 0000000..a94ef46 --- /dev/null +++ b/USRP2M17/codec2/qbase.cpp @@ -0,0 +1,247 @@ +#include +#include + +#include "qbase.h" + +/*---------------------------------------------------------------------------*\ + + quantise + + Quantises vec by choosing the nearest vector in codebook cb, and + returns the vector index. The squared error of the quantised vector + is added to se. + +\*---------------------------------------------------------------------------*/ + +long CQbase::quantise(const float *cb, float vec[], float w[], int k, int m, float *se) +/* float cb[][K]; current VQ codebook */ +/* float vec[]; vector to quantise */ +/* float w[]; weighting vector */ +/* int k; dimension of vectors */ +/* int m; size of codebook */ +/* float *se; accumulated squared error */ +{ + float e; /* current error */ + long besti; /* best index so far */ + float beste; /* best error so far */ + long j; + int i; + float diff; + + besti = 0; + beste = 1E32; + for(j=0; jWo/PI)*4000.0/50.0)/log10f(2); + x[1] = 10.0*log10f(1e-4 + e); + + compute_weights2(x, xq, w); + for (i=0; iWo_min; + float Wo_max = c2const->Wo_max; + + for (i=0; iWo = powf(2.0, xq[0])*(PI*50.0)/4000.0; + + /* bit errors can make us go out of range leading to all sorts of + probs like seg faults */ + + if (model->Wo > Wo_max) model->Wo = Wo_max; + if (model->Wo < Wo_min) model->Wo = Wo_min; + + model->L = PI/model->Wo; /* if we quantise Wo re-compute L */ + + *e = exp10f(xq[1]/10.0); +} + +void CQbase::compute_weights2(const float *x, const float *xp, float *w) +{ + w[0] = 30; + w[1] = 1; + if (x[1]<0) + { + w[0] *= .6; + w[1] *= .3; + } + if (x[1]<-10) + { + w[0] *= .3; + w[1] *= .3; + } + /* Higher weight if pitch is stable */ + if (fabsf(x[0]-xp[0])<.2) + { + w[0] *= 2; + w[1] *= 1.5; + } + else if (fabsf(x[0]-xp[0])>.5) /* Lower if not stable */ + { + w[0] *= .5; + } + + /* Lower weight for low energy */ + if (x[1] < xp[1]-10) + { + w[1] *= .5; + } + if (x[1] < xp[1]-20) + { + w[1] *= .5; + } + + //w[0] = 30; + //w[1] = 1; + + /* Square the weights because it's applied on the squared error */ + w[0] *= w[0]; + w[1] *= w[1]; + +} + +int CQbase::find_nearest_weighted(const float *codebook, int nb_entries, float *x, const float *w, int ndim) +{ + int i, j; + float min_dist = 1e15; + int nearest = 0; + + for (i=0; iWo_min; + float Wo_max = c2const->Wo_max; + float norm; + + norm = (log10f(Wo) - log10f(Wo_min))/(log10f(Wo_max) - log10f(Wo_min)); + index = floorf(Wo_levels * norm + 0.5); + if (index < 0 ) index = 0; + if (index > (Wo_levels-1)) index = Wo_levels-1; + + return index; +} + +/*---------------------------------------------------------------------------*\ + + FUNCTION....: decode_log_Wo() + AUTHOR......: David Rowe + DATE CREATED: 22/8/2010 + + Decodes Wo using a WO_LEVELS quantiser in the log domain. + +\*---------------------------------------------------------------------------*/ + +float CQbase::decode_log_Wo(C2CONST *c2const, int index, int bits) +{ + float Wo_min = c2const->Wo_min; + float Wo_max = c2const->Wo_max; + float step; + float Wo; + int Wo_levels = 1<. + +*/ + +#include +#include +#include +#include +#include +#include + +#include "defines.h" +#include "quantise.h" +#include "lpc.h" +#include "kiss_fft.h" + +extern CKissFFT kiss; + +#define LSP_DELTA1 0.01 /* grid spacing for LSP root searches */ + +/*---------------------------------------------------------------------------*\ + + FUNCTIONS + +\*---------------------------------------------------------------------------*/ + +int CQuantize::lsp_bits(int i) +{ + return lsp_cb[i].log2m; +} + +int CQuantize::lspd_bits(int i) +{ + return lsp_cbd[i].log2m; +} + + + +/*---------------------------------------------------------------------------*\ + + encode_lspds_scalar() + + Scalar/VQ LSP difference quantiser. + +\*---------------------------------------------------------------------------*/ + +void CQuantize::encode_lspds_scalar(int indexes[], float lsp[], int order) +{ + int i,k,m; + float lsp_hz[order]; + float lsp__hz[order]; + float dlsp[order]; + float dlsp_[order]; + float wt[order]; + const float *cb; + float se; + + for(i=0; i Ww[FFT_ENC/2+1]; /* weighting spectrum */ + float Rw[FFT_ENC/2+1]; /* R = WA */ + float e_before, e_after, gain; + float Pfw; + float max_Rw, min_Rw; + float coeff; + + /* Determine weighting filter spectrum W(exp(jw)) ---------------*/ + + for(i=0; i max_Rw) + max_Rw = Rw[i]; + if (Rw[i] < min_Rw) + min_Rw = Rw[i]; + + } + + /* create post filter mag spectrum and apply ------------------*/ + + /* measure energy before post filtering */ + + e_before = 1E-4; + for(i=0; i Aw[] /* output power spectrum */ +) +{ + int i,m; /* loop variables */ + int am,bm; /* limits of current band */ + float r; /* no. rads/bin */ + float Em; /* energy in band */ + float Am; /* spectral amplitude sample */ + float signal, noise; + + r = TWO_PI/(FFT_ENC); + + /* Determine DFT of A(exp(jw)) --------------------------------------------*/ + { + float a[FFT_ENC]; /* input to FFT for power spectrum */ + + for(i=0; iL; m++) + { + am = (int)((m - 0.5)*model->Wo/r + 0.5); + bm = (int)((m + 0.5)*model->Wo/r + 0.5); + + // FIXME: With arm_rfft_fast_f32 we have to use this + // otherwise sometimes a to high bm is calculated + // which causes trouble later in the calculation + // chain + // it seems for some reason model->Wo is calculated somewhat too high + if (bm>FFT_ENC/2) + { + bm = FFT_ENC/2; + } + Em = 0.0; + + for(i=am; iA[m]*model->A[m]; + noise += (model->A[m] - Am)*(model->A[m] - Am); + + /* This code significantly improves perf of LPC model, in + particular when combined with phase0. The LPC spectrum tends + to track just under the peaks of the spectral envelope, and + just above nulls. This algorithm does the reverse to + compensate - raising the amplitudes of spectral peaks, while + attenuating the null. This enhances the formants, and + supresses the energy between formants. */ + + if (sim_pf) + { + if (Am > model->A[m]) + Am *= 0.7; + if (Am < model->A[m]) + Am *= 1.4; + } + model->A[m] = Am; + } + *snr = 10.0*log10f(signal/noise); +} + +/*---------------------------------------------------------------------------*\ + + FUNCTION....: encode_Wo() + AUTHOR......: David Rowe + DATE CREATED: 22/8/2010 + + Encodes Wo using a WO_LEVELS quantiser. + +\*---------------------------------------------------------------------------*/ + +int CQuantize::encode_Wo(C2CONST *c2const, float Wo, int bits) +{ + int index, Wo_levels = 1<Wo_min; + float Wo_max = c2const->Wo_max; + float norm; + + norm = (Wo - Wo_min)/(Wo_max - Wo_min); + index = floorf(Wo_levels * norm + 0.5); + if (index < 0 ) index = 0; + if (index > (Wo_levels-1)) index = Wo_levels-1; + + return index; +} + +/*---------------------------------------------------------------------------*\ + + FUNCTION....: decode_Wo() + AUTHOR......: David Rowe + DATE CREATED: 22/8/2010 + + Decodes Wo using a WO_LEVELS quantiser. + +\*---------------------------------------------------------------------------*/ + +float CQuantize::decode_Wo(C2CONST *c2const, int index, int bits) +{ + float Wo_min = c2const->Wo_min; + float Wo_max = c2const->Wo_max; + float step; + float Wo; + int Wo_levels = 1<Wo < (PI*150.0/4000)) + { + model->A[1] *= 0.032; + } +} + +/*---------------------------------------------------------------------------*\ + + FUNCTION....: encode_energy() + AUTHOR......: David Rowe + DATE CREATED: 22/8/2010 + + Encodes LPC energy using an E_LEVELS quantiser. + +\*---------------------------------------------------------------------------*/ + +int CQuantize::encode_energy(float e, int bits) +{ + int index, e_levels = 1< (e_levels-1)) index = e_levels-1; + + return index; +} + +/*---------------------------------------------------------------------------*\ + + FUNCTION....: decode_energy() + AUTHOR......: David Rowe + DATE CREATED: 22/8/2010 + + Decodes energy using a E_LEVELS quantiser. + +\*---------------------------------------------------------------------------*/ + +float CQuantize::decode_energy(int index, int bits) +{ + float e_min = E_MIN_DB; + float e_max = E_MAX_DB; + float step; + float e; + int e_levels = 1<= -1.0)) + { + xr = xl - delta ; /* interval spacing */ + psumr = cheb_poly_eva(pt,xr,order);/* poly(xl-delta_x) */ + temp_psumr = psumr; + temp_xr = xr; + + /* if no sign change increment xr and re-evaluate + poly(xr). Repeat til sign change. if a sign change has + occurred the interval is bisected and then checked again + for a sign change which determines in which interval the + zero lies in. If there is no sign change between poly(xm) + and poly(xl) set interval between xm and xr else set + interval between xl and xr and repeat till root is located + within the specified limits */ + + if(((psumr*psuml)<0.0) || (psumr == 0.0)) + { + roots++; + + psumm=psuml; + for(k=0; k<=nb; k++) + { + xm = (xl+xr)/2; /* bisect the interval */ + psumm=cheb_poly_eva(pt,xm,order); + if(psumm*psuml>0.) + { + psuml=psumm; + xl=xm; + } + else + { + psumr=psumm; + xr=xm; + } + } + + /* once zero is found, reset initial interval to xr */ + freq[j] = (xm); + xl = xm; + flag = 0; /* reset flag for next search */ + } + else + { + psuml=temp_psumr; + xl=temp_xr; + } + } + } + + /* convert from x domain to radians */ + + for(i=0; i. +*/ + +#ifndef __QUANTISE__ +#define __QUANTISE__ + +#include + +#include "qbase.h" + +class CQuantize : public CQbase { +public: + void aks_to_M2(FFTR_STATE *fftr_fwd_cfg, float ak[], int order, MODEL *model, float E, float *snr, int sim_pf, int pf, int bass_boost, float beta, float gamma, std::complex Aw[]); + + int encode_Wo(C2CONST *c2const, float Wo, int bits); + float decode_Wo(C2CONST *c2const, int index, int bits); + void encode_lsps_scalar(int indexes[], float lsp[], int order); + void decode_lsps_scalar(float lsp[], int indexes[], int order); + void encode_lspds_scalar(int indexes[], float lsp[], int order); + void decode_lspds_scalar(float lsp[], int indexes[], int order); + + int encode_energy(float e, int bits); + float decode_energy(int index, int bits); + + void pack(unsigned char * bits, unsigned int *nbit, int index, unsigned int index_bits); + void pack_natural_or_gray(unsigned char * bits, unsigned int *nbit, int index, unsigned int index_bits, unsigned int gray); + int unpack(const unsigned char * bits, unsigned int *nbit, unsigned int index_bits); + int unpack_natural_or_gray(const unsigned char * bits, unsigned int *nbit, unsigned int index_bits, unsigned int gray); + + int lsp_bits(int i); + int lspd_bits(int i); + + void apply_lpc_correction(MODEL *model); + float speech_to_uq_lsps(float lsp[], float ak[], float Sn[], float w[], int m_pitch, int order); + int check_lsp_order(float lsp[], int lpc_order); + void bw_expand_lsps(float lsp[], int order, float min_sep_low, float min_sep_high); + +private: + void compute_weights(const float *x, float *w, int ndim); + int find_nearest(const float *codebook, int nb_entries, float *x, int ndim); + void lpc_post_filter(FFTR_STATE *fftr_fwd_cfg, float Pw[], float ak[], int order, float beta, float gamma, int bass_boost, float E); + int lpc_to_lsp (float *a, int lpcrdr, float *freq, int nb, float delta); + float cheb_poly_eva(float *coef,float x,int order); +}; + +#endif