mirror of
https://github.com/f4exb/sdrangel.git
synced 2025-10-30 20:40:20 -04:00
308 lines
8.7 KiB
C++
308 lines
8.7 KiB
C++
/* amd.c
|
|
|
|
This file is part of a program that implements a Software-Defined Radio.
|
|
|
|
Copyright (C) 2012, 2013 Warren Pratt, NR0V
|
|
Copyright (C) 2024 Edouard Griffiths, F4EXB Adapted to SDRangel
|
|
|
|
This program is free software; you can redistribute it and/or
|
|
modify it under the terms of the GNU General Public License
|
|
as published by the Free Software Foundation; either version 2
|
|
of the License, or (at your option) any later version.
|
|
|
|
This program is distributed in the hope that it will be useful,
|
|
but WITHOUT ANY WARRANTY; without even the implied warranty of
|
|
MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the
|
|
GNU General Public License for more details.
|
|
|
|
You should have received a copy of the GNU General Public License
|
|
along with this program; if not, write to the Free Software
|
|
Foundation, Inc., 51 Franklin Street, Fifth Floor, Boston, MA 02110-1301, USA.
|
|
|
|
The author can be reached by email at
|
|
|
|
warren@wpratt.com
|
|
|
|
*/
|
|
|
|
#include <cmath>
|
|
#include <array>
|
|
|
|
#include "comm.hpp"
|
|
#include "amd.hpp"
|
|
#include "anf.hpp"
|
|
#include "emnr.hpp"
|
|
#include "anr.hpp"
|
|
#include "snba.hpp"
|
|
|
|
namespace WDSP {
|
|
|
|
AMD::AMD
|
|
(
|
|
int _run,
|
|
int _buff_size,
|
|
float *_in_buff,
|
|
float *_out_buff,
|
|
int _mode,
|
|
int _levelfade,
|
|
int _sbmode,
|
|
int _sample_rate,
|
|
double _fmin,
|
|
double _fmax,
|
|
double _zeta,
|
|
double _omegaN,
|
|
double _tauR,
|
|
double _tauI
|
|
) :
|
|
run(_run),
|
|
buff_size(_buff_size),
|
|
in_buff(_in_buff),
|
|
out_buff(_out_buff),
|
|
mode(_mode),
|
|
sample_rate((double) _sample_rate),
|
|
fmin(_fmin),
|
|
fmax(_fmax),
|
|
zeta(_zeta),
|
|
omegaN(_omegaN),
|
|
tauR(_tauR),
|
|
tauI(_tauI),
|
|
sbmode(_sbmode),
|
|
levelfade(_levelfade)
|
|
{
|
|
init();
|
|
}
|
|
|
|
void AMD::init()
|
|
{
|
|
//pll
|
|
omega_min = 2 * M_PI * fmin / sample_rate;
|
|
omega_max = 2 * M_PI * fmax / sample_rate;
|
|
g1 = 1.0 - std::exp(-2.0 * omegaN * zeta / sample_rate);
|
|
g2 = -g1 + 2.0 * (1 - exp(-omegaN * zeta / sample_rate) * cos(omegaN / sample_rate * sqrt(1.0 - zeta * zeta)));
|
|
phs = 0.0;
|
|
fil_out = 0.0;
|
|
omega = 0.0;
|
|
|
|
//fade leveler
|
|
dc = 0.0;
|
|
dc_insert = 0.0;
|
|
mtauR = exp(-1.0 / (sample_rate * tauR));
|
|
onem_mtauR = 1.0 - mtauR;
|
|
mtauI = exp(-1.0 / (sample_rate * tauI));
|
|
onem_mtauI = 1.0 - mtauI;
|
|
|
|
//sideband separation
|
|
c0[0] = -0.328201924180698;
|
|
c0[1] = -0.744171491539427;
|
|
c0[2] = -0.923022915444215;
|
|
c0[3] = -0.978490468768238;
|
|
c0[4] = -0.994128272402075;
|
|
c0[5] = -0.998458978159551;
|
|
c0[6] = -0.999790306259206;
|
|
|
|
c1[0] = -0.0991227952747244;
|
|
c1[1] = -0.565619728761389;
|
|
c1[2] = -0.857467122550052;
|
|
c1[3] = -0.959123933111275;
|
|
c1[4] = -0.988739372718090;
|
|
c1[5] = -0.996959189310611;
|
|
c1[6] = -0.999282492800792;
|
|
}
|
|
|
|
void AMD::flush()
|
|
{
|
|
dc = 0.0;
|
|
dc_insert = 0.0;
|
|
}
|
|
|
|
void AMD::execute()
|
|
{
|
|
double audio;
|
|
std::array<double, 2> vco;
|
|
std::array<double, 2> corr;
|
|
double det;
|
|
double del_out;
|
|
double ai;
|
|
double bi;
|
|
double aq;
|
|
double bq;
|
|
double ai_ps;
|
|
double bi_ps;
|
|
double aq_ps;
|
|
double bq_ps;
|
|
|
|
if (run)
|
|
{
|
|
switch (mode)
|
|
{
|
|
|
|
case 0: //AM Demodulator
|
|
{
|
|
for (int i = 0; i < buff_size; i++)
|
|
{
|
|
double xr = in_buff[2 * i + 0];
|
|
double xi = in_buff[2 * i + 1];
|
|
audio = sqrt(xr*xr + xi*xi);
|
|
|
|
if (levelfade)
|
|
{
|
|
dc = mtauR * dc + onem_mtauR * audio;
|
|
dc_insert = mtauI * dc_insert + onem_mtauI * audio;
|
|
audio += dc_insert - dc;
|
|
}
|
|
|
|
out_buff[2 * i + 0] = (float) audio;
|
|
out_buff[2 * i + 1] = (float) audio;
|
|
}
|
|
|
|
break;
|
|
}
|
|
|
|
case 1: //Synchronous AM Demodulator with Sideband Separation
|
|
{
|
|
for (int i = 0; i < buff_size; i++)
|
|
{
|
|
vco[0] = cos(phs);
|
|
vco[1] = sin(phs);
|
|
|
|
ai = in_buff[2 * i + 0] * vco[0];
|
|
bi = in_buff[2 * i + 0] * vco[1];
|
|
aq = in_buff[2 * i + 1] * vco[0];
|
|
bq = in_buff[2 * i + 1] * vco[1];
|
|
|
|
if (sbmode != 0)
|
|
{
|
|
a[0] = dsI;
|
|
b[0] = bi;
|
|
c[0] = dsQ;
|
|
d[0] = aq;
|
|
dsI = ai;
|
|
dsQ = bq;
|
|
|
|
for (int j = 0; j < STAGES; j++)
|
|
{
|
|
int k = 3 * j;
|
|
a[k + 3] = c0[j] * (a[k] - a[k + 5]) + a[k + 2];
|
|
b[k + 3] = c1[j] * (b[k] - b[k + 5]) + b[k + 2];
|
|
c[k + 3] = c0[j] * (c[k] - c[k + 5]) + c[k + 2];
|
|
d[k + 3] = c1[j] * (d[k] - d[k + 5]) + d[k + 2];
|
|
}
|
|
|
|
ai_ps = a[OUT_IDX];
|
|
bi_ps = b[OUT_IDX];
|
|
bq_ps = c[OUT_IDX];
|
|
aq_ps = d[OUT_IDX];
|
|
|
|
for (int j = OUT_IDX + 2; j > 0; j--)
|
|
{
|
|
a[j] = a[j - 1];
|
|
b[j] = b[j - 1];
|
|
c[j] = c[j - 1];
|
|
d[j] = d[j - 1];
|
|
}
|
|
}
|
|
|
|
corr[0] = +ai + bq;
|
|
corr[1] = -bi + aq;
|
|
|
|
switch(sbmode)
|
|
{
|
|
case 0: //both sidebands
|
|
{
|
|
audio = corr[0];
|
|
break;
|
|
}
|
|
case 1: //LSB
|
|
{
|
|
audio = (ai_ps - bi_ps) + (aq_ps + bq_ps);
|
|
break;
|
|
}
|
|
case 2: //USB
|
|
{
|
|
audio = (ai_ps + bi_ps) - (aq_ps - bq_ps);
|
|
break;
|
|
}
|
|
default:
|
|
break;
|
|
}
|
|
|
|
if (levelfade)
|
|
{
|
|
dc = mtauR * dc + onem_mtauR * audio;
|
|
dc_insert = mtauI * dc_insert + onem_mtauI * corr[0];
|
|
audio += dc_insert - dc;
|
|
}
|
|
|
|
out_buff[2 * i + 0] = (float) audio;
|
|
out_buff[2 * i + 1] = (float) audio;
|
|
|
|
if ((corr[0] == 0.0) && (corr[1] == 0.0))
|
|
corr[0] = 1.0;
|
|
|
|
det = atan2(corr[1], corr[0]);
|
|
del_out = fil_out;
|
|
omega += g2 * det;
|
|
|
|
if (omega < omega_min)
|
|
omega = omega_min;
|
|
|
|
if (omega > omega_max)
|
|
omega = omega_max;
|
|
|
|
fil_out = g1 * det + omega;
|
|
phs += del_out;
|
|
|
|
while (phs >= 2 * M_PI)
|
|
phs -= 2 * M_PI;
|
|
|
|
while (phs < 0.0)
|
|
phs += 2 * M_PI;
|
|
}
|
|
|
|
break;
|
|
}
|
|
default:
|
|
break;
|
|
}
|
|
}
|
|
else if (in_buff != out_buff)
|
|
{
|
|
std::copy (in_buff, in_buff + buff_size * 2, out_buff);
|
|
}
|
|
}
|
|
|
|
void AMD::setBuffers(float* in, float* out)
|
|
{
|
|
in_buff = in;
|
|
out_buff = out;
|
|
}
|
|
|
|
void AMD::setSamplerate(int rate)
|
|
{
|
|
sample_rate = rate;
|
|
init();
|
|
}
|
|
|
|
void AMD::setSize(int size)
|
|
{
|
|
buff_size = size;
|
|
}
|
|
|
|
/********************************************************************************************************
|
|
* *
|
|
* Public Properties *
|
|
* *
|
|
********************************************************************************************************/
|
|
|
|
void AMD::setSBMode(int _sbmode)
|
|
{
|
|
sbmode = _sbmode;
|
|
}
|
|
|
|
void AMD::setFadeLevel(int _levelfade)
|
|
{
|
|
levelfade = _levelfade;
|
|
}
|
|
|
|
} // namesoace WDSP
|