You can not select more than 25 topics
Topics must start with a letter or number, can include dashes ('-') and can be up to 35 characters long.
305 lines
9.4 KiB
305 lines
9.4 KiB
/* 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 "comm.hpp" |
|
#include "amd.hpp" |
|
#include "anf.hpp" |
|
#include "emnr.hpp" |
|
#include "anr.hpp" |
|
#include "snba.hpp" |
|
#include "RXA.hpp" |
|
|
|
namespace WDSP { |
|
|
|
AMD* AMD::create_amd |
|
( |
|
int run, |
|
int buff_size, |
|
float *in_buff, |
|
float *out_buff, |
|
int mode, |
|
int levelfade, |
|
int sbmode, |
|
int sample_rate, |
|
float fmin, |
|
float fmax, |
|
float zeta, |
|
float omegaN, |
|
float tauR, |
|
float tauI |
|
) |
|
{ |
|
AMD *a = new AMD(); |
|
a->run = run; |
|
a->buff_size = buff_size; |
|
a->in_buff = in_buff; |
|
a->out_buff = out_buff; |
|
a->mode = mode; |
|
a->levelfade = levelfade; |
|
a->sbmode = sbmode; |
|
a->sample_rate = (float)sample_rate; |
|
a->fmin = fmin; |
|
a->fmax = fmax; |
|
a->zeta = zeta; |
|
a->omegaN = omegaN; |
|
a->tauR = tauR; |
|
a->tauI = tauI; |
|
|
|
init_amd(a); |
|
return a; |
|
} |
|
|
|
void AMD::destroy_amd(AMD *a) |
|
{ |
|
delete a; |
|
} |
|
|
|
void AMD::init_amd(AMD *a) |
|
{ |
|
//pll |
|
a->omega_min = 2 * M_PI * a->fmin / a->sample_rate; |
|
a->omega_max = 2 * M_PI * a->fmax / a->sample_rate; |
|
a->g1 = 1.0 - std::exp(-2.0 * a->omegaN * a->zeta / a->sample_rate); |
|
a->g2 = -a->g1 + 2.0 * (1 - exp(-a->omegaN * a->zeta / a->sample_rate) * cos(a->omegaN / a->sample_rate * sqrt(1.0 - a->zeta * a->zeta))); |
|
a->phs = 0.0; |
|
a->fil_out = 0.0; |
|
a->omega = 0.0; |
|
|
|
//fade leveler |
|
a->dc = 0.0; |
|
a->dc_insert = 0.0; |
|
a->mtauR = exp(-1.0 / (a->sample_rate * a->tauR)); |
|
a->onem_mtauR = 1.0 - a->mtauR; |
|
a->mtauI = exp(-1.0 / (a->sample_rate * a->tauI)); |
|
a->onem_mtauI = 1.0 - a->mtauI; |
|
|
|
//sideband separation |
|
a->c0[0] = -0.328201924180698; |
|
a->c0[1] = -0.744171491539427; |
|
a->c0[2] = -0.923022915444215; |
|
a->c0[3] = -0.978490468768238; |
|
a->c0[4] = -0.994128272402075; |
|
a->c0[5] = -0.998458978159551; |
|
a->c0[6] = -0.999790306259206; |
|
|
|
a->c1[0] = -0.0991227952747244; |
|
a->c1[1] = -0.565619728761389; |
|
a->c1[2] = -0.857467122550052; |
|
a->c1[3] = -0.959123933111275; |
|
a->c1[4] = -0.988739372718090; |
|
a->c1[5] = -0.996959189310611; |
|
a->c1[6] = -0.999282492800792; |
|
} |
|
|
|
void AMD::flush_amd (AMD *a) |
|
{ |
|
a->dc = 0.0; |
|
a->dc_insert = 0.0; |
|
} |
|
|
|
void AMD::xamd (AMD *a) |
|
{ |
|
int i; |
|
float audio; |
|
float vco[2]; |
|
float corr[2]; |
|
float det; |
|
float del_out; |
|
float ai, bi, aq, bq; |
|
float ai_ps, bi_ps, aq_ps, bq_ps; |
|
int j, k; |
|
if (a->run) |
|
{ |
|
switch (a->mode) |
|
{ |
|
|
|
case 0: //AM Demodulator |
|
{ |
|
for (i = 0; i < a->buff_size; i++) |
|
{ |
|
audio = sqrt(a->in_buff[2 * i + 0] * a->in_buff[2 * i + 0] + a->in_buff[2 * i + 1] * a->in_buff[2 * i + 1]); |
|
if (a->levelfade) |
|
{ |
|
a->dc = a->mtauR * a->dc + a->onem_mtauR * audio; |
|
a->dc_insert = a->mtauI * a->dc_insert + a->onem_mtauI * audio; |
|
audio += a->dc_insert - a->dc; |
|
} |
|
a->out_buff[2 * i + 0] = audio; |
|
a->out_buff[2 * i + 1] = audio; |
|
} |
|
break; |
|
} |
|
|
|
case 1: //Synchronous AM Demodulator with Sideband Separation |
|
{ |
|
for (i = 0; i < a->buff_size; i++) |
|
{ |
|
vco[0] = cos(a->phs); |
|
vco[1] = sin(a->phs); |
|
|
|
ai = a->in_buff[2 * i + 0] * vco[0]; |
|
bi = a->in_buff[2 * i + 0] * vco[1]; |
|
aq = a->in_buff[2 * i + 1] * vco[0]; |
|
bq = a->in_buff[2 * i + 1] * vco[1]; |
|
|
|
if (a->sbmode != 0) |
|
{ |
|
a->a[0] = a->dsI; |
|
a->b[0] = bi; |
|
a->c[0] = a->dsQ; |
|
a->d[0] = aq; |
|
a->dsI = ai; |
|
a->dsQ = bq; |
|
|
|
for (j = 0; j < STAGES; j++) |
|
{ |
|
k = 3 * j; |
|
a->a[k + 3] = a->c0[j] * (a->a[k] - a->a[k + 5]) + a->a[k + 2]; |
|
a->b[k + 3] = a->c1[j] * (a->b[k] - a->b[k + 5]) + a->b[k + 2]; |
|
a->c[k + 3] = a->c0[j] * (a->c[k] - a->c[k + 5]) + a->c[k + 2]; |
|
a->d[k + 3] = a->c1[j] * (a->d[k] - a->d[k + 5]) + a->d[k + 2]; |
|
} |
|
ai_ps = a->a[OUT_IDX]; |
|
bi_ps = a->b[OUT_IDX]; |
|
bq_ps = a->c[OUT_IDX]; |
|
aq_ps = a->d[OUT_IDX]; |
|
|
|
for (j = OUT_IDX + 2; j > 0; j--) |
|
{ |
|
a->a[j] = a->a[j - 1]; |
|
a->b[j] = a->b[j - 1]; |
|
a->c[j] = a->c[j - 1]; |
|
a->d[j] = a->d[j - 1]; |
|
} |
|
} |
|
|
|
corr[0] = +ai + bq; |
|
corr[1] = -bi + aq; |
|
|
|
switch(a->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; |
|
} |
|
} |
|
|
|
if (a->levelfade) |
|
{ |
|
a->dc = a->mtauR * a->dc + a->onem_mtauR * audio; |
|
a->dc_insert = a->mtauI * a->dc_insert + a->onem_mtauI * corr[0]; |
|
audio += a->dc_insert - a->dc; |
|
} |
|
a->out_buff[2 * i + 0] = audio; |
|
a->out_buff[2 * i + 1] = audio; |
|
|
|
if ((corr[0] == 0.0) && (corr[1] == 0.0)) corr[0] = 1.0; |
|
det = atan2(corr[1], corr[0]); |
|
del_out = a->fil_out; |
|
a->omega += a->g2 * det; |
|
if (a->omega < a->omega_min) a->omega = a->omega_min; |
|
if (a->omega > a->omega_max) a->omega = a->omega_max; |
|
a->fil_out = a->g1 * det + a->omega; |
|
a->phs += del_out; |
|
while (a->phs >= 2 * M_PI) a->phs -= 2 * M_PI; |
|
while (a->phs < 0.0) a->phs += 2 * M_PI; |
|
} |
|
break; |
|
} |
|
} |
|
} |
|
else if (a->in_buff != a->out_buff) |
|
{ |
|
memcpy (a->out_buff, a->in_buff, a->buff_size * sizeof(wcomplex)); |
|
} |
|
} |
|
|
|
void AMD::setBuffers_amd (AMD *a, float* in, float* out) |
|
{ |
|
a->in_buff = in; |
|
a->out_buff = out; |
|
} |
|
|
|
void AMD::setSamplerate_amd (AMD *a, int rate) |
|
{ |
|
a->sample_rate = rate; |
|
init_amd(a); |
|
} |
|
|
|
void AMD::setSize_amd (AMD *a, int size) |
|
{ |
|
a->buff_size = size; |
|
} |
|
|
|
/******************************************************************************************************** |
|
* * |
|
* RXA Properties * |
|
* * |
|
********************************************************************************************************/ |
|
|
|
void AMD::SetAMDRun(RXA& rxa, int run) |
|
{ |
|
AMD *a = rxa.amd.p; |
|
if (a->run != run) |
|
{ |
|
RXA::bp1Check (rxa, run, rxa.snba.p->run, rxa.emnr.p->run, |
|
rxa.anf.p->run, rxa.anr.p->run); |
|
rxa.csDSP.lock(); |
|
a->run = run; |
|
RXA::bp1Set (rxa); |
|
rxa.csDSP.unlock(); |
|
} |
|
} |
|
|
|
void AMD::SetAMDSBMode(RXA& rxa, int sbmode) |
|
{ |
|
rxa.csDSP.lock(); |
|
rxa.amd.p->sbmode = sbmode; |
|
rxa.csDSP.unlock(); |
|
} |
|
|
|
void AMD::SetAMDFadeLevel(RXA& rxa, int levelfade) |
|
{ |
|
rxa.csDSP.lock(); |
|
rxa.amd.p->levelfade = levelfade; |
|
rxa.csDSP.unlock(); |
|
} |
|
|
|
} // namesoace WDSP
|
|
|