clean up rds code and fix use before init

This commit is contained in:
AlexandreRouma 2024-01-29 18:43:46 +01:00
parent 9501371c6c
commit 14cb839863
4 changed files with 229 additions and 148 deletions

View File

@ -1,14 +1,7 @@
#pragma once
#include "../demod.h"
#include <dsp/demod/broadcast_fm.h>
#include <dsp/clock_recovery/mm.h>
#include <dsp/loop/fast_agc.h>
#include <dsp/loop/costas.h>
#include <dsp/taps/root_raised_cosine.h>
#include <dsp/digital/binary_slicer.h>
#include <dsp/digital/manchester_decoder.h>
#include <dsp/digital/differential_decoder.h>
#include <dsp/routing/doubler.h>
#include "../rds_demod.h"
#include <gui/widgets/symbol_diagram.h>
#include <fstream>
#include <rds.h>
@ -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<dsp::complex_t>(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<dsp::complex_t> agc;
dsp::loop::Costas<2> costas;
dsp::tap<dsp::complex_t> taps;
dsp::filter::FIR<dsp::complex_t, dsp::complex_t> fir;
dsp::loop::Costas<2> costas2;
dsp::convert::ComplexToReal c2r;
dsp::clock_recovery::MM<float> recov;
dsp::digital::BinarySlicer slice;
dsp::digital::DifferentialDecoder diff;
RDSDemod rdsDemod;
dsp::sink::Handler<uint8_t> hs;
EventHandler<ImGui::WaterFall::FFTRedrawArgs> fftRedrawHandler;
dsp::routing::Doubler<float> doubler;
dsp::buffer::Reshaper<float> reshape;
dsp::sink::Handler<float> diagHandler;
ImGui::SymbolDiagram diag;
rds::RDSDecoder rdsDecode;
rds::Decoder rdsDecode;
ConfigManager* _config = NULL;

View File

@ -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<std::mutex> lck(blockAMtx);
// If it didn't decode properly return
if (!blockAvail[BLOCK_TYPE_A]) { return; }
// Update timeout
std::lock_guard<std::mutex> 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<std::mutex> 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<std::mutex> lck(groupMtx);
auto now = std::chrono::high_resolution_clock::now();
void Decoder::decodeGroup0() {
// Acquire lock
std::lock_guard<std::mutex> 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<std::mutex> 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<std::chrono::milliseconds>(now - blockALastUpdate)).count() < 5000.0;
return (std::chrono::duration_cast<std::chrono::milliseconds>(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<std::chrono::milliseconds>(now - blockALastUpdate)).count() < 5000.0;
return (std::chrono::duration_cast<std::chrono::milliseconds>(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<std::chrono::milliseconds>(now - group0LastUpdate)).count() < 5000.0;
return (std::chrono::duration_cast<std::chrono::milliseconds>(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<std::chrono::milliseconds>(now - group2LastUpdate)).count() < 5000.0;
return (std::chrono::duration_cast<std::chrono::milliseconds>(now - group2LastUpdate)).count() < RDS_GROUP_2_TIMEOUT_MS;
}
}

View File

@ -4,6 +4,11 @@
#include <chrono>
#include <mutex>
#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<std::mutex> lck(groupMtx); return blockAValid(); }
uint16_t getPICode() { std::lock_guard<std::mutex> lck(groupMtx); return piCode; }
uint8_t getCountryCode() { std::lock_guard<std::mutex> lck(groupMtx); return countryCode; }
uint8_t getProgramCoverage() { std::lock_guard<std::mutex> lck(groupMtx); return programCoverage; }
uint8_t getProgramRefNumber() { std::lock_guard<std::mutex> lck(groupMtx); return programRefNumber; }
std::string getCallsign() { std::lock_guard<std::mutex> lck(groupMtx); return callsign; }
bool piCodeValid() { std::lock_guard<std::mutex> lck(blockAMtx); return blockAValid(); }
uint16_t getPICode() { std::lock_guard<std::mutex> lck(blockAMtx); return piCode; }
uint8_t getCountryCode() { std::lock_guard<std::mutex> lck(blockAMtx); return countryCode; }
uint8_t getProgramCoverage() { std::lock_guard<std::mutex> lck(blockAMtx); return programCoverage; }
uint8_t getProgramRefNumber() { std::lock_guard<std::mutex> lck(blockAMtx); return programRefNumber; }
std::string getCallsign() { std::lock_guard<std::mutex> lck(blockAMtx); return callsign; }
bool programTypeValid() { std::lock_guard<std::mutex> lck(groupMtx); return blockBValid(); }
ProgramType getProgramType() { std::lock_guard<std::mutex> lck(groupMtx); return programType; }
bool programTypeValid() { std::lock_guard<std::mutex> lck(blockBMtx); return blockBValid(); }
ProgramType getProgramType() { std::lock_guard<std::mutex> lck(blockBMtx); return programType; }
bool musicValid() { std::lock_guard<std::mutex> lck(groupMtx); return group0Valid(); }
bool getMusic() { std::lock_guard<std::mutex> lck(groupMtx); return music; }
bool PSNameValid() { std::lock_guard<std::mutex> lck(groupMtx); return group0Valid(); }
std::string getPSName() { std::lock_guard<std::mutex> lck(groupMtx); return programServiceName; }
bool musicValid() { std::lock_guard<std::mutex> lck(group0Mtx); return group0Valid(); }
bool getMusic() { std::lock_guard<std::mutex> lck(group0Mtx); return music; }
bool PSNameValid() { std::lock_guard<std::mutex> lck(group0Mtx); return group0Valid(); }
std::string getPSName() { std::lock_guard<std::mutex> lck(group0Mtx); return programServiceName; }
bool radioTextValid() { std::lock_guard<std::mutex> lck(groupMtx); return group2Valid(); }
std::string getRadioText() { std::lock_guard<std::mutex> lck(groupMtx); return radioText; }
bool radioTextValid() { std::lock_guard<std::mutex> lck(group2Mtx); return group2Valid(); }
std::string getRadioText() { std::lock_guard<std::mutex> 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<std::chrono::high_resolution_clock> 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<std::chrono::high_resolution_clock> 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<std::chrono::high_resolution_clock> 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<std::chrono::high_resolution_clock> group2LastUpdate{}; // 1970-01-01
bool rtAB = false;
std::string radioText = " ";

View File

@ -0,0 +1,87 @@
#pragma once
#include <dsp/processor.h>
#include <dsp/loop/fast_agc.h>
#include <dsp/loop/costas.h>
#include <dsp/taps/band_pass.h>
#include <dsp/filter/fir.h>
#include <dsp/convert/complex_to_real.h>
#include <dsp/clock_recovery/mm.h>
#include <dsp/digital/binary_slicer.h>
#include <dsp/digital/differential_decoder.h>
class RDSDemod : public dsp::Processor<dsp::complex_t, uint8_t> {
using base_type = dsp::Processor<dsp::complex_t, uint8_t>;
public:
RDSDemod() {}
RDSDemod(dsp::stream<dsp::complex_t>* in) { init(in); }
~RDSDemod() {}
void init(dsp::stream<dsp::complex_t>* in) {
// Initialize the DSP
agc.init(NULL, 1.0, 1e6, 0.1);
costas.init(NULL, 0.005f);
taps = dsp::taps::bandPass<dsp::complex_t>(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<std::recursive_mutex> 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<float> soft;
private:
dsp::loop::FastAGC<dsp::complex_t> agc;
dsp::loop::Costas<2> costas;
dsp::tap<dsp::complex_t> taps;
dsp::filter::FIR<dsp::complex_t, dsp::complex_t> fir;
dsp::loop::Costas<2> costas2;
dsp::clock_recovery::MM<float> recov;
dsp::digital::DifferentialDecoder diff;
};