From 1fd4ecbf9ada093f8235c27b8aafa8f16ffa1abb Mon Sep 17 00:00:00 2001 From: Jonathan Naylor Date: Tue, 27 Sep 2016 19:26:24 +0100 Subject: [PATCH] Simple P25 silence insertion. --- P25Control.cpp | 92 +++++++++++++++++++++++++++++++++++++++++++++++--- P25Control.h | 3 ++ P25Defines.h | 12 +------ 3 files changed, 92 insertions(+), 15 deletions(-) diff --git a/P25Control.cpp b/P25Control.cpp index a7cff14..b8237dd 100644 --- a/P25Control.cpp +++ b/P25Control.cpp @@ -24,6 +24,8 @@ #include "Log.h" #include +#include +#include #include // #define DUMP_P25 @@ -61,6 +63,7 @@ m_rfLSD(), m_netLSD(), m_netLDU1(NULL), m_netLDU2(NULL), +m_lastIMBE(NULL), m_fp(NULL) { assert(display != NULL); @@ -68,12 +71,19 @@ m_fp(NULL) m_netLDU1 = new unsigned char[9U * 25U]; m_netLDU2 = new unsigned char[9U * 25U]; + + ::memset(m_netLDU1, 0x00U, 9U * 25U); + ::memset(m_netLDU2, 0x00U, 9U * 25U); + + m_lastIMBE = new unsigned char[11U]; + ::memcpy(m_lastIMBE, P25_NULL_IMBE, 11U); } CP25Control::~CP25Control() { delete[] m_netLDU1; delete[] m_netLDU2; + delete[] m_lastIMBE; } bool CP25Control::writeModem(unsigned char* data, unsigned int len) @@ -374,7 +384,7 @@ void CP25Control::clock(unsigned int ms) m_networkWatchdog.clock(ms); if (m_networkWatchdog.hasExpired()) { - LogMessage("P25, network watchdog has expired, %.1f seconds, %u%% packet loss", float(m_netFrames) / 5.56F, (m_netLost * 100U) / m_netFrames); + LogMessage("P25, network watchdog has expired, %.1f seconds, %u%% packet loss", float(m_netFrames) / 50.0F, (m_netLost * 100U) / m_netFrames); m_display->clearP25(); m_networkWatchdog.stop(); m_netState = RS_NET_IDLE; @@ -463,6 +473,72 @@ void CP25Control::addBusyBits(unsigned char* data, unsigned int length, bool b1, } } +void CP25Control::insertMissingAudio(unsigned char* data) +{ + if (data[0U] == 0x00U) { + ::memcpy(data + 10U, m_lastIMBE, 11U); + m_netLost++; + } else { + ::memcpy(m_lastIMBE, data + 10U, 11U); + } + + if (data[25U] == 0x00U) { + ::memcpy(data + 26U, m_lastIMBE, 11U); + m_netLost++; + } else { + ::memcpy(m_lastIMBE, data + 26U, 11U); + } + + if (data[50U] == 0x00U) { + ::memcpy(data + 55U, m_lastIMBE, 11U); + m_netLost++; + } else { + ::memcpy(m_lastIMBE, data + 55U, 11U); + } + + if (data[75U] == 0x00U) { + ::memcpy(data + 80U, m_lastIMBE, 11U); + m_netLost++; + } else { + ::memcpy(m_lastIMBE, data + 80U, 11U); + } + + if (data[100U] == 0x00U) { + ::memcpy(data + 105U, m_lastIMBE, 11U); + m_netLost++; + } else { + ::memcpy(m_lastIMBE, data + 105U, 11U); + } + + if (data[125U] == 0x00U) { + ::memcpy(data + 130U, m_lastIMBE, 11U); + m_netLost++; + } else { + ::memcpy(m_lastIMBE, data + 130U, 11U); + } + + if (data[150U] == 0x00U) { + ::memcpy(data + 155U, m_lastIMBE, 11U); + m_netLost++; + } else { + ::memcpy(m_lastIMBE, data + 155U, 11U); + } + + if (data[175U] == 0x00U) { + ::memcpy(data + 180U, m_lastIMBE, 11U); + m_netLost++; + } else { + ::memcpy(m_lastIMBE, data + 180U, 11U); + } + + if (data[200U] == 0x00U) { + ::memcpy(data + 204U, m_lastIMBE, 11U); + m_netLost++; + } else { + ::memcpy(m_lastIMBE, data + 204U, 11U); + } +} + void CP25Control::createRFHeader() { unsigned char buffer[P25_HDR_FRAME_LENGTH_BYTES + 2U]; @@ -559,6 +635,8 @@ void CP25Control::createNetHeader() void CP25Control::createNetLDU1() { + insertMissingAudio(m_netLDU1); + unsigned char buffer[P25_LDU_FRAME_LENGTH_BYTES + 2U]; ::memset(buffer, 0x00U, P25_LDU_FRAME_LENGTH_BYTES + 2U); @@ -595,11 +673,15 @@ void CP25Control::createNetLDU1() writeQueueNet(buffer, P25_LDU_FRAME_LENGTH_BYTES + 2U); - m_netFrames++; + ::memset(m_netLDU1, 0x00U, 9U * 25U); + + m_netFrames += 9U; } void CP25Control::createNetLDU2() { + insertMissingAudio(m_netLDU2); + unsigned char buffer[P25_LDU_FRAME_LENGTH_BYTES + 2U]; ::memset(buffer, 0x00U, P25_LDU_FRAME_LENGTH_BYTES + 2U); @@ -636,7 +718,9 @@ void CP25Control::createNetLDU2() writeQueueNet(buffer, P25_LDU_FRAME_LENGTH_BYTES + 2U); - m_netFrames++; + ::memset(m_netLDU2, 0x00U, 9U * 25U); + + m_netFrames += 9U; } void CP25Control::createNetTerminator(const unsigned char* data) @@ -663,7 +747,7 @@ void CP25Control::createNetTerminator(const unsigned char* data) writeQueueNet(buffer, P25_TERM_FRAME_LENGTH_BYTES + 2U); - LogMessage("P25, network end of transmission, %.1f seconds, %u%% packet loss", float(m_netFrames) / 5.56F, (m_netLost * 100U) / m_netFrames); + LogMessage("P25, network end of transmission, %.1f seconds, %u%% packet loss", float(m_netFrames) / 50.0F, (m_netLost * 100U) / m_netFrames); m_display->clearP25(); m_netTimeout.stop(); diff --git a/P25Control.h b/P25Control.h index 79537d8..a737d04 100644 --- a/P25Control.h +++ b/P25Control.h @@ -72,6 +72,7 @@ private: CP25LowSpeedData m_netLSD; unsigned char* m_netLDU1; unsigned char* m_netLDU2; + unsigned char* m_lastIMBE; FILE* m_fp; void writeQueueRF(const unsigned char* data, unsigned int length); @@ -81,6 +82,8 @@ private: void addBusyBits(unsigned char* data, unsigned int length, bool b1, bool b2); + void insertMissingAudio(unsigned char* data); + void createRFHeader(); void createNetHeader(); diff --git a/P25Defines.h b/P25Defines.h index d929cf1..14e2915 100644 --- a/P25Defines.h +++ b/P25Defines.h @@ -55,16 +55,6 @@ const unsigned char P25_DUID_LDU2 = 0x0AU; const unsigned char P25_DUID_PDU = 0x0CU; const unsigned char P25_DUID_TERM_LC = 0x0FU; -const unsigned char P25_RAW_HDR[] = { - 0x00U, 0x00U, 0x00U, 0x00U, 0x00U, 0x00U, 0x00U, 0x00U, 0x00U, 0x00U, 0x00U, 0x00U, 0x00U, 0x00U, 0x00U, 0x00U, - 0x00U, 0x00U, 0x00U, 0x00U, 0x00U, 0x00U, 0x00U, 0x00U, 0x00U, 0x00U, 0x00U, 0x00U, 0x00U, 0x08U, 0xDCU, 0x60U, - 0x00U, 0x00U, 0x00U, 0x00U, 0x00U, 0x00U, 0x00U, 0x00U, 0x00U, 0x00U, 0x00U, 0x18U, 0xEBU, 0x5EU, 0x09U, 0x54U, - 0x9FU, 0x0AU, 0x88U, 0xD5U, 0x03U, 0x67U, 0xCDU, 0x1FU, 0xF1U, 0xD4U, 0x1EU, 0x42U, 0x1EU, 0xA2U, 0x35U, 0x8BU, - 0xFCU, 0xC4U, 0xA9U, 0x78U, 0xDCU, 0x61U, 0x2AU, 0x59U, 0x46U, 0xE3U, 0x0AU, 0x4FU, 0x80U, 0xC7U, 0x54U, 0xC7U, - 0x51U}; - -const unsigned char P25_RAW_LDU2[] = { - 0x00U, 0x00U, 0x00U, 0x00U, 0x00U, 0x00U, 0x00U, 0x00U, 0x00U, 0x00U, 0x00U, 0x00U, 0x00U, 0x00U, 0x00U, 0x83U, - 0x80U, 0x00U, 0x00U, 0x00U, 0xAEU, 0x8BU, 0x48U, 0xB6U, 0x49U, 0x9AU, 0xBDU, 0x3CU, 0x7FU, 0x58U}; +const unsigned char P25_NULL_IMBE[] = {0x04U, 0x0CU, 0xFDU, 0x7BU, 0xFBU, 0x7DU, 0xF2U, 0x7BU, 0x3DU, 0x9EU, 0x45U}; #endif