rewrote DSP

This commit is contained in:
Ryzerth 2020-06-22 16:45:57 +02:00
parent 61ba7f1420
commit b78c2cf415
29 changed files with 1331 additions and 1554 deletions

View File

@ -2,13 +2,21 @@ cmake_minimum_required(VERSION 3.9)
project(sdrpp) project(sdrpp)
# Compiler config # Compiler config
set(CMAKE_CXX_FLAGS "-OFast /std:c++17") set(CMAKE_CXX_FLAGS "-O2 /std:c++17")
# HackRF # HackRF
include_directories(sdrpp "C:/Program Files/PothosSDR/include/libhackrf/") include_directories(sdrpp "C:/Program Files/PothosSDR/include/libhackrf/")
link_directories(sdrpp "C:/Program Files/PothosSDR/lib/") link_directories(sdrpp "C:/Program Files/PothosSDR/lib/")
link_libraries(hackrf) link_libraries(hackrf)
# Volk
include_directories(sdrpp "C:/Program Files/PothosSDR/include/volk/")
link_libraries(volk)
# SoapySDR
include_directories(sdrpp "C:/Program Files/PothosSDR/include/")
link_libraries(SoapySDR)
# Main code # Main code
include_directories(sdrpp "src/") include_directories(sdrpp "src/")
include_directories(sdrpp "src/imgui") include_directories(sdrpp "src/imgui")

BIN
lib/SoapySDR.lib Normal file

Binary file not shown.

View File

@ -1,84 +0,0 @@
#ifdef _MSC_VER
#include <tmmintrin.h>
#else
#include <x86intrin.h>
#endif
#include <cdsp/types.h>
inline void cm_mul(__m128& ab, const __m128& xy)
{
//const __m128 aa = _mm_shuffle_ps(ab, ab, _MM_SHUFFLE(2, 2, 0, 0));
const __m128 aa = _mm_moveldup_ps(ab);
const __m128 bb = _mm_movehdup_ps(ab);
//const __m128 bb = _mm_shuffle_ps(ab, ab, _MM_SHUFFLE(3, 3, 1, 1));
const __m128 yx = _mm_shuffle_ps(xy, xy, _MM_SHUFFLE(2, 3, 0, 1));
const __m128 tmp = _mm_addsub_ps(_mm_mul_ps(aa, xy), _mm_mul_ps(bb, yx));
ab = tmp;
}
inline void do_mul(cdsp::complex_t* a, const cdsp::complex_t* b, int n)
{
const int vector_size = 16;
int simd_iterations = n - (n % vector_size);
//assert(simd_iterations % vector_size == 0);
for (int i = 0; i < simd_iterations; i += vector_size)
{
//__builtin_prefetch(a + i*4 + 64, 0);
//__builtin_prefetch(b + i*4 + 64, 0);
__m128 vec_a = _mm_load_ps((float*)&a[i]);
__m128 vec_b = _mm_load_ps((float*)&b[i]);
__m128 vec_a2 = _mm_load_ps((float*)&a[i+2]);
__m128 vec_b2 = _mm_load_ps((float*)&b[i+2]);
__m128 vec_a3 = _mm_load_ps((float*)&a[i+4]);
__m128 vec_b3 = _mm_load_ps((float*)&b[i+4]);
__m128 vec_a4 = _mm_load_ps((float*)&a[i+6]);
__m128 vec_b4 = _mm_load_ps((float*)&b[i+6]);
__m128 vec_a5 = _mm_load_ps((float*)&a[i+8]);
__m128 vec_b5 = _mm_load_ps((float*)&b[i+8]);
__m128 vec_a6 = _mm_load_ps((float*)&a[i+10]);
__m128 vec_b6 = _mm_load_ps((float*)&b[i+10]);
__m128 vec_a7 = _mm_load_ps((float*)&a[i+12]);
__m128 vec_b7 = _mm_load_ps((float*)&b[i+12]);
__m128 vec_a8 = _mm_load_ps((float*)&a[i+14]);
__m128 vec_b8 = _mm_load_ps((float*)&b[i+14]);
cm_mul(vec_a, vec_b);
_mm_store_ps((float*)&a[i], vec_a);
cm_mul(vec_a2, vec_b2);
_mm_store_ps((float*)&a[i+2], vec_a2);
cm_mul(vec_a3, vec_b3);
_mm_store_ps((float*)&a[i+4], vec_a3);
cm_mul(vec_a4, vec_b4);
_mm_store_ps((float*)&a[i+6], vec_a4);
cm_mul(vec_a5, vec_b5);
_mm_store_ps((float*)&a[i+8], vec_a5);
cm_mul(vec_a6, vec_b6);
_mm_store_ps((float*)&a[i+10], vec_a6);
cm_mul(vec_a7, vec_b7);
_mm_store_ps((float*)&a[i+12], vec_a7);
cm_mul(vec_a8, vec_b8);
_mm_store_ps((float*)&a[i+14], vec_a8);
}
// finish with scalar
for (int i = simd_iterations; i < n; i++)
{
cdsp::complex_t cm;
cm.q = a[i].q*b[i].q - a[i].i*b[i].i;
cm.i = a[i].q*b[i].i + b[i].q*a[i].i;
a[i] = cm;
}
}

View File

@ -1,127 +0,0 @@
#pragma once
// Code by: Stellaris
#include <cmath>
#include <complex>
#include <cassert>
#include <cdsp/types.h>
#define M_PI 3.14159265359
#define R2(n) n, n + 2*64, n + 1*64, n + 3*64
#define R4(n) R2(n), R2(n + 2*16), R2(n + 1*16), R2(n + 3*16)
#define R6(n) R4(n), R4(n + 2*4 ), R4(n + 1*4 ), R4(n + 3*4 )
// Lookup table that store the reverse of each table
uint8_t lut[256] = { R6(0), R6(2), R6(1), R6(3) };
inline uint16_t reverse_16(uint16_t val)
{
uint8_t lo = lut[val&0xFF];
uint8_t hi = lut[(val>>8)&0xFF];
return (lo << 8) | hi;
}
static size_t reverse_bits(size_t x, int n) {
size_t result = 0;
for (int i = 0; i < n; i++, x >>= 1)
result = (result << 1) | (x & 1U);
return result;
}
// struct complex
// {
// float re;
// float im;
// };
inline void bit_reverse_swap_aos(cdsp::complex_t* data, int n)
{
assert(n < 65536); // only up to 16-bit size
int power2 = 0;
for (size_t temp = n; temp > 1U; temp >>= 1)
power2++;
power2 = 16 - power2;
for (int i = 0; i < n; i++) {
int j = reverse_16(i << power2);
if (j > i) {
std::swap(data[i], data[j]);
}
}
}
struct trig_table
{
float* cos_table;
float* sin_table;
};
trig_table tables[14];
trig_table get_trig_table(int power2)
{
assert(power2 < 14);
trig_table& table = tables[power2];
if (table.cos_table == 0)
{
int n = 1 << (power2);
table.cos_table = (float*)malloc((n/2) * sizeof(float));
table.sin_table = (float*)malloc((n/2) * sizeof(float));
for (size_t i = 0; i < n / 2; i++) {
table.cos_table[i] = cos(2 * M_PI * i / n);
table.sin_table[i] = sin(2 * M_PI * i / n);
}
}
return table;
}
void fft_aos(cdsp::complex_t* data, int n) {
int power2 = 0;
for (size_t temp = n; temp > 1U; temp >>= 1)
power2++;
float* cos_table; float* sin_table;
trig_table table = get_trig_table(power2);
cos_table = table.cos_table; sin_table = table.sin_table;
size_t size = (n / 2) * sizeof(float);
// Bit-reversed addressing permutation
bit_reverse_swap_aos(data, n);
// Cooley-Tukey decimation-in-time radix-2 FFT
for (size_t size = 2; size <= n; size *= 2) {
size_t halfsize = size / 2;
size_t tablestep = n / size;
for (size_t i = 0; i < n; i += size) {
for (size_t j = i, k = 0; j < i + halfsize; j++, k += tablestep) {
size_t l = j + halfsize;
float tpre = data[l].q * cos_table[k] + data[l].i * sin_table[k];
float tpim = data[l].i * cos_table[k] - data[l].q * sin_table[k];
data[l].q = data[j].q - tpre;
data[l].i = data[j].i - tpim;
data[j].q += tpre;
data[j].i += tpim;
}
}
}
}
// for (int i = 0; i < 327680; i++) {
// complex cm;
// cm.q = complexes[i].q*sineGen[i].q - complexes[i].i*sineGen[i].i;
// cm.i = complexes[i].q*sineGen[i].i + sineGen[i].q*complexes[i].i;
// complexes[i] = cm;
// }
// ImGui::Begin("FFT");
// ImGui::PlotLines("I", [](void*data, int idx) { return ((float*)data)[idx]; }, endData, 1024, 0, 0, -1, 12, ImVec2(1024, 200));
// ImGui::InputFloat("Freq", &frequency, 100000.0f, 100000.0f);
// ImGui::End();

View File

@ -1,104 +0,0 @@
#pragma once
#include <thread>
#include <cdsp/stream.h>
#include <cdsp/types.h>
#include <fstream>
namespace cdsp {
#pragma pack(push, 1)
struct audio_sample_t {
int16_t l;
int16_t r;
};
#pragma pack(pop)
class RawFileSource {
public:
RawFileSource() {
}
RawFileSource(std::string path, int bufferSize) : output(bufferSize * 2) {
_bufferSize = bufferSize;
_file = std::ifstream(path.c_str(), std::ios::in | std::ios::binary);
}
void init(std::string path, int bufferSize) {
output.init(bufferSize * 2);
_bufferSize = bufferSize;
_file = std::ifstream(path.c_str(), std::ios::in | std::ios::binary);
}
void start() {
_workerThread = std::thread(_worker, this);
}
stream<float> output;
private:
static void _worker(RawFileSource* _this) {
audio_sample_t* inBuf = new audio_sample_t[_this->_bufferSize];
float* outBuf = new float[_this->_bufferSize];
while (true) {
//printf("%d\n", _this->_bufferSize * sizeof(audio_sample_t));
_this->_file.read((char*)inBuf, _this->_bufferSize * sizeof(audio_sample_t));
for (int i = 0; i < _this->_bufferSize; i++) {
outBuf[i] = ((float)inBuf[i].l + (float)inBuf[i].r) / (float)0xFFFF;
}
//printf("Writing file samples\n");
_this->output.write(outBuf, _this->_bufferSize);
}
}
int _bufferSize;
std::ifstream _file;
std::thread _workerThread;
};
class RawFileSink {
public:
RawFileSink() {
}
RawFileSink(std::string path, stream<float>* in, int bufferSize) {
_bufferSize = bufferSize;
_input = in;
_file = std::ofstream(path.c_str(), std::ios::out | std::ios::binary);
}
void init(std::string path, stream<float>* in, int bufferSize) {
_bufferSize = bufferSize;
_input = in;
_file = std::ofstream(path.c_str(), std::ios::out | std::ios::binary);
}
void start() {
_workerThread = std::thread(_worker, this);
}
private:
static void _worker(RawFileSink* _this) {
float* inBuf = new float[_this->_bufferSize];
audio_sample_t* outBuf = new audio_sample_t[_this->_bufferSize];
while (true) {
//printf("%d\n", _this->_bufferSize * sizeof(audio_sample_t));
_this->_input->read(inBuf, _this->_bufferSize);
for (int i = 0; i < _this->_bufferSize; i++) {
outBuf[i].l = inBuf[i] * 0x7FFF;
outBuf[i].r = outBuf[i].l;
}
//printf("Writing file samples\n");
_this->_file.write((char*)outBuf, _this->_bufferSize * sizeof(audio_sample_t));
}
}
int _bufferSize;
std::ofstream _file;
stream<float>* _input;
std::thread _workerThread;
};
};

View File

