From 14cb839863e4b75392b22642f5a371f958625ab4 Mon Sep 17 00:00:00 2001 From: AlexandreRouma Date: Mon, 29 Jan 2024 18:43:46 +0100 Subject: [PATCH] clean up rds code and fix use before init --- decoder_modules/radio/src/demodulators/wfm.h | 67 ++----- decoder_modules/radio/src/rds.cpp | 181 +++++++++++-------- decoder_modules/radio/src/rds.h | 42 +++-- decoder_modules/radio/src/rds_demod.h | 87 +++++++++ 4 files changed, 229 insertions(+), 148 deletions(-) create mode 100644 decoder_modules/radio/src/rds_demod.h diff --git a/decoder_modules/radio/src/demodulators/wfm.h b/decoder_modules/radio/src/demodulators/wfm.h index 8c2c03ea..43a5093d 100644 --- a/decoder_modules/radio/src/demodulators/wfm.h +++ b/decoder_modules/radio/src/demodulators/wfm.h @@ -1,14 +1,7 @@ #pragma once #include "../demod.h" #include -#include -#include -#include -#include -#include -#include -#include -#include +#include "../rds_demod.h" #include #include #include @@ -52,59 +45,30 @@ namespace demod { } _config->release(modified); - // Define structure + // Init DSP demod.init(input, bandwidth / 2.0f, getIFSampleRate(), _stereo, _lowPass, _rds); - agc.init(&demod.rdsOut, 1.0, 1e6, 0.1); - costas.init(&agc.out, 0.005f); - - taps = dsp::taps::bandPass(0, 2375, 100, 5000); - fir.init(&costas.out, taps); - double baudfreq = dsp::math::hzToRads(2375.0/2.0, 5000); - costas2.init(&fir.out, 0.01, 0.0, baudfreq, baudfreq - (baudfreq*0.1), baudfreq + (baudfreq*0.1)); - - c2r.init(&costas2.out); - recov.init(&c2r.out, 5000.0 / (2375.0 / 2.0), 1e-6, 0.01, 0.01); - slice.init(&doubler.outA); - diff.init(&slice.out, 2); - hs.init(&diff.out, rdsHandler, this); - - doubler.init(&recov.out); - reshape.init(&doubler.outB, 4096, (1187 / 30) - 4096); + rdsDemod.init(&demod.rdsOut); + hs.init(&rdsDemod.out, rdsHandler, this); + reshape.init(&rdsDemod.soft, 4096, (1187 / 30) - 4096); diagHandler.init(&reshape.out, _diagHandler, this); + + // Init RDS display diag.lines.push_back(-0.8); diag.lines.push_back(0.8); } void start() { - agc.start(); - costas.start(); - fir.start(); - costas2.start(); - c2r.start(); demod.start(); - recov.start(); - slice.start(); - diff.start(); + rdsDemod.start(); hs.start(); - - doubler.start(); reshape.start(); diagHandler.start(); } void stop() { - agc.stop(); - costas.stop(); - fir.stop(); - costas2.stop(); - c2r.stop(); demod.stop(); - recov.stop(); - slice.stop(); - diff.stop(); + rdsDemod.stop(); hs.stop(); - - c2r.stop(); reshape.stop(); diagHandler.stop(); } @@ -320,24 +284,15 @@ namespace demod { } dsp::demod::BroadcastFM demod; - dsp::loop::FastAGC agc; - dsp::loop::Costas<2> costas; - dsp::tap taps; - dsp::filter::FIR fir; - dsp::loop::Costas<2> costas2; - dsp::convert::ComplexToReal c2r; - dsp::clock_recovery::MM recov; - dsp::digital::BinarySlicer slice; - dsp::digital::DifferentialDecoder diff; + RDSDemod rdsDemod; dsp::sink::Handler hs; EventHandler fftRedrawHandler; - dsp::routing::Doubler doubler; dsp::buffer::Reshaper reshape; dsp::sink::Handler diagHandler; ImGui::SymbolDiagram diag; - rds::RDSDecoder rdsDecode; + rds::Decoder rdsDecode; ConfigManager* _config = NULL; diff --git a/decoder_modules/radio/src/rds.cpp b/decoder_modules/radio/src/rds.cpp index c2f37e3a..354cfddf 100644 --- a/decoder_modules/radio/src/rds.cpp +++ b/decoder_modules/radio/src/rds.cpp @@ -30,7 +30,7 @@ namespace rds { const int DATA_LEN = 16; const int POLY_LEN = 10; - void RDSDecoder::process(uint8_t* symbols, int count) { + void Decoder::process(uint8_t* symbols, int count) { for (int i = 0; i < count; i++) { // Shift in the bit shiftReg = ((shiftReg << 1) & 0x3FFFFFF) | (symbols[i] & 1); @@ -86,7 +86,7 @@ namespace rds { } } - uint16_t RDSDecoder::calcSyndrome(uint32_t block) { + uint16_t Decoder::calcSyndrome(uint32_t block) { uint16_t syn = 0; // Calculate the syndrome using a LFSR @@ -105,7 +105,7 @@ namespace rds { return syn; } - uint32_t RDSDecoder::correctErrors(uint32_t block, BlockType type, bool& recovered) { + uint32_t Decoder::correctErrors(uint32_t block, BlockType type, bool& recovered) { // Subtract the offset from block block ^= (uint32_t)OFFSETS[type]; uint32_t out = block; @@ -134,24 +134,28 @@ namespace rds { return out; } - void RDSDecoder::decodeBlockA() { + void Decoder::decodeBlockA() { + // Acquire lock + std::lock_guard lck(blockAMtx); + // If it didn't decode properly return if (!blockAvail[BLOCK_TYPE_A]) { return; } - // Update timeout - std::lock_guard lck(groupMtx); - auto now = std::chrono::high_resolution_clock::now(); - blockALastUpdate = now; - // Decode PI code piCode = (blocks[BLOCK_TYPE_A] >> 10) & 0xFFFF; countryCode = (blocks[BLOCK_TYPE_A] >> 22) & 0xF; programCoverage = (AreaCoverage)((blocks[BLOCK_TYPE_A] >> 18) & 0xF); programRefNumber = (blocks[BLOCK_TYPE_A] >> 10) & 0xFF; decodeCallsign(); + + // Update timeout + blockALastUpdate = std::chrono::high_resolution_clock::now();; } - void RDSDecoder::decodeBlockB() { + void Decoder::decodeBlockB() { + // Acquire lock + std::lock_guard lck(blockBMtx); + // If it didn't decode properly return if (!blockAvail[BLOCK_TYPE_B]) { return; } @@ -162,76 +166,101 @@ namespace rds { // Decode traffic program and program type trafficProgram = (blocks[BLOCK_TYPE_B] >> 20) & 1; programType = (ProgramType)((blocks[BLOCK_TYPE_B] >> 15) & 0x1F); + + // Update timeout + blockBLastUpdate = std::chrono::high_resolution_clock::now(); } - void RDSDecoder::decodeGroup() { - std::lock_guard lck(groupMtx); - auto now = std::chrono::high_resolution_clock::now(); + void Decoder::decodeGroup0() { + // Acquire lock + std::lock_guard lck(group0Mtx); + // Decode Block B data + trafficAnnouncement = (blocks[BLOCK_TYPE_B] >> 14) & 1; + music = (blocks[BLOCK_TYPE_B] >> 13) & 1; + uint8_t diBit = (blocks[BLOCK_TYPE_B] >> 12) & 1; + uint8_t offset = ((blocks[BLOCK_TYPE_B] >> 10) & 0b11); + uint8_t diOffset = 3 - offset; + uint8_t psOffset = offset * 2; + + // Decode Block C data + if (groupVer == GROUP_VER_A && blockAvail[BLOCK_TYPE_C]) { + alternateFrequency = (blocks[BLOCK_TYPE_C] >> 10) & 0xFFFF; + } + + // Write DI bit to the decoder identification + decoderIdent &= ~(1 << diOffset); + decoderIdent |= (diBit << diOffset); + + // Write chars at offset the PSName + if (blockAvail[BLOCK_TYPE_D]) { + programServiceName[psOffset] = (blocks[BLOCK_TYPE_D] >> 18) & 0xFF; + programServiceName[psOffset + 1] = (blocks[BLOCK_TYPE_D] >> 10) & 0xFF; + } + + // Update timeout + group0LastUpdate = std::chrono::high_resolution_clock::now(); + } + + void Decoder::decodeGroup2() { + // Acquire lock + std::lock_guard lck(group2Mtx); + + // Get char offset and write chars in the Radiotext + bool nAB = (blocks[BLOCK_TYPE_B] >> 14) & 1; + uint8_t offset = (blocks[BLOCK_TYPE_B] >> 10) & 0xF; + + // Clear text field if the A/B flag changed + if (nAB != rtAB) { + radioText = " "; + } + rtAB = nAB; + + // Write char at offset in Radiotext + if (groupVer == GROUP_VER_A) { + uint8_t rtOffset = offset * 4; + if (blockAvail[BLOCK_TYPE_C]) { + radioText[rtOffset] = (blocks[BLOCK_TYPE_C] >> 18) & 0xFF; + radioText[rtOffset + 1] = (blocks[BLOCK_TYPE_C] >> 10) & 0xFF; + } + if (blockAvail[BLOCK_TYPE_D]) { + radioText[rtOffset + 2] = (blocks[BLOCK_TYPE_D] >> 18) & 0xFF; + radioText[rtOffset + 3] = (blocks[BLOCK_TYPE_D] >> 10) & 0xFF; + } + } + else { + uint8_t rtOffset = offset * 2; + if (blockAvail[BLOCK_TYPE_D]) { + radioText[rtOffset] = (blocks[BLOCK_TYPE_D] >> 18) & 0xFF; + radioText[rtOffset + 1] = (blocks[BLOCK_TYPE_D] >> 10) & 0xFF; + } + } + + // Update timeout + group2LastUpdate = std::chrono::high_resolution_clock::now(); + } + + void Decoder::decodeGroup() { // Make sure blocks B is available if (!blockAvail[BLOCK_TYPE_B]) { return; } // Decode block B decodeBlockB(); - - if (groupType == 0) { - group0LastUpdate = now; - trafficAnnouncement = (blocks[BLOCK_TYPE_B] >> 14) & 1; - music = (blocks[BLOCK_TYPE_B] >> 13) & 1; - uint8_t diBit = (blocks[BLOCK_TYPE_B] >> 12) & 1; - uint8_t offset = ((blocks[BLOCK_TYPE_B] >> 10) & 0b11); - uint8_t diOffset = 3 - offset; - uint8_t psOffset = offset * 2; - if (groupVer == GROUP_VER_A && blockAvail[BLOCK_TYPE_C]) { - alternateFrequency = (blocks[BLOCK_TYPE_C] >> 10) & 0xFFFF; - } - - // Write DI bit to the decoder identification - decoderIdent &= ~(1 << diOffset); - decoderIdent |= (diBit << diOffset); - - // Write chars at offset the PSName - if (blockAvail[BLOCK_TYPE_D]) { - programServiceName[psOffset] = (blocks[BLOCK_TYPE_D] >> 18) & 0xFF; - programServiceName[psOffset + 1] = (blocks[BLOCK_TYPE_D] >> 10) & 0xFF; - } - } - else if (groupType == 2) { - group2LastUpdate = now; - // Get char offset and write chars in the Radiotext - bool nAB = (blocks[BLOCK_TYPE_B] >> 14) & 1; - uint8_t offset = (blocks[BLOCK_TYPE_B] >> 10) & 0xF; - - // Clear text field if the A/B flag changed - if (nAB != rtAB) { - radioText = " "; - } - rtAB = nAB; - - // Write char at offset in Radiotext - if (groupVer == GROUP_VER_A) { - uint8_t rtOffset = offset * 4; - if (blockAvail[BLOCK_TYPE_C]) { - radioText[rtOffset] = (blocks[BLOCK_TYPE_C] >> 18) & 0xFF; - radioText[rtOffset + 1] = (blocks[BLOCK_TYPE_C] >> 10) & 0xFF; - } - if (blockAvail[BLOCK_TYPE_D]) { - radioText[rtOffset + 2] = (blocks[BLOCK_TYPE_D] >> 18) & 0xFF; - radioText[rtOffset + 3] = (blocks[BLOCK_TYPE_D] >> 10) & 0xFF; - } - } - else { - uint8_t rtOffset = offset * 2; - if (blockAvail[BLOCK_TYPE_D]) { - radioText[rtOffset] = (blocks[BLOCK_TYPE_D] >> 18) & 0xFF; - radioText[rtOffset + 1] = (blocks[BLOCK_TYPE_D] >> 10) & 0xFF; - } - } + // Decode depending on group type + switch (groupType) { + case 0: + decodeGroup0(); + break; + case 2: + decodeGroup2(); + break; + default: + break; } } - void RDSDecoder::decodeCallsign() { + void Decoder::decodeCallsign() { // Determin first better based on offset bool w = (piCode >= 21672); callsign = w ? 'W' : 'K'; @@ -250,23 +279,23 @@ namespace rds { } } - bool RDSDecoder::blockAValid() { + bool Decoder::blockAValid() { auto now = std::chrono::high_resolution_clock::now(); - return (std::chrono::duration_cast(now - blockALastUpdate)).count() < 5000.0; + return (std::chrono::duration_cast(now - blockALastUpdate)).count() < RDS_BLOCK_A_TIMEOUT_MS; } - bool RDSDecoder::blockBValid() { + bool Decoder::blockBValid() { auto now = std::chrono::high_resolution_clock::now(); - return (std::chrono::duration_cast(now - blockALastUpdate)).count() < 5000.0; + return (std::chrono::duration_cast(now - blockBLastUpdate)).count() < RDS_BLOCK_B_TIMEOUT_MS; } - bool RDSDecoder::group0Valid() { + bool Decoder::group0Valid() { auto now = std::chrono::high_resolution_clock::now(); - return (std::chrono::duration_cast(now - group0LastUpdate)).count() < 5000.0; + return (std::chrono::duration_cast(now - group0LastUpdate)).count() < RDS_GROUP_0_TIMEOUT_MS; } - bool RDSDecoder::group2Valid() { + bool Decoder::group2Valid() { auto now = std::chrono::high_resolution_clock::now(); - return (std::chrono::duration_cast(now - group2LastUpdate)).count() < 5000.0; + return (std::chrono::duration_cast(now - group2LastUpdate)).count() < RDS_GROUP_2_TIMEOUT_MS; } } \ No newline at end of file diff --git a/decoder_modules/radio/src/rds.h b/decoder_modules/radio/src/rds.h index 0ecbb423..8f075f85 100644 --- a/decoder_modules/radio/src/rds.h +++ b/decoder_modules/radio/src/rds.h @@ -4,6 +4,11 @@ #include #include +#define RDS_BLOCK_A_TIMEOUT_MS 5000.0 +#define RDS_BLOCK_B_TIMEOUT_MS 5000.0 +#define RDS_GROUP_0_TIMEOUT_MS 5000.0 +#define RDS_GROUP_2_TIMEOUT_MS 5000.0 + namespace rds { enum BlockType { BLOCK_TYPE_A, @@ -205,33 +210,35 @@ namespace rds { DECODER_IDENT_VARIABLE_PTY = (1 << 0) }; - class RDSDecoder { + class Decoder { public: void process(uint8_t* symbols, int count); - bool piCodeValid() { std::lock_guard lck(groupMtx); return blockAValid(); } - uint16_t getPICode() { std::lock_guard lck(groupMtx); return piCode; } - uint8_t getCountryCode() { std::lock_guard lck(groupMtx); return countryCode; } - uint8_t getProgramCoverage() { std::lock_guard lck(groupMtx); return programCoverage; } - uint8_t getProgramRefNumber() { std::lock_guard lck(groupMtx); return programRefNumber; } - std::string getCallsign() { std::lock_guard lck(groupMtx); return callsign; } + bool piCodeValid() { std::lock_guard lck(blockAMtx); return blockAValid(); } + uint16_t getPICode() { std::lock_guard lck(blockAMtx); return piCode; } + uint8_t getCountryCode() { std::lock_guard lck(blockAMtx); return countryCode; } + uint8_t getProgramCoverage() { std::lock_guard lck(blockAMtx); return programCoverage; } + uint8_t getProgramRefNumber() { std::lock_guard lck(blockAMtx); return programRefNumber; } + std::string getCallsign() { std::lock_guard lck(blockAMtx); return callsign; } - bool programTypeValid() { std::lock_guard lck(groupMtx); return blockBValid(); } - ProgramType getProgramType() { std::lock_guard lck(groupMtx); return programType; } + bool programTypeValid() { std::lock_guard lck(blockBMtx); return blockBValid(); } + ProgramType getProgramType() { std::lock_guard lck(blockBMtx); return programType; } - bool musicValid() { std::lock_guard lck(groupMtx); return group0Valid(); } - bool getMusic() { std::lock_guard lck(groupMtx); return music; } - bool PSNameValid() { std::lock_guard lck(groupMtx); return group0Valid(); } - std::string getPSName() { std::lock_guard lck(groupMtx); return programServiceName; } + bool musicValid() { std::lock_guard lck(group0Mtx); return group0Valid(); } + bool getMusic() { std::lock_guard lck(group0Mtx); return music; } + bool PSNameValid() { std::lock_guard lck(group0Mtx); return group0Valid(); } + std::string getPSName() { std::lock_guard lck(group0Mtx); return programServiceName; } - bool radioTextValid() { std::lock_guard lck(groupMtx); return group2Valid(); } - std::string getRadioText() { std::lock_guard lck(groupMtx); return radioText; } + bool radioTextValid() { std::lock_guard lck(group2Mtx); return group2Valid(); } + std::string getRadioText() { std::lock_guard lck(group2Mtx); return radioText; } private: static uint16_t calcSyndrome(uint32_t block); static uint32_t correctErrors(uint32_t block, BlockType type, bool& recovered); void decodeBlockA(); void decodeBlockB(); + void decodeGroup0(); + void decodeGroup2(); void decodeGroup(); void decodeCallsign(); @@ -251,7 +258,7 @@ namespace rds { bool blockAvail[_BLOCK_TYPE_COUNT]; // Block A (All groups) - std::mutex groupMtx; + std::mutex blockAMtx; std::chrono::time_point blockALastUpdate{}; // 1970-01-01 uint16_t piCode; uint8_t countryCode; @@ -260,6 +267,7 @@ namespace rds { std::string callsign; // Block B (All groups) + std::mutex blockBMtx; std::chrono::time_point blockBLastUpdate{}; // 1970-01-01 uint8_t groupType; GroupVersion groupVer; @@ -267,6 +275,7 @@ namespace rds { ProgramType programType; // Group type 0 + std::mutex group0Mtx; std::chrono::time_point group0LastUpdate{}; // 1970-01-01 bool trafficAnnouncement; bool music; @@ -275,6 +284,7 @@ namespace rds { std::string programServiceName = " "; // Group type 2 + std::mutex group2Mtx; std::chrono::time_point group2LastUpdate{}; // 1970-01-01 bool rtAB = false; std::string radioText = " "; diff --git a/decoder_modules/radio/src/rds_demod.h b/decoder_modules/radio/src/rds_demod.h new file mode 100644 index 00000000..1824aec6 --- /dev/null +++ b/decoder_modules/radio/src/rds_demod.h @@ -0,0 +1,87 @@ +#pragma once +#include +#include +#include +#include +#include +#include +#include +#include +#include + +class RDSDemod : public dsp::Processor { + using base_type = dsp::Processor; +public: + RDSDemod() {} + RDSDemod(dsp::stream* in) { init(in); } + ~RDSDemod() {} + + void init(dsp::stream* in) { + // Initialize the DSP + agc.init(NULL, 1.0, 1e6, 0.1); + costas.init(NULL, 0.005f); + taps = dsp::taps::bandPass(0, 2375, 100, 5000); + fir.init(NULL, taps); + double baudfreq = dsp::math::hzToRads(2375.0/2.0, 5000); + costas2.init(NULL, 0.01, 0.0, baudfreq, baudfreq - (baudfreq*0.1), baudfreq + (baudfreq*0.1)); + recov.init(NULL, 5000.0 / (2375.0 / 2.0), 1e-6, 0.01, 0.01); + diff.init(NULL, 2); + + // Free useless buffers + agc.out.free(); + fir.out.free(); + costas2.out.free(); + recov.out.free(); + + // Init the rest + base_type::init(in); + } + + void reset() { + assert(base_type::_block_init); + std::lock_guard lck(base_type::ctrlMtx); + base_type::tempStop(); + agc.reset(); + costas.reset(); + fir.reset(); + costas2.reset(); + recov.reset(); + diff.reset(); + base_type::tempStart(); + } + + inline int process(int count, dsp::complex_t* in, float* softOut, uint8_t* hardOut) { + count = agc.process(count, in, costas.out.readBuf); + count = costas.process(count, costas.out.readBuf, costas.out.writeBuf); + count = fir.process(count, costas.out.writeBuf, costas.out.writeBuf); + count = costas2.process(count, costas.out.writeBuf, costas.out.readBuf); + count = dsp::convert::ComplexToReal::process(count, costas.out.readBuf, softOut); + count = recov.process(count, softOut, softOut); + count = dsp::digital::BinarySlicer::process(count, softOut, diff.out.readBuf); + count = diff.process(count, diff.out.readBuf, hardOut); + return count; + } + + int run() { + int count = base_type::_in->read(); + if (count < 0) { return -1; } + + count = process(count, base_type::_in->readBuf, soft.writeBuf, base_type::out.writeBuf); + + base_type::_in->flush(); + if (!base_type::out.swap(count)) { return -1; } + if (!soft.swap(count)) { return -1; } + return count; + } + + dsp::stream soft; + +private: + dsp::loop::FastAGC agc; + dsp::loop::Costas<2> costas; + dsp::tap taps; + dsp::filter::FIR fir; + dsp::loop::Costas<2> costas2; + dsp::clock_recovery::MM recov; + dsp::digital::DifferentialDecoder diff; +}; \ No newline at end of file