diff --git a/bladerf_source/src/main.cpp b/bladerf_source/src/main.cpp index 49cd3c62..fc2675ab 100644 --- a/bladerf_source/src/main.cpp +++ b/bladerf_source/src/main.cpp @@ -284,8 +284,8 @@ private: int16_t* samples16 = (int16_t*)samples; _this->currentBuffer = ((_this->currentBuffer + 1) % NUM_BUFFERS); for (size_t i = 0; i < num_samples; i++) { - _this->stream.writeBuf[i].i = (float)samples16[(2 * i)] / 32768.0f; - _this->stream.writeBuf[i].q = (float)samples16[(2 * i) + 1] / 32768.0f; + _this->stream.writeBuf[i].re = (float)samples16[(2 * i)] / 32768.0f; + _this->stream.writeBuf[i].im = (float)samples16[(2 * i) + 1] / 32768.0f; if (!_this->stream.swap(num_samples)) { return _this->streamBuffers[_this->currentBuffer];; } } diff --git a/core/src/dsp/audio.h b/core/src/dsp/audio.h index 21fef95b..795b58c3 100644 --- a/core/src/dsp/audio.h +++ b/core/src/dsp/audio.h @@ -8,8 +8,6 @@ namespace dsp { MonoToStereo(stream* in) { init(in); } - ~MonoToStereo() { generic_block::stop(); } - void init(stream* in) { _in = in; generic_block::registerInput(_in); @@ -26,13 +24,10 @@ namespace dsp { } int run() { - count = _in->read(); + int count = _in->read(); if (count < 0) { return -1; } - for (int i = 0; i < count; i++) { - out.writeBuf[i].l = _in->readBuf[i]; - out.writeBuf[i].r = _in->readBuf[i]; - } + volk_32f_x2_interleave_32fc((lv_32fc_t*)out.writeBuf, _in->readBuf, _in->readBuf, count); _in->flush(); if (!out.swap(count)) { return -1; } @@ -42,7 +37,6 @@ namespace dsp { stream out; private: - int count; stream* _in; }; @@ -53,8 +47,6 @@ namespace dsp { ChannelsToStereo(stream* in_left, stream* in_right) { init(in_left, in_right); } - ~ChannelsToStereo() { generic_block::stop(); } - void init(stream* in_left, stream* in_right) { _in_left = in_left; _in_right = in_right; @@ -76,19 +68,16 @@ namespace dsp { } int run() { - count_l = _in_left->read(); + int count_l = _in_left->read(); if (count_l < 0) { return -1; } - count_r = _in_right->read(); + int count_r = _in_right->read(); if (count_r < 0) { return -1; } if (count_l != count_r) { spdlog::warn("ChannelsToStereo block size missmatch"); } - for (int i = 0; i < count_l; i++) { - out.writeBuf[i].l = _in_left->readBuf[i]; - out.writeBuf[i].r = _in_right->readBuf[i]; - } + volk_32f_x2_interleave_32fc((lv_32fc_t*)out.writeBuf, _in_left->readBuf, _in_right->readBuf, count_l); _in_left->flush(); _in_right->flush(); @@ -99,8 +88,6 @@ namespace dsp { stream out; private: - int count_l; - int count_r; stream* _in_left; stream* _in_right; @@ -112,10 +99,16 @@ namespace dsp { StereoToMono(stream* in) { init(in); } - ~StereoToMono() { generic_block::stop(); } + ~StereoToMono() { + generic_block::stop(); + delete[] l_buf; + delete[] r_buf; + } void init(stream* in) { _in = in; + l_buf = new float[STREAM_BUFFER_SIZE]; + r_buf = new float[STREAM_BUFFER_SIZE]; generic_block::registerInput(_in); generic_block::registerOutput(&out); } @@ -130,14 +123,15 @@ namespace dsp { } int run() { - count = _in->read(); + int count = _in->read(); if (count < 0) { return -1; } for (int i = 0; i < count; i++) { - out.writeBuf[i] = (_in->readBuf[i].l + _in->readBuf[i].r) / 2.0f; + out.writeBuf[i] = (_in->readBuf[i].l + _in->readBuf[i].r) * 0.5f; } _in->flush(); + if (!out.swap(count)) { return -1; } return count; } @@ -145,7 +139,7 @@ namespace dsp { stream out; private: - int count; + float* l_buf, *r_buf; stream* _in; }; @@ -156,8 +150,6 @@ namespace dsp { StereoToChannels(stream* in) { init(in); } - ~StereoToChannels() { generic_block::stop(); } - void init(stream* in) { _in = in; generic_block::registerInput(_in); @@ -175,13 +167,10 @@ namespace dsp { } int run() { - count = _in->read(); + int count = _in->read(); if (count < 0) { return -1; } - for (int i = 0; i < count; i++) { - out_left.writeBuf[i] = _in->readBuf[i].l; - out_right.writeBuf[i] = _in->readBuf[i].r; - } + volk_32fc_deinterleave_32f_x2(out_left.writeBuf, out_right.writeBuf, (lv_32fc_t*)_in->readBuf, count); _in->flush(); if (!out_left.swap(count)) { return -1; } @@ -193,7 +182,6 @@ namespace dsp { stream out_right; private: - int count; stream* _in; }; diff --git a/core/src/dsp/block.h b/core/src/dsp/block.h index 4bfcaa4c..caf5964f 100644 --- a/core/src/dsp/block.h +++ b/core/src/dsp/block.h @@ -11,9 +11,17 @@ #define FL_M_PI 3.1415926535f namespace dsp { + + class generic_unnamed_block { + public: + virtual void start() {} + virtual void stop() {} + virtual int calcOutSize(int inSize) { return inSize; } + virtual int run() { return -1; } + }; template - class generic_block { + class generic_block : public generic_unnamed_block { public: virtual void init() {} @@ -125,4 +133,79 @@ namespace dsp { std::mutex ctrlMtx; }; + + template + class generic_hier_block { + public: + virtual void init() {} + + virtual ~generic_hier_block() { + stop(); + } + + virtual void start() { + std::lock_guard lck(ctrlMtx); + if (running) { + return; + } + running = true; + doStart(); + } + + virtual void stop() { + std::lock_guard lck(ctrlMtx); + if (!running) { + return; + } + doStop(); + running = false; + } + + virtual int calcOutSize(int inSize) { return inSize; } + + friend BLOCK; + + private: + void registerBlock(generic_unnamed_block* block) { + blocks.push_back(block); + } + + void unregisterBlock(generic_unnamed_block* block) { + blocks.erase(std::remove(blocks.begin(), blocks.end(), block), blocks.end()); + } + + virtual void doStart() { + for (auto& block : blocks) { + block->start(); + } + } + + virtual void doStop() { + for (auto& block : blocks) { + block->stop(); + } + } + + void tempStart() { + if (tempStopped) { + doStart(); + tempStopped = false; + } + } + + void tempStop() { + if (running && !tempStopped) { + doStop(); + tempStopped = true; + } + } + + std::vector blocks; + bool tempStopped = false; + bool running = false; + + protected: + std::mutex ctrlMtx; + + }; } \ No newline at end of file diff --git a/core/src/dsp/clock_recovery.h b/core/src/dsp/clock_recovery.h new file mode 100644 index 00000000..e3c47a0f --- /dev/null +++ b/core/src/dsp/clock_recovery.h @@ -0,0 +1,220 @@ +#pragma once +#include +#include +#include + +namespace dsp { + class EdgeTrigClockRecovery : public generic_block { + public: + EdgeTrigClockRecovery() {} + EdgeTrigClockRecovery(stream* in, int omega) { init(in, omega); } + + void init(stream* in, int omega) { + _in = in; + samplesPerSymbol = omega; + generic_block::registerInput(_in); + generic_block::registerOutput(&out); + } + + void setInput(stream* in) { + generic_block::tempStop(); + generic_block::unregisterInput(_in); + _in = in; + generic_block::registerInput(_in); + generic_block::tempStart(); + } + + int run() { + count = _in->read(); + if (count < 0) { return -1; } + + int outCount = 0; + + for (int i = 0; i < count; i++) { + if (DSP_SIGN(lastVal) != DSP_SIGN(_in->readBuf[i])) { + counter = samplesPerSymbol / 2; + lastVal = _in->readBuf[i]; + continue; + } + + if (counter >= samplesPerSymbol) { + counter = 0; + out.writeBuf[outCount] = _in->readBuf[i]; + outCount++; + } + else { + counter++; + } + + lastVal = _in->readBuf[i]; + } + + _in->flush(); + if (!out.swap(outCount)) { return -1; } + return count; + } + + stream out; + + private: + int count; + int samplesPerSymbol = 1; + int counter = 0; + float lastVal = 0; + stream* _in; + + }; + + template + class MMClockRecovery : public generic_block> { + public: + MMClockRecovery() {} + MMClockRecovery(stream* in, float omega, float gainOmega, float muGain, float omegaRelLimit) { + init(in, omega, gainOmega, muGain, omegaRelLimit); + } + + void init(stream* in, float omega, float gainOmega, float muGain, float omegaRelLimit) { + _in = in; + _omega = omega; + _muGain = muGain; + _gainOmega = gainOmega; + _omegaRelLimit = omegaRelLimit; + + omegaMin = _omega - (_omega * _omegaRelLimit); + omegaMax = _omega + (_omega * _omegaRelLimit); + _dynOmega = _omega; + + generic_block>::registerInput(_in); + generic_block>::registerOutput(&out); + } + + void setInput(stream* in) { + generic_block>::tempStop(); + generic_block>::unregisterInput(_in); + _in = in; + generic_block>::registerInput(_in); + generic_block>::tempStart(); + } + + int run() { + count = _in->read(); + if (count < 0) { return -1; } + + int outCount = 0; + float outVal; + float phaseError; + float roundedStep; + int maxOut = 2.0f * _omega * (float)count; + + // Copy the first 7 values to the delay buffer for fast computing + memcpy(&delay[7], _in->readBuf, 7 * sizeof(T)); + + int i = nextOffset; + for (; i < count && outCount < maxOut;) { + + if constexpr (std::is_same_v) { + // Calculate output value + // If we still need to use the old values, calculate using delay buf + // Otherwise, use normal buffer + if (i < 7) { + volk_32f_x2_dot_prod_32f(&outVal, &delay[i], INTERP_TAPS[(int)roundf(_mu * 128.0f)], 8); + } + else { + volk_32f_x2_dot_prod_32f(&outVal, &_in->readBuf[i - 7], INTERP_TAPS[(int)roundf(_mu * 128.0f)], 8); + } + out.writeBuf[outCount++] = outVal; + + // Cursed phase detect approximation (don't ask me how this approximation works) + phaseError = (DSP_STEP(lastOutput)*outVal) - (lastOutput*DSP_STEP(outVal)); + lastOutput = outVal; + } + if constexpr (std::is_same_v || std::is_same_v) { + // Propagate delay + _p_2T = _p_1T; + _p_1T = _p_0T; + + _c_2T = _c_1T; + _c_1T = _c_0T; + + // Perfrom interpolation the same way as for float values + if (i < 7) { + volk_32fc_32f_dot_prod_32fc(&_p_0T, &delay[i], INTERP_TAPS[(int)roundf(_mu * 128.0f)], 8); + } + else { + volk_32fc_32f_dot_prod_32fc(&_p_0T, &_in->readBuf[i - 7], INTERP_TAPS[(int)roundf(_mu * 128.0f)], 8); + } + out.writeBuf[outCount++] = _p_0T; + + // Slice output value + _c_0T = DSP_STEP_CPLX(_p_0T); + + // Cursed math to calculate the phase error + phaseError = (((_p_0T - _p_2T) * _c_1T.conj()) - ((_c_0T - _c_2T) * _p_1T.conj())).re; + } + + // Clamp phase error + if (phaseError > 1.0f) { phaseError = 1.0f; } + if (phaseError < -1.0f) { phaseError = -1.0f; } + + // Adjust the symbol rate using the phase error approximation and clamp + // TODO: Branchless clamp + _dynOmega = _dynOmega + (_gainOmega * phaseError); + if (_dynOmega > omegaMax) { _dynOmega = omegaMax; } + else if (_dynOmega < omegaMin) { _dynOmega = omegaMin; } + + // Adjust the symbol phase according to the phase error approximation + // It will now contain the phase delta needed to jump to the next symbol + // Rounded step will contain the rounded number of symbols + _mu = _mu + _dynOmega + (_muGain * phaseError); + roundedStep = floor(_mu); + + // Step to where the next symbol should be, and check for bogus input + i += (int)roundedStep; + if (i < 0) { i = 0; } + + // Now that we've stepped to the next symbol, keep only the offset inside the symbol + _mu -= roundedStep; + } + + nextOffset = i - count; + + // Save the last 7 values for the next round + memcpy(delay, &_in->readBuf[count - 7], 7 * sizeof(T)); + + _in->flush(); + if (!out.swap(outCount)) { return -1; } + return count; + } + + stream out; + + private: + int count; + + // Delay buffer + T delay[15]; + int nextOffset = 0; + + // Configuration + float _omega = 1.0f; + float _muGain = 1.0f; + float _gainOmega = 0.001f; + float _omegaRelLimit = 0.005; + + // Precalculated values + float omegaMin = _omega + (_omega * _omegaRelLimit); + float omegaMax = _omega + (_omega * _omegaRelLimit); + + // Runtime adjusted + float _dynOmega = _omega; + float _mu = 0.5f; + float lastOutput = 0.0f; + + // Cursed complex stuff + complex_t _p_0T = {0,0}, _p_1T = {0,0}, _p_2T = {0,0}; + complex_t _c_0T = {0,0}, _c_1T = {0,0}, _c_2T = {0,0}; + + stream* _in; + + }; +} \ No newline at end of file diff --git a/core/src/dsp/convertion.h b/core/src/dsp/convertion.h index 52c47496..90ac9d29 100644 --- a/core/src/dsp/convertion.h +++ b/core/src/dsp/convertion.h @@ -8,8 +8,6 @@ namespace dsp { ComplexToStereo(stream* in) { init(in); } - ~ComplexToStereo() { generic_block::stop(); } - static_assert(sizeof(complex_t) == sizeof(stereo_t)); void init(stream* in) { @@ -28,7 +26,7 @@ namespace dsp { } int run() { - count = _in->read(); + int count = _in->read(); if (count < 0) { return -1; } memcpy(out.writeBuf, _in->readBuf, count * sizeof(complex_t)); @@ -41,8 +39,6 @@ namespace dsp { stream out; private: - float avg; - int count; stream* _in; }; @@ -53,8 +49,6 @@ namespace dsp { ComplexToReal(stream* in) { init(in); } - ~ComplexToReal() { generic_block::stop(); } - void init(stream* in) { _in = in; generic_block::registerInput(_in); @@ -71,7 +65,7 @@ namespace dsp { } int run() { - count = _in->read(); + int count = _in->read(); if (count < 0) { return -1; } volk_32fc_deinterleave_real_32f(out.writeBuf, (lv_32fc_t*)_in->readBuf, count); @@ -84,8 +78,6 @@ namespace dsp { stream out; private: - float avg; - int count; stream* _in; }; @@ -96,8 +88,6 @@ namespace dsp { ComplexToImag(stream* in) { init(in); } - ~ComplexToImag() { generic_block::stop(); } - void init(stream* in) { _in = in; generic_block::registerInput(_in); @@ -114,7 +104,7 @@ namespace dsp { } int run() { - count = _in->read(); + int count = _in->read(); if (count < 0) { return -1; } volk_32fc_deinterleave_imag_32f(out.writeBuf, (lv_32fc_t*)_in->readBuf, count); @@ -127,8 +117,6 @@ namespace dsp { stream out; private: - float avg; - int count; stream* _in; }; @@ -163,7 +151,7 @@ namespace dsp { } int run() { - count = _in->read(); + int count = _in->read(); if (count < 0) { return -1; } volk_32f_x2_interleave_32fc((lv_32fc_t*)out.writeBuf, _in->readBuf, nullBuffer, count); @@ -176,8 +164,6 @@ namespace dsp { stream out; private: - float avg; - int count; float* nullBuffer; stream* _in; diff --git a/core/src/dsp/demodulator.h b/core/src/dsp/demodulator.h index 91dc3cde..0438ccaa 100644 --- a/core/src/dsp/demodulator.h +++ b/core/src/dsp/demodulator.h @@ -5,6 +5,8 @@ #include #include #include +#include +#include #define FAST_ATAN2_COEF1 FL_M_PI / 4.0f #define FAST_ATAN2_COEF2 3.0f * FAST_ATAN2_COEF1 @@ -34,8 +36,6 @@ namespace dsp { FloatFMDemod(stream* in, float sampleRate, float deviation) { init(in, sampleRate, deviation); } - ~FloatFMDemod() { generic_block::stop(); } - void init(stream* in, float sampleRate, float deviation) { _in = in; _sampleRate = sampleRate; @@ -79,14 +79,13 @@ namespace dsp { } int run() { - count = _in->read(); + int count = _in->read(); if (count < 0) { return -1; } // This is somehow faster than volk... - float diff, currentPhase; for (int i = 0; i < count; i++) { - currentPhase = fast_arctan2(_in->readBuf[i].i, _in->readBuf[i].q); + currentPhase = fast_arctan2(_in->readBuf[i].im, _in->readBuf[i].re); diff = currentPhase - phase; if (diff > 3.1415926535f) { diff -= 2 * 3.1415926535f; } else if (diff <= -3.1415926535f) { diff += 2 * 3.1415926535f; } @@ -102,8 +101,8 @@ namespace dsp { stream out; private: - int count; - float phase, phasorSpeed, _sampleRate, _deviation; + float phase = 0; + float phasorSpeed, _sampleRate, _deviation; stream* _in; }; @@ -114,8 +113,6 @@ namespace dsp { FMDemod(stream* in, float sampleRate, float deviation) { init(in, sampleRate, deviation); } - ~FMDemod() { generic_block::stop(); } - void init(stream* in, float sampleRate, float deviation) { _in = in; _sampleRate = sampleRate; @@ -159,14 +156,14 @@ namespace dsp { } int run() { - count = _in->read(); + int count = _in->read(); if (count < 0) { return -1; } // This is somehow faster than volk... float diff, currentPhase; for (int i = 0; i < count; i++) { - currentPhase = fast_arctan2(_in->readBuf[i].i, _in->readBuf[i].q); + currentPhase = fast_arctan2(_in->readBuf[i].im, _in->readBuf[i].re); diff = currentPhase - phase; if (diff > 3.1415926535f) { diff -= 2 * 3.1415926535f; } else if (diff <= -3.1415926535f) { diff += 2 * 3.1415926535f; } @@ -183,8 +180,8 @@ namespace dsp { stream out; private: - int count; - float phase, phasorSpeed, _sampleRate, _deviation; + float phase = 0; + float phasorSpeed, _sampleRate, _deviation; stream* _in; }; @@ -338,8 +335,6 @@ namespace dsp { AMDemod(stream* in) { init(in); } - ~AMDemod() { generic_block::stop(); } - void init(stream* in) { _in = in; generic_block::registerInput(_in); @@ -356,13 +351,14 @@ namespace dsp { } int run() { - count = _in->read(); + int count = _in->read(); if (count < 0) { return -1; } volk_32fc_magnitude_32f(out.writeBuf, (lv_32fc_t*)_in->readBuf, count); _in->flush(); + float avg; volk_32f_accumulator_s32f(&avg, out.writeBuf, count); avg /= (float)count; @@ -377,8 +373,6 @@ namespace dsp { stream out; private: - float avg; - int count; stream* _in; }; @@ -479,7 +473,7 @@ namespace dsp { } int run() { - count = _in->read(); + int count = _in->read(); if (count < 0) { return -1; } volk_32fc_s32fc_x2_rotator_32fc(buffer, (lv_32fc_t*)_in->readBuf, phaseDelta, &phase, count); @@ -493,7 +487,6 @@ namespace dsp { stream out; private: - int count; int _mode; float _sampleRate, _bandWidth; stream* _in; @@ -502,4 +495,25 @@ namespace dsp { lv_32fc_t phaseDelta; }; + + class MSKDemod : public generic_hier_block { + public: + MSKDemod() {} + MSKDemod(stream* input, float sampleRate, float deviation, float baudRate) { init(input, sampleRate, deviation, baudRate); } + + void init(stream* input, float sampleRate, float deviation, float baudRate) { + demod.init(input, sampleRate, deviation); + recov.init(&demod.out, sampleRate / baudRate, powf(0.01f, 2) / 4.0f, 0.01f, 100e-6f); + out = &recov.out; + + generic_hier_block::registerBlock(&demod); + generic_hier_block::registerBlock(&recov); + } + + stream* out = NULL; + + private: + FloatFMDemod demod; + MMClockRecovery recov; + }; } \ No newline at end of file diff --git a/core/src/dsp/falcon_fec.h b/core/src/dsp/falcon_fec.h new file mode 100644 index 00000000..560895d6 --- /dev/null +++ b/core/src/dsp/falcon_fec.h @@ -0,0 +1,127 @@ +#pragma once +#include +#include + +// WTF??? +extern "C" +{ +#include +} + +const uint8_t toDB[] = { +0x00, 0x7b, 0xaf, 0xd4, 0x99, 0xe2, 0x36, 0x4d, 0xfa, 0x81, 0x55, 0x2e, 0x63, 0x18, 0xcc, 0xb7, 0x86, 0xfd, 0x29, 0x52, 0x1f, + 0x64, 0xb0, 0xcb, 0x7c, 0x07, 0xd3, 0xa8, 0xe5, 0x9e, 0x4a, 0x31, 0xec, 0x97, 0x43, 0x38, 0x75, 0x0e, 0xda, 0xa1, 0x16, 0x6d, 0xb9, 0xc2, 0x8f, 0xf4, + 0x20, 0x5b, 0x6a, 0x11, 0xc5, 0xbe, 0xf3, 0x88, 0x5c, 0x27, 0x90, 0xeb, 0x3f, 0x44, 0x09, 0x72, 0xa6, 0xdd, 0xef, 0x94, 0x40, 0x3b, 0x76, 0x0d, 0xd9, + 0xa2, 0x15, 0x6e, 0xba, 0xc1, 0x8c, 0xf7, 0x23, 0x58, 0x69, 0x12, 0xc6, 0xbd, 0xf0, 0x8b, 0x5f, 0x24, 0x93, 0xe8, 0x3c, 0x47, 0x0a, 0x71, 0xa5, 0xde, + 0x03, 0x78, 0xac, 0xd7, 0x9a, 0xe1, 0x35, 0x4e, 0xf9, 0x82, 0x56, 0x2d, 0x60, 0x1b, 0xcf, 0xb4, 0x85, 0xfe, 0x2a, 0x51, 0x1c, 0x67, 0xb3, 0xc8, 0x7f, + 0x04, 0xd0, 0xab, 0xe6, 0x9d, 0x49, 0x32, 0x8d, 0xf6, 0x22, 0x59, 0x14, 0x6f, 0xbb, 0xc0, 0x77, 0x0c, 0xd8, 0xa3, 0xee, 0x95, 0x41, 0x3a, 0x0b, 0x70, + 0xa4, 0xdf, 0x92, 0xe9, 0x3d, 0x46, 0xf1, 0x8a, 0x5e, 0x25, 0x68, 0x13, 0xc7, 0xbc, 0x61, 0x1a, 0xce, 0xb5, 0xf8, 0x83, 0x57, 0x2c, 0x9b, 0xe0, 0x34, + 0x4f, 0x02, 0x79, 0xad, 0xd6, 0xe7, 0x9c, 0x48, 0x33, 0x7e, 0x05, 0xd1, 0xaa, 0x1d, 0x66, 0xb2, 0xc9, 0x84, 0xff, 0x2b, 0x50, 0x62, 0x19, 0xcd, 0xb6, + 0xfb, 0x80, 0x54, 0x2f, 0x98, 0xe3, 0x37, 0x4c, 0x01, 0x7a, 0xae, 0xd5, 0xe4, 0x9f, 0x4b, 0x30, 0x7d, 0x06, 0xd2, 0xa9, 0x1e, 0x65, 0xb1, 0xca, 0x87, + 0xfc, 0x28, 0x53, 0x8e, 0xf5, 0x21, 0x5a, 0x17, 0x6c, 0xb8, 0xc3, 0x74, 0x0f, 0xdb, 0xa0, 0xed, 0x96, 0x42, 0x39, 0x08, 0x73, 0xa7, 0xdc, 0x91, 0xea, + 0x3e, 0x45, 0xf2, 0x89, 0x5d, 0x26, 0x6b, 0x10, 0xc4, 0xbf +}; + +const uint8_t fromDB[] = { + 0x00, 0xcc, 0xac, 0x60, 0x79, 0xb5, 0xd5, 0x19, 0xf0, 0x3c, 0x5c, 0x90, 0x89, 0x45, 0x25, 0xe9, 0xfd, 0x31, 0x51, 0x9d, + 0x84, 0x48, 0x28, 0xe4, 0x0d, 0xc1, 0xa1, 0x6d, 0x74, 0xb8, 0xd8, 0x14, 0x2e, 0xe2, 0x82, 0x4e, 0x57, 0x9b, 0xfb, 0x37, 0xde, 0x12, 0x72, 0xbe, 0xa7, + 0x6b, 0x0b, 0xc7, 0xd3, 0x1f, 0x7f, 0xb3, 0xaa, 0x66, 0x06, 0xca, 0x23, 0xef, 0x8f, 0x43, 0x5a, 0x96, 0xf6, 0x3a, 0x42, 0x8e, 0xee, 0x22, 0x3b, 0xf7, + 0x97, 0x5b, 0xb2, 0x7e, 0x1e, 0xd2, 0xcb, 0x07, 0x67, 0xab, 0xbf, 0x73, 0x13, 0xdf, 0xc6, 0x0a, 0x6a, 0xa6, 0x4f, 0x83, 0xe3, 0x2f, 0x36, 0xfa, 0x9a, + 0x56, 0x6c, 0xa0, 0xc0, 0x0c, 0x15, 0xd9, 0xb9, 0x75, 0x9c, 0x50, 0x30, 0xfc, 0xe5, 0x29, 0x49, 0x85, 0x91, 0x5d, 0x3d, 0xf1, 0xe8, 0x24, 0x44, 0x88, + 0x61, 0xad, 0xcd, 0x01, 0x18, 0xd4, 0xb4, 0x78, 0xc5, 0x09, 0x69, 0xa5, 0xbc, 0x70, 0x10, 0xdc, 0x35, 0xf9, 0x99, 0x55, 0x4c, 0x80, 0xe0, 0x2c, 0x38, + 0xf4, 0x94, 0x58, 0x41, 0x8d, 0xed, 0x21, 0xc8, 0x04, 0x64, 0xa8, 0xb1, 0x7d, 0x1d, 0xd1, 0xeb, 0x27, 0x47, 0x8b, 0x92, 0x5e, 0x3e, 0xf2, 0x1b, 0xd7, + 0xb7, 0x7b, 0x62, 0xae, 0xce, 0x02, 0x16, 0xda, 0xba, 0x76, 0x6f, 0xa3, 0xc3, 0x0f, 0xe6, 0x2a, 0x4a, 0x86, 0x9f, 0x53, 0x33, 0xff, 0x87, 0x4b, 0x2b, + 0xe7, 0xfe, 0x32, 0x52, 0x9e, 0x77, 0xbb, 0xdb, 0x17, 0x0e, 0xc2, 0xa2, 0x6e, 0x7a, 0xb6, 0xd6, 0x1a, 0x03, 0xcf, 0xaf, 0x63, 0x8a, 0x46, 0x26, 0xea, + 0xf3, 0x3f, 0x5f, 0x93, 0xa9, 0x65, 0x05, 0xc9, 0xd0, 0x1c, 0x7c, 0xb0, 0x59, 0x95, 0xf5, 0x39, 0x20, 0xec, 0x8c, 0x40, 0x54, 0x98, 0xf8, 0x34, 0x2d, + 0xe1, 0x81, 0x4d, 0xa4, 0x68, 0x08, 0xc4, 0xdd, 0x11, 0x71, 0xbd +}; + +const uint8_t randVals[] = { + 0xFF, 0x48, 0x0E, 0xC0, 0x9A, 0x0D, 0x70, 0xBC, 0x8E, 0x2C, 0x93, 0xAD, 0xA7, 0xB7, 0x46, 0xCE, + 0x5A, 0x97, 0x7D, 0xCC, 0x32, 0xA2, 0xBF, 0x3E, 0x0A, 0x10, 0xF1, 0x88, 0x94, 0xCD, 0xEA, 0xB1, + 0xFE, 0x90, 0x1D, 0x81, 0x34, 0x1A, 0xE1, 0x79, 0x1C, 0x59, 0x27, 0x5B, 0x4F, 0x6E, 0x8D, 0x9C, + 0xB5, 0x2E, 0xFB, 0x98, 0x65, 0x45, 0x7E, 0x7C, 0x14, 0x21, 0xE3, 0x11, 0x29, 0x9B, 0xD5, 0x63, + 0xFD, 0x20, 0x3B, 0x02, 0x68, 0x35, 0xC2, 0xF2, 0x38, 0xB2, 0x4E, 0xB6, 0x9E, 0xDD, 0x1B, 0x39, + 0x6A, 0x5D, 0xF7, 0x30, 0xCA, 0x8A, 0xFC, 0xF8, 0x28, 0x43, 0xC6, 0x22, 0x53, 0x37, 0xAA, 0xC7, + 0xFA, 0x40, 0x76, 0x04, 0xD0, 0x6B, 0x85, 0xE4, 0x71, 0x64, 0x9D, 0x6D, 0x3D, 0xBA, 0x36, 0x72, + 0xD4, 0xBB, 0xEE, 0x61, 0x95, 0x15, 0xF9, 0xF0, 0x50, 0x87, 0x8C, 0x44, 0xA6, 0x6F, 0x55, 0x8F, + 0xF4, 0x80, 0xEC, 0x09, 0xA0, 0xD7, 0x0B, 0xC8, 0xE2, 0xC9, 0x3A, 0xDA, 0x7B, 0x74, 0x6C, 0xE5, + 0xA9, 0x77, 0xDC, 0xC3, 0x2A, 0x2B, 0xF3, 0xE0, 0xA1, 0x0F, 0x18, 0x89, 0x4C, 0xDE, 0xAB, 0x1F, + 0xE9, 0x01, 0xD8, 0x13, 0x41, 0xAE, 0x17, 0x91, 0xC5, 0x92, 0x75, 0xB4, 0xF6, 0xE8, 0xD9, 0xCB, + 0x52, 0xEF, 0xB9, 0x86, 0x54, 0x57, 0xE7, 0xC1, 0x42, 0x1E, 0x31, 0x12, 0x99, 0xBD, 0x56, 0x3F, + 0xD2, 0x03, 0xB0, 0x26, 0x83, 0x5C, 0x2F, 0x23, 0x8B, 0x24, 0xEB, 0x69, 0xED, 0xD1, 0xB3, 0x96, + 0xA5, 0xDF, 0x73, 0x0C, 0xA8, 0xAF, 0xCF, 0x82, 0x84, 0x3C, 0x62, 0x25, 0x33, 0x7A, 0xAC, 0x7F, + 0xA4, 0x07, 0x60, 0x4D, 0x06, 0xB8, 0x5E, 0x47, 0x16, 0x49, 0xD6, 0xD3, 0xDB, 0xA3, 0x67, 0x2D, + 0x4B, 0xBE, 0xE6, 0x19, 0x51, 0x5F, 0x9F, 0x05, 0x08, 0x78, 0xC4, 0x4A, 0x66, 0xF5, 0x58 +}; + +namespace dsp { + class FalconRS : public generic_block { + public: + FalconRS() {} + + FalconRS(stream* in) { init(in); } + + ~FalconRS() { + generic_block::stop(); + } + + void init(stream* in) { + _in = in; + + for (int i = 0; i < 5; i++) { memset(buffers[i], 0, 255); } + for (int i = 0; i < 5; i++) { memset(outBuffers[i], 0, 255); } + rs = correct_reed_solomon_create(correct_rs_primitive_polynomial_ccsds, 120, 11, 16); + if (rs == NULL) { printf("Error creating the reed solomon decoder\n"); } + + generic_block::registerInput(_in); + generic_block::registerOutput(&out); + } + + int run() { + count = _in->read(); + if (count < 0) { return -1; } + + uint8_t* data = _in->readBuf + 4; + + // Deinterleave + for (int i = 0; i < 255*5; i++) { + buffers[i%5][i/5] = fromDB[data[i]]; + } + + // Reed the solomon :weary: + int result = 0; + result = correct_reed_solomon_decode(rs, buffers[0], 255, outBuffers[0]); + if (result == -1) { _in->flush(); return count; } + result = correct_reed_solomon_decode(rs, buffers[1], 255, outBuffers[1]); + if (result == -1) { _in->flush(); return count; } + result = correct_reed_solomon_decode(rs, buffers[2], 255, outBuffers[2]); + if (result == -1) { _in->flush(); return count; } + result = correct_reed_solomon_decode(rs, buffers[3], 255, outBuffers[3]); + if (result == -1) { _in->flush(); return count; } + result = correct_reed_solomon_decode(rs, buffers[4], 255, outBuffers[4]); + if (result == -1) { _in->flush(); return count; } + + // Reinterleave + for (int i = 0; i < 255*5; i++) { + out.writeBuf[i] = toDB[outBuffers[i%5][i/5]] ^ randVals[i % 255]; + } + + out.swap(255*5); + + _in->flush(); + return count; + } + + stream out; + + private: + int count; + uint8_t buffers[5][255]; + uint8_t outBuffers[5][255]; + correct_reed_solomon* rs; + + stream* _in; + + }; +} \ No newline at end of file diff --git a/core/src/dsp/falcon_packet.h b/core/src/dsp/falcon_packet.h new file mode 100644 index 00000000..e1d7aca9 --- /dev/null +++ b/core/src/dsp/falcon_packet.h @@ -0,0 +1,121 @@ +#pragma once +#include +#include + +namespace dsp { + struct FalconFrameHeader { + uint32_t counter; + uint16_t packet; + }; + + class FalconPacketSync : public generic_block { + public: + FalconPacketSync() {} + + FalconPacketSync(stream* in) { init(in); } + + ~FalconPacketSync() { + generic_block::stop(); + } + + void init(stream* in) { + _in = in; + + generic_block::registerInput(_in); + generic_block::registerOutput(&out); + } + + int run() { + count = _in->read(); + if (count < 0) { return -1; } + + // Parse frame header + FalconFrameHeader header; + header.packet = (_in->readBuf[3] | ((_in->readBuf[2] & 0b111) << 8)); + header.counter = ((_in->readBuf[2] >> 3) | (_in->readBuf[1] << 5) | ((_in->readBuf[0] & 0b111111) << 13)); + + // Pointer to the data aera of the frame + uint8_t* data = _in->readBuf + 4; + int dataLen = 1191; + + // If a frame was missed, cancel reading the current packet + if (lastCounter + 1 != header.counter) { + packetRead = -1; + } + lastCounter = header.counter; + + // If frame is just a continuation of a single packet, save it + // If we're not currently reading a packet + if (header.packet == 2047 && packetRead >= 0) { + memcpy(packet + packetRead, data, dataLen); + packetRead += dataLen; + _in->flush(); + printf("Wow, all data\n"); + return count; + } + else if (header.packet == 2047) { + printf("Wow, all data\n"); + _in->flush(); + return count; + } + + // Finish reading the last package and send it + if (packetRead >= 0) { + memcpy(packet + packetRead, data, header.packet); + memcpy(out.writeBuf, packet, packetRead + header.packet); + out.swap(packetRead + header.packet); + packetRead = -1; + } + + // Iterate through every packet of the frame + for (int i = header.packet; i < dataLen;) { + // First, check if we can read the header. If not, save and wait for next frame + if (dataLen - i < 4) { + packetRead = dataLen - i; + memcpy(packet, &data[i], packetRead); + break; + } + + // Extract packet length + uint16_t length = (((data[i] & 0b1111) << 8) | data[i + 1]) + 2; + + // Check if it's not an invalid zero length packet + if (length <= 2) { + packetRead = -1; + break; + } + + uint64_t pktId = ((uint64_t)data[i + 2] << 56) | ((uint64_t)data[i + 3] << 48) | ((uint64_t)data[i + 4] << 40) | ((uint64_t)data[i + 5] << 32) + | ((uint64_t)data[i + 6] << 24) | ((uint64_t)data[i + 7] << 16) | ((uint64_t)data[i + 8] << 8) | data[i + 9]; + + // If the packet doesn't fit the frame, save and go to next frame + if (dataLen - i < length) { + packetRead = dataLen - i; + memcpy(packet, &data[i], packetRead); + break; + } + + // Here, the package fits fully, read it and jump to the next + memcpy(out.writeBuf, &data[i], length); + out.swap(length); + i += length; + + } + + _in->flush(); + return count; + } + + stream out; + + private: + int count; + uint32_t lastCounter = 0; + + int packetRead = -1; + uint8_t packet[0x4008]; + + stream* _in; + + }; +} \ No newline at end of file diff --git a/core/src/dsp/filter.h b/core/src/dsp/filter.h index 190b471c..b77ac1ff 100644 --- a/core/src/dsp/filter.h +++ b/core/src/dsp/filter.h @@ -49,7 +49,7 @@ namespace dsp { } int run() { - count = _in->read(); + int count = _in->read(); if (count < 0) { return -1; } memcpy(bufStart, _in->readBuf, count * sizeof(T)); @@ -76,7 +76,6 @@ namespace dsp { stream out; private: - int count; stream* _in; dsp::filter_window::generic_window* _window; diff --git a/core/src/dsp/math.h b/core/src/dsp/math.h index a43dfaba..b2f8e8ec 100644 --- a/core/src/dsp/math.h +++ b/core/src/dsp/math.h @@ -10,8 +10,6 @@ namespace dsp { Add(stream* a, stream* b) { init(a, b); } - ~Add() { generic_block>::stop(); } - void init(stream* a, stream* b) { _a = a; _b = b; @@ -21,9 +19,9 @@ namespace dsp { } int run() { - a_count = _a->read(); + int a_count = _a->read(); if (a_count < 0) { return -1; } - b_count = _b->read(); + int b_count = _b->read(); if (b_count < 0) { return -1; } if (a_count != b_count) { _a->flush(); @@ -47,7 +45,6 @@ namespace dsp { stream out; private: - int a_count, b_count; stream* _a; stream* _b; @@ -60,8 +57,6 @@ namespace dsp { Substract(stream* a, stream* b) { init(a, b); } - ~Substract() { generic_block>::stop(); } - void init(stream* a, stream* b) { _a = a; _b = b; @@ -71,9 +66,9 @@ namespace dsp { } int run() { - a_count = _a->read(); + int a_count = _a->read(); if (a_count < 0) { return -1; } - b_count = _b->read(); + int b_count = _b->read(); if (b_count < 0) { return -1; } if (a_count != b_count) { _a->flush(); @@ -85,7 +80,7 @@ namespace dsp { volk_32f_x2_subtract_32f((float*)out.writeBuf, (float*)_a->readBuf, (float*)_b->readBuf, a_count * 2); } else { - volk_32f_x2_add_32f(out.writeBuf, _a->readBuf, _b->readBuf, a_count); + volk_32f_x2_subtract_32f(out.writeBuf, _a->readBuf, _b->readBuf, a_count); } _a->flush(); @@ -97,7 +92,6 @@ namespace dsp { stream out; private: - int a_count, b_count; stream* _a; stream* _b; @@ -110,8 +104,6 @@ namespace dsp { Multiply(stream* a, stream* b) { init(a, b); } - ~Multiply() { generic_block::stop(); } - void init(stream* a, stream* b) { _a = a; _b = b; @@ -121,9 +113,9 @@ namespace dsp { } int run() { - a_count = _a->read(); + int a_count = _a->read(); if (a_count < 0) { return -1; } - b_count = _b->read(); + int b_count = _b->read(); if (b_count < 0) { return -1; } if (a_count != b_count) { _a->flush(); @@ -147,7 +139,6 @@ namespace dsp { stream out; private: - int a_count, b_count; stream* _a; stream* _b; diff --git a/core/src/dsp/measure.h b/core/src/dsp/measure.h index 58c8b271..2abe7557 100644 --- a/core/src/dsp/measure.h +++ b/core/src/dsp/measure.h @@ -13,8 +13,6 @@ namespace dsp { LevelMeter(stream* in) { init(in); } - ~LevelMeter() { generic_block::stop(); } - void init(stream* in) { _in = in; generic_block::registerInput(_in); @@ -30,13 +28,14 @@ namespace dsp { } int run() { - count = _in->read(); + int count = _in->read(); if (count < 0) { return -1; } float maxL = 0, maxR = 0; + float absL, absR; for (int i = 0; i < count; i++) { - float absL = fabs(_in->readBuf[i].l); - float absR = fabs(_in->readBuf[i].r); + absL = fabs(_in->readBuf[i].l); + absR = fabs(_in->readBuf[i].r); if (absL > maxL) { maxL = absL; } if (absR > maxR) { maxR = absR; } } @@ -74,7 +73,6 @@ namespace dsp { private: float lvlL = -90.0f; float lvlR = -90.0f; - int count; stream* _in; std::mutex lvlMtx; diff --git a/core/src/dsp/pll.h b/core/src/dsp/pll.h index 7d0c44ad..25752208 100644 --- a/core/src/dsp/pll.h +++ b/core/src/dsp/pll.h @@ -1,185 +1,117 @@ #pragma once #include #include - -#define DSP_SIGN(n) ((n) >= 0) -#define DSP_STEP(n) (((n) > 0.0f) ? 1.0f : -1.0f) +#include +#include namespace dsp { - class SqSymbolRecovery : public generic_block { + template + class CostasLoop: public generic_block> { public: - SqSymbolRecovery() {} + CostasLoop() {} + CostasLoop(stream* in, float loopBandwidth) { init(in, loopBandwidth); } - SqSymbolRecovery(stream* in, int omega) { init(in, omega); } + void init(stream* in, float loopBandwidth) { + _in = in; + lastVCO.re = 1.0f; + lastVCO.im = 0.0f; + _loopBandwidth = loopBandwidth; - ~SqSymbolRecovery() { - generic_block::stop(); + float dampningFactor = sqrtf(2.0f) / 2.0f; + float denominator = (1.0 + 2.0 * dampningFactor * _loopBandwidth + _loopBandwidth * _loopBandwidth); + _alpha = (4 * dampningFactor * _loopBandwidth) / denominator; + _beta = (4 * _loopBandwidth * _loopBandwidth) / denominator; + + generic_block>::registerInput(_in); + generic_block>::registerOutput(&out); } - void init(stream* in, int omega) { + void setInput(stream* in) { + generic_block>::tempStop(); + generic_block>::unregisterInput(_in); _in = in; - samplesPerSymbol = omega; - generic_block::registerInput(_in); - generic_block::registerOutput(&out); + generic_block>::registerInput(_in); + generic_block>::tempStart(); + } + + void setLoopBandwidth(float loopBandwidth) { + generic_block>::tempStop(); + _loopBandwidth = loopBandwidth; + float dampningFactor = sqrtf(2.0f) / 2.0f; + float denominator = (1.0 + 2.0 * dampningFactor * _loopBandwidth + _loopBandwidth * _loopBandwidth); + _alpha = (4 * dampningFactor * _loopBandwidth) / denominator; + _beta = (4 * _loopBandwidth * _loopBandwidth) / denominator; + generic_block>::tempStart(); } int run() { - count = _in->read(); + int count = _in->read(); if (count < 0) { return -1; } - int outCount = 0; + complex_t outVal; + float error; for (int i = 0; i < count; i++) { - if (DSP_SIGN(lastVal) != DSP_SIGN(_in->readBuf[i])) { - counter = samplesPerSymbol / 2; - lastVal = _in->readBuf[i]; - continue; - } - if (counter >= samplesPerSymbol) { - counter = 0; - out.writeBuf[outCount] = _in->readBuf[i]; - outCount++; - } - else { - counter++; - } + // Mix the VFO with the input to create the output value + outVal = lastVCO * _in->readBuf[i]; + out.writeBuf[i] = outVal; + + // Calculate the phase error estimation + if constexpr (ORDER == 2) { + error = outVal.re * outVal.im; + } + if constexpr (ORDER == 4) { + error = (DSP_STEP(outVal.re) * outVal.im) - (DSP_STEP(outVal.im) * outVal.re); + } + if constexpr (ORDER == 8) { + // This is taken from GR, I have no idea how it works but it does... + const float K = (sqrtf(2.0) - 1); + if (fabsf(outVal.re) >= fabsf(outVal.im)) { + error = ((outVal.re > 0.0f ? 1.0f : -1.0f) * outVal.im - + (outVal.im > 0.0f ? 1.0f : -1.0f) * outVal.re * K); + } else { + error = ((outVal.re > 0.0f ? 1.0f : -1.0f) * outVal.im * K - + (outVal.im > 0.0f ? 1.0f : -1.0f) * outVal.re); + } + } + + if (error > 1.0f) { error = 1.0f; } + if (error < -1.0f) { error = -1.0f; } + + // Integrate frequency and clamp it + vcoFrequency += _beta * error; + if (vcoFrequency > 1.0f) { vcoFrequency = 1.0f; } + if (vcoFrequency < -1.0f) { vcoFrequency = -1.0f; } + + // Calculate new phase and wrap it + vcoPhase += vcoFrequency + (_alpha * error); + while (vcoPhase > (2.0f * FL_M_PI)) { vcoPhase -= (2.0f * FL_M_PI); } + while (vcoPhase < (-2.0f * FL_M_PI)) { vcoPhase += (2.0f * FL_M_PI); } + + // Calculate output + lastVCO.re = cosf(vcoPhase); + lastVCO.im = sinf(vcoPhase); - lastVal = _in->readBuf[i]; } _in->flush(); - if (!out.swap(outCount)) { return -1; } + if (!out.swap(count)) { return -1; } return count; } - stream out; + stream out; private: - int count; - int samplesPerSymbol = 1; - int counter = 0; - float lastVal = 0; - stream* _in; + float _loopBandwidth = 1.0f; - }; + float _alpha; // Integral coefficient + float _beta; // Proportional coefficient + float vcoFrequency = 0.0f; + float vcoPhase = 0.0f; + complex_t lastVCO; - - - class MMClockRecovery : public generic_block { - public: - MMClockRecovery() {} - - MMClockRecovery(stream* in, float omega, float gainOmega, float muGain, float omegaRelLimit) { - init(in, omega, gainOmega, muGain, omegaRelLimit); - } - - ~MMClockRecovery() { - generic_block::stop(); - } - - void init(stream* in, float omega, float gainOmega, float muGain, float omegaRelLimit) { - _in = in; - _omega = omega; - _muGain = muGain; - _gainOmega = gainOmega; - _omegaRelLimit = omegaRelLimit; - - omegaMin = _omega - (_omega * _omegaRelLimit); - omegaMax = _omega + (_omega * _omegaRelLimit); - _dynOmega = _omega; - - generic_block::registerInput(_in); - generic_block::registerOutput(&out); - } - - int run() { - count = _in->read(); - if (count < 0) { return -1; } - - int outCount = 0; - float outVal; - float phaseError; - float roundedStep; - int maxOut = 2.0f * _omega * (float)count; - - // Copy the first 7 values to the delay buffer for fast computing - memcpy(&delay[7], _in->readBuf, 7); - - int i = nextOffset; - for (; i < count && outCount < maxOut;) { - // Calculate output value - // If we still need to use the old values, calculate using delay buf - // Otherwise, use normal buffer - if (i < 7) { - volk_32f_x2_dot_prod_32f(&outVal, &delay[i], INTERP_TAPS[(int)roundf(_mu * 128.0f)], 8); - } - else { - volk_32f_x2_dot_prod_32f(&outVal, &_in->readBuf[i - 7], INTERP_TAPS[(int)roundf(_mu * 128.0f)], 8); - } - out.writeBuf[outCount] = outVal; - - - // Cursed phase detect approximation (don't ask me how this approximation works) - phaseError = (DSP_STEP(lastOutput)*outVal) - (lastOutput*DSP_STEP(outVal)); - lastOutput = outVal; - outCount++; - - // Adjust the symbol rate using the phase error approximation and clamp - // TODO: Branchless clamp - _dynOmega = _dynOmega + (_gainOmega * phaseError); - if (_dynOmega > omegaMax) { _dynOmega = omegaMax; } - else if (_dynOmega < omegaMin) { _dynOmega = omegaMin; } - - // Adjust the symbol phase according to the phase error approximation - // It will now contain the phase delta needed to jump to the next symbol - // Rounded step will contain the rounded number of symbols - _mu = _mu + _dynOmega + (_muGain * phaseError); - roundedStep = floor(_mu); - - // Step to where the next symbol should be - i += (int)roundedStep; - - // Now that we've stepped to the next symbol, keep only the offset inside the symbol - _mu -= roundedStep; - } - - nextOffset = i - count; - - // Save the last 7 values for the next round - memcpy(delay, &_in->readBuf[count - 7], 7); - - _in->flush(); - if (!out.swap(outCount)) { return -1; } - return count; - } - - stream out; - - private: - int count; - - // Delay buffer - float delay[15]; - int nextOffset = 0; - - // Configuration - float _omega = 1.0f; - float _muGain = 1.0f; - float _gainOmega = 0.001f; - float _omegaRelLimit = 0.005; - - // Precalculated values - float omegaMin = _omega + (_omega * _omegaRelLimit); - float omegaMax = _omega + (_omega * _omegaRelLimit); - - // Runtime adjusted - float _dynOmega = _omega; - float _mu = 0.5f; - float lastOutput = 0.0f; - - - stream* _in; + stream* _in; }; } \ No newline at end of file diff --git a/core/src/dsp/processing.h b/core/src/dsp/processing.h index 1dccbce4..dd77bc8d 100644 --- a/core/src/dsp/processing.h +++ b/core/src/dsp/processing.h @@ -13,10 +13,6 @@ namespace dsp { FrequencyXlator(stream* in, float sampleRate, float freq) { init(in, sampleRate, freq); } - ~FrequencyXlator() { - generic_block>::stop(); - } - void init(stream* in, float sampleRate, float freq) { _in = in; _sampleRate = sampleRate; @@ -57,7 +53,7 @@ namespace dsp { } int run() { - count = _in->read(); + int count = _in->read(); if (count < 0) { return -1; } // TODO: Do float xlation @@ -76,7 +72,6 @@ namespace dsp { stream out; private: - int count; float _sampleRate; float _freq; lv_32fc_t phaseDelta; @@ -91,8 +86,6 @@ namespace dsp { AGC(stream* in, float fallRate, float sampleRate) { init(in, fallRate, sampleRate); } - ~AGC() { generic_block::stop(); } - void init(stream* in, float fallRate, float sampleRate) { _in = in; _sampleRate = sampleRate; @@ -124,7 +117,7 @@ namespace dsp { } int run() { - count = _in->read(); + int count = _in->read(); if (count < 0) { return -1; } level = pow(10, ((10.0f * log10f(level)) - (_CorrectedFallRate * count)) / 10.0f); @@ -143,7 +136,6 @@ namespace dsp { stream out; private: - int count; float level = 0.0f; float _fallRate; float _CorrectedFallRate; @@ -152,50 +144,64 @@ namespace dsp { }; - class FeedForwardAGC : public generic_block { + template + class FeedForwardAGC : public generic_block> { public: FeedForwardAGC() {} - FeedForwardAGC(stream* in) { init(in); } + FeedForwardAGC(stream* in) { init(in); } - ~FeedForwardAGC() { generic_block::stop(); } - - void init(stream* in) { + void init(stream* in) { _in = in; - generic_block::registerInput(_in); - generic_block::registerOutput(&out); + generic_block>::registerInput(_in); + generic_block>::registerOutput(&out); } - void setInput(stream* in) { - std::lock_guard lck(generic_block::ctrlMtx); - generic_block::tempStop(); - generic_block::unregisterInput(_in); + void setInput(stream* in) { + std::lock_guard lck(generic_block>::ctrlMtx); + generic_block>::tempStop(); + generic_block>::unregisterInput(_in); _in = in; - generic_block::registerInput(_in); - generic_block::tempStart(); + generic_block>::registerInput(_in); + generic_block>::tempStart(); } int run() { - count = _in->read(); + int count = _in->read(); if (count < 0) { return -1; } - float level = 0; - for (int i = 0; i < count; i++) { - if (fabs(_in->readBuf[i]) > level) { level = fabs(_in->readBuf[i]); } - } + float level = 1e-4; - volk_32f_s32f_multiply_32f(out.writeBuf, _in->readBuf, 1.0f / level, count); + // TODO: THIS AGC IS BAAAAAD!!!! + + if constexpr (std::is_same_v) { + for (int i = 0; i < count; i++) { + if (fabs(_in->readBuf[i]) > level) { level = fabs(_in->readBuf[i]); } + } + volk_32f_s32f_multiply_32f(out.writeBuf, _in->readBuf, 1.0f / level, count); + } + if constexpr (std::is_same_v) { + float reAbs, imAbs, val; + for (int i = 0; i < count; i++) { + reAbs = fabs(_in->readBuf[i].re); + imAbs = fabs(_in->readBuf[i].im); + if (reAbs > imAbs) { val = reAbs + 0.4 * imAbs; } + else { val = imAbs + 0.4 * reAbs; } + if (val > level) { level = val; } + } + lv_32fc_t cplxLvl = {1.0f / level, 1.0f / level}; + volk_32fc_s32fc_multiply_32fc((lv_32fc_t*)out.writeBuf, (lv_32fc_t*)_in->readBuf, cplxLvl, count); + } _in->flush(); if (!out.swap(count)) { return -1; } return count; } - stream out; + stream out; private: - int count; - stream* _in; + stream* _in; }; @@ -207,8 +213,6 @@ namespace dsp { Volume(stream* in, float volume) { init(in, volume); } - ~Volume() { generic_block>::stop(); } - void init(stream* in, float volume) { _in = in; _volume = volume; @@ -243,7 +247,7 @@ namespace dsp { } int run() { - count = _in->read(); + int count = _in->read(); if (count < 0) { return -1; } if (_muted) { @@ -271,7 +275,6 @@ namespace dsp { stream out; private: - int count; float level = 1.0f; float _volume = 1.0f; bool _muted = false; @@ -316,10 +319,10 @@ namespace dsp { } int run() { - count = _in->read(); + int count = _in->read(); if (count < 0) { return -1; } - float sum = 0.0f; + float sum; volk_32fc_magnitude_32f(normBuffer, (lv_32fc_t*)_in->readBuf, count); volk_32f_accumulator_s32f(&sum, normBuffer, count); sum /= (float)count; @@ -340,7 +343,6 @@ namespace dsp { private: - int count; float* normBuffer; float _level = -50.0f; stream* _in; @@ -354,10 +356,6 @@ namespace dsp { Packer(stream* in, int count) { init(in, count); } - ~Packer() { - generic_block>::stop(); - } - void init(stream* in, int count) { _in = in; samples = count; @@ -382,7 +380,7 @@ namespace dsp { } int run() { - count = _in->read(); + int count = _in->read(); if (count < 0) { read = 0; return -1; @@ -408,7 +406,6 @@ namespace dsp { stream out; private: - int count; int samples = 1; int read = 0; stream* _in; @@ -451,7 +448,7 @@ namespace dsp { } int run() { - count = _in->read(); + int count = _in->read(); if (count < 0) { return -1; } for (int i = 0; i < count; i++) { @@ -467,7 +464,6 @@ namespace dsp { private: - int count; float* normBuffer; float _level = -50.0f; stream* _in; diff --git a/core/src/dsp/resampling.h b/core/src/dsp/resampling.h index bf60b3d3..0241bc7f 100644 --- a/core/src/dsp/resampling.h +++ b/core/src/dsp/resampling.h @@ -97,7 +97,7 @@ namespace dsp { } virtual int run() override { - count = _in->read(); + int count = _in->read(); if (count < 0) { return -1; } @@ -172,7 +172,6 @@ namespace dsp { tapPhases.clear(); } - int count; stream* _in; dsp::filter_window::generic_window* _window; @@ -195,10 +194,6 @@ namespace dsp { PowerDecimator(stream* in, unsigned int power) { init(in, power); } - ~PowerDecimator() { - generic_block::stop(); - } - void init(stream* in, unsigned int power) { _in = in; _power = power; @@ -218,14 +213,12 @@ namespace dsp { void setPower(unsigned int power) { std::lock_guard lck(generic_block::ctrlMtx); generic_block::tempStop(); - generic_block::unregisterInput(_in); _power = power; - generic_block::registerInput(_in); generic_block::tempStart(); } int run() { - count = _in->read(); + int count = _in->read(); if (count < 0) { return -1; } if (_power == 0) { @@ -233,8 +226,8 @@ namespace dsp { } else if (_power == 1) { for (int j = 0; j < count; j += 2) { - out.writeBuf[j / 2].i = (_in->readBuf[j].i + _in->readBuf[j + 1].i) * 0.5f; - out.writeBuf[j / 2].q = (_in->readBuf[j].q + _in->readBuf[j + 1].q) * 0.5f; + out.writeBuf[j / 2].re = (_in->readBuf[j].re + _in->readBuf[j + 1].re) * 0.5f; + out.writeBuf[j / 2].im = (_in->readBuf[j].im + _in->readBuf[j + 1].im) * 0.5f; } count /= 2; } @@ -244,8 +237,8 @@ namespace dsp { if (_power > 1) { for (int i = 1; i < _power; i++) { for (int j = 0; j < count; j += 2) { - out.writeBuf[j / 2].i = (_in->readBuf[j].i + _in->readBuf[j + 1].i) * 0.5f; - out.writeBuf[j / 2].q = (_in->readBuf[j].q + _in->readBuf[j + 1].q) * 0.5f; + out.writeBuf[j / 2].re = (_in->readBuf[j].re + _in->readBuf[j + 1].re) * 0.5f; + out.writeBuf[j / 2].im = (_in->readBuf[j].im + _in->readBuf[j + 1].im) * 0.5f; } count /= 2; } @@ -259,7 +252,6 @@ namespace dsp { private: - int count; unsigned int _power = 0; stream* _in; diff --git a/core/src/dsp/routing.h b/core/src/dsp/routing.h index 42bf4446..3cd4fd65 100644 --- a/core/src/dsp/routing.h +++ b/core/src/dsp/routing.h @@ -13,8 +13,6 @@ namespace dsp { Splitter(stream* in) { init(in); } - ~Splitter() { generic_block::stop(); } - void init(stream* in) { _in = in; generic_block::registerInput(_in); @@ -72,8 +70,6 @@ namespace dsp { Reshaper(stream* in, int keep, int skip) { init(in, keep, skip); } - ~Reshaper() { generic_block>::stop(); } - void init(stream* in, int keep, int skip) { _in = in; _keep = keep; @@ -95,19 +91,15 @@ namespace dsp { void setKeep(int keep) { std::lock_guard lck(generic_block>::ctrlMtx); generic_block>::tempStop(); - generic_block>::unregisterInput(_in); _keep = keep; ringBuf.setMaxLatency(keep * 2); - generic_block>::registerInput(_in); generic_block>::tempStart(); } void setSkip(int skip) { std::lock_guard lck(generic_block>::ctrlMtx); generic_block>::tempStop(); - generic_block>::unregisterInput(_in); _skip = skip; - generic_block>::registerInput(_in); generic_block>::tempStart(); } @@ -167,8 +159,8 @@ namespace dsp { memmove(buf, delayStart, delaySize); if constexpr (std::is_same_v || std::is_same_v) { for (int i = 0; i < delayCount; i++) { - buf[i].i /= 10.0f; - buf[i].q /= 10.0f; + buf[i].re /= 10.0f; + buf[i].im /= 10.0f; } } } diff --git a/core/src/dsp/sink.h b/core/src/dsp/sink.h index b9890a37..7c219dd1 100644 --- a/core/src/dsp/sink.h +++ b/core/src/dsp/sink.h @@ -10,8 +10,6 @@ namespace dsp { HandlerSink(stream* in, void (*handler)(T* data, int count, void* ctx), void* ctx) { init(in, handler, ctx); } - ~HandlerSink() { generic_block>::stop(); } - void init(stream* in, void (*handler)(T* data, int count, void* ctx), void* ctx) { _in = in; _handler = handler; @@ -37,7 +35,7 @@ namespace dsp { } int run() { - count = _in->read(); + int count = _in->read(); if (count < 0) { return -1; } _handler(_in->readBuf, count, _ctx); _in->flush(); @@ -45,7 +43,6 @@ namespace dsp { } private: - int count; stream* _in; void (*_handler)(T* data, int count, void* ctx); void* _ctx; @@ -59,8 +56,6 @@ namespace dsp { RingBufferSink(stream* in) { init(in); } - ~RingBufferSink() { generic_block>::stop(); } - void init(stream* in) { _in = in; data.init(480); // TODO: Use an argument @@ -77,7 +72,7 @@ namespace dsp { } int run() { - count = _in->read(); + int count = _in->read(); if (count < 0) { return -1; } if (data.write(_in->readBuf, count) < 0) { return -1; } _in->flush(); @@ -97,7 +92,6 @@ namespace dsp { data.clearWriteStop(); } - int count; stream* _in; }; @@ -126,14 +120,13 @@ namespace dsp { } int run() { - count = _in->read(); + int count = _in->read(); if (count < 0) { return -1; } _in->flush(); return count; } private: - int count; stream* _in; }; diff --git a/core/src/dsp/source.h b/core/src/dsp/source.h index c7ed9da3..29416e9b 100644 --- a/core/src/dsp/source.h +++ b/core/src/dsp/source.h @@ -8,8 +8,6 @@ namespace dsp { SineSource(int blockSize, float sampleRate, float freq) { init(blockSize, sampleRate, freq); } - ~SineSource() { generic_block::stop(); } - void init(int blockSize, float sampleRate, float freq) { _blockSize = blockSize; _sampleRate = sampleRate; @@ -79,8 +77,6 @@ namespace dsp { HandlerSource(int (*handler)(T* data, void* ctx), void* ctx) { init(handler, ctx); } - ~HandlerSource() { generic_block>::stop(); } - void init(int (*handler)(T* data, void* ctx), void* ctx) { _handler = handler; _ctx = ctx; @@ -105,7 +101,6 @@ namespace dsp { stream out; private: - int count; int (*_handler)(T* data, void* ctx); void* _ctx; diff --git a/core/src/dsp/types.h b/core/src/dsp/types.h index a1730df0..787bb590 100644 --- a/core/src/dsp/types.h +++ b/core/src/dsp/types.h @@ -2,11 +2,43 @@ namespace dsp { struct complex_t { - float q; - float i; + complex_t& operator*(const float& b) { + return complex_t{re*b, im*b}; + } + + complex_t& operator*(const complex_t& b) { + return complex_t{(re*b.re) - (im*b.im), (im*b.re) + (re*b.im)}; + } + + complex_t& operator+(const complex_t& b) { + return complex_t{re+b.re, im+b.im}; + } + + complex_t& operator-(const complex_t& b) { + return complex_t{re-b.re, im-b.im}; + } + + inline complex_t conj() { + return complex_t{re, -im}; + } + + float re; + float im; }; struct stereo_t { + stereo_t& operator*(const float& b) { + return stereo_t{l*b, r*b}; + } + + stereo_t& operator+(const stereo_t& b) { + return stereo_t{l+b.l, r+b.r}; + } + + stereo_t& operator-(const stereo_t& b) { + return stereo_t{l-b.l, r-b.r}; + } + float l; float r; }; diff --git a/core/src/dsp/utils/ccsds.h b/core/src/dsp/utils/ccsds.h new file mode 100644 index 00000000..36958d9a --- /dev/null +++ b/core/src/dsp/utils/ccsds.h @@ -0,0 +1,121 @@ +#pragma once +#include + +namespace dsp { + namespace ccsds { + const uint8_t TO_DUAL_BASIS[256] = { + 0x00, 0x7b, 0xaf, 0xd4, 0x99, 0xe2, 0x36, 0x4d, 0xfa, 0x81, 0x55, 0x2e, 0x63, 0x18, 0xcc, 0xb7, + 0x86, 0xfd, 0x29, 0x52, 0x1f, 0x64, 0xb0, 0xcb, 0x7c, 0x07, 0xd3, 0xa8, 0xe5, 0x9e, 0x4a, 0x31, + 0xec, 0x97, 0x43, 0x38, 0x75, 0x0e, 0xda, 0xa1, 0x16, 0x6d, 0xb9, 0xc2, 0x8f, 0xf4, 0x20, 0x5b, + 0x6a, 0x11, 0xc5, 0xbe, 0xf3, 0x88, 0x5c, 0x27, 0x90, 0xeb, 0x3f, 0x44, 0x09, 0x72, 0xa6, 0xdd, + 0xef, 0x94, 0x40, 0x3b, 0x76, 0x0d, 0xd9, 0xa2, 0x15, 0x6e, 0xba, 0xc1, 0x8c, 0xf7, 0x23, 0x58, + 0x69, 0x12, 0xc6, 0xbd, 0xf0, 0x8b, 0x5f, 0x24, 0x93, 0xe8, 0x3c, 0x47, 0x0a, 0x71, 0xa5, 0xde, + 0x03, 0x78, 0xac, 0xd7, 0x9a, 0xe1, 0x35, 0x4e, 0xf9, 0x82, 0x56, 0x2d, 0x60, 0x1b, 0xcf, 0xb4, + 0x85, 0xfe, 0x2a, 0x51, 0x1c, 0x67, 0xb3, 0xc8, 0x7f, 0x04, 0xd0, 0xab, 0xe6, 0x9d, 0x49, 0x32, + 0x8d, 0xf6, 0x22, 0x59, 0x14, 0x6f, 0xbb, 0xc0, 0x77, 0x0c, 0xd8, 0xa3, 0xee, 0x95, 0x41, 0x3a, + 0x0b, 0x70, 0xa4, 0xdf, 0x92, 0xe9, 0x3d, 0x46, 0xf1, 0x8a, 0x5e, 0x25, 0x68, 0x13, 0xc7, 0xbc, + 0x61, 0x1a, 0xce, 0xb5, 0xf8, 0x83, 0x57, 0x2c, 0x9b, 0xe0, 0x34, 0x4f, 0x02, 0x79, 0xad, 0xd6, + 0xe7, 0x9c, 0x48, 0x33, 0x7e, 0x05, 0xd1, 0xaa, 0x1d, 0x66, 0xb2, 0xc9, 0x84, 0xff, 0x2b, 0x50, + 0x62, 0x19, 0xcd, 0xb6, 0xfb, 0x80, 0x54, 0x2f, 0x98, 0xe3, 0x37, 0x4c, 0x01, 0x7a, 0xae, 0xd5, + 0xe4, 0x9f, 0x4b, 0x30, 0x7d, 0x06, 0xd2, 0xa9, 0x1e, 0x65, 0xb1, 0xca, 0x87, 0xfc, 0x28, 0x53, + 0x8e, 0xf5, 0x21, 0x5a, 0x17, 0x6c, 0xb8, 0xc3, 0x74, 0x0f, 0xdb, 0xa0, 0xed, 0x96, 0x42, 0x39, + 0x08, 0x73, 0xa7, 0xdc, 0x91, 0xea, 0x3e, 0x45, 0xf2, 0x89, 0x5d, 0x26, 0x6b, 0x10, 0xc4, 0xbf + }; + + const uint8_t FROM_DUAL_BASIS[256] = { + 0x00, 0xcc, 0xac, 0x60, 0x79, 0xb5, 0xd5, 0x19, 0xf0, 0x3c, 0x5c, 0x90, 0x89, 0x45, 0x25, 0xe9, + 0xfd, 0x31, 0x51, 0x9d, 0x84, 0x48, 0x28, 0xe4, 0x0d, 0xc1, 0xa1, 0x6d, 0x74, 0xb8, 0xd8, 0x14, + 0x2e, 0xe2, 0x82, 0x4e, 0x57, 0x9b, 0xfb, 0x37, 0xde, 0x12, 0x72, 0xbe, 0xa7, 0x6b, 0x0b, 0xc7, + 0xd3, 0x1f, 0x7f, 0xb3, 0xaa, 0x66, 0x06, 0xca, 0x23, 0xef, 0x8f, 0x43, 0x5a, 0x96, 0xf6, 0x3a, + 0x42, 0x8e, 0xee, 0x22, 0x3b, 0xf7, 0x97, 0x5b, 0xb2, 0x7e, 0x1e, 0xd2, 0xcb, 0x07, 0x67, 0xab, + 0xbf, 0x73, 0x13, 0xdf, 0xc6, 0x0a, 0x6a, 0xa6, 0x4f, 0x83, 0xe3, 0x2f, 0x36, 0xfa, 0x9a, 0x56, + 0x6c, 0xa0, 0xc0, 0x0c, 0x15, 0xd9, 0xb9, 0x75, 0x9c, 0x50, 0x30, 0xfc, 0xe5, 0x29, 0x49, 0x85, + 0x91, 0x5d, 0x3d, 0xf1, 0xe8, 0x24, 0x44, 0x88, 0x61, 0xad, 0xcd, 0x01, 0x18, 0xd4, 0xb4, 0x78, + 0xc5, 0x09, 0x69, 0xa5, 0xbc, 0x70, 0x10, 0xdc, 0x35, 0xf9, 0x99, 0x55, 0x4c, 0x80, 0xe0, 0x2c, + 0x38, 0xf4, 0x94, 0x58, 0x41, 0x8d, 0xed, 0x21, 0xc8, 0x04, 0x64, 0xa8, 0xb1, 0x7d, 0x1d, 0xd1, + 0xeb, 0x27, 0x47, 0x8b, 0x92, 0x5e, 0x3e, 0xf2, 0x1b, 0xd7, 0xb7, 0x7b, 0x62, 0xae, 0xce, 0x02, + 0x16, 0xda, 0xba, 0x76, 0x6f, 0xa3, 0xc3, 0x0f, 0xe6, 0x2a, 0x4a, 0x86, 0x9f, 0x53, 0x33, 0xff, + 0x87, 0x4b, 0x2b, 0xe7, 0xfe, 0x32, 0x52, 0x9e, 0x77, 0xbb, 0xdb, 0x17, 0x0e, 0xc2, 0xa2, 0x6e, + 0x7a, 0xb6, 0xd6, 0x1a, 0x03, 0xcf, 0xaf, 0x63, 0x8a, 0x46, 0x26, 0xea, 0xf3, 0x3f, 0x5f, 0x93, + 0xa9, 0x65, 0x05, 0xc9, 0xd0, 0x1c, 0x7c, 0xb0, 0x59, 0x95, 0xf5, 0x39, 0x20, 0xec, 0x8c, 0x40, + 0x54, 0x98, 0xf8, 0x34, 0x2d, 0xe1, 0x81, 0x4d, 0xa4, 0x68, 0x08, 0xc4, 0xdd, 0x11, 0x71, 0xbd + }; + + const uint8_t SCRAMBLING_SEQUENCE[255] = { + 0xFF, 0x48, 0x0E, 0xC0, 0x9A, 0x0D, 0x70, 0xBC, 0x8E, 0x2C, 0x93, 0xAD, 0xA7, 0xB7, 0x46, 0xCE, + 0x5A, 0x97, 0x7D, 0xCC, 0x32, 0xA2, 0xBF, 0x3E, 0x0A, 0x10, 0xF1, 0x88, 0x94, 0xCD, 0xEA, 0xB1, + 0xFE, 0x90, 0x1D, 0x81, 0x34, 0x1A, 0xE1, 0x79, 0x1C, 0x59, 0x27, 0x5B, 0x4F, 0x6E, 0x8D, 0x9C, + 0xB5, 0x2E, 0xFB, 0x98, 0x65, 0x45, 0x7E, 0x7C, 0x14, 0x21, 0xE3, 0x11, 0x29, 0x9B, 0xD5, 0x63, + 0xFD, 0x20, 0x3B, 0x02, 0x68, 0x35, 0xC2, 0xF2, 0x38, 0xB2, 0x4E, 0xB6, 0x9E, 0xDD, 0x1B, 0x39, + 0x6A, 0x5D, 0xF7, 0x30, 0xCA, 0x8A, 0xFC, 0xF8, 0x28, 0x43, 0xC6, 0x22, 0x53, 0x37, 0xAA, 0xC7, + 0xFA, 0x40, 0x76, 0x04, 0xD0, 0x6B, 0x85, 0xE4, 0x71, 0x64, 0x9D, 0x6D, 0x3D, 0xBA, 0x36, 0x72, + 0xD4, 0xBB, 0xEE, 0x61, 0x95, 0x15, 0xF9, 0xF0, 0x50, 0x87, 0x8C, 0x44, 0xA6, 0x6F, 0x55, 0x8F, + 0xF4, 0x80, 0xEC, 0x09, 0xA0, 0xD7, 0x0B, 0xC8, 0xE2, 0xC9, 0x3A, 0xDA, 0x7B, 0x74, 0x6C, 0xE5, + 0xA9, 0x77, 0xDC, 0xC3, 0x2A, 0x2B, 0xF3, 0xE0, 0xA1, 0x0F, 0x18, 0x89, 0x4C, 0xDE, 0xAB, 0x1F, + 0xE9, 0x01, 0xD8, 0x13, 0x41, 0xAE, 0x17, 0x91, 0xC5, 0x92, 0x75, 0xB4, 0xF6, 0xE8, 0xD9, 0xCB, + 0x52, 0xEF, 0xB9, 0x86, 0x54, 0x57, 0xE7, 0xC1, 0x42, 0x1E, 0x31, 0x12, 0x99, 0xBD, 0x56, 0x3F, + 0xD2, 0x03, 0xB0, 0x26, 0x83, 0x5C, 0x2F, 0x23, 0x8B, 0x24, 0xEB, 0x69, 0xED, 0xD1, 0xB3, 0x96, + 0xA5, 0xDF, 0x73, 0x0C, 0xA8, 0xAF, 0xCF, 0x82, 0x84, 0x3C, 0x62, 0x25, 0x33, 0x7A, 0xAC, 0x7F, + 0xA4, 0x07, 0x60, 0x4D, 0x06, 0xB8, 0x5E, 0x47, 0x16, 0x49, 0xD6, 0xD3, 0xDB, 0xA3, 0x67, 0x2D, + 0x4B, 0xBE, 0xE6, 0x19, 0x51, 0x5F, 0x9F, 0x05, 0x08, 0x78, 0xC4, 0x4A, 0x66, 0xF5, 0x58 + }; + + const uint32_t ASM_VALUE = 0x1ACFFC1D; + const uint8_t ASM_BYTES[4] = {0x1A, 0xCF, 0xFC, 0x1D}; + const uint8_t ASM_SYMS[16] = {0b00, 0b01, 0b10, 0b10, 0b11, 0b00, 0b11, 0b11, 0b11, 0b11, 0b11, 0b00, 0b00, 0b01, 0b11, 0b01}; + const uint8_t ASM_BITS[32] = {0,0,0,1,1,0,1,0,1,1,0,0,1,1,1,1,1,1,1,1,1,1,0,0,0,0,0,1,1,1,0,1}; + + class FrameDataDecoder { + public: + FrameDataDecoder(int interleaving, bool dualBasis, int rsBlockSize, int rsParitySize) { + _interleaving = interleaving; + _dualBasis = dualBasis; + _rsBlockSize = rsBlockSize; + _rsParitySize = rsParitySize; + } + + void decode(uint8_t* in, uint8_t* out, int count) { + // Deinterleave + if (_dualBasis) { + for (int i = 0; i < count; i++) { + workBuffer[i % _interleaving][i / _interleaving] = FROM_DUAL_BASIS[in[i]]; + } + } + else { + for (int i = 0; i < count; i++) { + workBuffer[i % _interleaving][i / _interleaving] = in[i]; + } + } + + // Reed solomon + + // Reinterleave and descramble if needed + if (_dualBasis) { + for (int i = 0; i < count; i++) { + out[i] = TO_DUAL_BASIS[workOutputBuffer[i % _interleaving][i / _interleaving]]; + } + } + else { + for (int i = 0; i < count; i++) { + out[i] = workOutputBuffer[i % _interleaving][i / _interleaving]; + } + } + } + + private: + uint8_t workBuffer[5][255]; + uint8_t workOutputBuffer[5][255]; + int _interleaving; + bool _dualBasis; + int _rsBlockSize; + int _rsParitySize; + + }; + + inline void descramble(uint8_t* in, uint8_t* out, int count) { + for (int i = 0; i < count; i++){ + out[i] = in[i] ^ SCRAMBLING_SEQUENCE[i % 255]; + } + } + } +} \ No newline at end of file diff --git a/core/src/dsp/utils/macros.h b/core/src/dsp/utils/macros.h new file mode 100644 index 00000000..86f79ec0 --- /dev/null +++ b/core/src/dsp/utils/macros.h @@ -0,0 +1,6 @@ +#pragma once +#include + +#define DSP_SIGN(n) ((n) >= 0) +#define DSP_STEP_CPLX(c) (complex_t{(c.i > 0.0f) ? 1.0f : -1.0f, (c.q > 0.0f) ? 1.0f : -1.0f}) +#define DSP_STEP(n) (((n) > 0.0f) ? 1.0f : -1.0f) \ No newline at end of file diff --git a/core/src/gui/menus/source.cpp b/core/src/gui/menus/source.cpp index e8bdd27e..fd865daf 100644 --- a/core/src/gui/menus/source.cpp +++ b/core/src/gui/menus/source.cpp @@ -24,7 +24,8 @@ namespace sourecmenu { else { spdlog::warn("No source available..."); } - sigpath::sourceManager.setTuningOffset(core::configManager.conf["offset"]); + freqOffset = core::configManager.conf["offset"]; + sigpath::sourceManager.setTuningOffset(freqOffset); core::configManager.release(); } diff --git a/falcon9_decoder/src/main.cpp b/falcon9_decoder/src/main.cpp index 70be0e79..2afe322e 100644 --- a/falcon9_decoder/src/main.cpp +++ b/falcon9_decoder/src/main.cpp @@ -36,6 +36,8 @@ SDRPP_MOD_INFO { #define INPUT_SAMPLE_RATE 6000000 +std::ofstream file("output.ts"); + class Falcon9DecoderModule : public ModuleManager::Instance { public: Falcon9DecoderModule(std::string name) { @@ -46,7 +48,6 @@ public: // dsp::Splitter split; // dsp::Reshaper reshape; // dsp::HandlerSink symSink; - // dsp::stream thrInput; // dsp::Threshold thr; @@ -196,7 +197,7 @@ private: } else if (pktId == 0x01123201042E1403) { fwrite(data + 25, 1, 940, _this->ffplay); - //file.write((char*)(data + 25), 940); + file.write((char*)(data + 25), 940); } //printf("%016" PRIX64 ": %d bytes, %d full\n", pktId, length, count); @@ -219,7 +220,7 @@ private: // DSP Chain dsp::FloatFMDemod demod; - dsp::MMClockRecovery recov; + dsp::MMClockRecovery recov; dsp::Splitter split; diff --git a/file_source/src/main.cpp b/file_source/src/main.cpp index a9a64dad..2bdd01ba 100644 --- a/file_source/src/main.cpp +++ b/file_source/src/main.cpp @@ -120,8 +120,8 @@ private: while (true) { _this->reader->readSamples(inBuf, blockSize * 2 * sizeof(int16_t)); for (int i = 0; i < blockSize; i++) { - _this->stream.writeBuf[i].q = (float)inBuf[i * 2] / (float)0x7FFF; - _this->stream.writeBuf[i].i = (float)inBuf[(i * 2) + 1] / (float)0x7FFF; + _this->stream.writeBuf[i].re = (float)inBuf[i * 2] / (float)0x7FFF; + _this->stream.writeBuf[i].im = (float)inBuf[(i * 2) + 1] / (float)0x7FFF; } if (!_this->stream.swap(blockSize)) { break; }; } diff --git a/hackrf_source/src/main.cpp b/hackrf_source/src/main.cpp index a4349cae..5e43c502 100644 --- a/hackrf_source/src/main.cpp +++ b/hackrf_source/src/main.cpp @@ -226,8 +226,8 @@ private: int count = transfer->valid_length / 2; int8_t* buffer = (int8_t*)transfer->buffer; for (int i = 0; i < count; i++) { - _this->stream.writeBuf[i].i = (float)buffer[i * 2] / 128.0f; - _this->stream.writeBuf[i].q = (float)buffer[(i * 2) + 1] / 128.0f; + _this->stream.writeBuf[i].re = (float)buffer[i * 2] / 128.0f; + _this->stream.writeBuf[i].im = (float)buffer[(i * 2) + 1] / 128.0f; } if (!_this->stream.swap(count)) { return -1; } return 0; diff --git a/plutosdr_source/src/main.cpp b/plutosdr_source/src/main.cpp index 5fa3c6fc..4a785f7c 100644 --- a/plutosdr_source/src/main.cpp +++ b/plutosdr_source/src/main.cpp @@ -231,8 +231,8 @@ private: int16_t* buf = (int16_t*)iio_buffer_first(rxbuf, rx0_i); for (int i = 0; i < blockSize; i++) { - _this->stream.writeBuf[i].q = (float)buf[i * 2] / 32768.0f; - _this->stream.writeBuf[i].i = (float)buf[(i * 2) + 1] / 32768.0f; + _this->stream.writeBuf[i].re = (float)buf[i * 2] / 32768.0f; + _this->stream.writeBuf[i].im = (float)buf[(i * 2) + 1] / 32768.0f; } if (!_this->stream.swap(blockSize)) { break; }; } diff --git a/recorder/src/main.cpp b/recorder/src/main.cpp index afef5737..0bd5367d 100644 --- a/recorder/src/main.cpp +++ b/recorder/src/main.cpp @@ -41,7 +41,7 @@ std::string genFileName(std::string prefix, bool isVfo, std::string name = "") { if (isVfo) { freq += gui::waterfall.vfos[name]->generalOffset; } - sprintf(buf, "_%.0lfHz_%02d-%02d-%02d_%02d-%02d-%02d.wav", freq, ltm->tm_hour, ltm->tm_min, ltm->tm_sec, ltm->tm_mday, ltm->tm_mon + 1, ltm->tm_year + 1900); + sprintf(buf, "%.0lfHz_%02d-%02d-%02d_%02d-%02d-%02d.wav", freq, ltm->tm_hour, ltm->tm_min, ltm->tm_sec, ltm->tm_mday, ltm->tm_mon + 1, ltm->tm_year + 1900); return prefix + buf; } @@ -265,8 +265,8 @@ private: static void _basebandHandler(dsp::complex_t *data, int count, void *ctx) { RecorderModule* _this = (RecorderModule*)ctx; for (int i = 0; i < count; i++) { - _this->wavSampleBuf[(2*i)] = data[i].q * 32768.0f; - _this->wavSampleBuf[(2*i) + 1] = data[i].i * 32768.0f; + _this->wavSampleBuf[(2*i)] = data[i].re * 32768.0f; + _this->wavSampleBuf[(2*i) + 1] = data[i].im * 32768.0f; } _this->basebandWriter->writeSamples(_this->wavSampleBuf, count * 2 * sizeof(int16_t)); _this->samplesWritten += count; diff --git a/rtl_sdr_source/src/main.cpp b/rtl_sdr_source/src/main.cpp index a6688c70..bc4b3866 100644 --- a/rtl_sdr_source/src/main.cpp +++ b/rtl_sdr_source/src/main.cpp @@ -427,11 +427,10 @@ private: static void asyncHandler(unsigned char *buf, uint32_t len, void *ctx) { RTLSDRSourceModule* _this = (RTLSDRSourceModule*)ctx; - int sampCount = len / 2; for (int i = 0; i < sampCount; i++) { - _this->stream.writeBuf[i].i = (float)(buf[(i * 2) + 1] - 127) / 128.0f; - _this->stream.writeBuf[i].q = (float)(buf[i * 2] - 127) / 128.0f; + _this->stream.writeBuf[i].re = (float)(buf[i * 2] - 127) / 128.0f; + _this->stream.writeBuf[i].im = (float)(buf[(i * 2) + 1] - 127) / 128.0f; } if (!_this->stream.swap(sampCount)) { return; } } diff --git a/rtl_tcp_source/src/main.cpp b/rtl_tcp_source/src/main.cpp index 6c2d5cb5..327adf1f 100644 --- a/rtl_tcp_source/src/main.cpp +++ b/rtl_tcp_source/src/main.cpp @@ -219,8 +219,8 @@ private: // Read samples here _this->client.receiveData(inBuf, blockSize * 2); for (int i = 0; i < blockSize; i++) { - _this->stream.writeBuf[i].q = ((double)inBuf[i * 2] - 128.0) / 128.0; - _this->stream.writeBuf[i].i = ((double)inBuf[(i * 2) + 1] - 128.0) / 128.0; + _this->stream.writeBuf[i].re = ((double)inBuf[i * 2] - 128.0) / 128.0; + _this->stream.writeBuf[i].im = ((double)inBuf[(i * 2) + 1] - 128.0) / 128.0; } if (!_this->stream.swap(blockSize)) { break; }; } diff --git a/sdrplay_source/src/main.cpp b/sdrplay_source/src/main.cpp index bc0eea66..d0765fae 100644 --- a/sdrplay_source/src/main.cpp +++ b/sdrplay_source/src/main.cpp @@ -726,8 +726,8 @@ private: if (!_this->running) { return; } for (int i = 0; i < numSamples; i++) { int id = _this->bufferIndex++; - _this->stream.writeBuf[id].i = (float)xq[i] / 32768.0f; - _this->stream.writeBuf[id].q = (float)xi[i] / 32768.0f; + _this->stream.writeBuf[id].re = (float)xi[i] / 32768.0f; + _this->stream.writeBuf[id].im = (float)xq[i] / 32768.0f; if (_this->bufferIndex >= _this->bufferSize) { _this->stream.swap(_this->bufferSize);