diff --git a/AMBEFEC.cpp b/AMBEFEC.cpp index 2fa0c5c..31ccfaf 100644 --- a/AMBEFEC.cpp +++ b/AMBEFEC.cpp @@ -596,6 +596,48 @@ unsigned int CAMBEFEC::regenerateDStar(unsigned char* bytes) const return errors; } +unsigned int CAMBEFEC::regenerateYSF1(unsigned char* bytes) const +{ + assert(bytes != NULL); + + unsigned int a = 0U; + unsigned int b = 0U; + unsigned int c = 0U; + + unsigned int MASK = 0x800000U; + for (unsigned int i = 0U; i < 24U; i++) { + unsigned int aPos = DMR_A_TABLE[i]; + unsigned int bPos = DMR_B_TABLE[i]; + unsigned int cPos = DMR_C_TABLE[i]; + + if (READ_BIT(bytes, aPos)) + a |= MASK; + if (READ_BIT(bytes, bPos)) + b |= MASK; + if (READ_BIT(bytes, cPos)) + c |= MASK; + + MASK >>= 1; + } + + unsigned int errors = regenerate(a, b, c, true); + + MASK = 0x800000U; + for (unsigned int i = 0U; i < 24U; i++) { + unsigned int aPos = DMR_A_TABLE[i]; + unsigned int bPos = DMR_B_TABLE[i]; + unsigned int cPos = DMR_C_TABLE[i]; + + WRITE_BIT(bytes, aPos, a & MASK); + WRITE_BIT(bytes, bPos, b & MASK); + WRITE_BIT(bytes, cPos, c & MASK); + + MASK >>= 1; + } + + return errors; +} + unsigned int CAMBEFEC::regenerateYSF3(unsigned char* bytes) const { assert(bytes != NULL); diff --git a/AMBEFEC.h b/AMBEFEC.h index 35e8d27..19453db 100644 --- a/AMBEFEC.h +++ b/AMBEFEC.h @@ -28,6 +28,8 @@ public: unsigned int regenerateDStar(unsigned char* bytes) const; + unsigned int regenerateYSF1(unsigned char* bytes) const; + unsigned int regenerateYSF3(unsigned char* bytes) const; private: diff --git a/YSFPayload.cpp b/YSFPayload.cpp index ec0818c..19b32da 100644 --- a/YSFPayload.cpp +++ b/YSFPayload.cpp @@ -257,11 +257,11 @@ unsigned int CYSFPayload::processVDMode1Audio(unsigned char* data) // Regenerate the AMBE FEC unsigned int errors = 0U; - errors += m_fec.regenerateDMR(data + 9U); - errors += m_fec.regenerateDMR(data + 27U); - errors += m_fec.regenerateDMR(data + 45U); - errors += m_fec.regenerateDMR(data + 63U); - errors += m_fec.regenerateDMR(data + 81U); + errors += m_fec.regenerateYSF1(data + 9U); + errors += m_fec.regenerateYSF1(data + 27U); + errors += m_fec.regenerateYSF1(data + 45U); + errors += m_fec.regenerateYSF1(data + 63U); + errors += m_fec.regenerateYSF1(data + 81U); return errors; }