mirror of https://github.com/ShaYmez/MMDVM_CM.git
Add USRP2M17
This commit is contained in:
Doug McLain
parent
d684d0924c
commit
96184ff024
|
|
@ -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();
|
||||
|
|
|
|||
|
|
@ -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 <cstdio>
|
||||
#include <cstdlib>
|
||||
#include <cstring>
|
||||
#include <cctype>
|
||||
|
||||
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;
|
||||
}
|
||||
|
|
@ -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 <string>
|
||||
#include <vector>
|
||||
|
||||
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
|
||||
|
|
@ -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 <Windows.h>
|
||||
#else
|
||||
#include <sys/time.h>
|
||||
#endif
|
||||
|
||||
#include <cstdio>
|
||||
#include <cstdlib>
|
||||
#include <cstdarg>
|
||||
#include <ctime>
|
||||
#include <cassert>
|
||||
#include <cstring>
|
||||
|
||||
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);
|
||||
}
|
||||
}
|
||||
|
|
@ -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 <string>
|
||||
|
||||
#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
|
||||
|
|
@ -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 <cstdio>
|
||||
#include <cassert>
|
||||
#include <cstring>
|
||||
|
||||
|
||||
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");
|
||||
}
|
||||
|
|
@ -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 <cstdint>
|
||||
#include <string>
|
||||
|
||||
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
|
||||
|
|
@ -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 *~
|
||||
|
||||
|
|
@ -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 <cstdio>
|
||||
#include <cstring>
|
||||
|
||||
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];
|
||||
}
|
||||
|
|
@ -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<uint8_t> m_M17;
|
||||
CRingBuffer<int16_t> m_USRP;
|
||||
CCodec2 *m_c2;
|
||||
uint16_t m_m17GainMultiplier;
|
||||
bool m_m17Attenuate;
|
||||
uint16_t m_usrpGainMultiplier;
|
||||
bool m_usrpAttenuate;
|
||||
};
|
||||
|
||||
#endif
|
||||
|
|
@ -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.
|
||||
|
|
@ -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 <cstdio>
|
||||
#include <cassert>
|
||||
#include <cstring>
|
||||
|
||||
template<class T> 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
|
||||
|
|
@ -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 <cstdio>
|
||||
#include <ctime>
|
||||
|
||||
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
|
||||
|
|
@ -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 <windows.h>
|
||||
#else
|
||||
#include <sys/time.h>
|
||||
#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
|
||||
|
|
@ -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 <cstdio>
|
||||
#include <cassert>
|
||||
|
||||
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;
|
||||
}
|
||||
|
|
@ -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
|
||||
|
|
@ -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 <cassert>
|
||||
#include <cerrno>
|
||||
#include <cstring>
|
||||
|
||||
|
||||
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
|
||||
}
|
||||
|
|
@ -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 <string>
|
||||
|
||||
#include <netdb.h>
|
||||
#include <sys/time.h>
|
||||
#include <sys/types.h>
|
||||
#include <sys/socket.h>
|
||||
#include <unistd.h>
|
||||
#include <netinet/in.h>
|
||||
#include <arpa/inet.h>
|
||||
#include <errno.h>
|
||||
|
||||
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
|
||||
|
|
@ -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 <sys/time.h>
|
||||
#include <unistd.h>
|
||||
#include <signal.h>
|
||||
#include <fcntl.h>
|
||||
#include <pwd.h>
|
||||
|
||||
#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 <functional>
|
||||
#include <algorithm>
|
||||
#include <cstdio>
|
||||
#include <cstdlib>
|
||||
#include <cstring>
|
||||
#include <clocale>
|
||||
#include <cctype>
|
||||
|
||||
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<uint16_t>((::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;
|
||||
}
|
||||
|
|
@ -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 <string>
|
||||
|
||||
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
|
||||
|
|
@ -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
|
||||
|
||||
|
|
@ -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 <cstdio>
|
||||
|
||||
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);
|
||||
}
|
||||
|
|
@ -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 <string>
|
||||
#include <cstdint>
|
||||
|
||||
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
|
||||
|
|
@ -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 <cstdio>
|
||||
#include <cassert>
|
||||
|
||||
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;
|
||||
}
|
||||
|
|
@ -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 <string>
|
||||
|
||||
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
|
||||
|
|
@ -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
|
||||
|
|
@ -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 }
|
||||
};
|
||||
File diff suppressed because it is too large
Load Diff
|
|
@ -0,0 +1,100 @@
|
|||
/*---------------------------------------------------------------------------*\
|
||||
|
||||
FILE........: codec2.h
|
||||
AUTHOR......: David Rowe
|
||||
DATE CREATED: 21 August 2010
|
||||
|
||||
Codec 2 fully quantised encoder and decoder functions. If you want use
|
||||
Codec 2, these are the functions you need to call.
|
||||
|
||||
\*---------------------------------------------------------------------------*/
|
||||
|
||||
/*
|
||||
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 <http://www.gnu.org/licenses/>.
|
||||
*/
|
||||
|
||||
#ifndef __CODEC2__
|
||||
#define __CODEC2__
|
||||
|
||||
#include <complex>
|
||||
|
||||
#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<float> filter_phase[], std::complex<float> A[]);
|
||||
void phase_synth_zero_order(int n_samp, MODEL *model, float *ex_phase, std::complex<float> 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<float> Sw[], float Sn[], float w[]);
|
||||
void two_stage_pitch_refinement(C2CONST *c2const, MODEL *model, std::complex<float> Sw[]);
|
||||
void estimate_amplitudes(MODEL *model, std::complex<float> Sw[], int est_phase);
|
||||
float est_voicing_mbe(C2CONST *c2const, MODEL *model, std::complex<float> 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<float> 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<float> 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
|
||||
|
|
@ -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 <http://www.gnu.org/licenses/>.
|
||||
*/
|
||||
|
||||
#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<float> w; /* [m_pitch] time domain hamming window */
|
||||
std::vector<float> Pn; /* [2*n_samp] trapezoidal synthesis window */
|
||||
std::vector<float> Sn; /* [m_pitch] input speech */
|
||||
std::vector<float> Sn_; /* [2*n_samp] synthesised output speech */
|
||||
std::vector<float> bpf_buf; /* buffer for band pass filter */
|
||||
};
|
||||
|
||||
#endif
|
||||
|
|
@ -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 <http://www.gnu.org/licenses/>.
|
||||
*/
|
||||
|
||||
#ifndef __DEFINES__
|
||||
#define __DEFINES__
|
||||
|
||||
#include <complex>
|
||||
#include <vector>
|
||||
|
||||
/*---------------------------------------------------------------------------*\
|
||||
|
||||
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<std::complex<float>> twiddles;
|
||||
};
|
||||
|
||||
using FFTR_STATE = struct fftr_state_tag
|
||||
{
|
||||
FFT_STATE substate;
|
||||
std::vector<std::complex<float>> tmpbuf;
|
||||
std::vector<std::complex<float>> super_twiddles;
|
||||
};
|
||||
|
||||
extern const struct lsp_codebook lsp_cb[];
|
||||
extern const struct lsp_codebook lsp_cbd[];
|
||||
extern const struct lsp_codebook ge_cb[];
|
||||
|
||||
#endif
|
||||
|
|
@ -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 <cstring>
|
||||
#include <cassert>
|
||||
|
||||
#include "defines.h"
|
||||
#include "kiss_fft.h"
|
||||
|
||||
void CKissFFT::kf_bfly2(std::complex<float> *Fout, const size_t fstride, FFT_STATE &st, int m)
|
||||
{
|
||||
std::complex<float> *Fout2;
|
||||
std::complex<float> *tw1 = st.twiddles.data();
|
||||
std::complex<float> 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<float> * Fout, const size_t fstride, FFT_STATE &st, int m)
|
||||
{
|
||||
const size_t m2 = 2 * m;
|
||||
std::complex<float> *tw1,*tw2;
|
||||
std::complex<float> scratch[5];
|
||||
std::complex<float> 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<float> *Fout, const size_t fstride, FFT_STATE &st, int m)
|
||||
{
|
||||
std::complex<float> *tw1,*tw2,*tw3;
|
||||
std::complex<float> 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<float> * Fout, const size_t fstride, FFT_STATE &st, int m)
|
||||
{
|
||||
std::complex<float> scratch[13];
|
||||
std::complex<float> *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<m; ++u)
|
||||
{
|
||||
scratch[0] = *Fout0;
|
||||
|
||||
scratch[1] = *Fout1 * tw[u*fstride];
|
||||
scratch[2] = *Fout2 * tw[2*u*fstride];
|
||||
scratch[3] = *Fout3 * tw[3*u*fstride];
|
||||
scratch[4] = *Fout4 * tw[4*u*fstride];
|
||||
|
||||
scratch[7] = scratch[1] + scratch[4];
|
||||
scratch[10] = scratch[1] - scratch[4];
|
||||
scratch[8] = scratch[2] + scratch[3];
|
||||
scratch[9] = scratch[2] - scratch[3];
|
||||
|
||||
*Fout0 += scratch[7] + scratch[8];
|
||||
|
||||
scratch[5] = scratch[0] + (scratch[7] * ya.real()) + (scratch[8] * yb.real());
|
||||
|
||||
scratch[6].real( (scratch[10].imag() * ya.imag()) + (scratch[9].imag() * yb.imag()));
|
||||
scratch[6].imag(-(scratch[10].real() * ya.imag()) - (scratch[9].real() * yb.imag()));
|
||||
|
||||
*Fout1 = scratch[5] - scratch[6];
|
||||
*Fout4 = scratch[5] + scratch[6];
|
||||
|
||||
scratch[11] = scratch[0] + (scratch[7] * yb.real()) + (scratch[8] * ya.real());
|
||||
scratch[12].real(-(scratch[10].imag() * yb.imag()) + (scratch[9].imag() * ya.imag()));
|
||||
scratch[12].imag( (scratch[10].real() * yb.imag()) - (scratch[9].real() * ya.imag()));
|
||||
|
||||
*Fout2 = scratch[11] + scratch[12];
|
||||
*Fout3 = scratch[11] - scratch[12];
|
||||
|
||||
++Fout0;
|
||||
++Fout1;
|
||||
++Fout2;
|
||||
++Fout3;
|
||||
++Fout4;
|
||||
}
|
||||
}
|
||||
|
||||
/* perform the butterfly for one stage of a mixed radix FFT */
|
||||
void CKissFFT::kf_bfly_generic(std::complex<float> *Fout, const size_t fstride, FFT_STATE &st, int m, int p)
|
||||
{
|
||||
auto twiddles = st.twiddles.data();
|
||||
std::complex<float> t;
|
||||
int Norig = st.nfft;
|
||||
|
||||
std::vector<std::complex<float>> scratch(p);
|
||||
|
||||
for (int u=0; u<m; ++u)
|
||||
{
|
||||
int k = u;
|
||||
for (int q1=0 ; q1<p ; ++q1)
|
||||
{
|
||||
scratch[q1] = Fout[k];
|
||||
k += m;
|
||||
}
|
||||
|
||||
k = u;
|
||||
for (int q1=0 ; q1<p ; ++q1)
|
||||
{
|
||||
int twidx = 0;
|
||||
Fout[k] = scratch[0];
|
||||
for (int q=1; q<p; ++q)
|
||||
{
|
||||
twidx += fstride * k;
|
||||
if (twidx >= Norig) twidx-=Norig;
|
||||
t = scratch[q] * twiddles[twidx];
|
||||
Fout[k] += t;
|
||||
}
|
||||
k += m;
|
||||
}
|
||||
}
|
||||
scratch.clear();
|
||||
}
|
||||
|
||||
void CKissFFT::kf_work(std::complex<float> *Fout, const std::complex<float> *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<float> *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<nfft; ++i)
|
||||
{
|
||||
const double pi=3.141592653589793238462643383279502884197169399375105820974944;
|
||||
double phase = -2.0 * pi * i / nfft;
|
||||
if (state.inverse)
|
||||
phase *= -1.0;
|
||||
state.twiddles[i] = std::polar(1.0f, float(phase));
|
||||
}
|
||||
|
||||
kf_factor(nfft, state.factors);
|
||||
}
|
||||
|
||||
|
||||
void CKissFFT::fft_stride(FFT_STATE &st, const std::complex<float> *fin, std::complex<float> *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<std::complex<float>> tmpbuf(st.nfft);
|
||||
kf_work(tmpbuf.data(), fin, true, in_stride, st.factors, st);
|
||||
memcpy(fout, tmpbuf.data(), sizeof(std::complex<float>)*st.nfft);
|
||||
tmpbuf.clear();
|
||||
}
|
||||
else
|
||||
{
|
||||
kf_work(fout, fin, 1, in_stride, st.factors, st);
|
||||
}
|
||||
}
|
||||
|
||||
void CKissFFT::fft(FFT_STATE &cfg, const std::complex<float> *fin, std::complex<float> *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<nfft/2; ++i)
|
||||
{
|
||||
double phase = -3.141592653589793238462643383279502884197169399375105820974944 * (double(i+1) / nfft + .5);
|
||||
if (inverse_fft)
|
||||
phase *= -1.0;
|
||||
st.super_twiddles[i] = std::polar(1.0f, float(phase));
|
||||
}
|
||||
}
|
||||
|
||||
void CKissFFT::fftr(FFTR_STATE &st, const float *timedata, std::complex<float> *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<float>*)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<float> *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<float> *)timedata);
|
||||
}
|
||||
|
|
@ -0,0 +1,35 @@
|
|||
#ifndef KISS_FFT_H
|
||||
#define KISS_FFT_H
|
||||
|
||||
#include <complex>
|
||||
|
||||
#include <stdlib.h>
|
||||
#include <stdio.h>
|
||||
#include <math.h>
|
||||
#include <string.h>
|
||||
|
||||
#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<float> *fin, std::complex<float> *fout);
|
||||
void fft_stride(FFT_STATE &cfg, const std::complex<float> *fin, std::complex<float> *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<float> *freqdata);
|
||||
void fftri(FFTR_STATE &cfg,const std::complex<float> *freqdata,float *timedata);
|
||||
private:
|
||||
void kf_bfly2(std::complex<float> *Fout, const size_t fstride, FFT_STATE &st, int m);
|
||||
void kf_bfly3(std::complex<float> *Fout, const size_t fstride, FFT_STATE &st, int m);
|
||||
void kf_bfly4(std::complex<float> *Fout, const size_t fstride, FFT_STATE &st, int m);
|
||||
void kf_bfly5(std::complex<float> *Fout, const size_t fstride, FFT_STATE &st, int m);
|
||||
void kf_bfly_generic(std::complex<float> *Fout, const size_t fstride, FFT_STATE &st, int m, int p);
|
||||
void kf_work(std::complex<float> *Fout, const std::complex<float> *f, const size_t fstride, int in_stride, int *factors, FFT_STATE &st);
|
||||
void kf_factor(int n, int *facbuf);
|
||||
};
|
||||
#endif
|
||||
|
|
@ -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 <http://www.gnu.org/licenses/>.
|
||||
*/
|
||||
|
||||
#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 <assert.h>
|
||||
#include <math.h>
|
||||
#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<Nsam; i++)
|
||||
{
|
||||
Sn_pre[i] = Sn[i] - ALPHA * mem[0];
|
||||
mem[0] = Sn[i];
|
||||
}
|
||||
|
||||
}
|
||||
|
||||
|
||||
/*---------------------------------------------------------------------------*\
|
||||
|
||||
de_emp()
|
||||
|
||||
De-emphasis filter (low pass filter with a pole close to 0 Hz).
|
||||
|
||||
\*---------------------------------------------------------------------------*/
|
||||
|
||||
void Clpc::de_emp(
|
||||
float Sn_de[], /* 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<Nsam; i++)
|
||||
{
|
||||
Sn_de[i] = Sn[i] + BETA * mem[0];
|
||||
mem[0] = Sn_de[i];
|
||||
}
|
||||
|
||||
}
|
||||
|
||||
|
||||
/*---------------------------------------------------------------------------*\
|
||||
|
||||
hanning_window()
|
||||
|
||||
Hanning windows a frame of speech samples.
|
||||
|
||||
\*---------------------------------------------------------------------------*/
|
||||
|
||||
void Clpc::hanning_window(
|
||||
float Sn[], /* input frame of speech samples */
|
||||
float Wn[], /* output frame of windowed samples */
|
||||
int Nsam /* number of samples */
|
||||
)
|
||||
{
|
||||
int i; /* loop variable */
|
||||
|
||||
for(i=0; i<Nsam; i++)
|
||||
Wn[i] = Sn[i]*(0.5 - 0.5*cosf(2*PI*(float)i/(Nsam-1)));
|
||||
}
|
||||
|
||||
/*---------------------------------------------------------------------------*\
|
||||
|
||||
autocorrelate()
|
||||
|
||||
Finds the first P autocorrelation values of an array of windowed speech
|
||||
samples Sn[].
|
||||
|
||||
\*---------------------------------------------------------------------------*/
|
||||
|
||||
void Clpc::autocorrelate(
|
||||
float Sn[], /* frame of Nsam windowed speech samples */
|
||||
float Rn[], /* array of P+1 autocorrelation coefficients */
|
||||
int Nsam, /* number of windowed samples to use */
|
||||
int order /* order of LPC analysis */
|
||||
)
|
||||
{
|
||||
int i,j; /* loop variables */
|
||||
|
||||
for(j=0; j<order+1; j++)
|
||||
{
|
||||
Rn[j] = 0.0;
|
||||
for(i=0; i<Nsam-j; i++)
|
||||
Rn[j] += Sn[i]*Sn[i+j];
|
||||
}
|
||||
}
|
||||
|
||||
/*---------------------------------------------------------------------------*\
|
||||
|
||||
levinson_durbin()
|
||||
|
||||
Given P+1 autocorrelation coefficients, finds P Linear Prediction Coeff.
|
||||
(LPCs) where P is the order of the LPC all-pole model. The Levinson-Durbin
|
||||
algorithm is used, and is described in:
|
||||
|
||||
J. Makhoul
|
||||
"Linear prediction, a tutorial review"
|
||||
Proceedings of the IEEE
|
||||
Vol-63, No. 4, April 1975
|
||||
|
||||
\*---------------------------------------------------------------------------*/
|
||||
|
||||
void Clpc::levinson_durbin(
|
||||
float R[], /* order+1 autocorrelation coeff */
|
||||
float lpcs[], /* order+1 LPC's */
|
||||
int order /* order of the LPC analysis */
|
||||
)
|
||||
{
|
||||
float a[order+1][order+1];
|
||||
float sum, e, k;
|
||||
int i,j; /* loop variables */
|
||||
|
||||
e = R[0]; /* Equation 38a, Makhoul */
|
||||
|
||||
for(i=1; i<=order; i++)
|
||||
{
|
||||
sum = 0.0;
|
||||
for(j=1; j<=i-1; j++)
|
||||
sum += a[i-1][j]*R[i-j];
|
||||
k = -1.0*(R[i] + sum)/e; /* Equation 38b, Makhoul */
|
||||
if (fabsf(k) > 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<Nsam; i++)
|
||||
{
|
||||
res[i] = 0.0;
|
||||
for(j=0; j<=order; j++)
|
||||
res[i] += Sn[i-j]*a[j];
|
||||
}
|
||||
}
|
||||
|
||||
/*---------------------------------------------------------------------------*\
|
||||
|
||||
synthesis_filter()
|
||||
|
||||
C version of the Speech Synthesis Filter, 1/A(z). Given an array of
|
||||
residual or excitation samples, and the the LP filter coefficients, this
|
||||
function will produce an array of speech samples. This filter structure is
|
||||
IIR.
|
||||
|
||||
The synthesis filter has memory as well, this is treated in the same way
|
||||
as the memory for the inverse filter (see inverse_filter() notes above).
|
||||
The difference is that the memory for the synthesis filter is stored in
|
||||
the output array, wheras the memory of the inverse filter is stored in the
|
||||
input array.
|
||||
|
||||
Note: the calling function must update the filter memory.
|
||||
|
||||
\*---------------------------------------------------------------------------*/
|
||||
|
||||
void Clpc::synthesis_filter(
|
||||
float res[], /* Nsam input residual (excitation) samples */
|
||||
float a[], /* LPCs for this frame of speech samples */
|
||||
int Nsam, /* number of speech samples */
|
||||
int order, /* LPC order */
|
||||
float Sn_[] /* Nsam output synthesised speech samples */
|
||||
)
|
||||
{
|
||||
int i,j; /* loop variables */
|
||||
|
||||
/* Filter Nsam samples */
|
||||
|
||||
for(i=0; i<Nsam; i++)
|
||||
{
|
||||
Sn_[i] = res[i]*a[0];
|
||||
for(j=1; j<=order; j++)
|
||||
Sn_[i] -= Sn_[i-j]*a[j];
|
||||
}
|
||||
}
|
||||
|
||||
/*---------------------------------------------------------------------------*\
|
||||
|
||||
find_aks()
|
||||
|
||||
This function takes a frame of samples, and determines the linear
|
||||
prediction coefficients for that frame of samples.
|
||||
|
||||
\*---------------------------------------------------------------------------*/
|
||||
|
||||
void Clpc::find_aks(
|
||||
float Sn[], /* Nsam samples with order sample memory */
|
||||
float a[], /* order+1 LPCs with first coeff 1.0 */
|
||||
int Nsam, /* number of input speech samples */
|
||||
int order, /* order of the LPC analysis */
|
||||
float *E /* residual energy */
|
||||
)
|
||||
{
|
||||
float Wn[LPC_MAX_N]; /* windowed frame of Nsam speech samples */
|
||||
float R[order+1]; /* order+1 autocorrelation values of Sn[] */
|
||||
int i;
|
||||
|
||||
assert(Nsam < LPC_MAX_N);
|
||||
|
||||
hanning_window(Sn,Wn,Nsam);
|
||||
autocorrelate(Wn,R,Nsam,order);
|
||||
levinson_durbin(R,a,order);
|
||||
|
||||
*E = 0.0;
|
||||
for(i=0; i<=order; i++)
|
||||
*E += a[i]*R[i];
|
||||
if (*E < 0.0)
|
||||
*E = 1E-12;
|
||||
}
|
||||
|
||||
/*---------------------------------------------------------------------------*\
|
||||
|
||||
weight()
|
||||
|
||||
Weights a vector of LPCs.
|
||||
|
||||
\*---------------------------------------------------------------------------*/
|
||||
|
||||
void Clpc::weight(
|
||||
float ak[], /* vector of order+1 LPCs */
|
||||
float gamma, /* weighting factor */
|
||||
int order, /* num LPCs (excluding leading 1.0) */
|
||||
float akw[] /* weighted vector of order+1 LPCs */
|
||||
)
|
||||
{
|
||||
int i;
|
||||
|
||||
for(i=1; i<=order; i++)
|
||||
akw[i] = ak[i]*powf(gamma,(float)i);
|
||||
}
|
||||
|
|
@ -0,0 +1,47 @@
|
|||
/*---------------------------------------------------------------------------*\
|
||||
|
||||
FILE........: lpc.h
|
||||
AUTHOR......: David Rowe
|
||||
DATE CREATED: 24/8/09
|
||||
|
||||
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 <http://www.gnu.org/licenses/>.
|
||||
*/
|
||||
|
||||
#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
|
||||
|
|
@ -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 <http://www.gnu.org/licenses/>.
|
||||
*/
|
||||
|
||||
#include <assert.h>
|
||||
#include <math.h>
|
||||
#include <stdlib.h>
|
||||
|
||||
#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<FDMDV_OS_TAPS_16K; i++)
|
||||
{
|
||||
snlp.Sn16k[i] = 0.0;
|
||||
}
|
||||
|
||||
/* most processing occurs at 8 kHz sample rate so halve m */
|
||||
|
||||
m /= 2;
|
||||
}
|
||||
|
||||
assert(m <= PMAX_M);
|
||||
|
||||
for(i=0; i<m/DEC; i++)
|
||||
{
|
||||
snlp.w[i] = 0.5 - 0.5*cosf(2*PI*i/(m/DEC-1));
|
||||
}
|
||||
|
||||
for(i=0; i<PMAX_M; i++)
|
||||
snlp.sq[i] = 0.0;
|
||||
snlp.mem_x = 0.0;
|
||||
snlp.mem_y = 0.0;
|
||||
for(i=0; i<NLP_NTAP; i++)
|
||||
snlp.mem_fir[i] = 0.0;
|
||||
|
||||
kiss.fft_alloc(snlp.fft_cfg, PE_FFT_SIZE, false);
|
||||
}
|
||||
|
||||
/*---------------------------------------------------------------------------*\
|
||||
|
||||
nlp_destroy()
|
||||
|
||||
Shut down function for NLP pitch estimator.
|
||||
|
||||
\*---------------------------------------------------------------------------*/
|
||||
|
||||
void Cnlp::nlp_destroy()
|
||||
{
|
||||
snlp.fft_cfg.twiddles.clear();
|
||||
}
|
||||
|
||||
/*---------------------------------------------------------------------------*\
|
||||
|
||||
nlp()
|
||||
|
||||
Determines the pitch in samples using the Non Linear Pitch (NLP)
|
||||
algorithm [1]. Returns the fundamental in Hz. Note that the actual
|
||||
pitch estimate is for the centre of the M sample Sn[] vector, not
|
||||
the current N sample input vector. This is (I think) a delay of 2.5
|
||||
frames with N=80 samples. You should align further analysis using
|
||||
this pitch estimate to be centred on the middle of Sn[].
|
||||
|
||||
Two post processors have been tried, the MBE version (as discussed
|
||||
in [1]), and a post processor that checks sub-multiples. Both
|
||||
suffer occasional gross pitch errors (i.e. neither are perfect). In
|
||||
the presence of background noise the sub-multiple algorithm tends
|
||||
towards low F0 which leads to better sounding background noise than
|
||||
the MBE post processor.
|
||||
|
||||
A good way to test and develop the NLP pitch estimator is using the
|
||||
tnlp (codec2/unittest) and the codec2/octave/plnlp.m Octave script.
|
||||
|
||||
A pitch tracker searching a few frames forward and backward in time
|
||||
would be a useful addition.
|
||||
|
||||
References:
|
||||
|
||||
[1] http://rowetel.com/downloads/1997_rowe_phd_thesis.pdf Chapter 4
|
||||
|
||||
\*---------------------------------------------------------------------------*/
|
||||
|
||||
float Cnlp::nlp(
|
||||
float Sn[], /* input speech vector */
|
||||
int n, /* frames shift (no. new samples in Sn[]) */
|
||||
float *pitch, /* estimated pitch period in samples at current Fs */
|
||||
// std::complex<float> 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<float> 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<m; i++)
|
||||
{
|
||||
snlp.sq[i] = Sn[i]*Sn[i];
|
||||
}
|
||||
}
|
||||
else
|
||||
{
|
||||
assert(snlp.Fs == 16000);
|
||||
|
||||
/* re-sample at 8 KHz */
|
||||
|
||||
for(i=0; i<n; i++)
|
||||
{
|
||||
snlp.Sn16k[FDMDV_OS_TAPS_16K+i] = Sn[m-n+i];
|
||||
}
|
||||
|
||||
m /= 2;
|
||||
n /= 2;
|
||||
|
||||
float Sn8k[n];
|
||||
fdmdv_16_to_8(Sn8k, &snlp.Sn16k[FDMDV_OS_TAPS_16K], n);
|
||||
|
||||
/* Square latest input samples */
|
||||
|
||||
for(i=m-n, j=0; i<m; i++, j++)
|
||||
{
|
||||
snlp.sq[i] = Sn8k[j]*Sn8k[j];
|
||||
}
|
||||
assert(j <= n);
|
||||
}
|
||||
|
||||
for(i=m-n; i<m; i++) /* notch filter at DC */
|
||||
{
|
||||
notch = snlp.sq[i] - snlp.mem_x;
|
||||
notch += COEFF*snlp.mem_y;
|
||||
snlp.mem_x = snlp.sq[i];
|
||||
snlp.mem_y = notch;
|
||||
snlp.sq[i] = notch + 1.0; /* With 0 input vectors to codec,
|
||||
kiss_fft() would take a long
|
||||
time to execute when running in
|
||||
real time. Problem was traced
|
||||
to kiss_fft function call in
|
||||
this function. Adding this small
|
||||
constant fixed problem. Not
|
||||
exactly sure why. */
|
||||
}
|
||||
|
||||
for(i=m-n; i<m; i++) /* FIR filter vector */
|
||||
{
|
||||
|
||||
for(j=0; j<NLP_NTAP-1; j++)
|
||||
snlp.mem_fir[j] = snlp.mem_fir[j+1];
|
||||
snlp.mem_fir[NLP_NTAP-1] = snlp.sq[i];
|
||||
|
||||
snlp.sq[i] = 0.0;
|
||||
for(j=0; j<NLP_NTAP; j++)
|
||||
snlp.sq[i] += snlp.mem_fir[j]*nlp_fir[j];
|
||||
}
|
||||
|
||||
/* Decimate and DFT */
|
||||
|
||||
for(i=0; i<PE_FFT_SIZE; i++)
|
||||
{
|
||||
Fw[i].real(0);
|
||||
Fw[i].imag(0);
|
||||
}
|
||||
for(i=0; i<m/DEC; i++)
|
||||
{
|
||||
Fw[i].real(snlp.sq[i*DEC]*snlp.w[i]);
|
||||
}
|
||||
|
||||
// FIXME: check if this can be converted to a real fft
|
||||
// since all imag inputs are 0
|
||||
codec2_fft_inplace(snlp.fft_cfg, Fw);
|
||||
|
||||
for(i=0; i<PE_FFT_SIZE; i++)
|
||||
Fw[i].real(Fw[i].real() * Fw[i].real() + Fw[i].imag() * Fw[i].imag());
|
||||
|
||||
/* todo: express everything in f0, as pitch in samples is dep on Fs */
|
||||
|
||||
int pmin = floor(SAMPLE_RATE*P_MIN_S);
|
||||
int pmax = floor(SAMPLE_RATE*P_MAX_S);
|
||||
|
||||
/* find global peak */
|
||||
|
||||
gmax = 0.0;
|
||||
gmax_bin = PE_FFT_SIZE*DEC/pmax;
|
||||
for(i=PE_FFT_SIZE*DEC/pmax; i<=PE_FFT_SIZE*DEC/pmin; i++)
|
||||
{
|
||||
if (Fw[i].real() > 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<m-n; i++)
|
||||
snlp.sq[i] = snlp.sq[i+n];
|
||||
|
||||
/* return pitch period in samples and F0 estimate */
|
||||
|
||||
*pitch = (float)snlp.Fs/best_f0;
|
||||
|
||||
*prev_f0 = best_f0;
|
||||
|
||||
return(best_f0);
|
||||
}
|
||||
|
||||
/*---------------------------------------------------------------------------*\
|
||||
|
||||
post_process_sub_multiples()
|
||||
|
||||
Given the global maximma of Fw[] we search integer submultiples for
|
||||
local maxima. If local maxima exist and they are above an
|
||||
experimentally derived threshold (OK a magic number I pulled out of
|
||||
the air) we choose the submultiple as the F0 estimate.
|
||||
|
||||
The rational for this is that the lowest frequency peak of Fw[]
|
||||
should be F0, as Fw[] can be considered the autocorrelation function
|
||||
of Sw[] (the speech spectrum). However sometimes due to phase
|
||||
effects the lowest frequency maxima may not be the global maxima.
|
||||
|
||||
This works OK in practice and favours low F0 values in the presence
|
||||
of background noise which means the sinusoidal codec does an OK job
|
||||
of synthesising the background noise. High F0 in background noise
|
||||
tends to sound more periodic introducing annoying artifacts.
|
||||
|
||||
\*---------------------------------------------------------------------------*/
|
||||
|
||||
float Cnlp::post_process_sub_multiples(std::complex<float> 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<n; i+=FDMDV_OS, k++)
|
||||
{
|
||||
acc = 0.0;
|
||||
for(j=0; j<FDMDV_OS_TAPS_16K; j++)
|
||||
acc += fdmdv_os_filter[j]*in16k[i-j];
|
||||
out8k[k] = acc;
|
||||
}
|
||||
|
||||
/* update filter memory */
|
||||
|
||||
for(i=-FDMDV_OS_TAPS_16K; i<0; i++)
|
||||
in16k[i] = in16k[i + n*FDMDV_OS];
|
||||
}
|
||||
|
||||
// there is a little overhead for inplace kiss_fft but this is
|
||||
// on the powerful platforms like the Raspberry or even x86 PC based ones
|
||||
// not noticeable
|
||||
// the reduced usage of RAM and increased performance on STM32 platforms
|
||||
// should be worth it.
|
||||
void Cnlp::codec2_fft_inplace(FFT_STATE &cfg, std::complex<float> *inout)
|
||||
{
|
||||
std::complex<float> 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<float>));
|
||||
kiss.fft(cfg, in, inout);
|
||||
}
|
||||
else
|
||||
{
|
||||
kiss.fft(cfg, inout, inout);
|
||||
}
|
||||
}
|
||||
|
|
@ -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 <http://www.gnu.org/licenses/>.
|
||||
*/
|
||||
|
||||
#ifndef __NLP__
|
||||
#define __NLP__
|
||||
|
||||
#include <complex>
|
||||
#include <vector>
|
||||
|
||||
#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<float> 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<float> *inout);
|
||||
|
||||
private:
|
||||
float post_process_sub_multiples(std::complex<float> 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
|
||||
|
|
@ -0,0 +1,139 @@
|
|||
/*
|
||||
Copyright (C) 2010 Perens LLC <bruce@perens.com>
|
||||
|
||||
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 <http://www.gnu.org/licenses/>.
|
||||
*/
|
||||
|
||||
#include "defines.h"
|
||||
#include "quantise.h"
|
||||
#include <stdio.h>
|
||||
|
||||
/* 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;
|
||||
}
|
||||
|
|
@ -0,0 +1,247 @@
|
|||
#include <assert.h>
|
||||
#include <math.h>
|
||||
|
||||
#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; j<m; j++)
|
||||
{
|
||||
e = 0.0;
|
||||
for(i=0; i<k; i++)
|
||||
{
|
||||
diff = cb[j*k+i]-vec[i];
|
||||
e += (diff*w[i] * diff*w[i]);
|
||||
}
|
||||
if (e < beste)
|
||||
{
|
||||
beste = e;
|
||||
besti = j;
|
||||
}
|
||||
}
|
||||
|
||||
*se += beste;
|
||||
|
||||
return(besti);
|
||||
}
|
||||
|
||||
/*---------------------------------------------------------------------------*\
|
||||
|
||||
FUNCTION....: encode_WoE()
|
||||
AUTHOR......: Jean-Marc Valin & David Rowe
|
||||
DATE CREATED: 11 May 2012
|
||||
|
||||
Joint Wo and LPC energy vector quantiser developed my Jean-Marc
|
||||
Valin. Returns index, and updated states xq[].
|
||||
|
||||
\*---------------------------------------------------------------------------*/
|
||||
|
||||
int CQbase::encode_WoE(MODEL *model, float e, float xq[])
|
||||
{
|
||||
int i, n1;
|
||||
float x[2];
|
||||
float err[2];
|
||||
float w[2];
|
||||
const float *codebook1 = ge_cb[0].cb;
|
||||
int nb_entries = ge_cb[0].m;
|
||||
int ndim = ge_cb[0].k;
|
||||
|
||||
assert((1<<WO_E_BITS) == nb_entries);
|
||||
|
||||
if (e < 0.0) e = 0; /* occasional small negative energies due LPC round off I guess */
|
||||
|
||||
x[0] = log10f((model->Wo/PI)*4000.0/50.0)/log10f(2);
|
||||
x[1] = 10.0*log10f(1e-4 + e);
|
||||
|
||||
compute_weights2(x, xq, w);
|
||||
for (i=0; i<ndim; i++)
|
||||
err[i] = x[i]-ge_coeff[i]*xq[i];
|
||||
n1 = find_nearest_weighted(codebook1, nb_entries, err, w, ndim);
|
||||
|
||||
for (i=0; i<ndim; i++)
|
||||
{
|
||||
xq[i] = ge_coeff[i]*xq[i] + codebook1[ndim*n1+i];
|
||||
err[i] -= codebook1[ndim*n1+i];
|
||||
}
|
||||
|
||||
//printf("enc: %f %f (%f)(%f) \n", xq[0], xq[1], e, 10.0*log10(1e-4 + e));
|
||||
return n1;
|
||||
}
|
||||
|
||||
|
||||
/*---------------------------------------------------------------------------*\
|
||||
|
||||
FUNCTION....: decode_WoE()
|
||||
AUTHOR......: Jean-Marc Valin & David Rowe
|
||||
DATE CREATED: 11 May 2012
|
||||
|
||||
Joint Wo and LPC energy vector quantiser developed my Jean-Marc
|
||||
Valin. Given index and states xq[], returns Wo & E, and updates
|
||||
states xq[].
|
||||
|
||||
\*---------------------------------------------------------------------------*/
|
||||
|
||||
void CQbase::decode_WoE(C2CONST *c2const, MODEL *model, float *e, float xq[], int n1)
|
||||
{
|
||||
int i;
|
||||
const float *codebook1 = ge_cb[0].cb;
|
||||
int ndim = ge_cb[0].k;
|
||||
float Wo_min = c2const->Wo_min;
|
||||
float Wo_max = c2const->Wo_max;
|
||||
|
||||
for (i=0; i<ndim; i++)
|
||||
{
|
||||
xq[i] = ge_coeff[i]*xq[i] + codebook1[ndim*n1+i];
|
||||
}
|
||||
|
||||
//printf("dec: %f %f\n", xq[0], xq[1]);
|
||||
model->Wo = 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; i<nb_entries; i++)
|
||||
{
|
||||
float dist=0;
|
||||
for (j=0; j<ndim; j++)
|
||||
dist += w[j]*(x[j]-codebook[i*ndim+j])*(x[j]-codebook[i*ndim+j]);
|
||||
if (dist<min_dist)
|
||||
{
|
||||
min_dist = dist;
|
||||
nearest = i;
|
||||
}
|
||||
}
|
||||
return nearest;
|
||||
}
|
||||
|
||||
/*---------------------------------------------------------------------------*\
|
||||
|
||||
FUNCTION....: encode_log_Wo()
|
||||
AUTHOR......: David Rowe
|
||||
DATE CREATED: 22/8/2010
|
||||
|
||||
Encodes Wo in the log domain using a WO_LEVELS quantiser.
|
||||
|
||||
\*---------------------------------------------------------------------------*/
|
||||
|
||||
int CQbase::encode_log_Wo(C2CONST *c2const, float Wo, int bits)
|
||||
{
|
||||
int index, Wo_levels = 1<<bits;
|
||||
float Wo_min = c2const->Wo_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<<bits;
|
||||
|
||||
step = (log10f(Wo_max) - log10f(Wo_min))/Wo_levels;
|
||||
Wo = log10f(Wo_min) + step*(index);
|
||||
|
||||
return exp10f(Wo);
|
||||
}
|
||||
|
|
@ -0,0 +1,39 @@
|
|||
#ifndef QBASE_H
|
||||
#define QBASE_H
|
||||
|
||||
#include "defines.h"
|
||||
|
||||
#define WO_BITS 7
|
||||
#define WO_LEVELS (1<<WO_BITS)
|
||||
#define WO_DT_BITS 3
|
||||
|
||||
#define E_BITS 5
|
||||
#define E_LEVELS (1<<E_BITS)
|
||||
#define E_MIN_DB -10.0
|
||||
#define E_MAX_DB 40.0
|
||||
|
||||
#define LSP_SCALAR_INDEXES 10
|
||||
#define LSPD_SCALAR_INDEXES 10
|
||||
#define LSP_PRED_VQ_INDEXES 3
|
||||
|
||||
#define WO_E_BITS 8
|
||||
|
||||
#define LPCPF_GAMMA 0.5
|
||||
#define LPCPF_BETA 0.2
|
||||
|
||||
class CQbase {
|
||||
public:
|
||||
int encode_WoE(MODEL *model, float e, float xq[]);
|
||||
void decode_WoE(C2CONST *c2const, MODEL *model, float *e, float xq[], int n1);
|
||||
int encode_log_Wo(C2CONST *c2const, float Wo, int bits);
|
||||
float decode_log_Wo(C2CONST *c2const, int index, int bits);
|
||||
protected:
|
||||
long quantise(const float * cb, float vec[], float w[], int k, int m, float *se);
|
||||
void compute_weights2(const float *x, const float *xp, float *w);
|
||||
int find_nearest_weighted(const float *codebook, int nb_entries, float *x, const float *w, int ndim);
|
||||
|
||||
const float ge_coeff[2] = { 0.8, 0.9 };
|
||||
|
||||
};
|
||||
|
||||
#endif
|
||||
|
|
@ -0,0 +1,897 @@
|
|||
/*---------------------------------------------------------------------------*\
|
||||
|
||||
FILE........: quantise.c
|
||||
AUTHOR......: David Rowe
|
||||
DATE CREATED: 31/5/92
|
||||
|
||||
Quantisation functions for the sinusoidal coder.
|
||||
|
||||
\*---------------------------------------------------------------------------*/
|
||||
|
||||
/*
|
||||
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 <http://www.gnu.org/licenses/>.
|
||||
|
||||
*/
|
||||
|
||||
#include <assert.h>
|
||||
#include <ctype.h>
|
||||
#include <stdio.h>
|
||||
#include <stdlib.h>
|
||||
#include <string.h>
|
||||
#include <math.h>
|
||||
|
||||
#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<order; i++)
|
||||
{
|
||||
wt[i] = 1.0;
|
||||
}
|
||||
|
||||
/* convert from radians to Hz so we can use human readable
|
||||
frequencies */
|
||||
|
||||
for(i=0; i<order; i++)
|
||||
lsp_hz[i] = (4000.0/PI)*lsp[i];
|
||||
|
||||
wt[0] = 1.0;
|
||||
for(i=0; i<order; i++)
|
||||
{
|
||||
|
||||
/* find difference from previous qunatised lsp */
|
||||
|
||||
if (i)
|
||||
dlsp[i] = lsp_hz[i] - lsp__hz[i-1];
|
||||
else
|
||||
dlsp[0] = lsp_hz[0];
|
||||
|
||||
k = lsp_cbd[i].k;
|
||||
m = lsp_cbd[i].m;
|
||||
cb = lsp_cbd[i].cb;
|
||||
indexes[i] = quantise(cb, &dlsp[i], wt, k, m, &se);
|
||||
dlsp_[i] = cb[indexes[i]*k];
|
||||
|
||||
|
||||
if (i)
|
||||
lsp__hz[i] = lsp__hz[i-1] + dlsp_[i];
|
||||
else
|
||||
lsp__hz[0] = dlsp_[0];
|
||||
}
|
||||
|
||||
}
|
||||
|
||||
|
||||
void CQuantize::decode_lspds_scalar( float lsp_[], int indexes[], int order)
|
||||
{
|
||||
int i,k;
|
||||
float lsp__hz[order];
|
||||
float dlsp_[order];
|
||||
const float *cb;
|
||||
|
||||
for(i=0; i<order; i++)
|
||||
{
|
||||
|
||||
k = lsp_cbd[i].k;
|
||||
cb = lsp_cbd[i].cb;
|
||||
dlsp_[i] = cb[indexes[i]*k];
|
||||
|
||||
if (i)
|
||||
lsp__hz[i] = lsp__hz[i-1] + dlsp_[i];
|
||||
else
|
||||
lsp__hz[0] = dlsp_[0];
|
||||
|
||||
lsp_[i] = (PI/4000.0)*lsp__hz[i];
|
||||
}
|
||||
|
||||
}
|
||||
|
||||
#define MIN(a,b) ((a)<(b)?(a):(b))
|
||||
#define MAX_ENTRIES 16384
|
||||
|
||||
void CQuantize::compute_weights(const float *x, float *w, int ndim)
|
||||
{
|
||||
int i;
|
||||
w[0] = MIN(x[0], x[1]-x[0]);
|
||||
for (i=1; i<ndim-1; i++)
|
||||
w[i] = MIN(x[i]-x[i-1], x[i+1]-x[i]);
|
||||
w[ndim-1] = MIN(x[ndim-1]-x[ndim-2], PI-x[ndim-1]);
|
||||
|
||||
for (i=0; i<ndim; i++)
|
||||
w[i] = 1./(.01+w[i]);
|
||||
}
|
||||
|
||||
int CQuantize::find_nearest(const float *codebook, int nb_entries, float *x, int ndim)
|
||||
{
|
||||
int i, j;
|
||||
float min_dist = 1e15;
|
||||
int nearest = 0;
|
||||
|
||||
for (i=0; i<nb_entries; i++)
|
||||
{
|
||||
float dist=0;
|
||||
for (j=0; j<ndim; j++)
|
||||
dist += (x[j]-codebook[i*ndim+j])*(x[j]-codebook[i*ndim+j]);
|
||||
if (dist<min_dist)
|
||||
{
|
||||
min_dist = dist;
|
||||
nearest = i;
|
||||
}
|
||||
}
|
||||
return nearest;
|
||||
}
|
||||
|
||||
int CQuantize::check_lsp_order(float lsp[], int order)
|
||||
{
|
||||
int i;
|
||||
float tmp;
|
||||
int swaps = 0;
|
||||
|
||||
for(i=1; i<order; i++)
|
||||
if (lsp[i] < lsp[i-1])
|
||||
{
|
||||
//fprintf(stderr, "swap %d\n",i);
|
||||
swaps++;
|
||||
tmp = lsp[i-1];
|
||||
lsp[i-1] = lsp[i]-0.1;
|
||||
lsp[i] = tmp+0.1;
|
||||
i = 1; /* start check again, as swap may have caused out of order */
|
||||
}
|
||||
|
||||
return swaps;
|
||||
}
|
||||
|
||||
|
||||
/*---------------------------------------------------------------------------*\
|
||||
|
||||
lpc_post_filter()
|
||||
|
||||
Applies a post filter to the LPC synthesis filter power spectrum
|
||||
Pw, which supresses the inter-formant energy.
|
||||
|
||||
The algorithm is from p267 (Section 8.6) of "Digital Speech",
|
||||
edited by A.M. Kondoz, 1994 published by Wiley and Sons. Chapter 8
|
||||
of this text is on the MBE vocoder, and this is a freq domain
|
||||
adaptation of post filtering commonly used in CELP.
|
||||
|
||||
I used the Octave simulation lpcpf.m to get an understanding of the
|
||||
algorithm.
|
||||
|
||||
Requires two more FFTs which is significantly more MIPs. However
|
||||
it should be possible to implement this more efficiently in the
|
||||
time domain. Just not sure how to handle relative time delays
|
||||
between the synthesis stage and updating these coeffs. A smaller
|
||||
FFT size might also be accetable to save CPU.
|
||||
|
||||
TODO:
|
||||
[ ] sync var names between Octave and C version
|
||||
[ ] doc gain normalisation
|
||||
[ ] I think the first FFT is not rqd as we do the same
|
||||
thing in aks_to_M2().
|
||||
|
||||
\*---------------------------------------------------------------------------*/
|
||||
|
||||
void CQuantize::lpc_post_filter(FFTR_STATE *fftr_fwd_cfg, float Pw[], float ak[], int order, float beta, float gamma, int bass_boost, float E)
|
||||
{
|
||||
int i;
|
||||
float x[FFT_ENC]; /* input to FFTs */
|
||||
std::complex<float> 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<FFT_ENC; i++)
|
||||
{
|
||||
x[i] = 0.0;
|
||||
}
|
||||
|
||||
x[0] = ak[0];
|
||||
coeff = gamma;
|
||||
for(i=1; i<=order; i++)
|
||||
{
|
||||
x[i] = ak[i] * coeff;
|
||||
coeff *= gamma;
|
||||
}
|
||||
kiss.fftr(*fftr_fwd_cfg, x, Ww);
|
||||
|
||||
for(i=0; i<FFT_ENC/2; i++)
|
||||
{
|
||||
Ww[i].real(Ww[i].real() * Ww[i].real() + Ww[i].imag() * Ww[i].imag());
|
||||
}
|
||||
|
||||
/* Determined combined filter R = WA ---------------------------*/
|
||||
|
||||
max_Rw = 0.0;
|
||||
min_Rw = 1E32;
|
||||
for(i=0; i<FFT_ENC/2; i++)
|
||||
{
|
||||
Rw[i] = sqrtf(Ww[i].real() * Pw[i]);
|
||||
if (Rw[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<FFT_ENC/2; i++)
|
||||
e_before += Pw[i];
|
||||
|
||||
/* apply post filter and measure energy */
|
||||
|
||||
|
||||
e_after = 1E-4;
|
||||
for(i=0; i<FFT_ENC/2; i++)
|
||||
{
|
||||
Pfw = powf(Rw[i], beta);
|
||||
Pw[i] *= Pfw * Pfw;
|
||||
e_after += Pw[i];
|
||||
}
|
||||
gain = e_before/e_after;
|
||||
|
||||
/* apply gain factor to normalise energy, and LPC Energy */
|
||||
|
||||
gain *= E;
|
||||
for(i=0; i<FFT_ENC/2; i++)
|
||||
{
|
||||
Pw[i] *= gain;
|
||||
}
|
||||
|
||||
if (bass_boost)
|
||||
{
|
||||
/* add 3dB to first 1 kHz to account for LP effect of PF */
|
||||
|
||||
for(i=0; i<FFT_ENC/8; i++)
|
||||
{
|
||||
Pw[i] *= 1.4*1.4;
|
||||
}
|
||||
}
|
||||
}
|
||||
|
||||
|
||||
/*---------------------------------------------------------------------------*\
|
||||
|
||||
aks_to_M2()
|
||||
|
||||
Transforms the linear prediction coefficients to spectral amplitude
|
||||
samples. This function determines A(m) from the average energy per
|
||||
band using an FFT.
|
||||
|
||||
\*---------------------------------------------------------------------------*/
|
||||
|
||||
void CQuantize::aks_to_M2(
|
||||
FFTR_STATE * fftr_fwd_cfg,
|
||||
float ak[], /* LPC's */
|
||||
int order,
|
||||
MODEL *model, /* sinusoidal model parameters for this frame */
|
||||
float E, /* energy term */
|
||||
float *snr, /* signal to noise ratio for this frame in dB */
|
||||
int sim_pf, /* true to simulate a post filter */
|
||||
int pf, /* true to enable actual LPC post filter */
|
||||
int bass_boost, /* enable LPC filter 0-1kHz 3dB boost */
|
||||
float beta,
|
||||
float gamma, /* LPC post filter parameters */
|
||||
std::complex<float> 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; i<FFT_ENC; i++)
|
||||
{
|
||||
a[i] = 0.0;
|
||||
}
|
||||
|
||||
for(i=0; i<=order; i++)
|
||||
a[i] = ak[i];
|
||||
kiss.fftr(*fftr_fwd_cfg, a, Aw);
|
||||
}
|
||||
|
||||
/* Determine power spectrum P(w) = E/(A(exp(jw))^2 ------------------------*/
|
||||
|
||||
float Pw[FFT_ENC/2];
|
||||
|
||||
for(i=0; i<FFT_ENC/2; i++)
|
||||
{
|
||||
Pw[i] = 1.0/(Aw[i].real() * Aw[i].real() + Aw[i].imag() * Aw[i].imag() + 1E-6);
|
||||
}
|
||||
|
||||
if (pf)
|
||||
lpc_post_filter(fftr_fwd_cfg, Pw, ak, order, beta, gamma, bass_boost, E);
|
||||
else
|
||||
{
|
||||
for(i=0; i<FFT_ENC/2; i++)
|
||||
{
|
||||
Pw[i] *= E;
|
||||
}
|
||||
}
|
||||
|
||||
/* Determine magnitudes from P(w) ----------------------------------------*/
|
||||
|
||||
/* when used just by decoder {A} might be all zeroes so init signal
|
||||
and noise to prevent log(0) errors */
|
||||
|
||||
signal = 1E-30;
|
||||
noise = 1E-32;
|
||||
|
||||
for(m=1; m<=model->L; 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; i<bm; i++)
|
||||
Em += Pw[i];
|
||||
Am = sqrtf(Em);
|
||||
|
||||
signal += model->A[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<<bits;
|
||||
float Wo_min = c2const->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<<bits;
|
||||
|
||||
step = (Wo_max - Wo_min)/Wo_levels;
|
||||
Wo = Wo_min + step*(index);
|
||||
|
||||
return Wo;
|
||||
}
|
||||
|
||||
/*---------------------------------------------------------------------------*\
|
||||
|
||||
FUNCTION....: speech_to_uq_lsps()
|
||||
AUTHOR......: David Rowe
|
||||
DATE CREATED: 22/8/2010
|
||||
|
||||
Analyse a windowed frame of time domain speech to determine LPCs
|
||||
which are the converted to LSPs for quantisation and transmission
|
||||
over the channel.
|
||||
|
||||
\*---------------------------------------------------------------------------*/
|
||||
|
||||
float CQuantize::speech_to_uq_lsps(float lsp[], float ak[], float Sn[], float w[], int m_pitch, int order)
|
||||
{
|
||||
int i, roots;
|
||||
float Wn[m_pitch];
|
||||
float R[order+1];
|
||||
float e, E;
|
||||
Clpc lpc;
|
||||
|
||||
e = 0.0;
|
||||
for(i=0; i<m_pitch; i++)
|
||||
{
|
||||
Wn[i] = Sn[i]*w[i];
|
||||
e += Wn[i]*Wn[i];
|
||||
}
|
||||
|
||||
/* trap 0 energy case as LPC analysis will fail */
|
||||
|
||||
if (e == 0.0)
|
||||
{
|
||||
for(i=0; i<order; i++)
|
||||
lsp[i] = (PI/order)*(float)i;
|
||||
return 0.0;
|
||||
}
|
||||
|
||||
lpc.autocorrelate(Wn, R, m_pitch, order);
|
||||
lpc.levinson_durbin(R, ak, order);
|
||||
|
||||
E = 0.0;
|
||||
for(i=0; i<=order; i++)
|
||||
E += ak[i]*R[i];
|
||||
|
||||
/* 15 Hz BW expansion as I can't hear the difference and it may help
|
||||
help occasional fails in the LSP root finding. Important to do this
|
||||
after energy calculation to avoid -ve energy values.
|
||||
*/
|
||||
|
||||
for(i=0; i<=order; i++)
|
||||
ak[i] *= powf(0.994,(float)i);
|
||||
|
||||
roots = lpc_to_lsp(ak, order, lsp, 5, LSP_DELTA1);
|
||||
if (roots != order)
|
||||
{
|
||||
/* if root finding fails use some benign LSP values instead */
|
||||
for(i=0; i<order; i++)
|
||||
lsp[i] = (PI/order)*(float)i;
|
||||
}
|
||||
|
||||
return E;
|
||||
}
|
||||
|
||||
/*---------------------------------------------------------------------------*\
|
||||
|
||||
FUNCTION....: encode_lsps_scalar()
|
||||
AUTHOR......: David Rowe
|
||||
DATE CREATED: 22/8/2010
|
||||
|
||||
Thirty-six bit sclar LSP quantiser. From a vector of unquantised
|
||||
(floating point) LSPs finds the quantised LSP indexes.
|
||||
|
||||
\*---------------------------------------------------------------------------*/
|
||||
|
||||
void CQuantize::encode_lsps_scalar(int indexes[], float lsp[], int order)
|
||||
{
|
||||
int i,k,m;
|
||||
float wt[1];
|
||||
float lsp_hz[order];
|
||||
const float *cb;
|
||||
float se;
|
||||
|
||||
/* convert from radians to Hz so we can use human readable
|
||||
frequencies */
|
||||
|
||||
for(i=0; i<order; i++)
|
||||
lsp_hz[i] = (4000.0/PI)*lsp[i];
|
||||
|
||||
/* scalar quantisers */
|
||||
|
||||
wt[0] = 1.0;
|
||||
for(i=0; i<order; i++)
|
||||
{
|
||||
k = lsp_cb[i].k;
|
||||
m = lsp_cb[i].m;
|
||||
cb = lsp_cb[i].cb;
|
||||
indexes[i] = quantise(cb, &lsp_hz[i], wt, k, m, &se);
|
||||
}
|
||||
}
|
||||
|
||||
/*---------------------------------------------------------------------------*\
|
||||
|
||||
FUNCTION....: decode_lsps_scalar()
|
||||
AUTHOR......: David Rowe
|
||||
DATE CREATED: 22/8/2010
|
||||
|
||||
From a vector of quantised LSP indexes, returns the quantised
|
||||
(floating point) LSPs.
|
||||
|
||||
\*---------------------------------------------------------------------------*/
|
||||
|
||||
void CQuantize::decode_lsps_scalar(float lsp[], int indexes[], int order)
|
||||
{
|
||||
int i,k;
|
||||
float lsp_hz[order];
|
||||
const float *cb;
|
||||
|
||||
for(i=0; i<order; i++)
|
||||
{
|
||||
k = lsp_cb[i].k;
|
||||
cb = lsp_cb[i].cb;
|
||||
lsp_hz[i] = cb[indexes[i]*k];
|
||||
}
|
||||
|
||||
/* convert back to radians */
|
||||
|
||||
for(i=0; i<order; i++)
|
||||
lsp[i] = (PI/4000.0)*lsp_hz[i];
|
||||
}
|
||||
|
||||
/*---------------------------------------------------------------------------*\
|
||||
|
||||
FUNCTION....: bw_expand_lsps()
|
||||
AUTHOR......: David Rowe
|
||||
DATE CREATED: 22/8/2010
|
||||
|
||||
Applies Bandwidth Expansion (BW) to a vector of LSPs. Prevents any
|
||||
two LSPs getting too close together after quantisation. We know
|
||||
from experiment that LSP quantisation errors < 12.5Hz (25Hz step
|
||||
size) are inaudible so we use that as the minimum LSP separation.
|
||||
|
||||
\*---------------------------------------------------------------------------*/
|
||||
|
||||
void CQuantize::bw_expand_lsps(float lsp[], int order, float min_sep_low, float min_sep_high)
|
||||
{
|
||||
int i;
|
||||
|
||||
for(i=1; i<4; i++)
|
||||
{
|
||||
|
||||
if ((lsp[i] - lsp[i-1]) < min_sep_low*(PI/4000.0))
|
||||
lsp[i] = lsp[i-1] + min_sep_low*(PI/4000.0);
|
||||
|
||||
}
|
||||
|
||||
/* As quantiser gaps increased, larger BW expansion was required
|
||||
to prevent twinkly noises. This may need more experiment for
|
||||
different quanstisers.
|
||||
*/
|
||||
|
||||
for(i=4; i<order; i++)
|
||||
{
|
||||
if (lsp[i] - lsp[i-1] < min_sep_high*(PI/4000.0))
|
||||
lsp[i] = lsp[i-1] + min_sep_high*(PI/4000.0);
|
||||
}
|
||||
}
|
||||
|
||||
/*---------------------------------------------------------------------------*\
|
||||
|
||||
FUNCTION....: apply_lpc_correction()
|
||||
AUTHOR......: David Rowe
|
||||
DATE CREATED: 22/8/2010
|
||||
|
||||
Apply first harmonic LPC correction at decoder. This helps improve
|
||||
low pitch males after LPC modelling, like hts1a and morig.
|
||||
|
||||
\*---------------------------------------------------------------------------*/
|
||||
|
||||
void CQuantize::apply_lpc_correction(MODEL *model)
|
||||
{
|
||||
if (model->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<<bits;
|
||||
float e_min = E_MIN_DB;
|
||||
float e_max = E_MAX_DB;
|
||||
float norm;
|
||||
|
||||
e = 10.0*log10f(e);
|
||||
norm = (e - e_min)/(e_max - e_min);
|
||||
index = floorf(e_levels * norm + 0.5);
|
||||
if (index < 0 ) index = 0;
|
||||
if (index > (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<<bits;
|
||||
|
||||
step = (e_max - e_min)/e_levels;
|
||||
e = e_min + step*(index);
|
||||
e = exp10f(e/10.0);
|
||||
|
||||
return e;
|
||||
}
|
||||
|
||||
/*---------------------------------------------------------------------------*\
|
||||
|
||||
FUNCTION....: lpc_to_lsp()
|
||||
AUTHOR......: David Rowe
|
||||
DATE CREATED: 24/2/93
|
||||
|
||||
This function converts LPC coefficients to LSP coefficients.
|
||||
|
||||
\*---------------------------------------------------------------------------*/
|
||||
|
||||
int CQuantize::lpc_to_lsp(float *a, int order, float *freq, int nb, float delta)
|
||||
/* float *a lpc coefficients */
|
||||
/* int order order of LPC coefficients (10) */
|
||||
/* float *freq LSP frequencies in radians */
|
||||
/* int nb number of sub-intervals (4) */
|
||||
/* float delta grid spacing interval (0.02) */
|
||||
{
|
||||
float psuml,psumr,psumm,temp_xr,xl,xr,xm = 0;
|
||||
float temp_psumr;
|
||||
int i,j,m,flag,k;
|
||||
float *px; /* ptrs of respective P'(z) & Q'(z) */
|
||||
float *qx;
|
||||
float *p;
|
||||
float *q;
|
||||
float *pt; /* ptr used for cheb_poly_eval()
|
||||
whether P' or Q' */
|
||||
int roots=0; /* number of roots found */
|
||||
float Q[order + 1];
|
||||
float P[order + 1];
|
||||
|
||||
flag = 1;
|
||||
m = order/2; /* order of P'(z) & Q'(z) polynimials */
|
||||
|
||||
/* Allocate memory space for polynomials */
|
||||
|
||||
/* determine P'(z)'s and Q'(z)'s coefficients where
|
||||
P'(z) = P(z)/(1 + z^(-1)) and Q'(z) = Q(z)/(1-z^(-1)) */
|
||||
|
||||
px = P; /* initilaise ptrs */
|
||||
qx = Q;
|
||||
p = px;
|
||||
q = qx;
|
||||
*px++ = 1.0;
|
||||
*qx++ = 1.0;
|
||||
for(i=1; i<=m; i++)
|
||||
{
|
||||
*px++ = a[i]+a[order+1-i]-*p++;
|
||||
*qx++ = a[i]-a[order+1-i]+*q++;
|
||||
}
|
||||
px = P;
|
||||
qx = Q;
|
||||
for(i=0; i<m; i++)
|
||||
{
|
||||
*px = 2**px;
|
||||
*qx = 2**qx;
|
||||
px++;
|
||||
qx++;
|
||||
}
|
||||
px = P; /* re-initialise ptrs */
|
||||
qx = Q;
|
||||
|
||||
/* Search for a zero in P'(z) polynomial first and then alternate to Q'(z).
|
||||
Keep alternating between the two polynomials as each zero is found */
|
||||
|
||||
xr = 0; /* initialise xr to zero */
|
||||
xl = 1.0; /* start at point xl = 1 */
|
||||
|
||||
|
||||
for(j=0; j<order; j++)
|
||||
{
|
||||
if(j%2) /* determines whether P' or Q' is eval. */
|
||||
pt = qx;
|
||||
else
|
||||
pt = px;
|
||||
|
||||
psuml = cheb_poly_eva(pt,xl,order); /* evals poly. at xl */
|
||||
flag = 1;
|
||||
while(flag && (xr >= -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<order; i++)
|
||||
{
|
||||
freq[i] = acosf(freq[i]);
|
||||
}
|
||||
|
||||
return(roots);
|
||||
}
|
||||
|
||||
/*---------------------------------------------------------------------------*\
|
||||
|
||||
FUNCTION....: cheb_poly_eva()
|
||||
AUTHOR......: David Rowe
|
||||
DATE CREATED: 24/2/93
|
||||
|
||||
This function evalutes a series of chebyshev polynomials
|
||||
|
||||
FIXME: performing memory allocation at run time is very inefficient,
|
||||
replace with stack variables of MAX_P size.
|
||||
|
||||
\*---------------------------------------------------------------------------*/
|
||||
|
||||
float CQuantize::cheb_poly_eva(float *coef,float x,int order)
|
||||
/* float coef[] coefficients of the polynomial to be evaluated */
|
||||
/* float x the point where polynomial is to be evaluated */
|
||||
/* int order order of the polynomial */
|
||||
{
|
||||
int i;
|
||||
float *t,*u,*v,sum;
|
||||
float T[(order / 2) + 1];
|
||||
|
||||
/* Initialise pointers */
|
||||
|
||||
t = T; /* T[i-2] */
|
||||
*t++ = 1.0;
|
||||
u = t--; /* T[i-1] */
|
||||
*u++ = x;
|
||||
v = u--; /* T[i] */
|
||||
|
||||
/* Evaluate chebyshev series formulation using iterative approach */
|
||||
|
||||
for(i=2; i<=order/2; i++)
|
||||
*v++ = (2*x)*(*u++) - *t++; /* T[i] = 2*x*T[i-1] - T[i-2] */
|
||||
|
||||
sum=0.0; /* initialise sum to zero */
|
||||
t = T; /* reset pointer */
|
||||
|
||||
/* Evaluate polynomial and return value also free memory space */
|
||||
|
||||
for(i=0; i<=order/2; i++)
|
||||
sum+=coef[(order/2)-i]**t++;
|
||||
|
||||
return sum;
|
||||
}
|
||||
|
|
@ -0,0 +1,68 @@
|
|||
/*---------------------------------------------------------------------------*\
|
||||
|
||||
FILE........: quantise.h
|
||||
AUTHOR......: David Rowe
|
||||
DATE CREATED: 31/5/92
|
||||
|
||||
Quantisation functions for the sinusoidal coder.
|
||||
|
||||
\*---------------------------------------------------------------------------*/
|
||||
|
||||
/*
|
||||
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 <http://www.gnu.org/licenses/>.
|
||||
*/
|
||||
|
||||
#ifndef __QUANTISE__
|
||||
#define __QUANTISE__
|
||||
|
||||
#include <complex>
|
||||
|
||||
#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<float> 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
|
||||
Loading…
Reference in New Issue