@ -1,156 +0,0 @@
#pragma once
#include <thread>
#include <cdsp/stream.h>
#include <cdsp/types.h>
namespace cdsp {
class SineSource {
public:
SineSource() {
}
SineSource(float frequency, long sampleRate, int bufferSize) : output(bufferSize * 2) {
_bufferSize = bufferSize;
_phasorSpeed = (2 * 3.1415926535) / (sampleRate / frequency);
_phase = 0;
}
void init(float frequency, long sampleRate, int bufferSize) {
output.init(bufferSize * 2);
_bufferSize = bufferSize;
_phasorSpeed = (2 * 3.1415926535) / (sampleRate / frequency);
_phase = 0;
}
void start() {
_workerThread = std::thread(_worker, this);
}
stream<float> output;
private:
static void _worker(SineSource* _this) {
float* outBuf = new float[_this->_bufferSize];
while (true) {
for (int i = 0; i < _this->_bufferSize; i++) {
_this->_phase += _this->_phasorSpeed;
outBuf[i] = cos(_this->_phase);
}
_this->_phase = fmodf(_this->_phase, 2.0f * 3.1415926535);
_this->output.write(outBuf, _this->_bufferSize);
}
}
int _bufferSize;
float _phasorSpeed;
float _phase;
std::thread _workerThread;
};
class RandomSource {
public:
RandomSource() {
}
RandomSource(float frequency, long sampleRate, int bufferSize) : output(bufferSize * 2) {
_bufferSize = bufferSize;
}
void init(float frequency, long sampleRate, int bufferSize) {
output.init(bufferSize * 2);
_bufferSize = bufferSize;
}
void start() {
_workerThread = std::thread(_worker, this);
}
stream<float> output;
private:
static void _worker(RandomSource* _this) {
float* outBuf = new float[_this->_bufferSize];
while (true) {
for (int i = 0; i < _this->_bufferSize; i++) {
outBuf[i] = ((float)rand() / ((float)RAND_MAX / 2.0f)) - 1.0f;
}
_this->output.write(outBuf, _this->_bufferSize);
}
}
int _bufferSize;
std::thread _workerThread;
};
class ComplexSineSource {
public:
ComplexSineSource() {
}
ComplexSineSource(float frequency, long sampleRate, int bufferSize) : output(bufferSize * 2) {
_bufferSize = bufferSize;
_sampleRate = sampleRate;
_phasorSpeed = (2 * 3.1415926535) / (sampleRate / frequency);
_phase = 0;
running = false;
}
void init(float frequency, long sampleRate, int bufferSize) {
output.init(bufferSize * 2);
_sampleRate = sampleRate;
_bufferSize = bufferSize;
_phasorSpeed = (2 * 3.1415926535) / (sampleRate / frequency);
_phase = 0;
running = false;
}
void start() {
if (running) {
return;
}
_workerThread = std::thread(_worker, this);
running = true;
}
void stop() {
if (!running) {
return;
}
output.stopWriter();
_workerThread.join();
output.clearWriteStop();
running = false;
}
void setFrequency(float frequency) {
_phasorSpeed = (2 * 3.1415926535) / (_sampleRate / frequency);
}
stream<complex_t> output;
private:
static void _worker(ComplexSineSource* _this) {
complex_t* outBuf = new complex_t[_this->_bufferSize];
while (true) {
for (int i = 0; i < _this->_bufferSize; i++) {
_this->_phase += _this->_phasorSpeed;
outBuf[i].i = sin(_this->_phase);
outBuf[i].q = cos(_this->_phase);
}
_this->_phase = fmodf(_this->_phase, 2.0f * 3.1415926535);
if (_this->output.write(outBuf, _this->_bufferSize) < 0) { break; };
}
delete[] outBuf;
}
int _bufferSize;
float _phasorSpeed;
float _phase;
long _sampleRate;
std::thread _workerThread;
bool running;
};
};

View File

@ -1,194 +0,0 @@
#pragma once
#include <thread>
#include <cdsp/stream.h>
#include <cdsp/types.h>
#include <fstream>
#include <hackrf.h>
#include <Windows.h>
namespace cdsp {
#pragma pack(push, 1)
struct hackrf_sample_t {
int8_t q;
int8_t i;
};
#pragma pack(pop)
class Complex2HackRF {
public:
Complex2HackRF() {
}
Complex2HackRF(stream<complex_t>* in, int bufferSize) : output(bufferSize * 2) {
_input = in;
_bufferSize = bufferSize;
}
void init(stream<complex_t>* in, int bufferSize) {
output.init(bufferSize * 2);
_input = in;
_bufferSize = bufferSize;
}
stream<hackrf_sample_t> output;
void start() {
_workerThread = std::thread(_worker, this);
}
private:
static void _worker(Complex2HackRF* _this) {
complex_t* inBuf = new complex_t[_this->_bufferSize];
hackrf_sample_t* outBuf = new hackrf_sample_t[_this->_bufferSize];
while (true) {
_this->_input->read(inBuf, _this->_bufferSize);
for (int i = 0; i < _this->_bufferSize; i++) {
outBuf[i].i = inBuf[i].i * 127.0f;
outBuf[i].q = inBuf[i].q * 127.0f;
}
_this->output.write(outBuf, _this->_bufferSize);
}
}
int _bufferSize;
stream<complex_t>* _input;
std::thread _workerThread;
};
class HackRF2Complex {
public:
HackRF2Complex() {
}
HackRF2Complex(stream<complex_t>* out, int bufferSize) : input(bufferSize * 2) {
_output = out;
_bufferSize = bufferSize;
}
void init(stream<complex_t>* out, int bufferSize) {
input.init(bufferSize * 2);
_output = out;
_bufferSize = bufferSize;
}
void start() {
_workerThread = std::thread(_worker, this);
}
stream<hackrf_sample_t> input;
private:
static void _worker(HackRF2Complex* _this) {
hackrf_sample_t* inBuf = new hackrf_sample_t[_this->_bufferSize];
complex_t* outBuf = new complex_t[_this->_bufferSize];
while (true) {
_this->input.read(inBuf, _this->_bufferSize);
for (int i = 0; i < _this->_bufferSize; i++) {
outBuf[i].i = (float)inBuf[i].i / 127.0f;
outBuf[i].q = (float)inBuf[i].q / 127.0f;
}
_this->_output->write(outBuf, _this->_bufferSize);
}
}
int _bufferSize;
stream<complex_t>* _output;
std::thread _workerThread;
};
class HackRFSink {
public:
HackRFSink() {
}
HackRFSink(hackrf_device* dev, int bufferSize, stream<complex_t>* input) : gen(input, bufferSize) {
_input = input;
_dev = dev;
gen.start();
}
void init(hackrf_device* dev, int bufferSize, stream<complex_t>* input) {
gen.init(input, bufferSize);
_input = input;
_dev = dev;
gen.start();
}
void start() {
streaming = true;
hackrf_start_tx(_dev, _worker, this);
}
void stop() {
streaming = false;
Sleep(500);
hackrf_stop_tx(_dev);
}
private:
static int _worker(hackrf_transfer* transfer) {
if (!((HackRFSink*)transfer->tx_ctx)->streaming) {
return -1;
}
hackrf_sample_t* buf = (hackrf_sample_t*)transfer->buffer;
((HackRFSink*)transfer->tx_ctx)->gen.output.read(buf, transfer->buffer_length / 2);
return 0;
}
Complex2HackRF gen;
bool streaming;
stream<complex_t>* _input;
hackrf_device* _dev;
};
class HackRFSource {
public:
HackRFSource() {
}
HackRFSource(hackrf_device* dev, int bufferSize) : output(bufferSize * 2), gen(&output, bufferSize) {
_dev = dev;
gen.start();
}
void init(hackrf_device* dev, int bufferSize) {
output.init(bufferSize * 2);
gen.init(&output, bufferSize);
_dev = dev;
gen.start();
}
void start() {
streaming = true;
hackrf_start_rx(_dev, _worker, this);
}
void stop() {
streaming = false;
Sleep(500);
hackrf_stop_rx(_dev);
}
stream<complex_t> output;
private:
static int _worker(hackrf_transfer* transfer) {
if (!((HackRFSource*)transfer->rx_ctx)->streaming) {
return -1;
}
hackrf_sample_t* buf = (hackrf_sample_t*)transfer->buffer;
//printf("Writing samples\n");
((HackRFSource*)transfer->rx_ctx)->gen.input.write(buf, transfer->buffer_length / 2);
return 0;
}
HackRF2Complex gen;
bool streaming;
hackrf_device* _dev;
};
};

View File

@ -1,70 +0,0 @@
#pragma once
#include <thread>
#include <cdsp/stream.h>
#include <cdsp/types.h>
#include <cdsp/fast_math.h>
namespace cdsp {
class Multiplier {
public:
Multiplier() {
}
Multiplier(stream<complex_t>* a, stream<complex_t>* b, int bufferSize) : output(bufferSize * 2) {
_a = a;
_b = b;
_bufferSize = bufferSize;
}
void init(stream<complex_t>* a, stream<complex_t>* b, int bufferSize) {
output.init(bufferSize * 2);
_a = a;
_b = b;
_bufferSize = bufferSize;
}
void start() {
if (running) {
return;
}
running = true;
_workerThread = std::thread(_worker, this);
}
void stop() {
if (!running) {
return;
}
_a->stopReader();
_b->stopReader();
output.stopWriter();
_workerThread.join();
running = false;
_a->clearReadStop();
_b->clearReadStop();
output.clearWriteStop();
}
stream<complex_t> output;
private:
static void _worker(Multiplier* _this) {
complex_t* aBuf = new complex_t[_this->_bufferSize];
complex_t* bBuf = new complex_t[_this->_bufferSize];
complex_t* outBuf = new complex_t[_this->_bufferSize];
while (true) {
if (_this->_a->read(aBuf, _this->_bufferSize) < 0) { printf("A Received stop signal\n"); return; };
if (_this->_b->read(bBuf, _this->_bufferSize) < 0) { printf("B Received stop signal\n"); return; };
do_mul(aBuf, bBuf, _this->_bufferSize);
if (_this->output.write(aBuf, _this->_bufferSize) < 0) { printf("OUT Received stop signal\n"); return; };
}
}
stream<complex_t>* _a;
stream<complex_t>* _b;
int _bufferSize;
bool running = false;
std::thread _workerThread;
};
};

View File

@ -1,57 +0,0 @@
#pragma once
#include <thread>
#include <cdsp/stream.h>
#include <cdsp/types.h>
#include <cmath>
namespace cdsp {
class FMModulator {
public:
FMModulator() {
}
FMModulator(stream<float>* in, float deviation, long sampleRate, int bufferSize) : output(bufferSize * 2) {
_input = in;
_bufferSize = bufferSize;
_phase = 0.0f;
_phasorSpeed = (2 * 3.1415926535) / (sampleRate / deviation);
}
void init(stream<float>* in, float deviation, long sampleRate, int bufferSize) {
output.init(bufferSize * 2);
_input = in;
_bufferSize = bufferSize;
_phase = 0.0f;
_phasorSpeed = (2 * 3.1415926535) / (sampleRate / deviation);
}
void start() {
_workerThread = std::thread(_worker, this);
}
stream<complex_t> output;
private:
static void _worker(FMModulator* _this) {
float* inBuf = new float[_this->_bufferSize];
complex_t* outBuf = new complex_t[_this->_bufferSize];
while (true) {
_this->_input->read(inBuf, _this->_bufferSize);
for (int i = 0; i < _this->_bufferSize; i++) {
_this->_phase += inBuf[i] * _this->_phasorSpeed;
outBuf[i].i = std::sinf(_this->_phase);
outBuf[i].q = std::cosf(_this->_phase);
}
_this->_phase = fmodf(_this->_phase, 2.0f * 3.1415926535);
_this->output.write(outBuf, _this->_bufferSize);
}
}
stream<float>* _input;
int _bufferSize;
float _phase;
float _phasorSpeed;
std::thread _workerThread;
};
};

View File

