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.
703 lines
22 KiB
703 lines
22 KiB
/* SPDX-License-Identifier: MIT |
|
* Copyright (c) 2026 Dan Chokola, KD2HSG |
|
* |
|
* Streaming receiver (public chirpmodem_rx API), the C counterpart of |
|
* stream_rx.py. |
|
* |
|
* Each push is shifted to 0 Hz, FIR decimated and resampled to the modem |
|
* rate (filter state carried across pushes), decimated to the chip rate and |
|
* run through the preamble detector (each window's spectrum computed once). |
|
* A candidate waits for its header, then for the whole frame, and is decoded |
|
* with cm_receive_frame. Candidates are decoded in time order; one inside a |
|
* frame still being received waits for it (that frame's pilots are |
|
* up-chirps and can look like a preamble). Not yet: narrowband excision. |
|
*/ |
|
#include <stdarg.h> |
|
#include <stdio.h> |
|
#include <stdlib.h> |
|
#include <string.h> |
|
#include <math.h> |
|
#include <time.h> |
|
#if defined(_WIN32) |
|
#include <windows.h> |
|
#endif |
|
#include "cm.h" |
|
#include "chirpmodem.h" |
|
|
|
/* Symbols of slack in "candidate inside the last decoded frame": a frame can |
|
* start exactly where the previous one ended, whose last pilot is an up-chirp |
|
* too and can pull the detector's peak a window early. */ |
|
#define BUSY_SLACK 8 |
|
|
|
static double now(void) |
|
{ |
|
#if defined(_WIN32) |
|
LARGE_INTEGER f, c; |
|
QueryPerformanceFrequency(&f); |
|
QueryPerformanceCounter(&c); |
|
return (double)c.QuadPart / (double)f.QuadPart; |
|
#elif defined(CLOCK_MONOTONIC) |
|
struct timespec t; |
|
clock_gettime(CLOCK_MONOTONIC, &t); |
|
return t.tv_sec + 1e-9 * t.tv_nsec; |
|
#else |
|
return (double)clock() / CLOCKS_PER_SEC; |
|
#endif |
|
} |
|
|
|
/* ------------------------------------------------------------- rate conversion */ |
|
typedef struct { |
|
double fs_in, fs_out, f_shift, fs_mid, step, delay_s; |
|
int D, L, frac; |
|
double *h; /* FIR taps */ |
|
cf64 *hist; /* last L-1 shifted input samples */ |
|
long n_in; /* input samples so far */ |
|
cm_resampler *rs; /* fractional stage */ |
|
int hw; |
|
cf64 *buf; /* mid-rate samples not yet consumed by the fractional stage */ |
|
long nbuf, capbuf, b0, j; |
|
} conv_t; |
|
|
|
static double bessel_i0(double x) |
|
{ |
|
double s = 1, t = 1, q = 0.25 * x * x; |
|
for (int k = 1; k < 500 && t > 1e-17 * s; k++) { |
|
t *= q / ((double)k * k); |
|
s += t; |
|
} |
|
return s; |
|
} |
|
|
|
/* iq_io.lowpass_taps: Kaiser-windowed sinc, 80 dB */ |
|
static double *lowpass_taps(double cutoff, double transition, int *n_out) |
|
{ |
|
double atten = 80.0, beta = 0.1102 * (atten - 8.7); |
|
int n = (int)ceil((atten - 8) / (2.285 * 2 * M_PI * transition)) | 1; |
|
double *h = malloc(sizeof(double) * n), s = 0, i0b = bessel_i0(beta); |
|
if (!h) |
|
return NULL; |
|
for (int i = 0; i < n; i++) { |
|
double k = i - (n - 1) / 2.0, a = 2 * cutoff * k; |
|
double sinc = a == 0 ? 1.0 : sin(M_PI * a) / (M_PI * a); |
|
double u = 2.0 * i / (n - 1) - 1.0; |
|
h[i] = 2 * cutoff * sinc * bessel_i0(beta * sqrt(1 - u * u)) / i0b; |
|
s += h[i]; |
|
} |
|
for (int i = 0; i < n; i++) |
|
h[i] /= s; |
|
*n_out = n; |
|
return h; |
|
} |
|
|
|
static int conv_init(conv_t *c, double fs_in, double fs_out, double f_shift, double passband) |
|
{ |
|
memset(c, 0, sizeof *c); |
|
c->fs_in = fs_in; |
|
c->fs_out = fs_out; |
|
c->f_shift = f_shift; |
|
c->D = (int)floor(fs_in / (fs_out * 1.2)); |
|
if (c->D < 1) |
|
c->D = 1; |
|
c->fs_mid = fs_in / c->D; |
|
if (c->D > 1) { |
|
double stop = c->fs_mid - passband; |
|
c->h = lowpass_taps((passband + stop) / 2 / fs_in, (stop - passband) / fs_in, &c->L); |
|
c->hist = calloc((size_t)c->L, sizeof(cf64)); |
|
if (!c->h || !c->hist) |
|
return -1; |
|
c->delay_s = (c->L - 1) / 2.0 / fs_in; |
|
} |
|
c->step = c->fs_mid / fs_out; |
|
c->frac = fabs(c->step - 1.0) > 1e-9; |
|
if (c->frac) { |
|
double cut = 1.0 / c->step < 1.0 ? 1.0 / c->step : 1.0; |
|
c->rs = cm_resampler_new(cut); |
|
if (!c->rs) |
|
return -1; |
|
c->hw = cm_resampler_halfwidth(c->rs) + 2; |
|
} |
|
return 0; |
|
} |
|
|
|
static void conv_free(conv_t *c) |
|
{ |
|
free(c->h); |
|
free(c->hist); |
|
free(c->buf); |
|
if (c->rs) |
|
cm_resampler_free(c->rs); |
|
} |
|
|
|
/* x (n samples) -> modem-rate samples in *out (grown as needed); returns the count */ |
|
static long conv_process(conv_t *c, const cf64 *x, long n, cf64 **out, long *cap) |
|
{ |
|
/* shift + integer FIR decimation: output at every absolute input index |
|
* a = 0, D, 2D, ... (sum_t h[t] x[a - t]) */ |
|
long nmid_max = n / c->D + 2; |
|
cf64 *mid = malloc(sizeof(cf64) * (size_t)nmid_max), *xs = malloc(sizeof(cf64) * (size_t)(n + c->L)); |
|
long nmid = 0; |
|
double fn = c->f_shift / c->fs_in; |
|
cf64 ph = cexp(-2.0 * M_PI * I * fmod(fn * (double)c->n_in, 1.0)), rot = cexp(-2.0 * M_PI * I * fn); |
|
int Lm1 = c->D > 1 ? c->L - 1 : 0; |
|
if (Lm1) |
|
memcpy(xs, c->hist, sizeof(cf64) * (size_t)Lm1); |
|
for (long i = 0; i < n; i++) { |
|
if ((i & 4095) == 0) |
|
ph = cexp(-2.0 * M_PI * I * fmod(fn * (double)(c->n_in + i), 1.0)); |
|
xs[Lm1 + i] = x[i] * ph; |
|
ph *= rot; |
|
} |
|
if (c->D == 1) { |
|
memcpy(mid, xs, sizeof(cf64) * (size_t)n); |
|
nmid = n; |
|
} else { |
|
long first = (c->D - c->n_in % c->D) % c->D; /* block index of the first a */ |
|
for (long i = first; i < n; i += c->D) { |
|
const cf64 *e = xs + Lm1 + i; /* x[a] */ |
|
cf64 s = 0; |
|
for (int t = 0; t < c->L; t++) |
|
s += c->h[t] * e[-t]; |
|
mid[nmid++] = s; |
|
} |
|
memcpy(c->hist, xs + n, sizeof(cf64) * (size_t)Lm1); |
|
} |
|
c->n_in += n; |
|
free(xs); |
|
|
|
long nout = 0; |
|
if (!c->frac) { |
|
nout = nmid; |
|
} else { |
|
if (c->nbuf + nmid > c->capbuf) { |
|
c->capbuf = 2 * (c->nbuf + nmid); |
|
c->buf = realloc(c->buf, sizeof(cf64) * (size_t)c->capbuf); |
|
} |
|
memcpy(c->buf + c->nbuf, mid, sizeof(cf64) * (size_t)nmid); |
|
c->nbuf += nmid; |
|
long end = c->b0 + c->nbuf; |
|
long j_hi = (long)floor((end - c->hw) / c->step); |
|
if (j_hi >= c->j) |
|
nout = j_hi - c->j + 1; |
|
} |
|
if (nout > 0) { |
|
while (*cap < nout) { |
|
*cap = *cap ? 2 * *cap : 1 << 16; |
|
*out = realloc(*out, sizeof(cf64) * (size_t)*cap); |
|
} |
|
if (!c->frac) { |
|
memcpy(*out, mid, sizeof(cf64) * (size_t)nout); |
|
} else { |
|
double *pos = malloc(sizeof(double) * (size_t)nout); |
|
for (long i = 0; i < nout; i++) |
|
pos[i] = (c->j + i) * c->step - c->b0; |
|
cm_resample(c->rs, c->buf, c->nbuf, pos, nout, *out); |
|
free(pos); |
|
c->j += nout; |
|
long keep = (long)floor(c->j * c->step) - c->hw - c->b0; |
|
if (keep > 0) { |
|
memmove(c->buf, c->buf + keep, sizeof(cf64) * (size_t)(c->nbuf - keep)); |
|
c->nbuf -= keep; |
|
c->b0 += keep; |
|
} |
|
} |
|
} |
|
free(mid); |
|
return nout; |
|
} |
|
|
|
/* ------------------------------------------------------------- stream state */ |
|
typedef struct { |
|
long c0, need; |
|
} cand_t; |
|
|
|
struct chirpmodem_rx { |
|
chirpmodem_rx_config cfg; |
|
cm_phy phy; |
|
conv_t cv; |
|
cm_rx *rx; |
|
const cm_phy *p; |
|
cm_resampler *kd; /* modem rate -> chip rate (cm_decimate's kernel) */ |
|
double freq, t_offset; |
|
int effort, max_effort, auto_effort; |
|
long max_chips, hdr_chips; |
|
cf64 *x; /* modem-rate samples, x[0] = chip x0 */ |
|
long nx, capx, x0; |
|
cf64 *r; /* chip rate, r[0] = chip x0 */ |
|
long nr, capr; |
|
double *eu; /* detector spectra of windows w0 .. w0 + neu - 1 */ |
|
double *met; /* metric of windows w0 .. */ |
|
long w0, neu, cap_eu; |
|
long k_next; |
|
cand_t *pend; |
|
int npend, cappend; |
|
long busy_until; |
|
long n_ok, n_fail; |
|
double t_conv, t_det, t_dec, load; |
|
double hist_stream[50], hist_proc[50]; |
|
int nhist; |
|
cf64 *xin, *y; /* conversion scratch */ |
|
long capin, capy, n_in; |
|
}; |
|
|
|
typedef struct chirpmodem_rx stream_t; |
|
|
|
static long chips_in(const stream_t *s) { return s->x0 + s->nx / s->p->osr; } |
|
|
|
static int grow(void **p, long *cap, long need, size_t elem) |
|
{ |
|
if (need <= *cap) |
|
return 0; |
|
long c = *cap ? *cap : 1024; |
|
while (c < need) |
|
c *= 2; |
|
void *q = realloc(*p, elem * (size_t)c); |
|
if (!q) |
|
return -1; |
|
*p = q; |
|
*cap = c; |
|
return 0; |
|
} |
|
|
|
#if defined(__GNUC__) |
|
__attribute__((format(printf, 2, 3))) |
|
#endif |
|
static void say(stream_t *s, const char *fmt, ...) |
|
{ |
|
if (!s->cfg.on_log) |
|
return; |
|
char msg[256]; |
|
va_list ap; |
|
va_start(ap, fmt); |
|
vsnprintf(msg, sizeof msg, fmt, ap); |
|
va_end(ap); |
|
s->cfg.on_log(s->cfg.user, msg); |
|
} |
|
|
|
static void decimate_new(stream_t *s) |
|
{ |
|
int osr = s->p->osr, margin = 16; |
|
long n_have = s->x0 + s->nr, n_can = chips_in(s) - margin; |
|
if (n_can <= n_have) |
|
return; |
|
long lo = (n_have - s->x0 - margin) * osr; |
|
if (lo < 0) |
|
lo = 0; |
|
long hi = (n_can - s->x0 + margin) * osr; |
|
if (hi > s->nx) |
|
hi = s->nx; |
|
long nnew = n_can - n_have; |
|
double *pos = malloc(sizeof(double) * (size_t)nnew); |
|
for (long i = 0; i < nnew; i++) |
|
pos[i] = (double)(n_have + i - s->x0) * osr - lo; |
|
grow((void **)&s->r, &s->capr, s->nr + nnew, sizeof(cf64)); |
|
cm_resample(s->kd, s->x + lo, hi - lo, pos, nnew, s->r + s->nr); |
|
s->nr += nnew; |
|
free(pos); |
|
} |
|
|
|
static void detect(stream_t *s) |
|
{ |
|
const cm_phy *p = s->p; |
|
int M = p->M, L = p->n_preamble - 1, n_pre = p->n_preamble, N2 = 2 * M; |
|
long nw = (s->x0 + s->nr) / M, k_last = nw - L, k_final = k_last - n_pre; |
|
if (k_final < s->k_next) |
|
return; |
|
/* spectra for any new windows */ |
|
long have = s->w0 + s->neu; |
|
if (s->neu == 0) { |
|
s->w0 = s->x0 / M; |
|
have = s->w0; |
|
if (s->k_next - n_pre > s->w0) |
|
s->w0 = have = s->k_next - n_pre; |
|
} |
|
for (long w = have; w < nw; w++) { |
|
grow((void **)&s->eu, &s->cap_eu, (s->neu + 1) * N2, sizeof(double)); |
|
s->met = realloc(s->met, sizeof(double) * (size_t)(s->cap_eu / N2 + 1)); |
|
cm_window_power(s->rx, s->r + (w * M - s->x0), s->eu + (size_t)s->neu * N2); |
|
s->met[s->neu] = -1; /* metric not computed yet */ |
|
s->neu++; |
|
} |
|
/* metric for every k whose L windows are in */ |
|
double *S = malloc(sizeof(double) * (size_t)N2); |
|
for (long k = s->w0; k <= k_last; k++) { |
|
long i = k - s->w0; |
|
if (s->met[i] >= 0) |
|
continue; |
|
memset(S, 0, sizeof(double) * (size_t)N2); |
|
for (int t = 0; t < L; t++) { |
|
const double *e = s->eu + (size_t)(i + t) * N2; |
|
for (int n = 0; n < N2; n++) |
|
S[n] += e[n]; |
|
} |
|
int B = 0; |
|
for (int n = 1; n < N2; n++) |
|
if (S[n] > S[B]) |
|
B = n; |
|
double lg = 0; |
|
for (int t = 0; t < L; t++) { |
|
const double *e = s->eu + (size_t)(i + t) * N2; |
|
double m = 0; |
|
for (int n = 0; n < N2; n++) |
|
m += e[n]; |
|
/* an all-zero window (digital silence) has no peak: ratio 1, |
|
* not 0/0, which made the metric NaN and passed the threshold */ |
|
lg += log((m > 0 ? e[B] / (m / N2) : 1.0) + 1e-12); |
|
} |
|
s->met[i] = exp(lg / L); |
|
} |
|
free(S); |
|
/* accept local maxima above threshold once their neighbourhood is in */ |
|
for (long k = s->k_next; k <= k_final; k++) { |
|
long i = k - s->w0; |
|
if (i < 0 || !(s->met[i] >= 4.0)) /* written so that NaN fails too */ |
|
continue; |
|
int best = 1; |
|
for (long j = k - n_pre; j <= k + n_pre && best; j++) { |
|
long q = j - s->w0; |
|
if (q < 0 || q >= s->neu || j == k || s->met[q] < 0) |
|
continue; |
|
if (s->met[q] > s->met[i] || (j < k && s->met[q] == s->met[i])) |
|
best = 0; |
|
} |
|
long c0 = (k - 3) * M; |
|
if (!best || c0 + BUSY_SLACK * M < s->busy_until) |
|
continue; |
|
if (s->npend == s->cappend) { |
|
s->cappend = s->cappend ? 2 * s->cappend : 16; |
|
s->pend = realloc(s->pend, sizeof(cand_t) * (size_t)s->cappend); |
|
} |
|
s->pend[s->npend].c0 = c0 < 0 ? 0 : c0; |
|
s->pend[s->npend].need = c0 + 6L * M + s->hdr_chips + 8L * M; |
|
s->npend++; |
|
} |
|
s->k_next = k_final + 1; |
|
/* drop spectra no longer needed */ |
|
long drop = (s->k_next - 2 * n_pre) - s->w0; |
|
if (drop > 0 && drop <= s->neu) { |
|
memmove(s->eu, s->eu + (size_t)drop * N2, sizeof(double) * (size_t)(s->neu - drop) * N2); |
|
memmove(s->met, s->met + drop, sizeof(double) * (size_t)(s->neu - drop)); |
|
s->neu -= drop; |
|
s->w0 += drop; |
|
} |
|
} |
|
|
|
static void deliver(stream_t *s, const cm_result *r, double t) |
|
{ |
|
chirpmodem_frame f; |
|
memset(&f, 0, sizeof f); |
|
f.status = r->stage == CM_OK ? CHIRPMODEM_FRAME_OK |
|
: r->stage == CM_HEADER_FAIL ? CHIRPMODEM_FRAME_HEADER_FAIL |
|
: CHIRPMODEM_FRAME_PAYLOAD_FAIL; |
|
f.t = t; |
|
f.snr_db = r->snr_db; |
|
f.offset_hz = r->sync.f_hz; |
|
f.offset_ppm = r->sync.f_hz / s->freq * 1e6; |
|
if (r->have_header) { |
|
f.profile = r->hdr.profile; |
|
f.coherent = r->hdr.mode == CM_MODE_COHERENT; |
|
f.encrypted = r->hdr.encrypted; |
|
} |
|
f.policy_ok = 1; |
|
if (r->stage == CM_OK) { |
|
f.coherent = r->coherent; |
|
f.ramp_retry = r->ramp_retry; |
|
f.ramp_hz_per_s = r->ramp_hz_per_s; |
|
f.drift_retry = r->drift_retry; |
|
f.clock_ppm = r->sync.eps * 1e6; |
|
f.dd_passes = r->dd_passes; |
|
f.mac = r->mac; |
|
f.mac_len = r->mac_len; |
|
if (cm_mac_unpack(r->mac, r->mac_len, f.src, f.dst, &f.body, &f.body_len)) { |
|
f.src[0] = f.dst[0] = 0; /* malformed MAC: whole frame as body */ |
|
f.body = r->mac; |
|
f.body_len = r->mac_len; |
|
} |
|
if (f.profile == CM_LICENSED && (!f.src[0] || f.encrypted)) |
|
f.policy_ok = 0; |
|
} |
|
s->cfg.on_frame(s->cfg.user, &f); |
|
} |
|
|
|
static void service(stream_t *s) |
|
{ |
|
const cm_phy *p = s->p; |
|
int M = p->M, osr = p->osr, keep = 0; |
|
long hold_until = -1; /* an earlier candidate that may contain later ones is unresolved */ |
|
/* candidates in start order (insertion sort; the list is short and nearly sorted) */ |
|
for (int i = 1; i < s->npend; i++) { |
|
cand_t c = s->pend[i]; |
|
int j = i; |
|
while (j > 0 && s->pend[j - 1].c0 > c.c0) { |
|
s->pend[j] = s->pend[j - 1]; |
|
j--; |
|
} |
|
s->pend[j] = c; |
|
} |
|
for (int i = 0; i < s->npend; i++) { |
|
cand_t c = s->pend[i]; |
|
if (c.c0 < s->x0 || c.c0 + BUSY_SLACK * M < s->busy_until) |
|
continue; |
|
if (c.c0 < hold_until) { |
|
/* inside the span of an earlier frame still being received: its pilots |
|
* are up-chirps and can look like a preamble; wait until that frame is |
|
* decoded (busy_until then drops this one) */ |
|
s->pend[keep++] = c; |
|
continue; |
|
} |
|
if (chips_in(s) < c.need) { |
|
s->pend[keep++] = c; |
|
hold_until = c.c0 + s->max_chips; |
|
continue; |
|
} |
|
cm_result r; |
|
const cf64 *w = s->x + (c.c0 - s->x0) * osr; |
|
long nw = s->nx - (c.c0 - s->x0) * osr; |
|
cm_receive_frame(s->rx, w, (int)nw, 6 * M, s->effort, 4.0, &r); |
|
if (r.stage == CM_NO_SYNC && !r.truncated) |
|
continue; /* false alarm */ |
|
double start = c.c0 + r.sync.tau0; |
|
if (start + M / 2.0 < s->busy_until) |
|
continue; /* synced onto the tail of the frame just decoded |
|
* (pilots are up-chirps; BUSY_SLACK let it in) */ |
|
if (r.truncated) { |
|
long need; |
|
if (!r.have_header) { |
|
need = (long)start + s->hdr_chips + 16; |
|
} else { |
|
int nh, np; |
|
cm_frame_n_data(p, r.hdr.length, r.hdr.mode == CM_MODE_COHERENT, &nh, &np); |
|
need = (long)start + cm_payload_start(p) + |
|
(long)cm_layout_slots(nh + np, p->pilot_every) * M + 16; |
|
} |
|
c.need = need > c.need + M ? need : c.need + M; |
|
s->pend[keep++] = c; |
|
hold_until = c.c0 + s->max_chips; |
|
continue; |
|
} |
|
double t = start / p->bw + s->t_offset; |
|
if (r.stage == CM_OK) { |
|
s->n_ok++; |
|
s->busy_until = (long)start + r.frame_chips; |
|
deliver(s, &r, t); |
|
} else { |
|
s->n_fail++; |
|
if (s->cfg.report_failures) |
|
deliver(s, &r, t); |
|
} |
|
} |
|
s->npend = keep; |
|
} |
|
|
|
static void trim(stream_t *s) |
|
{ |
|
const cm_phy *p = s->p; |
|
int M = p->M, osr = p->osr; |
|
long keep = (s->k_next - p->n_preamble - 2) * M; |
|
for (int i = 0; i < s->npend; i++) |
|
if (s->pend[i].c0 < keep) |
|
keep = s->pend[i].c0; |
|
long oldest = chips_in(s) - (s->max_chips + 16L * M); |
|
if (keep < oldest) |
|
keep = oldest; |
|
int k2 = 0; |
|
for (int i = 0; i < s->npend; i++) |
|
if (s->pend[i].c0 >= keep) |
|
s->pend[k2++] = s->pend[i]; |
|
s->npend = k2; |
|
keep = (keep / M) * M; |
|
if (keep > s->x0) { |
|
long d = keep - s->x0; |
|
if (d * osr > s->nx) |
|
d = s->nx / osr; |
|
memmove(s->x, s->x + d * osr, sizeof(cf64) * (size_t)(s->nx - d * osr)); |
|
s->nx -= d * osr; |
|
long dr = d < s->nr ? d : s->nr; |
|
memmove(s->r, s->r + dr, sizeof(cf64) * (size_t)(s->nr - dr)); |
|
s->nr -= dr; |
|
s->x0 += d; |
|
} |
|
} |
|
|
|
static void adapt(stream_t *s, double stream_s, double proc_s) |
|
{ |
|
s->hist_stream[s->nhist % 50] = stream_s; |
|
s->hist_proc[s->nhist % 50] = proc_s; |
|
s->nhist++; |
|
int n = s->nhist < 50 ? s->nhist : 50; |
|
double a = 0, b = 0; |
|
for (int i = 0; i < n; i++) { |
|
a += s->hist_stream[i]; |
|
b += s->hist_proc[i]; |
|
} |
|
if (a > 0) |
|
s->load = b / a; |
|
if (!s->auto_effort || s->nhist < 20) |
|
return; |
|
double load = s->load; |
|
if (load > 0.7 && s->effort > 0) { |
|
s->effort--; |
|
say(s, "load %.0f%%: effort lowered to %d", 100 * load, s->effort); |
|
s->nhist = 0; |
|
} else if (load < 0.3 && s->effort < s->max_effort) { |
|
s->effort++; |
|
say(s, "load %.0f%%: effort raised to %d", 100 * load, s->effort); |
|
s->nhist = 0; |
|
} |
|
} |
|
|
|
static void push_modem(stream_t *s, const cf64 *y, long n) |
|
{ |
|
double t0 = now(); |
|
grow((void **)&s->x, &s->capx, s->nx + n, sizeof(cf64)); |
|
memcpy(s->x + s->nx, y, sizeof(cf64) * (size_t)n); |
|
s->nx += n; |
|
decimate_new(s); |
|
detect(s); |
|
double t1 = now(); |
|
service(s); |
|
double t2 = now(); |
|
trim(s); |
|
s->t_det += t1 - t0; |
|
s->t_dec += t2 - t1; |
|
adapt(s, n / (s->p->bw * s->p->osr), t2 - t0); |
|
} |
|
|
|
/* ------------------------------------------------------------- public API */ |
|
void chirpmodem_rx_config_init(chirpmodem_rx_config *c) |
|
{ |
|
memset(c, 0, sizeof *c); |
|
c->sf = 9; |
|
c->bw = 62.5e3; |
|
c->fs = 1e6; |
|
c->if_offset = 250e3; |
|
c->freq = 223.58e6; |
|
c->max_bytes = 255; |
|
c->effort = 2; |
|
} |
|
|
|
void chirpmodem_rx_free(chirpmodem_rx *s) |
|
{ |
|
if (!s) |
|
return; |
|
conv_free(&s->cv); |
|
if (s->rx) |
|
cm_rx_free(s->rx); |
|
if (s->kd) |
|
cm_resampler_free(s->kd); |
|
free(s->x); |
|
free(s->r); |
|
free(s->eu); |
|
free(s->met); |
|
free(s->pend); |
|
free(s->xin); |
|
free(s->y); |
|
free(s); |
|
} |
|
|
|
chirpmodem_rx *chirpmodem_rx_new(const chirpmodem_rx_config *cfg, int *err) |
|
{ |
|
int e = CHIRPMODEM_EINVAL; |
|
stream_t *s = NULL; |
|
if (!cfg || !cfg->on_frame || cfg->sf < 7 || cfg->sf > 12 || !(cfg->bw > 0) || |
|
!(cfg->fs > 0) || !(cfg->freq > 0) || cfg->effort < 0 || cfg->effort > 2 || |
|
cfg->max_bytes < 1 || cfg->max_bytes > CHIRPMODEM_MAX_MAC || |
|
fabs(cfg->if_offset) + cfg->bw / 2 > cfg->fs / 2) |
|
goto fail; |
|
e = CHIRPMODEM_ENOMEM; |
|
s = calloc(1, sizeof *s); |
|
if (!s) |
|
goto fail; |
|
s->cfg = *cfg; |
|
cm_phy_init(&s->phy, cfg->sf, cfg->bw, cfg->freq); |
|
if (conv_init(&s->cv, cfg->fs, cfg->bw * s->phy.osr, cfg->if_offset, cfg->bw / 2 + 20e3)) |
|
goto fail; |
|
s->rx = cm_rx_new(&s->phy); |
|
s->kd = cm_resampler_new(1.0 / s->phy.osr); |
|
if (!s->rx || !s->kd) |
|
goto fail; |
|
s->p = cm_rx_phy(s->rx); |
|
s->freq = cfg->freq; |
|
s->t_offset = s->cv.delay_s; |
|
s->effort = s->max_effort = cfg->effort; |
|
s->auto_effort = cfg->auto_effort; |
|
int nh, np; |
|
cm_frame_n_data(&s->phy, cfg->max_bytes, 0, &nh, &np); |
|
s->max_chips = cm_payload_start(&s->phy) + (long)cm_layout_slots(nh + np, s->phy.pilot_every) * s->phy.M; |
|
s->hdr_chips = cm_payload_start(&s->phy) + (long)cm_layout_slots(nh, s->phy.pilot_every) * s->phy.M; |
|
if (err) |
|
*err = CHIRPMODEM_OK; |
|
return s; |
|
fail: |
|
chirpmodem_rx_free(s); |
|
if (err) |
|
*err = e; |
|
return NULL; |
|
} |
|
|
|
int chirpmodem_rx_push(chirpmodem_rx *s, const void *samples, long n, chirpmodem_format fmt) |
|
{ |
|
if (!s || n < 0 || (n && !samples) || fmt < CHIRPMODEM_CU8 || fmt > CHIRPMODEM_CF32) |
|
return CHIRPMODEM_EINVAL; |
|
if (!n) |
|
return CHIRPMODEM_OK; |
|
double t0 = now(); |
|
if (grow((void **)&s->xin, &s->capin, n, sizeof(cf64))) |
|
return CHIRPMODEM_ENOMEM; |
|
cf64 *x = s->xin; |
|
const uint8_t *raw = samples; |
|
for (long i = 0; i < n; i++) { |
|
switch (fmt) { |
|
case CHIRPMODEM_CU8: |
|
x[i] = (raw[2 * i] - 127.5) / 127.5 + I * ((raw[2 * i + 1] - 127.5) / 127.5); |
|
break; |
|
case CHIRPMODEM_CI8: |
|
x[i] = ((int8_t)raw[2 * i]) / 128.0 + I * (((int8_t)raw[2 * i + 1]) / 128.0); |
|
break; |
|
case CHIRPMODEM_CI16: |
|
x[i] = ((int16_t)(raw[4 * i] | raw[4 * i + 1] << 8)) / 32768.0 + |
|
I * (((int16_t)(raw[4 * i + 2] | raw[4 * i + 3] << 8)) / 32768.0); |
|
break; |
|
default: { |
|
float f[2]; |
|
memcpy(f, raw + 8 * i, 8); |
|
x[i] = f[0] + I * f[1]; |
|
} |
|
} |
|
} |
|
long ny = conv_process(&s->cv, x, n, &s->y, &s->capy); |
|
s->n_in += n; |
|
s->t_conv += now() - t0; |
|
if (ny) |
|
push_modem(s, s->y, ny); |
|
return CHIRPMODEM_OK; |
|
} |
|
|
|
int chirpmodem_rx_finish(chirpmodem_rx *s) |
|
{ |
|
if (!s) |
|
return CHIRPMODEM_EINVAL; |
|
for (int i = 0; i < s->npend; i++) |
|
s->pend[i].need = 0; |
|
service(s); |
|
return CHIRPMODEM_OK; |
|
} |
|
|
|
void chirpmodem_rx_get_stats(const chirpmodem_rx *s, chirpmodem_rx_stats *st) |
|
{ |
|
memset(st, 0, sizeof *st); |
|
if (!s) |
|
return; |
|
st->frames_ok = s->n_ok; |
|
st->frames_failed = s->n_fail; |
|
st->seconds_in = s->n_in / s->cfg.fs; |
|
st->cpu_convert = s->t_conv; |
|
st->cpu_detect = s->t_det; |
|
st->cpu_decode = s->t_dec; |
|
st->effort = s->effort; |
|
st->load = s->load; |
|
} |
|
|
|
double chirpmodem_rx_delay(const chirpmodem_rx *s) |
|
{ |
|
return s ? s->t_offset : 0.0; |
|
}
|
|
|