GitHub/dsd-fme
1
0
Fork 0
mirror of https://github.com/lwvmobile/dsd-fme.git synced 2026-11-13 01:07:24 -05:00
Code Issues Packages Projects Releases Wiki Activity

rtl_sdr_fm: FLL rotator - improve sine/cos approximation

Browse Source
This commit is contained in:
arancormonk
2025-09-04 10:20:39 -04:00
parent f052271a16
commit e094f95b03
1 changed files with 110 additions and 19 deletions
Download Patch File Download Diff File Expand all files Collapse all files

129
src/rtl_sdr_fm.cpp
View File

@@ -103,6 +103,10 @@ static int atan_lut_size = 131072; /* 512 KB */
static int atan_lut_coef = 8;
static pthread_once_t atan_lut_once = PTHREAD_ONCE_INIT;
static pthread_mutex_t atan_lut_mutex = PTHREAD_MUTEX_INITIALIZER;
/* Optional quarter-wave sine LUT for FLL rotator (Q15). */
static int16_t fll_qsine_q15_lut[1025]; /* 0..pi/2 in 1024 steps, +1 guard for exact pi/2 */
static pthread_once_t fll_lut_once = PTHREAD_ONCE_INIT;
static int fll_lut_enabled = 0; /* DSD_FME_FLL_LUT (0 default: use fast approx) */
/* Debug/compat toggles via env */
static int combine_rotate_enabled = 1; /* DSD_FME_COMBINE_ROT (1 default) */
static int upsample_fixedpoint_enabled = 1;/* DSD_FME_UPSAMPLE_FP (1 default) */
@@ -659,6 +663,71 @@ static void atan_lut_once_init(void)
}
}
/* Build quarter-wave sine LUT in Q15: sin(theta) where theta in [0, pi/2] */
static void fll_lut_once_init(void)
{
for (int i = 0; i <= 1024; i++) {
double theta = (kPi * 0.5) * ((double)i / 1024.0);
int v = (int)lrint(sin(theta) * 32767.0);
if (v > 32767) v = 32767;
if (v < -32767) v = -32767;
fll_qsine_q15_lut[i] = (int16_t)v;
}
}
/* Return cos/sin for phase in Q15 using quarter-wave LUT with linear interpolation. */
static inline void fll_sin_cos_q15_from_phase_lut(int phase_q15, int16_t *c_out, int16_t *s_out)
{
/* phase_q15 wraps at 1<<15 mapping to 2*pi */
int p = phase_q15 & 0x7FFF; /* 0..32767 */
int quad = p >> 13; /* 0..3 */
int r = p & 0x1FFF; /* position within quadrant: 0..8191 */
/* Helper to sample quarter-wave S(r) with r in [0..8192] using 1024-segment linear interp */
auto sample_quarter = [](int r8192) -> int16_t {
if (r8192 < 0) r8192 = 0;
if (r8192 > 8192) r8192 = 8192;
int idx = r8192 >> 3; /* 0..1024 */
int frac = r8192 & 7; /* 0..7 */
int16_t s0 = fll_qsine_q15_lut[idx];
int16_t s1 = fll_qsine_q15_lut[(idx < 1024) ? (idx + 1) : 1024];
int diff = (int)s1 - (int)s0;
int interp = (int)s0 + ((diff * frac + 4) >> 3); /* rounded */
if (interp > 32767) interp = 32767;
if (interp < -32767) interp = -32767;
return (int16_t)interp;
};
int16_t s_pos, c_pos;
/* Cosine within quadrant uses complementary angle in the quarter-wave */
switch (quad) {
case 0: /* [0, pi/2) */
s_pos = sample_quarter(r);
c_pos = sample_quarter(8192 - r);
*s_out = s_pos;
*c_out = c_pos;
break;
case 1: /* [pi/2, pi) */
s_pos = sample_quarter(8192 - r);
c_pos = sample_quarter(r);
*s_out = s_pos;
*c_out = (int16_t)(-c_pos);
break;
case 2: /* [pi, 3pi/2) */
s_pos = sample_quarter(r);
c_pos = sample_quarter(8192 - r);
*s_out = (int16_t)(-s_pos);
*c_out = (int16_t)(-c_pos);
break;
default: /* 3: [3pi/2, 2pi) */
s_pos = sample_quarter(8192 - r);
c_pos = sample_quarter(r);
*s_out = (int16_t)(-s_pos);
*c_out = c_pos;
break;
}
}
//UDP -- keep for compatibility reasons
#include <netinet/in.h>
#include <arpa/inet.h>
@@ -1738,25 +1807,42 @@ static inline void fll_mix_and_update(struct demod_state *d)
const int N = d->lp_len;
int phase = d->fll_phase_q15; /* Q15 wraps at 1<<15 ~ 2*pi */
const int freq = d->fll_freq_q15; /* Q15 increment per sample */
/* Use small LUT-free rotator: sin/cos approximation via angle wrapping into quadrants. */
for (int i = 0; i + 1 < N; i += 2) {
int p = phase & 0x7FFF; /* 0..32767 */
int q = p >> 13; /* quadrant 0..3 */
int16_t c = 32767, s = 0;
int16_t r = (int16_t)(p & 0x1FFF); /* 0..8191 */
switch (q) {
case 0: c = 32767; s = (int16_t)((r * 4)); break;
case 1: c = (int16_t)(32767 - (r * 4)); s = 32767; break;
case 2: c = -32767; s = (int16_t)(32767 - (r * 4)); break;
default: c = (int16_t)(-32767 + (r * 4)); s = -32767; break;
}
int xr = x[i];
int xj = x[i+1];
int32_t yr = ((int32_t)xr * c + (int32_t)xj * s) >> 15;
int32_t yj = ((int32_t)xj * c - (int32_t)xr * s) >> 15;
x[i] = (int16_t)yr;
x[i+1] = (int16_t)yj;
phase += freq;
/* Optional: higher-quality quarter-wave LUT rotator (linear interp), enabled via DSD_FME_FLL_LUT=1. */
if (fll_lut_enabled) {
/* Ensure LUT is initialized once before use */
pthread_once(&fll_lut_once, fll_lut_once_init);
for (int i = 0; i + 1 < N; i += 2) {
int16_t c, s;
fll_sin_cos_q15_from_phase_lut(phase, &c, &s);
int xr = x[i];
int xj = x[i+1];
int32_t yr = ((int32_t)xr * c + (int32_t)xj * s) >> 15;
int32_t yj = ((int32_t)xj * c - (int32_t)xr * s) >> 15;
x[i] = (int16_t)yr;
x[i+1] = (int16_t)yj;
phase += freq;
}
} else {
/* Fast LUT-free rotator: piecewise-linear sin/cos within quadrants. */
for (int i = 0; i + 1 < N; i += 2) {
int p = phase & 0x7FFF; /* 0..32767 */
int q = p >> 13; /* quadrant 0..3 */
int16_t c = 32767, s = 0;
int16_t r = (int16_t)(p & 0x1FFF); /* 0..8191 */
switch (q) {
case 0: c = 32767; s = (int16_t)((r * 4)); break;
case 1: c = (int16_t)(32767 - (r * 4)); s = 32767; break;
case 2: c = -32767; s = (int16_t)(32767 - (r * 4)); break;
default: c = (int16_t)(-32767 + (r * 4)); s = -32767; break;
}
int xr = x[i];
int xj = x[i+1];
int32_t yr = ((int32_t)xr * c + (int32_t)xj * s) >> 15;
int32_t yj = ((int32_t)xj * c - (int32_t)xr * s) >> 15;
x[i] = (int16_t)yr;
x[i+1] = (int16_t)yj;
phase += freq;
}
}
d->fll_phase_q15 = phase & 0x7FFF;
}
@@ -2953,6 +3039,11 @@ void open_rtlsdr_stream(dsd_opts *opts)
/* Configure FLL/TED via envs. Defaults: FLL on with small gains; TED off by default. */
const char *fll = getenv("DSD_FME_FLL");
demod.fll_enabled = (!fll || fll[0] == '\0' || fll[0] == '1') ? 1 : 0;
/* Optional: enable LUT-based FLL rotator via DSD_FME_FLL_LUT=1 */
{
const char *flut = getenv("DSD_FME_FLL_LUT");
fll_lut_enabled = (flut && flut[0] == '1') ? 1 : 0;
}
/* Gains in Q15; very conservative defaults */
const char *fa = getenv("DSD_FME_FLL_ALPHA");
const char *fb = getenv("DSD_FME_FLL_BETA");