@ -1,317 +0,0 @@
#pragma once
#include <thread>
#include <cdsp/stream.h>
#include <cdsp/types.h>
#include <cdsp/filter.h>
#include <numeric>
namespace cdsp {
class Interpolator {
public:
Interpolator() {
}
Interpolator(stream<float>* in, float interpolation, int bufferSize) : output(bufferSize * 2) {
_input = in;
_interpolation = interpolation;
_bufferSize = bufferSize;
running = false;
}
void init(stream<float>* in, float interpolation, int bufferSize) {
output.init(bufferSize * 2);
_input = in;
_interpolation = interpolation;
_bufferSize = bufferSize;
running = false;
}
void start() {
if (running) {
return;
}
running = true;
_workerThread = std::thread(_worker, this);
}
void stop() {
if (!running) {
return;
}
running = false;
_input->stopReader();
output.stopWriter();
_workerThread.join();
_input->clearReadStop();
output.clearWriteStop();
}
void setInterpolation(float interpolation) {
if (running) {
return;
}
_interpolation = interpolation;
}
void setInput(stream<float>* in) {
if (running) {
return;
}
_input = in;
}
stream<float> output;
private:
static void _worker(Interpolator* _this) {
float* inBuf = new float[(int)((float)_this->_bufferSize / _this->_interpolation)];
float* outBuf = new float[_this->_bufferSize];
while (true) {
if (_this->_input->read(inBuf, (int)((float)_this->_bufferSize / _this->_interpolation)) < 0) { break; };
for (int i = 0; i < _this->_bufferSize; i++) {
outBuf[i] = inBuf[(int)((float)i / _this->_interpolation)];
}
if (_this->output.write(outBuf, _this->_bufferSize) < 0) { break; };
}
delete[] inBuf;
delete[] outBuf;
}
stream<float>* _input;
int _bufferSize;
float _interpolation;
std::thread _workerThread;
bool running;
};
class IQInterpolator {
public:
IQInterpolator() {
}
IQInterpolator(stream<complex_t>* in, float interpolation, int bufferSize) : output(bufferSize * 2) {
_input = in;
_interpolation = interpolation;
_bufferSize = bufferSize;
running = false;
}
void init(stream<complex_t>* in, float interpolation, int bufferSize) {
output.init(bufferSize * 2);
_input = in;
_interpolation = interpolation;
_bufferSize = bufferSize;
running = false;
}
void start() {
if (running) {
return;
}
_workerThread = std::thread(_worker, this);
running = true;
}
void stop() {
if (!running) {
return;
}
_input->stopReader();
output.stopWriter();
_workerThread.join();
_input->clearReadStop();
output.clearWriteStop();
running = false;
}
void setInterpolation(float interpolation) {
if (running) {
return;
}
_interpolation = interpolation;
}
stream<complex_t> output;
private:
static void _worker(IQInterpolator* _this) {
complex_t* inBuf = new complex_t[_this->_bufferSize];
complex_t* outBuf = new complex_t[_this->_bufferSize * _this->_interpolation];
int outCount = _this->_bufferSize * _this->_interpolation;
while (true) {
if (_this->_input->read(inBuf, _this->_bufferSize) < 0) { break; };
for (int i = 0; i < outCount; i++) {
outBuf[i] = inBuf[(int)((float)i / _this->_interpolation)];
}
if (_this->output.write(outBuf, _this->_bufferSize) < 0) { break; };
}
delete[] inBuf;
delete[] outBuf;
}
stream<complex_t>* _input;
int _bufferSize;
float _interpolation;
std::thread _workerThread;
bool running;
};
class BlockDecimator {
public:
BlockDecimator() {
}
BlockDecimator(stream<complex_t>* in, int skip, int bufferSize) : output(bufferSize * 2) {
_input = in;
_skip = skip;
_bufferSize = bufferSize;
}
void init(stream<complex_t>* in, int skip, int bufferSize) {
output.init(bufferSize * 2);
_input = in;
_skip = skip;
_bufferSize = bufferSize;
}
void start() {
_workerThread = std::thread(_worker, this);
}
stream<complex_t> output;
private:
static void _worker(BlockDecimator* _this) {
complex_t* buf = new complex_t[_this->_bufferSize];
while (true) {
_this->_input->readAndSkip(buf, _this->_bufferSize, _this->_skip);
_this->output.write(buf, _this->_bufferSize);
}
}
stream<complex_t>* _input;
int _bufferSize;
int _skip;
std::thread _workerThread;
};
class FractionalResampler {
public:
FractionalResampler() {
}
void init(stream<float>* input, float inputSampleRate, float outputSampleRate, int bufferSize, float customCutoff = INFINITY) {
_input = input;
float lowestFreq = std::min<float>(inputSampleRate, outputSampleRate);
int _gcd = std::gcd((int)inputSampleRate, (int)outputSampleRate);
_interp = outputSampleRate / _gcd;
_decim = inputSampleRate / _gcd;
_inputSampleRate = inputSampleRate;
_outputSampleRate = outputSampleRate;
running = false;
interpolator.init(input, _interp, bufferSize);
BlackmanWindow(decimTaps, inputSampleRate * _interp, lowestFreq / 2.0f, lowestFreq / 2.0f);
if (_interp != 1) {
printf("FR Interpolation needed\n");
decimator.init(&interpolator.output, decimTaps, bufferSize * _interp, _decim);
}
else {
decimator.init(input, decimTaps, bufferSize, _decim);
printf("FR Interpolation NOT needed: %d %d %d\n", bufferSize / _decim, _decim, _interp);
}
output = &decimator.output;
}
void start() {
if (_interp != 1) {
interpolator.start();
}
decimator.start();
running = true;
}
void stop() {
interpolator.stop();
decimator.stop();
running = false;
}
void setInputSampleRate(float inputSampleRate) {
if (running) {
return;
}
float lowestFreq = std::min<float>(inputSampleRate, _outputSampleRate);
int _gcd = std::gcd((int)inputSampleRate, (int)_outputSampleRate);
_interp = _outputSampleRate / _gcd;
_decim = inputSampleRate / _gcd;
// TODO: Add checks from VFO to remove the need to stop both
interpolator.setInterpolation(_interp);
decimator.setDecimation(_decim);
if (_interp != 1) {
decimator.setInput(&interpolator.output);
}
else {
decimator.setInput(_input);
}
}
void setOutputSampleRate(float outputSampleRate) {
if (running) {
return;
}
float lowestFreq = std::min<float>(_inputSampleRate, outputSampleRate);
int _gcd = std::gcd((int)_inputSampleRate, (int)outputSampleRate);
_interp = outputSampleRate / _gcd;
_decim = _inputSampleRate / _gcd;
// TODO: Add checks from VFO to remove the need to stop both
interpolator.setInterpolation(_interp);
decimator.setDecimation(_decim);
if (_interp != 1) {
decimator.setInput(&interpolator.output);
}
else {
decimator.setInput(_input);
}
}
void setInput(stream<float>* input) {
if (running) {
return;
}
_input = input;
if (_interp != 1) {
interpolator.setInput(input);
decimator.setInput(&interpolator.output);
}
else {
decimator.setInput(input);
}
}
stream<float>* output;
private:
Interpolator interpolator;
FloatDecimatingFIRFilter decimator;
std::vector<float> decimTaps;
stream<float>* _input;
int _interp;
int _decim;
int _bufferSize;
float _inputSampleRate;
float _outputSampleRate;
bool running;
};
};

63
src/dsp/correction.h Normal file
View File

@ -0,0 +1,63 @@
#pragma once
#include <thread>
#include <dsp/stream.h>
#include <dsp/types.h>
#include <vector>
namespace dsp {
class DCBiasRemover {
public:
DCBiasRemover() {
}
DCBiasRemover(stream<complex_t>* input, int bufferSize) : output(bufferSize * 2) {
_in = input;
_bufferSize = bufferSize;
bypass = false;
}
void init(stream<complex_t>* input, int bufferSize) {
output.init(bufferSize * 2);
_in = input;
_bufferSize = bufferSize;
bypass = false;
}
void start() {
_workerThread = std::thread(_worker, this);
}
stream<complex_t> output;
bool bypass;
private:
static void _worker(DCBiasRemover* _this) {
complex_t* buf = new complex_t[_this->_bufferSize];
float ibias = 0.0f;
float qbias = 0.0f;
while (true) {
_this->_in->read(buf, _this->_bufferSize);
if (_this->bypass) {
_this->output.write(buf, _this->_bufferSize);
continue;
}
for (int i = 0; i < _this->_bufferSize; i++) {
ibias += buf[i].i;
qbias += buf[i].q;
}
ibias /= _this->_bufferSize;
qbias /= _this->_bufferSize;
for (int i = 0; i < _this->_bufferSize; i++) {
buf[i].i -= ibias;
buf[i].q -= qbias;
}
_this->output.write(buf, _this->_bufferSize);
}
}
stream<complex_t>* _in;
int _bufferSize;
std::thread _workerThread;
};
};

View File

