SDRPlusPlus/core/src/dsp/demodulator.h

279 lines
8.8 KiB
C
Raw Normal View History

2020-06-10 04:13:56 +02:00
#pragma once
2020-11-02 03:57:44 +01:00
#include <dsp/block.h>
2020-11-12 00:53:38 +01:00
#include <volk/volk.h>
2020-06-22 16:45:57 +02:00
2020-11-02 21:13:28 +01:00
#include <spdlog/spdlog.h>
2020-11-02 03:57:44 +01:00
#define FAST_ATAN2_COEF1 FL_M_PI / 4.0f
#define FAST_ATAN2_COEF2 3.0f * FAST_ATAN2_COEF1
2020-06-15 15:53:45 +02:00
2020-06-22 16:45:57 +02:00
inline float fast_arctan2(float y, float x) {
2020-11-02 03:57:44 +01:00
float abs_y = fabsf(y);
float r, angle;
if (x == 0.0f && y == 0.0f) { return 0.0f; }
if (x>=0.0f) {
r = (x - abs_y) / (x + abs_y);
angle = FAST_ATAN2_COEF1 - FAST_ATAN2_COEF1 * r;
}
else {
r = (x + abs_y) / (abs_y - x);
angle = FAST_ATAN2_COEF2 - FAST_ATAN2_COEF1 * r;
}
if (y < 0.0f) {
return -angle;
}
2020-06-15 15:53:45 +02:00
return angle;
}
2020-06-10 04:13:56 +02:00
2020-06-22 16:45:57 +02:00
namespace dsp {
2020-11-02 03:57:44 +01:00
class FMDemod : public generic_block<FMDemod> {
2020-06-10 04:13:56 +02:00
public:
2020-11-02 03:57:44 +01:00
FMDemod() {}
2020-06-10 18:52:07 +02:00
2020-11-02 03:57:44 +01:00
FMDemod(stream<complex_t>* in, float sampleRate, float deviation) { init(in, sampleRate, deviation); }
2020-06-10 04:13:56 +02:00
2020-11-02 03:57:44 +01:00
~FMDemod() { generic_block<FMDemod>::stop(); }
2020-06-10 04:13:56 +02:00
2020-11-02 03:57:44 +01:00
void init(stream<complex_t>* in, float sampleRate, float deviation) {
_in = in;
_sampleRate = sampleRate;
_deviation = deviation;
2020-11-04 00:42:39 +01:00
phasorSpeed = (2 * FL_M_PI) / (_sampleRate / _deviation);
2020-11-02 03:57:44 +01:00
generic_block<FMDemod>::registerInput(_in);
generic_block<FMDemod>::registerOutput(&out);
2020-06-15 15:53:45 +02:00
}
2020-11-02 03:57:44 +01:00
void setInput(stream<complex_t>* in) {
std::lock_guard<std::mutex> lck(generic_block<FMDemod>::ctrlMtx);
generic_block<FMDemod>::tempStop();
2020-11-02 17:48:17 +01:00
generic_block<FMDemod>::unregisterInput(_in);
2020-11-02 03:57:44 +01:00
_in = in;
2020-11-02 17:48:17 +01:00
generic_block<FMDemod>::registerInput(_in);
2020-11-02 03:57:44 +01:00
generic_block<FMDemod>::tempStart();
2020-06-22 16:45:57 +02:00
}
2020-07-19 15:59:44 +02:00
void setSampleRate(float sampleRate) {
2020-11-02 03:57:44 +01:00
std::lock_guard<std::mutex> lck(generic_block<FMDemod>::ctrlMtx);
generic_block<FMDemod>::tempStop();
2020-07-19 15:59:44 +02:00
_sampleRate = sampleRate;
2020-11-04 00:42:39 +01:00
phasorSpeed = (2 * FL_M_PI) / (_sampleRate / _deviation);
2020-11-02 03:57:44 +01:00
generic_block<FMDemod>::tempStart();
}
float getSampleRate() {
return _sampleRate;
2020-07-19 15:59:44 +02:00
}
void setDeviation(float deviation) {
2020-11-02 03:57:44 +01:00
std::lock_guard<std::mutex> lck(generic_block<FMDemod>::ctrlMtx);
generic_block<FMDemod>::tempStop();
2020-07-19 15:59:44 +02:00
_deviation = deviation;
2020-11-04 00:42:39 +01:00
phasorSpeed = (2 * FL_M_PI) / (_sampleRate / _deviation);
2020-11-02 03:57:44 +01:00
generic_block<FMDemod>::tempStart();
}
float getDeviation() {
return _deviation;
2020-07-19 15:59:44 +02:00
}
2020-11-02 03:57:44 +01:00
int run() {
count = _in->read();
if (count < 0) { return -1; }
2020-06-10 04:13:56 +02:00
2020-11-02 03:57:44 +01:00
// 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);
2020-11-02 03:57:44 +01:00
diff = currentPhase - phase;
2020-11-04 00:42:39 +01:00
if (diff > 3.1415926535f) { diff -= 2 * 3.1415926535f; }
else if (diff <= -3.1415926535f) { diff += 2 * 3.1415926535f; }
out.writeBuf[i] = diff / phasorSpeed;
2020-11-02 03:57:44 +01:00
phase = currentPhase;
2020-06-10 04:13:56 +02:00
}
2020-11-02 03:57:44 +01:00
_in->flush();
if (!out.swap(count)) { return -1; }
2020-11-02 03:57:44 +01:00
return count;
2020-06-10 04:13:56 +02:00
}
2020-11-02 03:57:44 +01:00
stream<float> out;
2020-06-15 15:53:45 +02:00
2020-11-02 03:57:44 +01:00
private:
int count;
float phase, phasorSpeed, _sampleRate, _deviation;
stream<complex_t>* _in;
2020-06-15 15:53:45 +02:00
2020-11-02 03:57:44 +01:00
};
class AMDemod : public generic_block<AMDemod> {
2020-06-15 15:53:45 +02:00
public:
2020-11-02 03:57:44 +01:00
AMDemod() {}
2020-06-15 15:53:45 +02:00
2020-11-02 03:57:44 +01:00
AMDemod(stream<complex_t>* in) { init(in); }
2020-06-15 15:53:45 +02:00
2020-11-02 03:57:44 +01:00
~AMDemod() { generic_block<AMDemod>::stop(); }
2020-06-15 15:53:45 +02:00
2020-11-02 03:57:44 +01:00
void init(stream<complex_t>* in) {
_in = in;
generic_block<AMDemod>::registerInput(_in);
generic_block<AMDemod>::registerOutput(&out);
2020-06-15 15:53:45 +02:00
}
2020-11-02 03:57:44 +01:00
void setInput(stream<complex_t>* in) {
std::lock_guard<std::mutex> lck(generic_block<AMDemod>::ctrlMtx);
generic_block<AMDemod>::tempStop();
2020-11-02 17:48:17 +01:00
generic_block<AMDemod>::unregisterInput(_in);
2020-11-02 03:57:44 +01:00
_in = in;
2020-11-02 17:48:17 +01:00
generic_block<AMDemod>::registerInput(_in);
2020-11-02 03:57:44 +01:00
generic_block<AMDemod>::tempStart();
2020-06-15 15:53:45 +02:00
}
2020-11-02 03:57:44 +01:00
int run() {
count = _in->read();
if (count < 0) { return -1; }
volk_32fc_magnitude_32f(out.writeBuf, (lv_32fc_t*)_in->readBuf, count);
2020-11-02 03:57:44 +01:00
_in->flush();
2020-11-12 00:53:38 +01:00
volk_32f_accumulator_s32f(&avg, out.writeBuf, count);
2020-11-12 00:53:38 +01:00
avg /= (float)count;
for (int i = 0; i < count; i++) {
out.writeBuf[i] -= avg;
2020-11-12 00:53:38 +01:00
}
if (!out.swap(count)) { return -1; }
2020-11-02 03:57:44 +01:00
return count;
2020-06-22 16:45:57 +02:00
}
2020-11-02 03:57:44 +01:00
stream<float> out;
2020-06-15 15:53:45 +02:00
private:
2020-11-12 00:53:38 +01:00
float avg;
2020-11-02 03:57:44 +01:00
int count;
stream<complex_t>* _in;
2020-06-15 15:53:45 +02:00
};
2020-07-19 15:59:44 +02:00
2020-11-02 03:57:44 +01:00
class SSBDemod : public generic_block<SSBDemod> {
2020-07-19 15:59:44 +02:00
public:
2020-11-02 03:57:44 +01:00
SSBDemod() {}
2020-07-19 15:59:44 +02:00
2020-11-02 03:57:44 +01:00
SSBDemod(stream<complex_t>* in, float sampleRate, float bandWidth, int mode) { init(in, sampleRate, bandWidth, mode); }
2020-07-19 15:59:44 +02:00
2020-12-06 19:51:56 +01:00
~SSBDemod() {
generic_block<SSBDemod>::stop();
delete[] buffer;
}
2020-11-02 03:57:44 +01:00
enum {
MODE_USB,
MODE_LSB,
MODE_DSB
};
void init(stream<complex_t>* in, float sampleRate, float bandWidth, int mode) {
_in = in;
_sampleRate = sampleRate;
2020-07-19 15:59:44 +02:00
_bandWidth = bandWidth;
2020-11-02 03:57:44 +01:00
_mode = mode;
phase = lv_cmake(1.0f, 0.0f);
switch (_mode) {
case MODE_USB:
phaseDelta = lv_cmake(std::cos((_bandWidth / _sampleRate) * FL_M_PI), std::sin((_bandWidth / _sampleRate) * FL_M_PI));
break;
case MODE_LSB:
phaseDelta = lv_cmake(std::cos(-(_bandWidth / _sampleRate) * FL_M_PI), std::sin(-(_bandWidth / _sampleRate) * FL_M_PI));
break;
case MODE_DSB:
phaseDelta = lv_cmake(1.0f, 0.0f);
break;
}
2020-12-06 19:51:56 +01:00
buffer = new lv_32fc_t[STREAM_BUFFER_SIZE];
2020-11-02 03:57:44 +01:00
generic_block<SSBDemod>::registerInput(_in);
generic_block<SSBDemod>::registerOutput(&out);
2020-07-19 15:59:44 +02:00
}
2020-11-02 03:57:44 +01:00
void setInput(stream<complex_t>* in) {
std::lock_guard<std::mutex> lck(generic_block<SSBDemod>::ctrlMtx);
generic_block<SSBDemod>::tempStop();
2020-11-02 17:48:17 +01:00
generic_block<SSBDemod>::unregisterInput(_in);
2020-11-02 03:57:44 +01:00
_in = in;
2020-11-02 17:48:17 +01:00
generic_block<SSBDemod>::registerInput(_in);
2020-11-02 03:57:44 +01:00
generic_block<SSBDemod>::tempStart();
2020-07-19 15:59:44 +02:00
}
2020-11-02 03:57:44 +01:00
void setSampleRate(float sampleRate) {
// No need to restart
_sampleRate = sampleRate;
switch (_mode) {
case MODE_USB:
phaseDelta = lv_cmake(std::cos((_bandWidth / _sampleRate) * FL_M_PI), std::sin((_bandWidth / _sampleRate) * FL_M_PI));
break;
case MODE_LSB:
phaseDelta = lv_cmake(std::cos(-(_bandWidth / _sampleRate) * FL_M_PI), std::sin(-(_bandWidth / _sampleRate) * FL_M_PI));
break;
case MODE_DSB:
phaseDelta = lv_cmake(1.0f, 0.0f);
break;
}
2020-07-19 15:59:44 +02:00
}
2020-11-02 03:57:44 +01:00
void setBandWidth(float bandWidth) {
// No need to restart
_bandWidth = bandWidth;
switch (_mode) {
case MODE_USB:
phaseDelta = lv_cmake(std::cos((_bandWidth / _sampleRate) * FL_M_PI), std::sin((_bandWidth / _sampleRate) * FL_M_PI));
break;
case MODE_LSB:
phaseDelta = lv_cmake(std::cos(-(_bandWidth / _sampleRate) * FL_M_PI), std::sin(-(_bandWidth / _sampleRate) * FL_M_PI));
break;
case MODE_DSB:
phaseDelta = lv_cmake(1.0f, 0.0f);
break;
2020-07-19 15:59:44 +02:00
}
}
void setMode(int mode) {
_mode = mode;
2020-11-02 03:57:44 +01:00
switch (_mode) {
case MODE_USB:
phaseDelta = lv_cmake(std::cos((_bandWidth / _sampleRate) * FL_M_PI), std::sin((_bandWidth / _sampleRate) * FL_M_PI));
break;
case MODE_LSB:
phaseDelta = lv_cmake(std::cos(-(_bandWidth / _sampleRate) * FL_M_PI), std::sin(-(_bandWidth / _sampleRate) * FL_M_PI));
break;
case MODE_DSB:
phaseDelta = lv_cmake(1.0f, 0.0f);
break;
2020-08-20 18:29:23 +02:00
}
2020-07-19 15:59:44 +02:00
}
2020-11-02 03:57:44 +01:00
int run() {
count = _in->read();
if (count < 0) { return -1; }
2020-07-19 15:59:44 +02:00
volk_32fc_s32fc_x2_rotator_32fc(buffer, (lv_32fc_t*)_in->readBuf, phaseDelta, &phase, count);
volk_32fc_deinterleave_real_32f(out.writeBuf, buffer, count);
2020-07-19 15:59:44 +02:00
2020-11-02 03:57:44 +01:00
_in->flush();
if (!out.swap(count)) { return -1; }
2020-11-02 03:57:44 +01:00
return count;
2020-07-19 15:59:44 +02:00
}
2020-11-02 03:57:44 +01:00
stream<float> out;
2020-08-20 18:29:23 +02:00
2020-11-02 03:57:44 +01:00
private:
int count;
int _mode;
float _sampleRate, _bandWidth;
stream<complex_t>* _in;
2020-12-06 19:51:56 +01:00
lv_32fc_t* buffer;
2020-11-02 03:57:44 +01:00
lv_32fc_t phase;
lv_32fc_t phaseDelta;
2020-08-20 18:29:23 +02:00
2020-11-02 03:57:44 +01:00
};
}