From 1d9f4dc29e77a2a3125f346ddd58dd410a535a73 Mon Sep 17 00:00:00 2001 From: Jonathan Naylor Date: Thu, 21 Apr 2016 18:00:26 +0100 Subject: [PATCH] Start restructuring the YSF code. --- YSFControl.cpp | 222 ++++++++++++++++++++++++++++++++++--------------- YSFControl.h | 4 +- YSFFICH.cpp | 24 +----- YSFFICH.h | 4 - YSFPayload.cpp | 171 ++++++++++++++++++++++++++++--------- YSFPayload.h | 5 +- 6 files changed, 289 insertions(+), 141 deletions(-) diff --git a/YSFControl.cpp b/YSFControl.cpp index 8468e7c..c0822db 100644 --- a/YSFControl.cpp +++ b/YSFControl.cpp @@ -12,23 +12,18 @@ */ #include "YSFControl.h" +#include "YSFFICH.h" #include "Utils.h" #include "Sync.h" #include "Log.h" #include #include +#include #include // #define DUMP_YSF -/* - * TODO: - * AMBE FEC reconstruction. - * Callsign extraction + late entry. - * Uplink and downlink callsign addition. - */ - CYSFControl::CYSFControl(const std::string& callsign, IDisplay* display, unsigned int timeout, bool duplex, bool parrot) : m_display(display), m_duplex(duplex), @@ -37,7 +32,6 @@ m_state(RS_RF_LISTENING), m_timeoutTimer(1000U, timeout), m_interval(), m_frames(0U), -m_fich(), m_source(NULL), m_dest(NULL), m_payload(), @@ -79,10 +73,11 @@ bool CYSFControl::writeModem(unsigned char *data) if (type == TAG_LOST) return false; - bool valid = m_fich.decode(data + 2U); + CYSFFICH fich; + bool valid = fich.decode(data + 2U); if (valid && m_state == RS_RF_LISTENING) { - unsigned char fi = m_fich.getFI(); + unsigned char fi = fich.getFI(); if (fi == YSF_FI_TERMINATOR) return false; @@ -98,27 +93,85 @@ bool CYSFControl::writeModem(unsigned char *data) if (m_state != RS_RF_AUDIO) return false; - unsigned char fi = m_fich.getFI(); - if (valid && fi == YSF_FI_TERMINATOR) { + unsigned char fi = fich.getFI(); + if (valid && fi == YSF_FI_HEADER) { CSync::addYSFSync(data + 2U); - m_fich.encode(data + 2U); + unsigned char fn = fich.getFN(); + unsigned char ft = fich.getFT(); + unsigned char dt = fich.getDT(); + unsigned char cm = fich.getCM(); - unsigned char fn = m_fich.getFN(); - unsigned char ft = m_fich.getFT(); - unsigned char dt = m_fich.getDT(); + unsigned char orig[YSF_FRAME_LENGTH_BYTES]; + ::memcpy(orig, data + 2U, YSF_FRAME_LENGTH_BYTES); - unsigned int errs = m_fich.getErrors(); + fich.encode(data + 2U); + valid = m_payload.processHeader(data + 2U); - LogDebug("YSF, FI=%X FN=%u FT=%u DT=%X Errs=%u", fi, fn, ft, dt, errs); + unsigned int errs = calculateBER(orig, data + 2U); + LogDebug("YSF, FI=%u FN=%u FT=%u DT=%u BER=%.1f%%", fi, fn, ft, dt, float(errs) / 9.6F); - // m_payload.process(data + 2U, fi, fn, ft, dt); + if (m_duplex) { + fich.setMR(YSF_MR_BUSY); + fich.encode(data + 2U); + + data[0U] = TAG_DATA; + data[1U] = 0x00U; + writeQueue(data); + } + + if (m_parrot != NULL) { + fich.setMR(YSF_MR_NOT_BUSY); + fich.encode(data + 2U); + + data[0U] = TAG_DATA; + data[1U] = 0x00U; + writeParrot(data); + } + + if (valid) + m_source = m_payload.getSource(); + + if (cm == YSF_CM_GROUP) { + m_dest = (unsigned char*)"CQCQCQ "; + } else { + if (valid) + m_dest = m_payload.getDest(); + } + +#if defined(DUMP_YSF) + writeFile(data + 2U); +#endif + + if (m_source != NULL && m_dest != NULL) + LogMessage("YSF, received header from %10.10s to %10.10s", m_source, m_dest); + else if (m_source == NULL && m_dest != NULL) + LogMessage("YSF, received header from ?????????? to %10.10s", m_dest); + else if (m_source != NULL && m_dest == NULL) + LogMessage("YSF, received header from %10.10s to ??????????", m_source); + else + LogMessage("YSF, received header from ?????????? to ??????????"); + } else if (valid && fi == YSF_FI_TERMINATOR) { + CSync::addYSFSync(data + 2U); + + unsigned char fn = fich.getFN(); + unsigned char ft = fich.getFT(); + unsigned char dt = fich.getDT(); + + unsigned char orig[YSF_FRAME_LENGTH_BYTES]; + ::memcpy(orig, data + 2U, YSF_FRAME_LENGTH_BYTES); + + fich.encode(data + 2U); + m_payload.processTrailer(data + 2U); + + unsigned int errs = calculateBER(orig, data + 2U); + LogDebug("YSF, FI=%u FN=%u FT=%u DT=%u BER=%.1f%%", fi, fn, ft, dt, float(errs) / 9.6F); m_frames++; if (m_duplex) { - m_fich.setMR(YSF_MR_BUSY); - m_fich.encode(data + 2U); + fich.setMR(YSF_MR_BUSY); + fich.encode(data + 2U); data[0U] = TAG_EOT; data[1U] = 0x00U; @@ -126,8 +179,8 @@ bool CYSFControl::writeModem(unsigned char *data) } if (m_parrot != NULL) { - m_fich.setMR(YSF_MR_NOT_BUSY); - m_fich.encode(data + 2U); + fich.setMR(YSF_MR_NOT_BUSY); + fich.encode(data + 2U); data[0U] = TAG_EOT; data[1U] = 0x00U; @@ -142,66 +195,62 @@ bool CYSFControl::writeModem(unsigned char *data) writeEndOfTransmission(); return false; - } else { + } else if (valid) { CSync::addYSFSync(data + 2U); - if (valid) { - m_fich.encode(data + 2U); + unsigned char cm = fich.getCM(); + unsigned char fn = fich.getFN(); + unsigned char ft = fich.getFT(); + unsigned char dt = fich.getDT(); - unsigned char cm = m_fich.getCM(); - unsigned char fn = m_fich.getFN(); - unsigned char ft = m_fich.getFT(); - unsigned char dt = m_fich.getDT(); + unsigned char orig[YSF_FRAME_LENGTH_BYTES]; + ::memcpy(orig, data + 2U, YSF_FRAME_LENGTH_BYTES); - unsigned int errs = m_fich.getErrors(); + fich.encode(data + 2U); + m_payload.processData(data + 2U, fn, dt); - LogDebug("YSF, FI=%X FN=%u FT=%u DT=%X Errs=%u", fi, fn, ft, dt, errs); + unsigned int errs = calculateBER(orig, data + 2U); + LogDebug("YSF, FI=%u FN=%u FT=%u DT=%u BER=%.1f%%", fi, fn, ft, dt, float(errs) / 9.6F); - // m_payload.process(data + 2U, fi, fn, ft, dt); + bool change = false; - bool change = false; - - if (m_dest == NULL) { - if (cm == YSF_CM_GROUP) { - m_dest = (unsigned char*)"CQCQCQ "; - change = true; - } else { - m_dest = m_payload.getDest(); - if (m_dest != NULL) - change = true; - } - } - - if (m_source == NULL) { - m_source = m_payload.getSource(); - if (m_source != NULL) + if (m_dest == NULL) { + if (cm == YSF_CM_GROUP) { + m_dest = (unsigned char*)"CQCQCQ "; + change = true; + } else { + m_dest = m_payload.getDest(); + if (m_dest != NULL) change = true; } + } - if (change) { - if (m_source != NULL && m_dest != NULL) { - m_display->writeFusion((char*)m_source, (char*)m_dest); - LogMessage("YSF, received transmission from %10.10s to %10.10s", m_source, m_dest); - } - if (m_source != NULL && m_dest == NULL) { - m_display->writeFusion((char*)m_source, "??????????"); - LogMessage("YSF, received transmission from %10.10s to ??????????", m_source); - } - if (m_source == NULL && m_dest != NULL) { - m_display->writeFusion("??????????", (char*)m_dest); - LogMessage("YSF, received transmission from ?????????? to %10.10s", m_dest); - } + if (m_source == NULL) { + m_source = m_payload.getSource(); + if (m_source != NULL) + change = true; + } + + if (change) { + if (m_source != NULL && m_dest != NULL) { + m_display->writeFusion((char*)m_source, (char*)m_dest); + LogMessage("YSF, received transmission from %10.10s to %10.10s", m_source, m_dest); + } + if (m_source != NULL && m_dest == NULL) { + m_display->writeFusion((char*)m_source, "??????????"); + LogMessage("YSF, received transmission from %10.10s to ??????????", m_source); + } + if (m_source == NULL && m_dest != NULL) { + m_display->writeFusion("??????????", (char*)m_dest); + LogMessage("YSF, received transmission from ?????????? to %10.10s", m_dest); } - } else { - // Reconstruct FICH based on the last valid frame - m_fich.setFI(YSF_FI_COMMUNICATIONS); // Communication channel } m_frames++; if (m_duplex) { - m_fich.setMR(YSF_MR_BUSY); - m_fich.encode(data + 2U); + fich.setMR(YSF_MR_BUSY); + fich.encode(data + 2U); data[0U] = TAG_DATA; data[1U] = 0x00U; @@ -209,14 +258,34 @@ bool CYSFControl::writeModem(unsigned char *data) } if (m_parrot != NULL) { - m_fich.setMR(YSF_MR_NOT_BUSY); - m_fich.encode(data + 2U); + fich.setMR(YSF_MR_NOT_BUSY); + fich.encode(data + 2U); data[0U] = TAG_DATA; data[1U] = 0x00U; writeParrot(data); } +#if defined(DUMP_YSF) + writeFile(data + 2U); +#endif + } else { + CSync::addYSFSync(data + 2U); + + m_frames++; + + if (m_duplex) { + data[0U] = TAG_DATA; + data[1U] = 0x00U; + writeQueue(data); + } + + if (m_parrot != NULL) { + data[0U] = TAG_DATA; + data[1U] = 0x00U; + writeParrot(data); + } + #if defined(DUMP_YSF) writeFile(data + 2U); #endif @@ -352,3 +421,18 @@ void CYSFControl::closeFile() m_fp = NULL; } } + +unsigned int CYSFControl::calculateBER(const unsigned char* orig, const unsigned char *curr) const +{ + unsigned int errors = 0U; + + for (unsigned int i = 0U; i < YSF_FRAME_LENGTH_BYTES; i++) { + unsigned char v = orig[i] ^ curr[i]; + while (v != 0U) { + v &= v - 1U; + errors++; + } + } + + return errors; +} diff --git a/YSFControl.h b/YSFControl.h index 04b744e..5dbd80b 100644 --- a/YSFControl.h +++ b/YSFControl.h @@ -26,7 +26,6 @@ #include "YSFParrot.h" #include "Display.h" #include "Defines.h" -#include "YSFFICH.h" #include "Timer.h" #include "Modem.h" @@ -51,7 +50,6 @@ private: CTimer m_timeoutTimer; CStopWatch m_interval; unsigned int m_frames; - CYSFFICH m_fich; unsigned char* m_source; unsigned char* m_dest; CYSFPayload m_payload; @@ -66,6 +64,8 @@ private: bool openFile(); bool writeFile(const unsigned char* data); void closeFile(); + + unsigned int calculateBER(const unsigned char* orig, const unsigned char* curr) const; }; #endif diff --git a/YSFFICH.cpp b/YSFFICH.cpp index 45080de..877f2f5 100644 --- a/YSFFICH.cpp +++ b/YSFFICH.cpp @@ -55,17 +55,13 @@ const unsigned int INTERLEAVE_TABLE[] = { 38U, 78U, 118U, 158U, 198U}; CYSFFICH::CYSFFICH() : -m_bytes(NULL), -m_fich(NULL), -m_errors(0U) +m_fich(NULL) { - m_bytes = new unsigned char[YSF_FICH_LENGTH_BYTES]; m_fich = new unsigned char[6U]; } CYSFFICH::~CYSFFICH() { - delete[] m_bytes; delete[] m_fich; } @@ -76,9 +72,6 @@ bool CYSFFICH::decode(const unsigned char* bytes) // Skip the sync bytes bytes += YSF_SYNC_LENGTH_BYTES; - // Save a copy of the FICH for later error rate calculations - ::memcpy(m_bytes, bytes, YSF_FICH_LENGTH_BYTES); - CYSFConvolution viterbi; viterbi.start(); @@ -164,16 +157,6 @@ void CYSFFICH::encode(unsigned char* bytes) n++; WRITE_BIT1(bytes, n, s1); } - - // Calculate the errors - m_errors = 0U; - for (unsigned int i = 0U; i < YSF_FICH_LENGTH_BYTES; i++) { - unsigned char v = bytes[i] ^ m_bytes[i]; - while (v != 0U) { - v &= v - 1U; - m_errors++; - } - } } unsigned char CYSFFICH::getFI() const @@ -220,8 +203,3 @@ void CYSFFICH::setVoIP(bool on) else m_fich[2U] &= 0xFBU; } - -unsigned int CYSFFICH::getErrors() const -{ - return m_errors; -} diff --git a/YSFFICH.h b/YSFFICH.h index f308259..6e8cbe5 100644 --- a/YSFFICH.h +++ b/YSFFICH.h @@ -28,8 +28,6 @@ public: void encode(unsigned char* bytes); - unsigned int getErrors() const; - unsigned char getFI() const; unsigned char getCM() const; unsigned char getFN() const; @@ -41,9 +39,7 @@ public: void setVoIP(bool set); private: - unsigned char* m_bytes; unsigned char* m_fich; - unsigned int m_errors; }; #endif diff --git a/YSFPayload.cpp b/YSFPayload.cpp index c88ed79..76a2bff 100644 --- a/YSFPayload.cpp +++ b/YSFPayload.cpp @@ -91,16 +91,10 @@ CYSFPayload::~CYSFPayload() delete[] m_dest; } -void CYSFPayload::process(unsigned char* bytes, unsigned char fi, unsigned char fn, unsigned char ft, unsigned char dt) +void CYSFPayload::processData(unsigned char* bytes, unsigned char fn, unsigned char dt) { assert(bytes != NULL); - // Header and trailer - if (fi == YSF_FI_HEADER || fi == YSF_FI_TERMINATOR) { - processHeader(bytes + YSF_SYNC_LENGTH_BYTES + YSF_FICH_LENGTH_BYTES); - return; - } - switch (dt) { case YSF_DT_VD_MODE1: processVDMode1(bytes + YSF_SYNC_LENGTH_BYTES + YSF_FICH_LENGTH_BYTES, fn); @@ -120,7 +114,7 @@ void CYSFPayload::process(unsigned char* bytes, unsigned char fi, unsigned char } } -void CYSFPayload::processHeader(unsigned char* data) +bool CYSFPayload::processHeader(unsigned char* data) { unsigned char dch[45U]; @@ -147,13 +141,13 @@ void CYSFPayload::processHeader(unsigned char* data) unsigned char output[23U]; conv.chainback(output, 180U); - bool ret = CCRC::checkCCITT162(output, 22U); - if (ret) { + bool valid = CCRC::checkCCITT162(output, 22U); + if (valid) { for (unsigned int i = 0U; i < 20U; i++) output[i] ^= WHITENING_DATA[i]; - CUtils::dump("Header/Trailer, Source", output + 0U, 10U); - CUtils::dump("Header/Trailer, Destination", output + 10U, 10U); + CUtils::dump("Header, Source", output + 0U, 10U); + CUtils::dump("Header, Destination", output + 10U, 10U); if (m_source == NULL) { m_source = new unsigned char[10U]; @@ -236,19 +230,129 @@ void CYSFPayload::processHeader(unsigned char* data) ::memcpy(p1, p2, 9U); p1 += 18U; p2 += 9U; } + + return valid; +} + +void CYSFPayload::processTrailer(unsigned char* data) +{ + unsigned char dch[45U]; + + unsigned char* p1 = data; + unsigned char* p2 = dch; + for (unsigned int i = 0U; i < 5U; i++) { + ::memcpy(p2, p1, 9U); + p1 += 18U; p2 += 9U; + } + + CYSFConvolution conv; + conv.start(); + + for (unsigned int i = 0U; i < 180U; i++) { + unsigned int n = INTERLEAVE_TABLE_9_20[i]; + uint8_t s0 = READ_BIT1(dch, n) ? 1U : 0U; + + n++; + uint8_t s1 = READ_BIT1(dch, n) ? 1U : 0U; + + conv.decode(s0, s1); + } + + unsigned char output[23U]; + conv.chainback(output, 180U); + + bool valid = CCRC::checkCCITT162(output, 22U); + if (valid) { + for (unsigned int i = 0U; i < 20U; i++) + output[i] ^= WHITENING_DATA[i]; + + CUtils::dump("Trailer, Source", output + 0U, 10U); + CUtils::dump("Trailer, Destination", output + 10U, 10U); + + for (unsigned int i = 0U; i < 20U; i++) + output[i] ^= WHITENING_DATA[i]; + + CCRC::addCCITT162(output, 22U); + output[22U] = 0x00U; + + unsigned char convolved[45U]; + conv.encode(output, convolved, 180U); + + unsigned char bytes[45U]; + unsigned int j = 0U; + for (unsigned int i = 0U; i < 180U; i++) { + unsigned int n = INTERLEAVE_TABLE_9_20[i]; + + bool s0 = READ_BIT1(convolved, j) != 0U; + j++; + + bool s1 = READ_BIT1(convolved, j) != 0U; + j++; + + WRITE_BIT1(bytes, n, s0); + + n++; + WRITE_BIT1(bytes, n, s1); + } + + p1 = data; + p2 = bytes; + for (unsigned int i = 0U; i < 5U; i++) { + ::memcpy(p1, p2, 9U); + p1 += 18U; p2 += 9U; + } + } + + ::memset(output, ' ', 20U); + if (m_downlink != NULL) + ::memcpy(output + 0U, m_downlink, 10U); + if (m_uplink != NULL) + ::memcpy(output + 10U, m_uplink, 10U); + for (unsigned int i = 0U; i < 20U; i++) + output[i] ^= WHITENING_DATA[i]; + + CCRC::addCCITT162(output, 22U); + output[22U] = 0x00U; + + unsigned char convolved[45U]; + conv.encode(output, convolved, 180U); + + unsigned char bytes[45U]; + unsigned int j = 0U; + for (unsigned int i = 0U; i < 180U; i++) { + unsigned int n = INTERLEAVE_TABLE_9_20[i]; + + bool s0 = READ_BIT1(convolved, j) != 0U; + j++; + + bool s1 = READ_BIT1(convolved, j) != 0U; + j++; + + WRITE_BIT1(bytes, n, s0); + + n++; + WRITE_BIT1(bytes, n, s1); + } + + p1 = data + 9U; + p2 = bytes; + for (unsigned int i = 0U; i < 5U; i++) { + ::memcpy(p1, p2, 9U); + p1 += 18U; p2 += 9U; + } } void CYSFPayload::processVDMode1(unsigned char* data, unsigned char fn) { // 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); + // 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); - LogMessage("YSF, V/D Mode 1, AMBE FEC %u/235 (%.1f%%)", errors, float(errors) / 2.35F); + // LogMessage("YSF, V/D Mode 1, AMBE FEC %u/235 (%.1f%%)", errors, float(errors) / 2.35F); unsigned char dch[45U]; @@ -297,11 +401,6 @@ void CYSFPayload::processVDMode1(unsigned char* data, unsigned char fn) break; - case 2U: - CUtils::dump("V/D Mode 1, Rem 1+2", output + 0U, 10U); - CUtils::dump("V/D Mode 1, Rem 3+4", output + 10U, 10U); - break; - default: break; } @@ -403,12 +502,6 @@ void CYSFPayload::processVDMode2(unsigned char* data, unsigned char fn) ::memcpy(m_source, output, 10U); } break; - case 4U: - CUtils::dump("V/D Mode 2, Rem 1+2", output, 10U); - break; - case 5U: - CUtils::dump("V/D Mode 2, Rem 3+4", output, 10U); - break; default: break; } @@ -516,10 +609,6 @@ void CYSFPayload::processDataFRMode(unsigned char* data, unsigned char fn) break; - case 1U: - CUtils::dump("Data FR Mode, Rem 1+2", output + 0U, 10U); - CUtils::dump("Data FR Mode, Rem 3+4", output + 10U, 10U); - break; default: break; } @@ -633,14 +722,14 @@ void CYSFPayload::processDataFRMode(unsigned char* data, unsigned char fn) void CYSFPayload::processVoiceFRMode(unsigned char* data) { // Regenerate the AMBE FEC - unsigned int errors = 0U; - errors += m_fec.regenerateYSF3(data + 0U); - errors += m_fec.regenerateYSF3(data + 18U); - errors += m_fec.regenerateYSF3(data + 36U); - errors += m_fec.regenerateYSF3(data + 54U); - errors += m_fec.regenerateYSF3(data + 72U); + // unsigned int errors = 0U; + // errors += m_fec.regenerateYSF3(data + 0U); + // errors += m_fec.regenerateYSF3(data + 18U); + // errors += m_fec.regenerateYSF3(data + 36U); + // errors += m_fec.regenerateYSF3(data + 54U); + // errors += m_fec.regenerateYSF3(data + 72U); - LogMessage("YSF, V Mode 3, AMBE FEC %u/720 (%.1f%%)", errors, float(errors) / 7.2F); + // LogMessage("YSF, V Mode 3, AMBE FEC %u/720 (%.1f%%)", errors, float(errors) / 7.2F); } void CYSFPayload::setUplink(const std::string& callsign) diff --git a/YSFPayload.h b/YSFPayload.h index e9ec5ca..88fb62a 100644 --- a/YSFPayload.h +++ b/YSFPayload.h @@ -28,7 +28,9 @@ public: CYSFPayload(); ~CYSFPayload(); - void process(unsigned char* bytes, unsigned char fi, unsigned char fn, unsigned char ft, unsigned char dt); + bool processHeader(unsigned char* bytes); + void processData(unsigned char* bytes, unsigned char fn, unsigned char dt); + void processTrailer(unsigned char* bytes); unsigned char* getSource(); unsigned char* getDest(); @@ -45,7 +47,6 @@ private: unsigned char* m_dest; CAMBEFEC m_fec; - void processHeader(unsigned char* bytes); void processVDMode1(unsigned char* bytes, unsigned char fn); void processVDMode2(unsigned char* bytes, unsigned char fn); void processDataFRMode(unsigned char* bytes, unsigned char fn);