@ -1,13 +1,17 @@
#pragma once #pragma once
#include <thread> #include <thread>
#include <cdsp/stream.h> #include <dsp/stream.h>
#include <cdsp/types.h> #include <dsp/types.h>
/*
TODO:
- Add a sample rate ajustment function to all demodulators
*/
#define FAST_ATAN2_COEF1 3.1415926535f / 4.0f #define FAST_ATAN2_COEF1 3.1415926535f / 4.0f
#define FAST_ATAN2_COEF2 3.0f * FAST_ATAN2_COEF1 #define FAST_ATAN2_COEF2 3.0f * FAST_ATAN2_COEF1
inline float fast_arctan2(float y, float x) inline float fast_arctan2(float y, float x) {
{
float abs_y = fabs(y)+1e-10; float abs_y = fabs(y)+1e-10;
float r, angle; float r, angle;
if (x>=0) if (x>=0)
@ -26,26 +30,26 @@ inline float fast_arctan2(float y, float x)
return angle; return angle;
} }
namespace cdsp { namespace dsp {
class FMDemodulator { class FMDemodulator {
public: public:
FMDemodulator() { FMDemodulator() {
} }
FMDemodulator(stream<complex_t>* in, float deviation, long sampleRate, int bufferSize) : output(bufferSize * 2) { FMDemodulator(stream<complex_t>* in, float deviation, long sampleRate, int blockSize) : output(blockSize * 2) {
running = false; running = false;
_input = in; _input = in;
_bufferSize = bufferSize; _blockSize = blockSize;
_phase = 0.0f; _phase = 0.0f;
_phasorSpeed = (2 * 3.1415926535) / (sampleRate / deviation); _phasorSpeed = (2 * 3.1415926535) / (sampleRate / deviation);
} }
void init(stream<complex_t>* in, float deviation, long sampleRate, int bufferSize) { void init(stream<complex_t>* in, float deviation, long sampleRate, int blockSize) {
output.init(bufferSize * 2); output.init(blockSize * 2);
running = false; running = false;
_input = in; _input = in;
_bufferSize = bufferSize; _blockSize = blockSize;
_phase = 0.0f; _phase = 0.0f;
_phasorSpeed = (2 * 3.1415926535) / (sampleRate / deviation); _phasorSpeed = (2 * 3.1415926535) / (sampleRate / deviation);
} }
@ -70,17 +74,25 @@ namespace cdsp {
output.clearWriteStop(); output.clearWriteStop();
} }
void setBlockSize(int blockSize) {
if (running) {
return;
}
_blockSize = blockSize;
output.setMaxLatency(_blockSize * 2);
}
stream<float> output; stream<float> output;
private: private:
static void _worker(FMDemodulator* _this) { static void _worker(FMDemodulator* _this) {
complex_t* inBuf = new complex_t[_this->_bufferSize]; complex_t* inBuf = new complex_t[_this->_blockSize];
float* outBuf = new float[_this->_bufferSize]; float* outBuf = new float[_this->_blockSize];
float diff = 0; float diff = 0;
float currentPhase = 0; float currentPhase = 0;
while (true) { while (true) {
if (_this->_input->read(inBuf, _this->_bufferSize) < 0) { return; }; if (_this->_input->read(inBuf, _this->_blockSize) < 0) { return; };
for (int i = 0; i < _this->_bufferSize; i++) { for (int i = 0; i < _this->_blockSize; i++) {
currentPhase = fast_arctan2(inBuf[i].i, inBuf[i].q); currentPhase = fast_arctan2(inBuf[i].i, inBuf[i].q);
diff = currentPhase - _this->_phase; diff = currentPhase - _this->_phase;
if (diff > 3.1415926535f) { diff -= 2 * 3.1415926535f; } if (diff > 3.1415926535f) { diff -= 2 * 3.1415926535f; }
@ -88,13 +100,13 @@ namespace cdsp {
outBuf[i] = diff / _this->_phasorSpeed; outBuf[i] = diff / _this->_phasorSpeed;
_this->_phase = currentPhase; _this->_phase = currentPhase;
} }
if (_this->output.write(outBuf, _this->_bufferSize) < 0) { return; }; if (_this->output.write(outBuf, _this->_blockSize) < 0) { return; };
} }
} }
stream<complex_t>* _input; stream<complex_t>* _input;
bool running; bool running;
int _bufferSize; int _blockSize;
float _phase; float _phase;
float _phasorSpeed; float _phasorSpeed;
std::thread _workerThread; std::thread _workerThread;
@ -107,17 +119,17 @@ namespace cdsp {
} }
AMDemodulator(stream<complex_t>* in, int bufferSize) : output(bufferSize * 2) { AMDemodulator(stream<complex_t>* in, int blockSize) : output(blockSize * 2) {
running = false; running = false;
_input = in; _input = in;
_bufferSize = bufferSize; _blockSize = blockSize;
} }
void init(stream<complex_t>* in, int bufferSize) { void init(stream<complex_t>* in, int blockSize) {
output.init(bufferSize * 2); output.init(blockSize * 2);
running = false; running = false;
_input = in; _input = in;
_bufferSize = bufferSize; _blockSize = blockSize;
} }
void start() { void start() {
@ -140,18 +152,26 @@ namespace cdsp {
output.clearWriteStop(); output.clearWriteStop();
} }
void setBlockSize(int blockSize) {
if (running) {
return;
}
_blockSize = blockSize;
output.setMaxLatency(_blockSize * 2);
}
stream<float> output; stream<float> output;
private: private:
static void _worker(AMDemodulator* _this) { static void _worker(AMDemodulator* _this) {
complex_t* inBuf = new complex_t[_this->_bufferSize]; complex_t* inBuf = new complex_t[_this->_blockSize];
float* outBuf = new float[_this->_bufferSize]; float* outBuf = new float[_this->_blockSize];
float min, max, amp; float min, max, amp;
while (true) { while (true) {
if (_this->_input->read(inBuf, _this->_bufferSize) < 0) { return; }; if (_this->_input->read(inBuf, _this->_blockSize) < 0) { break; };
min = INFINITY; min = INFINITY;
max = 0.0f; max = 0.0f;
for (int i = 0; i < _this->_bufferSize; i++) { for (int i = 0; i < _this->_blockSize; i++) {
outBuf[i] = sqrt((inBuf[i].i*inBuf[i].i) + (inBuf[i].q*inBuf[i].q)); outBuf[i] = sqrt((inBuf[i].i*inBuf[i].i) + (inBuf[i].q*inBuf[i].q));
if (outBuf[i] < min) { if (outBuf[i] < min) {
min = outBuf[i]; min = outBuf[i];
@ -161,16 +181,18 @@ namespace cdsp {
} }
} }
amp = (max - min); amp = (max - min);
for (int i = 0; i < _this->_bufferSize; i++) { for (int i = 0; i < _this->_blockSize; i++) {
outBuf[i] = (outBuf[i] - min) / (max - min); outBuf[i] = (outBuf[i] - min) / (max - min);
} }
if (_this->output.write(outBuf, _this->_bufferSize) < 0) { return; }; if (_this->output.write(outBuf, _this->_blockSize) < 0) { break; };
} }
delete[] inBuf;
delete[] outBuf;
} }
stream<complex_t>* _input; stream<complex_t>* _input;
bool running; bool running;
int _bufferSize; int _blockSize;
std::thread _workerThread; std::thread _workerThread;
}; };
}; };

View File

@ -1,41 +1,40 @@
#pragma once #pragma once
#include <thread> #include <thread>
#include <cdsp/stream.h> #include <dsp/stream.h>
#include <cdsp/types.h> #include <dsp/types.h>
#include <vector> #include <vector>
#include <dsp/math.h>
#define GET_FROM_RIGHT_BUF(buffer, delayLine, delayLineSz, n) (((n) < 0) ? delayLine[(delayLineSz) + (n)] : buffer[(n)]) #define GET_FROM_RIGHT_BUF(buffer, delayLine, delayLineSz, n) (((n) < 0) ? delayLine[(delayLineSz) + (n)] : buffer[(n)])
#define M_PI 3.1415926535f
namespace dsp {
inline void BlackmanWindow(std::vector<float>& taps, float sampleRate, float cutoff, float transWidth) { inline void BlackmanWindow(std::vector<float>& taps, float sampleRate, float cutoff, float transWidth) {
taps.clear(); taps.clear();
float fc = cutoff / sampleRate; float fc = cutoff / sampleRate;
int _M = 4.0f / (transWidth / sampleRate); int _M = 4.0f / (transWidth / sampleRate);
if (_M % 2 == 0) { _M++; } if (_M % 2 == 0) { _M++; }
float M = _M; float M = _M;
float sum = 0.0f; float sum = 0.0f;
for (int i = 0; i < _M; i++) { for (int i = 0; i < _M; i++) {
float val = (sin(2.0f * M_PI * fc * ((float)i - (M / 2))) / ((float)i - (M / 2))) * (0.42f - (0.5f * cos(2.0f * M_PI / M)) + (0.8f * cos(4.0f * M_PI / M))); float val = (sin(2.0f * M_PI * fc * ((float)i - (M / 2))) / ((float)i - (M / 2))) * (0.42f - (0.5f * cos(2.0f * M_PI / M)) + (0.8f * cos(4.0f * M_PI / M)));
taps.push_back(val); taps.push_back(val);
sum += val; sum += val;
}
for (int i = 0; i < M; i++) {
taps[i] /= sum;
}
} }
for (int i = 0; i < M; i++) {
taps[i] /= sum;
}
}
namespace cdsp {
class DecimatingFIRFilter { class DecimatingFIRFilter {
public: public:
DecimatingFIRFilter() { DecimatingFIRFilter() {
} }
DecimatingFIRFilter(stream<complex_t>* input, std::vector<float> taps, int bufferSize, float decim) : output(bufferSize * 2) { DecimatingFIRFilter(stream<complex_t>* input, std::vector<float> taps, int blockSize, float decim) : output(blockSize * 2) {
output.init(bufferSize * 2); output.init(blockSize * 2);
_in = input; _in = input;
_bufferSize = bufferSize; _blockSize = blockSize;
_tapCount = taps.size(); _tapCount = taps.size();
delayBuf = new complex_t[_tapCount]; delayBuf = new complex_t[_tapCount];
@ -54,10 +53,10 @@ namespace cdsp {
running = false; running = false;
} }
void init(stream<complex_t>* input, std::vector<float>& taps, int bufferSize, float decim) { void init(stream<complex_t>* input, std::vector<float>& taps, int blockSize, float decim) {
output.init(bufferSize * 2); output.init(blockSize * 2);
_in = input; _in = input;
_bufferSize = bufferSize; _blockSize = blockSize;
_tapCount = taps.size(); _tapCount = taps.size();
delayBuf = new complex_t[_tapCount]; delayBuf = new complex_t[_tapCount];
@ -101,8 +100,11 @@ namespace cdsp {
return; return;
} }
_tapCount = taps.size(); _tapCount = taps.size();
printf("[%d]\n", _tapCount);
delete[] _taps; delete[] _taps;
delete[] delayBuf;
_taps = new float[_tapCount]; _taps = new float[_tapCount];
delayBuf = new complex_t[_tapCount];
for (int i = 0; i < _tapCount; i++) { for (int i = 0; i < _tapCount; i++) {
_taps[i] = taps[i]; _taps[i] = taps[i];
} }
@ -120,28 +122,30 @@ namespace cdsp {
return; return;
} }
_decim = decimation; _decim = decimation;
output.setMaxLatency((_blockSize * 2) / _decim);
} }
void setBufferSize(int bufferSize) { void setBlockSize(int blockSize) {
if (running) { if (running) {
return; return;
} }
_bufferSize = bufferSize; _blockSize = blockSize;
output.setMaxLatency((_blockSize * 2) / _decim);
} }
stream<complex_t> output; stream<complex_t> output;
private: private:
static void _worker(DecimatingFIRFilter* _this) { static void _worker(DecimatingFIRFilter* _this) {
int outputSize = _this->_bufferSize / _this->_decim; int outputSize = _this->_blockSize / _this->_decim;
complex_t* inBuf = new complex_t[_this->_bufferSize]; complex_t* inBuf = new complex_t[_this->_blockSize];
complex_t* outBuf = new complex_t[outputSize]; complex_t* outBuf = new complex_t[outputSize];
float tap = 0.0f; float tap = 0.0f;
int delayOff; int delayOff;
void* delayStart = &inBuf[_this->_bufferSize - (_this->_tapCount - 1)]; void* delayStart = &inBuf[_this->_blockSize - (_this->_tapCount - 1)];
int delaySize = (_this->_tapCount - 1) * sizeof(complex_t); int delaySize = (_this->_tapCount - 1) * sizeof(complex_t);
int bufferSize = _this->_bufferSize; int blockSize = _this->_blockSize;
int outBufferLength = outputSize * sizeof(complex_t); int outBufferLength = outputSize * sizeof(complex_t);
int tapCount = _this->_tapCount; int tapCount = _this->_tapCount;
int decim = _this->_decim; int decim = _this->_decim;
@ -149,7 +153,7 @@ namespace cdsp {
int id = 0; int id = 0;
while (true) { while (true) {
if (_this->_in->read(inBuf, bufferSize) < 0) { break; }; if (_this->_in->read(inBuf, blockSize) < 0) { break; };
memset(outBuf, 0, outBufferLength); memset(outBuf, 0, outBufferLength);
for (int t = 0; t < tapCount; t++) { for (int t = 0; t < tapCount; t++) {
@ -161,7 +165,7 @@ namespace cdsp {
delayOff = tapCount - t; delayOff = tapCount - t;
id = 0; id = 0;
for (int i = 0; i < bufferSize; i += decim) { for (int i = 0; i < blockSize; i += decim) {
if (i < t) { if (i < t) {
outBuf[id].i += tap * delayBuf[delayOff + i].i; outBuf[id].i += tap * delayBuf[delayOff + i].i;
outBuf[id].q += tap * delayBuf[delayOff + i].q; outBuf[id].q += tap * delayBuf[delayOff + i].q;
@ -182,7 +186,7 @@ namespace cdsp {
stream<complex_t>* _in; stream<complex_t>* _in;
complex_t* delayBuf; complex_t* delayBuf;
int _bufferSize; int _blockSize;
int _tapCount = 0; int _tapCount = 0;
float _decim; float _decim;
std::thread _workerThread; std::thread _workerThread;
@ -190,16 +194,17 @@ namespace cdsp {
bool running; bool running;
}; };
class FloatDecimatingFIRFilter { class FloatDecimatingFIRFilter {
public: public:
FloatDecimatingFIRFilter() { FloatDecimatingFIRFilter() {
} }
FloatDecimatingFIRFilter(stream<float>* input, std::vector<float> taps, int bufferSize, float decim) : output(bufferSize * 2) { FloatDecimatingFIRFilter(stream<float>* input, std::vector<float> taps, int blockSize, float decim) : output(blockSize * 2) {
output.init(bufferSize * 2); output.init(blockSize * 2);
_in = input; _in = input;
_bufferSize = bufferSize; _blockSize = blockSize;
_tapCount = taps.size(); _tapCount = taps.size();
delayBuf = new float[_tapCount]; delayBuf = new float[_tapCount];
@ -217,10 +222,10 @@ namespace cdsp {
running = false; running = false;
} }
void init(stream<float>* input, std::vector<float>& taps, int bufferSize, float decim) { void init(stream<float>* input, std::vector<float>& taps, int blockSize, float decim) {
output.init(bufferSize * 2); output.init(blockSize * 2);
_in = input; _in = input;
_bufferSize = bufferSize; _blockSize = blockSize;
_tapCount = taps.size(); _tapCount = taps.size();
delayBuf = new float[_tapCount]; delayBuf = new float[_tapCount];
@ -239,11 +244,17 @@ namespace cdsp {
} }
void start() { void start() {
if (running) {
return;
}
running = true; running = true;
_workerThread = std::thread(_worker, this); _workerThread = std::thread(_worker, this);
} }
void stop() { void stop() {
if (!running) {
return;
}
_in->stopReader(); _in->stopReader();
output.stopWriter(); output.stopWriter();
_workerThread.join(); _workerThread.join();
@ -258,7 +269,9 @@ namespace cdsp {
} }
_tapCount = taps.size(); _tapCount = taps.size();
delete[] _taps; delete[] _taps;
delete[] delayBuf;
_taps = new float[_tapCount]; _taps = new float[_tapCount];
delayBuf = new float[_tapCount];
for (int i = 0; i < _tapCount; i++) { for (int i = 0; i < _tapCount; i++) {
_taps[i] = taps[i]; _taps[i] = taps[i];
} }
@ -276,29 +289,38 @@ namespace cdsp {
return; return;
} }
_decim = decimation; _decim = decimation;
output.setMaxLatency((_blockSize * 2) / _decim);
}
void setBlockSize(int blockSize) {
if (running) {
return;
}
_blockSize = blockSize;
output.setMaxLatency((_blockSize * 2) / _decim);
} }
stream<float> output; stream<float> output;
private: private:
static void _worker(FloatDecimatingFIRFilter* _this) { static void _worker(FloatDecimatingFIRFilter* _this) {
int outputSize = _this->_bufferSize / _this->_decim; int outputSize = _this->_blockSize / _this->_decim;
float* inBuf = new float[_this->_bufferSize]; float* inBuf = new float[_this->_blockSize];
float* outBuf = new float[outputSize]; float* outBuf = new float[outputSize];
float tap = 0.0f; float tap = 0.0f;
int delayOff; int delayOff;
void* delayStart = &inBuf[_this->_bufferSize - (_this->_tapCount - 1)]; void* delayStart = &inBuf[_this->_blockSize - (_this->_tapCount - 1)];
int delaySize = (_this->_tapCount - 1) * sizeof(float); int delaySize = (_this->_tapCount - 1) * sizeof(float);
int bufferSize = _this->_bufferSize; int blockSize = _this->_blockSize;
int outBufferLength = outputSize * sizeof(float); int outBufferLength = outputSize * sizeof(complex_t);
int tapCount = _this->_tapCount; int tapCount = _this->_tapCount;
int decim = _this->_decim; int decim = _this->_decim;
float* delayBuf = _this->delayBuf; float* delayBuf = _this->delayBuf;
int id = 0; int id = 0;
while (true) { while (true) {
if (_this->_in->read(inBuf, bufferSize) < 0) { break; }; if (_this->_in->read(inBuf, blockSize) < 0) { break; };
memset(outBuf, 0, outBufferLength); memset(outBuf, 0, outBufferLength);
for (int t = 0; t < tapCount; t++) { for (int t = 0; t < tapCount; t++) {
@ -310,7 +332,7 @@ namespace cdsp {
delayOff = tapCount - t; delayOff = tapCount - t;
id = 0; id = 0;
for (int i = 0; i < bufferSize; i += decim) { for (int i = 0; i < blockSize; i += decim) {
if (i < t) { if (i < t) {
outBuf[id] += tap * delayBuf[delayOff + i]; outBuf[id] += tap * delayBuf[delayOff + i];
id++; id++;
@ -329,150 +351,11 @@ namespace cdsp {
stream<float>* _in; stream<float>* _in;
float* delayBuf; float* delayBuf;
int _bufferSize; int _blockSize;
int _tapCount = 0; int _tapCount = 0;
float _decim; float _decim;
std::thread _workerThread; std::thread _workerThread;
float* _taps; float* _taps;
bool running; bool running;
}; };
class DCBiasRemover {
public:
DCBiasRemover() {
}
DCBiasRemover(stream<complex_t>* input, int bufferSize) : output(bufferSize * 2) {
_in = input;
_bufferSize = bufferSize;
bypass = false;
}
void init(stream<complex_t>* input, int bufferSize) {
output.init(bufferSize * 2);
_in = input;
_bufferSize = bufferSize;
bypass = false;
}
void start() {
_workerThread = std::thread(_worker, this);
}
stream<complex_t> output;
bool bypass;
private:
static void _worker(DCBiasRemover* _this) {
complex_t* buf = new complex_t[_this->_bufferSize];
float ibias = 0.0f;
float qbias = 0.0f;
while (true) {
_this->_in->read(buf, _this->_bufferSize);
if (_this->bypass) {
_this->output.write(buf, _this->_bufferSize);
continue;
}
for (int i = 0; i < _this->_bufferSize; i++) {
ibias += buf[i].i;
qbias += buf[i].q;
}
ibias /= _this->_bufferSize;
qbias /= _this->_bufferSize;
for (int i = 0; i < _this->_bufferSize; i++) {
buf[i].i -= ibias;
buf[i].q -= qbias;
}
_this->output.write(buf, _this->_bufferSize);
}
}
stream<complex_t>* _in;
int _bufferSize;
std::thread _workerThread;
};
class HandlerSink {
public:
HandlerSink() {
}
HandlerSink(stream<complex_t>* input, complex_t* buffer, int bufferSize, void handler(complex_t*)) {
_in = input;
_bufferSize = bufferSize;
_buffer = buffer;
_handler = handler;
}
void init(stream<complex_t>* input, complex_t* buffer, int bufferSize, void handler(complex_t*)) {
_in = input;
_bufferSize = bufferSize;
_buffer = buffer;
_handler = handler;
}
void start() {
_workerThread = std::thread(_worker, this);
}
bool bypass;
private:
static void _worker(HandlerSink* _this) {
while (true) {
_this->_in->read(_this->_buffer, _this->_bufferSize);
_this->_handler(_this->_buffer);
}
}
stream<complex_t>* _in;
int _bufferSize;
complex_t* _buffer;
std::thread _workerThread;
void (*_handler)(complex_t*);
};
class Splitter {
public:
Splitter() {
}
Splitter(stream<complex_t>* input, int bufferSize) {
_in = input;
_bufferSize = bufferSize;
output_a.init(bufferSize);
output_b.init(bufferSize);
}
void init(stream<complex_t>* input, int bufferSize) {
_in = input;
_bufferSize = bufferSize;
output_a.init(bufferSize);
output_b.init(bufferSize);
}
void start() {
_workerThread = std::thread(_worker, this);
}
stream<complex_t> output_a;
stream<complex_t> output_b;
private:
static void _worker(Splitter* _this) {
complex_t* buf = new complex_t[_this->_bufferSize];
while (true) {
_this->_in->read(buf, _this->_bufferSize);
_this->output_a.write(buf, _this->_bufferSize);
_this->output_b.write(buf, _this->_bufferSize);
}
}
stream<complex_t>* _in;
int _bufferSize;
std::thread _workerThread;
};
}; };

85
src/dsp/math.h Normal file
View File

@ -0,0 +1,85 @@
#pragma once
#include <thread>
#include <dsp/stream.h>
#include <dsp/types.h>
#include <volk.h>
#ifndef M_PI
#define M_PI 3.1415926535f
#endif
namespace dsp {
class Multiplier {
public:
Multiplier() {
}
Multiplier(stream<complex_t>* a, stream<complex_t>* b, int blockSize) : output(blockSize * 2) {
_a = a;
_b = b;
_blockSize = blockSize;
}
void init(stream<complex_t>* a, stream<complex_t>* b, int blockSize) {
output.init(blockSize * 2);
_a = a;
_b = b;
_blockSize = blockSize;
}
void start() {
if (running) {
return;
}
running = true;
_workerThread = std::thread(_worker, this);
}
void stop() {
if (!running) {
return;
}
_a->stopReader();
_b->stopReader();
output.stopWriter();
_workerThread.join();
running = false;
_a->clearReadStop();
_b->clearReadStop();
output.clearWriteStop();
}
void setBlockSize(int blockSize) {
if (running) {
return;
}
_blockSize = blockSize;
output.setMaxLatency(blockSize * 2);
}
stream<complex_t> output;
private:
static void _worker(Multiplier* _this) {
complex_t* aBuf = (complex_t*)volk_malloc(sizeof(complex_t) * _this->_blockSize, volk_get_alignment());
complex_t* bBuf = (complex_t*)volk_malloc(sizeof(complex_t) * _this->_blockSize, volk_get_alignment());
complex_t* outBuf = (complex_t*)volk_malloc(sizeof(complex_t) * _this->_blockSize, volk_get_alignment());
while (true) {
if (_this->_a->read(aBuf, _this->_blockSize) < 0) { break; };
if (_this->_b->read(bBuf, _this->_blockSize) < 0) { break; };
volk_32fc_x2_multiply_32fc((lv_32fc_t*)outBuf, (lv_32fc_t*)aBuf, (lv_32fc_t*)bBuf, _this->_blockSize);
if (_this->output.write(outBuf, _this->_blockSize) < 0) { break; };
}
volk_free(aBuf);
volk_free(bBuf);
volk_free(outBuf);
}
stream<complex_t>* _a;
stream<complex_t>* _b;
int _blockSize;
bool running = false;
std::thread _workerThread;
};
};

425
src/dsp/resampling.h Normal file
View File

@ -0,0 +1,425 @@
#pragma once
#include <thread>
#include <dsp/filter.h>
#include <dsp/stream.h>
#include <dsp/types.h>
#include <numeric>
namespace dsp {
template <class T>
class Interpolator {
public:
Interpolator() {
}
Interpolator(stream<T>* in, float interpolation, int blockSize) : output(blockSize * interpolation * 2) {
_input = in;
_interpolation = interpolation;
_blockSize = blockSize;
}
void init(stream<T>* in, float interpolation, int blockSize) {
output.init(blockSize * 2);
_input = in;
_interpolation = interpolation;
_blockSize = blockSize;
}
void start() {
if (running) {
return;
}
_workerThread = std::thread(_worker, this);
running = true;
}
void stop() {
if (!running) {
return;
}
_input->stopReader();
output.stopWriter();
_workerThread.join();
_input->clearReadStop();
output.clearWriteStop();
running = false;
}
void setInterpolation(float interpolation) {
if (running) {
return;
}
_interpolation = interpolation;
output.setMaxLatency(_blockSize * _interpolation * 2);
}
void setBlockSize(int blockSize) {
if (running) {
return;
}
_blockSize = blockSize;
output.setMaxLatency(_blockSize * _interpolation * 2);
}
void setInput(stream<T>* input) {
if (running) {
return;
}
_input = input;
}
stream<T> output;
private:
static void _worker(Interpolator<T>* _this) {
T* inBuf = new T[_this->_blockSize];
T* outBuf = new T[_this->_blockSize * _this->_interpolation];
int outCount = _this->_blockSize * _this->_interpolation;
while (true) {
if (_this->_input->read(inBuf, _this->_blockSize) < 0) { break; };
for (int i = 0; i < outCount; i++) {
outBuf[i] = inBuf[(int)((float)i / _this->_interpolation)];
}
if (_this->output.write(outBuf, outCount) < 0) { break; };
}
delete[] inBuf;
delete[] outBuf;
}
stream<T>* _input;
int _blockSize;
float _interpolation;
std::thread _workerThread;
bool running = false;
};
class BlockDecimator {
public:
BlockDecimator() {
}
BlockDecimator(stream<complex_t>* in, int skip, int blockSize) : output(blockSize * 2) {
_input = in;
_skip = skip;
_blockSize = blockSize;
}
void init(stream<complex_t>* in, int skip, int blockSize) {
output.init(blockSize * 2);
_input = in;
_skip = skip;
_blockSize = blockSize;
}
void start() {
if (running) {
return;
}
_workerThread = std::thread(_worker, this);
}
void stop() {
if (!running) {
return;
}
_input->stopReader();
output.stopWriter();
_workerThread.join();
_input->clearReadStop();
output.clearWriteStop();
running = false;
}
void setBlockSize(int blockSize) {
if (running) {
return;
}
_blockSize = blockSize;
output.setMaxLatency(blockSize * 2);
}
void setSkip(int skip) {
if (running) {
return;
}
_skip = skip;
}
stream<complex_t> output;
private:
static void _worker(BlockDecimator* _this) {
complex_t* buf = new complex_t[_this->_blockSize];
while (true) {
_this->_input->readAndSkip(buf, _this->_blockSize, _this->_skip);
_this->output.write(buf, _this->_blockSize);
}
}
stream<complex_t>* _input;
int _blockSize;
int _skip;
std::thread _workerThread;
bool running = false;
};
class Resampler {
public:
Resampler() {
}
void init(stream<complex_t>* in, float inputSampleRate, float outputSampleRate, float bandWidth, int blockSize) {
_input = in;
_outputSampleRate = outputSampleRate;
_inputSampleRate = inputSampleRate;
int _gcd = std::gcd((int)inputSampleRate, (int)outputSampleRate);
_interp = outputSampleRate / _gcd;
_decim = inputSampleRate / _gcd;
_blockSize = blockSize;
output = &decim.output;
dsp::BlackmanWindow(_taps, inputSampleRate * _interp, outputSampleRate / 2.0f, outputSampleRate / 2.0f);
interp.init(in, _interp, blockSize);
if (_interp == 1) {
decim.init(in, _taps, blockSize, _decim);
}
else {
decim.init(&interp.output, _taps, blockSize * _interp, _decim);
}
}
void start() {
if (_interp != 1) {
interp.start();
}
decim.start();
running = true;
}
void stop() {
interp.stop();
decim.stop();
running = false;
}
void setInputSampleRate(float inputSampleRate, int blockSize = -1) {
stop();
_inputSampleRate = inputSampleRate;
int _gcd = std::gcd((int)inputSampleRate, (int)_outputSampleRate);
_interp = _outputSampleRate / _gcd;
_decim = inputSampleRate / _gcd;
dsp::BlackmanWindow(_taps, inputSampleRate * _interp, _outputSampleRate / 2.0f, _outputSampleRate / 2.0f);
decim.setTaps(_taps);
interp.setInterpolation(_interp);
decim.setDecimation(_decim);
if (blockSize > 0) {
_blockSize = blockSize;
interp.setBlockSize(_blockSize);
}
decim.setBlockSize(_blockSize * _interp);
if (_interp == 1) {
decim.setInput(_input);
}
else {
decim.setInput(&interp.output);
interp.start();
}
start();
}
void setOutputSampleRate(float outputSampleRate) {
stop();
_outputSampleRate = outputSampleRate;
int _gcd = std::gcd((int)_inputSampleRate, (int)outputSampleRate);
_interp = outputSampleRate / _gcd;
_decim = _inputSampleRate / _gcd;
dsp::BlackmanWindow(_taps, _inputSampleRate * _interp, outputSampleRate / 2.0f, outputSampleRate / 2.0f);
decim.setTaps(_taps);
interp.setInterpolation(_interp);
decim.setDecimation(_decim);
decim.setBlockSize(_blockSize * _interp);
if (_interp == 1) {
decim.setInput(_input);
}
else {
decim.setInput(&interp.output);
}
start();
}
void setBlockSize(int blockSize) {
stop();
_blockSize = blockSize;
interp.setBlockSize(_blockSize);
decim.setBlockSize(_blockSize * _interp);
start();
}
void setInput(stream<complex_t>* input) {
if (running) {
return;
}
_input = input;
interp.setInput(_input);
if (_interp == 1) {
decim.setInput(_input);
}
}
stream<complex_t>* output;
private:
Interpolator<complex_t> interp;
DecimatingFIRFilter decim;
stream<complex_t>* _input;
std::vector<float> _taps;
int _interp;
int _decim;
float _outputSampleRate;
float _inputSampleRate;
float _blockSize;
bool running = false;
};
class FloatResampler {
public:
FloatResampler() {
}
void init(stream<float>* in, float inputSampleRate, float outputSampleRate, float bandWidth, int blockSize) {
_input = in;
_outputSampleRate = outputSampleRate;
_inputSampleRate = inputSampleRate;
int _gcd = std::gcd((int)inputSampleRate, (int)outputSampleRate);
_interp = outputSampleRate / _gcd;
_decim = inputSampleRate / _gcd;
_blockSize = blockSize;
output = &decim.output;
dsp::BlackmanWindow(_taps, inputSampleRate * _interp, outputSampleRate / 2.0f, outputSampleRate / 2.0f);
interp.init(in, _interp, blockSize);
if (_interp == 1) {
decim.init(in, _taps, blockSize, _decim);
}
else {
decim.init(&interp.output, _taps, blockSize * _interp, _decim);
}
}
void start() {
if (_interp != 1) {
interp.start();
}
decim.start();
running = true;
}
void stop() {
interp.stop();
decim.stop();
running = false;
}
void setInputSampleRate(float inputSampleRate, int blockSize = -1) {
stop();
_inputSampleRate = inputSampleRate;
int _gcd = std::gcd((int)inputSampleRate, (int)_outputSampleRate);
_interp = _outputSampleRate / _gcd;
_decim = inputSampleRate / _gcd;
dsp::BlackmanWindow(_taps, inputSampleRate * _interp, _outputSampleRate / 2.0f, _outputSampleRate / 2.0f);
decim.setTaps(_taps);
interp.setInterpolation(_interp);
decim.setDecimation(_decim);
if (blockSize > 0) {
_blockSize = blockSize;
interp.setBlockSize(_blockSize);
}
decim.setBlockSize(_blockSize * _interp);
if (_interp == 1) {
decim.setInput(_input);
}
else {
decim.setInput(&interp.output);
}
start();
}
void setOutputSampleRate(float outputSampleRate) {
stop();
_outputSampleRate = outputSampleRate;
int _gcd = std::gcd((int)_inputSampleRate, (int)outputSampleRate);
_interp = outputSampleRate / _gcd;
_decim = _inputSampleRate / _gcd;
dsp::BlackmanWindow(_taps, _inputSampleRate * _interp, outputSampleRate / 2.0f, outputSampleRate / 2.0f);
decim.setTaps(_taps);
interp.setInterpolation(_interp);
decim.setDecimation(_decim);
decim.setBlockSize(_blockSize * _interp);
if (_interp == 1) {
decim.setInput(_input);
}
else {
decim.setInput(&interp.output);
}
start();
}
void setBlockSize(int blockSize) {
stop();
_blockSize = blockSize;
interp.setBlockSize(_blockSize);
decim.setBlockSize(_blockSize * _interp);
start();
}
void setInput(stream<float>* input) {
if (running) {
return;
}
_input = input;
interp.setInput(_input);
if (_interp == 1) {
decim.setInput(_input);
}
}
stream<float>* output;
private:
Interpolator<float> interp;
FloatDecimatingFIRFilter decim;
stream<float>* _input;
std::vector<float> _taps;
int _interp;
int _decim;
float _outputSampleRate;
float _inputSampleRate;
float _blockSize;
bool running = false;
};
};

49
src/dsp/routing.h Normal file
View File

@ -0,0 +1,49 @@
#pragma once
#include <thread>
#include <dsp/stream.h>
#include <dsp/types.h>
#include <vector>
namespace dsp {
class Splitter {
public:
Splitter() {
}
Splitter(stream<complex_t>* input, int bufferSize) {
_in = input;
_bufferSize = bufferSize;
output_a.init(bufferSize);
output_b.init(bufferSize);
}
void init(stream<complex_t>* input, int bufferSize) {
_in = input;
_bufferSize = bufferSize;
output_a.init(bufferSize);
output_b.init(bufferSize);
}
void start() {
_workerThread = std::thread(_worker, this);
}
stream<complex_t> output_a;
stream<complex_t> output_b;
private:
static void _worker(Splitter* _this) {
complex_t* buf = new complex_t[_this->_bufferSize];
while (true) {
_this->_in->read(buf, _this->_bufferSize);
_this->output_a.write(buf, _this->_bufferSize);
_this->output_b.write(buf, _this->_bufferSize);
}
}
stream<complex_t>* _in;
int _bufferSize;
std::thread _workerThread;
};
};

118
src/dsp/sink.h Normal file
View File

@ -0,0 +1,118 @@
#pragma once
#include <thread>
#include <dsp/stream.h>
#include <dsp/types.h>
#include <vector>
namespace dsp {
class HandlerSink {
public:
HandlerSink() {
}
HandlerSink(stream<complex_t>* input, complex_t* buffer, int bufferSize, void handler(complex_t*)) {
_in = input;
_bufferSize = bufferSize;
_buffer = buffer;
_handler = handler;
}
void init(stream<complex_t>* input, complex_t* buffer, int bufferSize, void handler(complex_t*)) {
_in = input;
_bufferSize = bufferSize;
_buffer = buffer;
_handler = handler;
}
void start() {
_workerThread = std::thread(_worker, this);
}
bool bypass;
private:
static void _worker(HandlerSink* _this) {
while (true) {
_this->_in->read(_this->_buffer, _this->_bufferSize);
_this->_handler(_this->_buffer);
}
}
stream<complex_t>* _in;
int _bufferSize;
complex_t* _buffer;
std::thread _workerThread;
void (*_handler)(complex_t*);
};
class NullSink {
public:
NullSink() {
}
NullSink(stream<complex_t>* input, int bufferSize) {
_in = input;
_bufferSize = bufferSize;
}
void init(stream<complex_t>* input, int bufferSize) {
_in = input;
_bufferSize = bufferSize;
}
void start() {
_workerThread = std::thread(_worker, this);
}
bool bypass;
private:
static void _worker(NullSink* _this) {
complex_t* buf = new complex_t[_this->_bufferSize];
while (true) {
_this->_in->read(buf, _this->_bufferSize);
}
}
stream<complex_t>* _in;
int _bufferSize;
std::thread _workerThread;
};
class FloatNullSink {
public:
FloatNullSink() {
}
FloatNullSink(stream<float>* input, int bufferSize) {
_in = input;
_bufferSize = bufferSize;
}
void init(stream<float>* input, int bufferSize) {
_in = input;
_bufferSize = bufferSize;
}
void start() {
_workerThread = std::thread(_worker, this);
}
bool bypass;
private:
static void _worker(FloatNullSink* _this) {
float* buf = new float[_this->_bufferSize];
while (true) {
_this->_in->read(buf, _this->_bufferSize);
}
}
stream<float>* _in;
int _bufferSize;
std::thread _workerThread;
};
};

82
src/dsp/source.h Normal file
View File

@ -0,0 +1,82 @@
#pragma once
#include <thread>
#include <dsp/stream.h>
#include <dsp/types.h>
#include <volk.h>
namespace dsp {
class SineSource {
public:
SineSource() {
}
SineSource(float frequency, long sampleRate, int blockSize) : output(blockSize * 2) {
_blockSize = blockSize;
_sampleRate = sampleRate;
_phasorSpeed = (2 * 3.1415926535) / (sampleRate / frequency);
_phase = 0;
}
void init(float frequency, long sampleRate, int blockSize) {
output.init(blockSize * 2);
_sampleRate = sampleRate;
_blockSize = blockSize;
_phasorSpeed = (2 * 3.1415926535) / (sampleRate / frequency);
_phase = 0;
}
void start() {
if (running) {
return;
}
_workerThread = std::thread(_worker, this);
running = true;
}
void stop() {
if (!running) {
return;
}
output.stopWriter();
_workerThread.join();
output.clearWriteStop();
running = false;
}
void setFrequency(float frequency) {
_phasorSpeed = (2 * 3.1415926535) / (_sampleRate / frequency);
}
void setBlockSize(int blockSize) {
if (running) {
return;
}
_blockSize = blockSize;
}
stream<complex_t> output;
private:
static void _worker(SineSource* _this) {
complex_t* outBuf = new complex_t[_this->_blockSize];
while (true) {
for (int i = 0; i < _this->_blockSize; i++) {
_this->_phase += _this->_phasorSpeed;
outBuf[i].i = sin(_this->_phase);
outBuf[i].q = cos(_this->_phase);
}
_this->_phase = fmodf(_this->_phase, 2.0f * 3.1415926535);
if (_this->output.write(outBuf, _this->_blockSize) < 0) { break; };
}
delete[] outBuf;
}
int _blockSize;
float _phasorSpeed;
float _phase;
long _sampleRate;
std::thread _workerThread;
bool running = false;
};
};

View File

@ -3,7 +3,9 @@
#include <algorithm> #include <algorithm>
#include <math.h> #include <math.h>
namespace cdsp { #define STREAM_BUF_SZ 1000000
namespace dsp {
template <class T> template <class T>
class stream { class stream {
public: public:
@ -11,20 +13,22 @@ namespace cdsp {
} }
stream(int size) { stream(int maxLatency) {
size = STREAM_BUF_SZ;
_buffer = new T[size]; _buffer = new T[size];
_stopReader = false; _stopReader = false;
_stopWriter = false; _stopWriter = false;
this->size = size; this->maxLatency = maxLatency;
writec = 0; writec = 0;
readc = size - 1; readc = size - 1;
} }
void init(int size) { void init(int maxLatency) {
size = STREAM_BUF_SZ;
_buffer = new T[size]; _buffer = new T[size];
_stopReader = false; _stopReader = false;
_stopWriter = false; _stopWriter = false;
this->size = size; this->maxLatency = maxLatency;
writec = 0; writec = 0;
readc = size - 1; readc = size - 1;
} }
@ -57,6 +61,7 @@ namespace cdsp {
readc_mtx.unlock(); readc_mtx.unlock();
canWriteVar.notify_one(); canWriteVar.notify_one();
} }
return len;
} }
int readAndSkip(T* data, int len, int skip) { int readAndSkip(T* data, int len, int skip) {
@ -95,6 +100,7 @@ namespace cdsp {
readc_mtx.unlock(); readc_mtx.unlock();
canWriteVar.notify_one(); canWriteVar.notify_one();
} }
return len;
} }
int waitUntilReadable() { int waitUntilReadable() {
@ -167,7 +173,7 @@ namespace cdsp {
if (_rc < writec) { if (_rc < writec) {
writeable = (this->size + writeable); writeable = (this->size + writeable);
} }
return writeable - 1; return std::min<float>(writeable - 1, maxLatency - readable(false) - 1);
} }
void stopReader() { void stopReader() {
@ -196,11 +202,16 @@ namespace cdsp {
_stopWriter = false; _stopWriter = false;
} }
void setMaxLatency(int maxLatency) {
this->maxLatency = maxLatency;
}
private: private:
T* _buffer; T* _buffer;
int size; int size;
int readc; int readc;
int writec; int writec;
int maxLatency;
bool _stopReader; bool _stopReader;
bool _stopWriter; bool _stopWriter;
std::mutex readc_mtx; std::mutex readc_mtx;

View File

@ -1,6 +1,6 @@
#pragma once #pragma once
namespace cdsp { namespace dsp {
struct complex_t { struct complex_t {
float q; float q;
float i; float i;

158
src/dsp/vfo.h Normal file
View File

@ -0,0 +1,158 @@
#pragma once
#include <dsp/source.h>
#include <dsp/math.h>
#include <dsp/resampling.h>
#include <dsp/filter.h>
namespace dsp {
class VFO {
public:
VFO() {
}
void init(stream<complex_t>* in, float inputSampleRate, float outputSampleRate, float bandWidth, float offset, int blockSize) {
_input = in;
_outputSampleRate = outputSampleRate;
_inputSampleRate = inputSampleRate;
int _gcd = std::gcd((int)inputSampleRate, (int)outputSampleRate);
_interp = outputSampleRate / _gcd;
_decim = inputSampleRate / _gcd;
_bandWidth = bandWidth;
_blockSize = blockSize;
output = &decim.output;
dsp::BlackmanWindow(_taps, inputSampleRate * _interp, bandWidth / 2.0f, bandWidth / 2.0f);
lo.init(offset, inputSampleRate, blockSize);
mixer.init(in, &lo.output, blockSize);
interp.init(&mixer.output, _interp, blockSize);
if (_interp == 1) {
decim.init(&mixer.output, _taps, blockSize, _decim);
}
else {
decim.init(&interp.output, _taps, blockSize * _interp, _decim);
}
}
void start() {
lo.start();
mixer.start();
if (_interp != 1) {
printf("UH OH INTERPOLATOR STARTED :/\n");
interp.start();
}
decim.start();
}
void stop() {
lo.stop();
mixer.stop();
interp.stop();
decim.stop();
}
void setInputSampleRate(float inputSampleRate, int blockSize = -1) {
interp.stop();
decim.stop();
_inputSampleRate = inputSampleRate;
int _gcd = std::gcd((int)inputSampleRate, (int)_outputSampleRate);
_interp = _outputSampleRate / _gcd;
_decim = inputSampleRate / _gcd;
dsp::BlackmanWindow(_taps, inputSampleRate * _interp, _bandWidth / 2.0f, _bandWidth / 2.0f);
interp.setInterpolation(_interp);
decim.setDecimation(_decim);
if (blockSize > 0) {
lo.stop();
mixer.stop();
_blockSize = blockSize;
lo.setBlockSize(_blockSize);
mixer.setBlockSize(_blockSize);
interp.setBlockSize(_blockSize);
lo.start();
mixer.start();
}
decim.setBlockSize(_blockSize * _interp);
if (_interp == 1) {
decim.setInput(&mixer.output);
}
else {
decim.setInput(&interp.output);
interp.start();
}
decim.start();
}
void setOutputSampleRate(float outputSampleRate, float bandWidth = -1) {
interp.stop();
decim.stop();
if (bandWidth > 0) {
_bandWidth = bandWidth;
}
_outputSampleRate = outputSampleRate;
int _gcd = std::gcd((int)_inputSampleRate, (int)outputSampleRate);
_interp = outputSampleRate / _gcd;
_decim = _inputSampleRate / _gcd;
dsp::BlackmanWindow(_taps, _inputSampleRate * _interp, _bandWidth / 2.0f, _bandWidth / 2.0f);
decim.setTaps(_taps);
interp.setInterpolation(_interp);
decim.setDecimation(_decim);
decim.setBlockSize(_blockSize * _interp);
if (_interp == 1) {
decim.setInput(&mixer.output);
}
else {
decim.setInput(&interp.output);
interp.start();
}
decim.start();
}
void setBandwidth(float bandWidth) {
decim.stop();
dsp::BlackmanWindow(_taps, _inputSampleRate * _interp, _bandWidth / 2.0f, _bandWidth / 2.0f);
decim.setTaps(_taps);
decim.start();
}
void setOffset(float offset) {
lo.setFrequency(-offset);
}
void setBlockSize(int blockSize) {
stop();
_blockSize = blockSize;
lo.setBlockSize(_blockSize);
mixer.setBlockSize(_blockSize);
interp.setBlockSize(_blockSize);
decim.setBlockSize(_blockSize * _interp);
start();
}
stream<complex_t>* output;
private:
SineSource lo;
Multiplier mixer;
Interpolator<complex_t> interp;
DecimatingFIRFilter decim;
stream<complex_t>* _input;
std::vector<float> _taps;
int _interp;
int _decim;
float _outputSampleRate;
float _inputSampleRate;
float _bandWidth;
float _blockSize;
};
};

View File

@ -1,18 +1,18 @@
#pragma once #pragma once
#include <thread> #include <thread>
#include <cdsp/stream.h> #include <dsp/stream.h>
#include <cdsp/types.h> #include <dsp/types.h>
#include <fstream> #include <fstream>
#include <portaudio.h> #include <portaudio.h>
namespace cdsp { namespace io {
class AudioSink { class AudioSink {
public: public:
AudioSink() { AudioSink() {
} }
AudioSink(stream<float>* in, int bufferSize) { AudioSink(dsp::stream<float>* in, int bufferSize) {
_bufferSize = bufferSize; _bufferSize = bufferSize;
_input = in; _input = in;
buffer = new float[_bufferSize * 2]; buffer = new float[_bufferSize * 2];
@ -20,7 +20,7 @@ namespace cdsp {
Pa_Initialize(); Pa_Initialize();
} }
void init(stream<float>* in, int bufferSize) { void init(dsp::stream<float>* in, int bufferSize) {
_bufferSize = bufferSize; _bufferSize = bufferSize;
_input = in; _input = in;
buffer = new float[_bufferSize * 2]; buffer = new float[_bufferSize * 2];
@ -67,7 +67,7 @@ namespace cdsp {
} }
int _bufferSize; int _bufferSize;
stream<float>* _input; dsp::stream<float>* _input;
float* buffer; float* buffer;
float _volume; float _volume;
PaStream *stream; PaStream *stream;

106
src/io/soapy.h Normal file
View File

@ -0,0 +1,106 @@
#include <string>
#include <SoapySDR/Device.hpp>
#include <SoapySDR/Modules.hpp>
#include <dsp/stream.h>
#include <dsp/types.h>
namespace io {
class SoapyWrapper {
public:
SoapyWrapper() {
output.init(64000);
refresh();
setDevice(devList[0]);
}
void start() {
if (running) {
return;
}
dev = SoapySDR::Device::make(args);
_stream = dev->setupStream(SOAPY_SDR_RX, "CF32");
dev->activateStream(_stream);
running = true;
_workerThread = std::thread(_worker, this);
}
void stop() {
if (!running) {
return;
}
running = false;
dev->deactivateStream(_stream);
dev->closeStream(_stream);
_workerThread.join();
SoapySDR::Device::unmake(dev);
}
void refresh() {
if (running) {
return;
}
devList = SoapySDR::Device::enumerate();
txtDevList = "";
for (int i = 0; i < devList.size(); i++) {
txtDevList += devList[i]["label"];
txtDevList += '\0';
}
}
void setDevice(SoapySDR::Kwargs devArgs) {
if (running) {
return;
}
args = devArgs;
dev = SoapySDR::Device::make(devArgs);
txtSampleRateList = "";
sampleRates = dev->listSampleRates(SOAPY_SDR_RX, 0);
for (int i = 0; i < sampleRates.size(); i++) {
txtSampleRateList += std::to_string((int)sampleRates[i]);
txtSampleRateList += '\0';
}
}
void setSampleRate(float sampleRate) {
if (running) {
return;
}
dev->setSampleRate(SOAPY_SDR_RX, 0, sampleRate);
}
void setFrequency(float freq) {
dev->setFrequency(SOAPY_SDR_RX, 0, freq);
}
bool isRunning() {
return running;
}
SoapySDR::KwargsList devList;
std::string txtDevList;
std::vector<double> sampleRates;
std::string txtSampleRateList;
dsp::stream<dsp::complex_t> output;
private:
static void _worker(SoapyWrapper* _this) {
dsp::complex_t* buf = new dsp::complex_t[32000];
int flags = 0;
long long timeMs = 0;
while (_this->running) {
_this->dev->readStream(_this->_stream, (void**)&buf, 32000, flags, timeMs);
_this->output.write(buf, 32000);
}
printf("Read worker terminated\n");
delete[] buf;
}
SoapySDR::Kwargs args;
SoapySDR::Device* dev;
SoapySDR::Stream* _stream;
std::thread _workerThread;
bool running = false;
};
};

View File

@ -7,6 +7,7 @@
#include <main_window.h> #include <main_window.h>
#include <styles.h> #include <styles.h>
#ifdef _WIN32 #ifdef _WIN32
#include <Windows.h> #include <Windows.h>
#endif #endif

View File

@ -1,22 +1,20 @@
#include <main_window.h> #include <main_window.h>
#include <imgui_plot.h> #include <imgui_plot.h>
#include <hackrf.h> #include <dsp/resampling.h>
#include <cdsp/hackrf.h> #include <dsp/demodulator.h>
#include <cdsp/resampling.h> #include <dsp/filter.h>
#include <cdsp/demodulation.h>
#include <cdsp/filter.h>
#include <thread> #include <thread>
#include <complex> #include <complex>
#include <cdsp/generator.h> #include <dsp/source.h>
#include <cdsp/math.h> #include <dsp/math.h>
#include <waterfall.h> #include <waterfall.h>
#include <fftw3.h> #include <fftw3.h>
#include <signal_path.h> #include <signal_path.h>
#include <io/soapy.h>
std::thread worker; std::thread worker;
std::mutex fft_mtx; std::mutex fft_mtx;
ImGui::WaterFall wtf; ImGui::WaterFall wtf;
hackrf_device* dev;
fftwf_complex *fft_in, *fft_out; fftwf_complex *fft_in, *fft_out;
fftwf_plan p; fftwf_plan p;
float* tempData; float* tempData;
@ -25,11 +23,13 @@ int fftSize = 8192 * 8;
bool dcbias = true; bool dcbias = true;
cdsp::HackRFSource src; io::SoapyWrapper soapy;
//dsp::HackRFSource src;
SignalPath sigPath; SignalPath sigPath;
std::vector<float> _data; std::vector<float> _data;
std::vector<float> fftTaps; std::vector<float> fftTaps;
void fftHandler(cdsp::complex_t* samples) { void fftHandler(dsp::complex_t* samples) {
fftwf_execute(p); fftwf_execute(p);
int half = fftSize / 2; int half = fftSize / 2;
@ -61,29 +61,34 @@ void windowInit() {
printf("Starting DSP Thread!\n"); printf("Starting DSP Thread!\n");
hackrf_init(); // hackrf_init();
hackrf_device_list_t* list = hackrf_device_list(); // hackrf_device_list_t* list = hackrf_device_list();
int err = hackrf_device_list_open(list, 0, &dev); // int err = hackrf_device_list_open(list, 0, &dev);
if (err != 0) { // if (err != 0) {
printf("Error while opening HackRF: %d\n", err); // printf("Error while opening HackRF: %d\n", err);
return; // return;
} // }
hackrf_set_freq(dev, 90500000); // hackrf_set_freq(dev, 90500000);
//hackrf_set_txvga_gain(dev, 10); // //hackrf_set_txvga_gain(dev, 10);
hackrf_set_amp_enable(dev, 1); // hackrf_set_amp_enable(dev, 1);
hackrf_set_lna_gain(dev, 24); // hackrf_set_lna_gain(dev, 24);
hackrf_set_vga_gain(dev, 20); // hackrf_set_vga_gain(dev, 20);
hackrf_set_baseband_filter_bandwidth(dev, sampleRate); // hackrf_set_baseband_filter_bandwidth(dev, sampleRate);
hackrf_set_sample_rate(dev, sampleRate); // hackrf_set_sample_rate(dev, sampleRate);
src.init(dev, 64000); //src.init(dev, 64000);
sigPath.init(sampleRate, 20, fftSize, &src.output, (cdsp::complex_t*)fft_in, fftHandler); sigPath.init(sampleRate, 20, fftSize, &soapy.output, (dsp::complex_t*)fft_in, fftHandler);
sigPath.start(); sigPath.start();
} }
int Current = 0; int devId = 0;
int _devId = -1;
int srId = 0;
int _srId = -1;
bool showExample = false; bool showExample = false;
int freq = 90500; int freq = 90500;
@ -102,7 +107,8 @@ void drawWindow() {
if (freq != _freq) { if (freq != _freq) {
_freq = freq; _freq = freq;
wtf.centerFrequency = freq * 1000; wtf.centerFrequency = freq * 1000;
hackrf_set_freq(dev, freq * 1000); soapy.setFrequency(freq * 1000);
//hackrf_set_freq(dev, freq * 1000);
} }
if (vfoFreq != lastVfoFreq) { if (vfoFreq != lastVfoFreq) {
@ -115,6 +121,15 @@ void drawWindow() {
sigPath.setVolume(volume); sigPath.setVolume(volume);
} }
if (devId != _devId) {
_devId = devId;
soapy.setDevice(soapy.devList[devId]);
}
if (srId != _srId) {
soapy.setSampleRate(soapy.sampleRates[srId]);
}
if (ImGui::BeginMenuBar()) if (ImGui::BeginMenuBar())
{ {
@ -148,16 +163,22 @@ void drawWindow() {
ImGui::BeginChild("Left Column"); ImGui::BeginChild("Left Column");
if (ImGui::CollapsingHeader("Source")) { if (ImGui::CollapsingHeader("Source")) {
ImGui::PushItemWidth(ImGui::GetWindowSize().x);
//ImGui::Combo("Source", &Current, "HackRF One\0RTL-SDR"); ImGui::Combo("##_0_", &devId, soapy.txtDevList.c_str());
ImGui::SliderFloat("Volume", &volume, 0.0f, 1.0f); ImGui::Combo("##_1_", &srId, soapy.txtSampleRateList.c_str());
ImGui::SliderFloat("##_2_", &volume, 0.0f, 1.0f, "");
if (ImGui::Button("Start") && !state) { if (ImGui::Button("Start") && !state) {
state = true; state = true;
src.start(); soapy.start();
} }
ImGui::SameLine();
if (ImGui::Button("Stop") && state) { if (ImGui::Button("Stop") && state) {
state = false; state = false;
src.stop(); soapy.stop();
}
if (ImGui::Button("Refresh")) {
soapy.refresh();
} }
} }

View File

@ -4,26 +4,17 @@ SignalPath::SignalPath() {
} }
std::vector<float> iftaps; void SignalPath::init(uint64_t sampleRate, int fftRate, int fftSize, dsp::stream<dsp::complex_t>* input, dsp::complex_t* fftBuffer, void fftHandler(dsp::complex_t*)) {
std::vector<float> ataps;
void SignalPath::init(uint64_t sampleRate, int fftRate, int fftSize, cdsp::stream<cdsp::complex_t>* input, cdsp::complex_t* fftBuffer, void fftHandler(cdsp::complex_t*)) {
this->sampleRate = sampleRate; this->sampleRate = sampleRate;
this->fftRate = fftRate; this->fftRate = fftRate;
this->fftSize = fftSize; this->fftSize = fftSize;
BlackmanWindow(iftaps, sampleRate, 100000, 200000);
BlackmanWindow(ataps, 200000, 20000, 10000);
// for (int i = 0; i < iftaps.size(); i++) { // for (int i = 0; i < iftaps.size(); i++) {
// printf("%f\n", iftaps[i]); // printf("%f\n", iftaps[i]);
// } // }
_demod = DEMOD_FM; _demod = DEMOD_FM;
printf("%d\n", iftaps.size());
printf("%d\n", ataps.size());
dcBiasRemover.init(input, 32000); dcBiasRemover.init(input, 32000);
dcBiasRemover.bypass = true; dcBiasRemover.bypass = true;
split.init(&dcBiasRemover.output, 32000); split.init(&dcBiasRemover.output, 32000);
@ -31,13 +22,15 @@ void SignalPath::init(uint64_t sampleRate, int fftRate, int fftSize, cdsp::strea
fftBlockDec.init(&split.output_a, (sampleRate / fftRate) - fftSize, fftSize); fftBlockDec.init(&split.output_a, (sampleRate / fftRate) - fftSize, fftSize);
fftHandlerSink.init(&fftBlockDec.output, fftBuffer, fftSize, fftHandler); fftHandlerSink.init(&fftBlockDec.output, fftBuffer, fftSize, fftHandler);
mainVFO.init(&split.output_b, 0, sampleRate, 200000, 32000); mainVFO.init(&split.output_b, sampleRate, 200000, 200000, 0, 32000);
demod.init(mainVFO.output, 100000, 200000, 800); demod.init(mainVFO.output, 100000, 200000, 800);
amDemod.init(mainVFO.output, 800); amDemod.init(mainVFO.output, 50);
audioResamp.init(&demod.output, 200000, 40000, 800, 20000); audioResamp.init(&demod.output, 200000, 40000, 20000, 800);
audio.init(audioResamp.output, 160); audio.init(audioResamp.output, 160);
ns.init(mainVFO.output, 800);
} }
void SignalPath::setVFOFrequency(long frequency) { void SignalPath::setVFOFrequency(long frequency) {
@ -69,16 +62,16 @@ void SignalPath::setDemodulator(int demId) {
// Set input of the audio resampler // Set input of the audio resampler
if (demId == DEMOD_FM) { if (demId == DEMOD_FM) {
printf("Starting FM demodulator\n"); printf("Starting FM demodulator\n");
mainVFO.setBandwidth(200000); mainVFO.setOutputSampleRate(200000, 200000);
audioResamp.setInput(&demod.output); audioResamp.setInput(&demod.output);
audioResamp.setInputSampleRate(200000); audioResamp.setInputSampleRate(200000, 800);
demod.start(); demod.start();
} }
else if (demId == DEMOD_AM) { else if (demId == DEMOD_AM) {
printf("Starting AM demodulator\n"); printf("Starting AM demodulator\n");
mainVFO.setBandwidth(12000); mainVFO.setOutputSampleRate(12500, 12500);
audioResamp.setInput(&amDemod.output); audioResamp.setInput(&amDemod.output);
audioResamp.setInputSampleRate(12000); audioResamp.setInputSampleRate(12500, 50);
amDemod.start(); amDemod.start();
} }
@ -94,6 +87,7 @@ void SignalPath::start() {
mainVFO.start(); mainVFO.start();
demod.start(); demod.start();
//ns.start();
audioResamp.start(); audioResamp.start();
audio.start(); audio.start();

View File

@ -1,16 +1,19 @@
#pragma once #pragma once
#include <cdsp/filter.h> #include <dsp/filter.h>
#include <cdsp/resampling.h> #include <dsp/resampling.h>
#include <cdsp/generator.h> #include <dsp/source.h>
#include <cdsp/math.h> #include <dsp/math.h>
#include <cdsp/demodulation.h> #include <dsp/demodulator.h>
#include <cdsp/audio.h> #include <dsp/routing.h>
#include <vfo.h> #include <dsp/sink.h>
#include <dsp/correction.h>
#include <dsp/vfo.h>
#include <io/audio.h>
class SignalPath { class SignalPath {
public: public:
SignalPath(); SignalPath();
void init(uint64_t sampleRate, int fftRate, int fftSize, cdsp::stream<cdsp::complex_t>* input, cdsp::complex_t* fftBuffer, void fftHandler(cdsp::complex_t*)); void init(uint64_t sampleRate, int fftRate, int fftSize, dsp::stream<dsp::complex_t>* input, dsp::complex_t* fftBuffer, void fftHandler(dsp::complex_t*));
void start(); void start();
void setSampleRate(float sampleRate); void setSampleRate(float sampleRate);
void setDCBiasCorrection(bool enabled); void setDCBiasCorrection(bool enabled);
@ -28,22 +31,26 @@ public:
}; };
private: private:
cdsp::DCBiasRemover dcBiasRemover; dsp::DCBiasRemover dcBiasRemover;
cdsp::Splitter split; dsp::Splitter split;
// FFT // FFT
cdsp::BlockDecimator fftBlockDec; dsp::BlockDecimator fftBlockDec;
cdsp::HandlerSink fftHandlerSink; dsp::HandlerSink fftHandlerSink;
// VFO // VFO
VFO mainVFO; dsp::VFO mainVFO;
cdsp::FMDemodulator demod; // Demodulators
cdsp::AMDemodulator amDemod; dsp::FMDemodulator demod;
dsp::AMDemodulator amDemod;
//cdsp::FloatDecimatingFIRFilter audioDecFilt; // Audio output
cdsp::FractionalResampler audioResamp; dsp::FloatResampler audioResamp;
cdsp::AudioSink audio; io::AudioSink audio;
// DEBUG
dsp::NullSink ns;
float sampleRate; float sampleRate;
float fftRate; float fftRate;

View File

@ -1,104 +0,0 @@
#include <vfo.h>
#include <numeric>
VFO::VFO() {
}
void VFO::init(cdsp::stream<cdsp::complex_t>* input, float offset, float inputSampleRate, float bandWidth, int bufferSize) {
_input = input;
outputSampleRate = ceilf(bandWidth / OUTPUT_SR_ROUND) * OUTPUT_SR_ROUND;
_inputSampleRate = inputSampleRate;
int _gcd = std::gcd((int)inputSampleRate, (int)outputSampleRate);
_interp = outputSampleRate / _gcd;
_decim = inputSampleRate / _gcd;
_bandWidth = bandWidth;
_bufferSize = bufferSize;
lo.init(offset, inputSampleRate, bufferSize);
mixer.init(&lo.output, input, bufferSize);
interp.init(&mixer.output, _interp, bufferSize);
BlackmanWindow(decimTaps, inputSampleRate * _interp, bandWidth / 2.0f, bandWidth / 2.0f);
if (_interp != 1) {
printf("Interpolation needed\n");
decFir.init(&interp.output, decimTaps, bufferSize * _interp, _decim);
}
else {
decFir.init(&mixer.output, decimTaps, bufferSize, _decim);
printf("Interpolation NOT needed: %d %d %d\n", bufferSize / _decim, _decim, _interp);
}
output = &decFir.output;
}
void VFO::start() {
lo.start();
mixer.start();
if (_interp != 1) {
interp.start();
}
decFir.start();
}
void VFO::stop() {
// TODO: Stop LO
mixer.stop();
interp.stop();
decFir.stop();
}
void VFO::setOffset(float freq) {
lo.setFrequency(-freq);
}
void VFO::setBandwidth(float bandWidth) {
if (bandWidth == _bandWidth) {
return;
}
outputSampleRate = ceilf(bandWidth / OUTPUT_SR_ROUND) * OUTPUT_SR_ROUND;
int _gcd = std::gcd((int)_inputSampleRate, (int)outputSampleRate);
int interpol = outputSampleRate / _gcd;
int decim = _inputSampleRate / _gcd;
_bandWidth = bandWidth;
BlackmanWindow(decimTaps, _inputSampleRate * _interp, bandWidth / 2, bandWidth);
decFir.stop();
decFir.setTaps(decimTaps);
decFir.setDecimation(decim);
if (interpol != _interp) {
interp.stop();
if (interpol == 1) {
decFir.setBufferSize(_bufferSize);
decFir.setInput(&mixer.output);
}
else if (_interp == 1) {
decFir.setInput(&interp.output);
decFir.setBufferSize(_bufferSize * _interp);
interp.setInterpolation(interpol);
interp.start();
}
else {
decFir.setBufferSize(_bufferSize * _interp);
interp.setInterpolation(interpol);
interp.start();
}
}
_interp = interpol;
_decim = decim;
decFir.start();
}
void VFO::setSampleRate(int sampleRate) {
}
int VFO::getOutputSampleRate() {
return outputSampleRate;
}

View File

@ -1,43 +0,0 @@
#pragma once
#include <cdsp/math.h>
#include <cdsp/generator.h>
#include <cdsp/resampling.h>
#include <cdsp/filter.h>
// Round up to next 5KHz multiple frequency
#define OUTPUT_SR_ROUND 5000.0f
class VFO {
public:
VFO();
void init(cdsp::stream<cdsp::complex_t>* input, float offset, float sampleRate, float bandWidth, int bufferSize);
void start();
void stop();
void setOffset(float freq);
void setBandwidth(float bandwidth);
void setSampleRate(int sampleRate);
int getOutputSampleRate();
cdsp::stream<cdsp::complex_t>* output;
private:
cdsp::ComplexSineSource lo;
cdsp::Multiplier mixer;
cdsp::IQInterpolator interp;
cdsp::DecimatingFIRFilter decFir;
std::vector<float> decimTaps;
int _interp;
int _decim;
float _inputSampleRate;
float _outputSampleRate;
float _bandWidth;
int _bufferSize;
int outputSampleRate;
cdsp::stream<cdsp::complex_t>* _input;
};