ryzerth/sdrpp
1
0
Fork 0
mirror of https://github.com/AlexandreRouma/SDRPlusPlus.git synced 2026-04-19 14:52:43 +00:00
Code Issues Packages Projects Releases Wiki Activity

new fixes

Browse Source
This commit is contained in:
Ryzerth
2020-10-20 00:32:17 +02:00
parent 027054b57e
commit 6e5450ed24
19 changed files with 298 additions and 96 deletions
Download Patch File Download Diff File Expand all files Collapse all files

77
core/src/dsp/correction.h
View File

@@ -63,26 +63,95 @@ namespace dsp {
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) {
// if (_this->_in->read(buf, _this->_bufferSize) < 0) { break; };
// if (_this->bypass) {
// if (_this->output.write(buf, _this->_bufferSize) < 0) { break; };
// 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;
// }
// if (_this->output.write(buf, _this->_bufferSize) < 0) { break; };
// }
// delete[] buf;
// }
static void _worker(DCBiasRemover* _this) {
complex_t* buf = new complex_t[_this->_bufferSize];
float ibias = 0.0f;
float qbias = 0.0f;
complex_t* mixBuf = new complex_t[_this->_bufferSize];
float currentPhase = 0.0f;
float lastPhase = 0.0f;
double phase = 0.0f;
while (true) {
float ibias = 0.0f;
float qbias = 0.0f;
if (_this->_in->read(buf, _this->_bufferSize) < 0) { break; };
if (_this->bypass) {
if (_this->output.write(buf, _this->_bufferSize) < 0) { break; };
continue;
}
// Detect the frequency of the signal
double avgDiff = 0.0f;
// for (int i = 0; i < _this->_bufferSize; i++) {
// currentPhase = fast_arctan2(buf[i].i, buf[i].q);
// float diff = currentPhase - lastPhase;
// if (diff > 3.1415926535f) { diff -= 2 * 3.1415926535f; }
// else if (diff <= -3.1415926535f) { diff += 2 * 3.1415926535f; }
// avgDiff += diff;
// lastPhase = currentPhase;
// }
// avgDiff /= (double)_this->_bufferSize;
// avgDiff /= (double)_this->_bufferSize;
// Average the samples to "filter" the signal to the block frequency
for (int i = 0; i < _this->_bufferSize; i++) {
ibias += buf[i].i;
qbias += buf[i].q;
}
ibias /= _this->_bufferSize;
qbias /= _this->_bufferSize;
// Get the phase difference from the last block
currentPhase = fast_arctan2(ibias, qbias);
float diff = currentPhase - lastPhase;
if (diff > 3.1415926535f) { diff -= 2 * 3.1415926535f; }
else if (diff <= -3.1415926535f) { diff += 2 * 3.1415926535f; }
avgDiff += diff;
lastPhase = currentPhase;
avgDiff /= (double)_this->_bufferSize;
// Generate a correction signal using the phase difference
for (int i = 0; i < _this->_bufferSize; i++) {
buf[i].i -= ibias;
buf[i].q -= qbias;
mixBuf[i].i = sin(phase);
mixBuf[i].q = cos(phase);
phase -= avgDiff;
phase = fmodl(phase, 2.0 * 3.1415926535);
}
// Mix the correction signal with the original signal to shift the unwanted signal
// to the center. Also, null out the real component so that symetric
// frequencies are removed (at least I hope...)
float tq;
for (int i = 0; i < _this->_bufferSize; i++) {
buf[i].i = ((mixBuf[i].i * buf[i].q) + (mixBuf[i].q * buf[i].i)) * 1.4142;
buf[i].q = 0;
}
if (_this->output.write(buf, _this->_bufferSize) < 0) { break; };
}
delete[] buf;