VK3FNG/soundmodem
1
0
Fork 0
mirror of https://github.com/VK3FNG/soundmodem.git synced 2025-12-06 03:01:59 +01:00
Code Issues Actions Packages Projects Releases Wiki Activity
soundmodem/afsk/modem.c
Matt Grice f4c4438e0e First commit
2018-06-17 22:19:22 +10:00

447 lines
14 KiB
C
Raw Blame History

/*****************************************************************************/
/*
* modem.c -- Linux Userland Soundmodem AFSK modem.
*
* Copyright (C) 1998-2000, 2003
* Thomas Sailer <t.sailer@alumni.ethz.ch>
*
* 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., 675 Mass Ave, Cambridge, MA 02139, USA.
*
*
*/
/*****************************************************************************/
#include <math.h>
#include <stdlib.h>
#include <string.h>
#include <stdarg.h>
#include "modem.h"
#include "costab.h"
/* --------------------------------------------------------------------- */
struct modstate {
struct modemchannel *chan;
unsigned int bps, f0, f1, notdiff, maxbitlen;
unsigned int bitinc, bitph;
unsigned int dds, ddsinc[2];
unsigned int bit;
};
static const struct modemparams modparams[] = {
{ "bps", "Bits/s", "Bits per second", "1200", MODEMPAR_NUMERIC, { n: { 100, 9600, 100, 1200 } } },
{ "f0", "Frequency 0", "Frequency 0", "1200", MODEMPAR_NUMERIC, { n: { 0, 38400, 100, 1000 } } },
{ "f1", "Frequency 1", "Frequency 1", "2200", MODEMPAR_NUMERIC, { n: { 0, 38400, 100, 1000 } } },
{ "diffenc", "Differential Encoding", "Enable for differentially encoded waveforms (normally on!)", "1", MODEMPAR_CHECKBUTTON },
{ NULL }
};
static void *modconfig(struct modemchannel *chan, unsigned int *samplerate, const char *params[])
{
struct modstate *s;
if (!(s = malloc(sizeof(struct modstate))))
logprintf(MLOG_FATAL, "out of memory\n");
s->chan = chan;
if (params[0]) {
s->bps = strtoul(params[0], NULL, 0);
if (s->bps < 100)
s->bps = 100;
if (s->bps > 9600)
s->bps= 9600;
} else
s->bps = 1200;
if (params[1]) {
s->f0 = strtoul(params[1], NULL, 0);
if (s->f0 > s->bps * 4)
s->f0 = s->bps * 4;
} else
s->f0 = 1200;
if (params[2]) {
s->f1 = strtoul(params[2], NULL, 0);
if (s->f1 > s->bps * 4)
s->f1 = s->bps * 4;
} else
s->f1 = 2200;
s->notdiff = params[3] ? !strtoul(params[3], NULL, 0) : 0;
*samplerate = 8 * s->bps;
return s;
}
static void modinit(void *state, unsigned int samplerate)
{
struct modstate *s = (struct modstate *)state;
s->maxbitlen = (samplerate + s->bps - 1) / s->bps;
s->bitinc = (s->bps << 16) / samplerate;
s->ddsinc[0] = (s->f0 << 16) / samplerate;
s->ddsinc[1] = (s->f1 << 16) / samplerate;
s->bit = 0;
}
static void modsendbits(struct modstate *s, unsigned int bits, unsigned int nrsyms)
{
int16_t sbuf[512];
int16_t *sptr = sbuf, *eptr = sbuf + sizeof(sbuf)/sizeof(sbuf[0]);
while (nrsyms > 0) {
if (s->bitph >= 0x10000) {
s->bitph &= 0xffff;
s->bit = (~((s->bit | s->notdiff) ^ bits)) & 1;
bits >>= 1;
nrsyms--;
}
*sptr++ = COS(s->dds);
s->dds += s->ddsinc[s->bit];
s->bitph += s->bitinc;
if (sptr >= eptr) {
audiowrite(s->chan, sbuf, sptr - sbuf);
sptr = sbuf;
}
}
audiowrite(s->chan, sbuf, sptr - sbuf);
}
static void modmodulate(void *state, unsigned int txdelay)
{
struct modstate *s = (struct modstate *)state;
int i;
unsigned char ch;
i = txdelay * s->bps / 1000;
do {
modsendbits(s, 0x7e, 8);
i -= 8;
} while (i > 0);
while (pktget(s->chan, &ch, 1))
modsendbits(s, ch, 8);
modsendbits(s, 0x7e, 8);
modsendbits(s, 0x7e, 8);
}
struct modulator afskmodulator = {
NULL,
"afsk",
modparams,
modconfig,
modinit,
modmodulate,
free
};
/* --------------------------------------------------------------------- */
#define max(a, b) (((a) > (b)) ? (a) : (b))
/* RxFilter */
#define WINDOWEXPAND 1.5
#define RXFILTLEN 16
#define RXFILTOVERBITS 3
#define RXFILTOVER (1<<(RXFILTOVERBITS))
#define RXFILTFIDX(x) (((x)>>(16-(RXFILTOVERBITS)))&(RXFILTOVER-1))
#define RXFILTFSAMP(x) ((x)>>16)
struct demodstate {
struct modemchannel *chan;
unsigned int bps, f0, f1, notdiff, firlen;
unsigned int srate;
unsigned int rxbits;
unsigned int rxphase;
unsigned int rxphinc;
int dcdcnt;
unsigned int dcdtim;
unsigned int div, divcnt;
unsigned int pllinc, pll, pllthresh, pllcorr;
unsigned int shreg, lastbit;
int dcd_sum0, dcd_sum1, dcd_sum2;
unsigned int dcd_time;
u_int32_t raws, rawb;
unsigned char dcd;
union {
struct {
int32_t f0r[RXFILTOVER][RXFILTLEN];
int32_t f0i[RXFILTOVER][RXFILTLEN];
int32_t f1r[RXFILTOVER][RXFILTLEN];
int32_t f1i[RXFILTOVER][RXFILTLEN];
} f32;
struct {
int16_t f0r[RXFILTOVER][RXFILTLEN];
int16_t f0i[RXFILTOVER][RXFILTLEN];
int16_t f1r[RXFILTOVER][RXFILTLEN];
int16_t f1i[RXFILTOVER][RXFILTLEN];
} f16;
} f;
};
/* --------------------------------------------------------------------- */
static const struct modemparams demodparams[] = {
{ "bps", "Bits/s", "Bits per second", "1200", MODEMPAR_NUMERIC, { n: { 100, 9600, 100, 1200 } } },
{ "f0", "Frequency 0", "Frequency 0", "1200", MODEMPAR_NUMERIC, { n: { 0, 38400, 100, 1000 } } },
{ "f1", "Frequency 1", "Frequency 1", "2200", MODEMPAR_NUMERIC, { n: { 0, 38400, 100, 1000 } } },
{ "diffdec", "Differential Decoding", "Enable for differentially encoded waveforms (normally on!)", "1", MODEMPAR_CHECKBUTTON },
{ NULL }
};
static void *demodconfig(struct modemchannel *chan, unsigned int *samplerate, const char *params[])
{
struct demodstate *s;
unsigned int f;
if (!(s = malloc(sizeof(struct demodstate))))
logprintf(MLOG_FATAL, "out of memory\n");
s->chan = chan;
if (params[0]) {
s->bps = strtoul(params[0], NULL, 0);
if (s->bps < 100)
s->bps = 100;
if (s->bps > 9600)
s->bps= 9600;
} else
s->bps = 1200;
if (params[1]) {
s->f0 = strtoul(params[1], NULL, 0);
if (s->f0 > s->bps * 4)
s->f0 = s->bps * 4;
} else
s->f0 = 1200;
if (params[2]) {
s->f1 = strtoul(params[2], NULL, 0);
if (s->f1 > s->bps * 4)
s->f1 = s->bps * 4;
} else
s->f1 = 2200;
s->notdiff = params[3] ? !strtoul(params[3], NULL, 0) : 0;
f = s->f0;
if (s->f1 > f)
f = s->f1;
f += s->bps/2;
f = (2*f) + (f >> 1);
*samplerate = f;
return s;
}
static int demfilter(struct demodstate *state, const int16_t *val, unsigned int phase)
{
unsigned int fidx = RXFILTFIDX(phase);
const int *c0r = state->f.f32.f0r[fidx];
const int *c0i = state->f.f32.f0i[fidx];
const int *c1r = state->f.f32.f1r[fidx];
const int *c1i = state->f.f32.f1i[fidx];
const int16_t *v1, *v2;
unsigned int i;
int st = 0, s;
v1 = val + RXFILTFSAMP(phase);
for (v2 = v1, s = 0, i = 0; i < RXFILTLEN; i++, v2++, c1r++)
s += (*v2) * (*c1r);
s >>= 16;
st = s * s;
for (v2 = v1, s = 0, i = 0; i < RXFILTLEN; i++, v2++, c1i++)
s += (*v2) * (*c1i);
s >>= 16;
st += s * s;
for (v2 = v1, s = 0, i = 0; i < RXFILTLEN; i++, v2++, c0r++)
s += (*v2) * (*c0r);
s >>= 16;
st -= s * s;
for (v2 = v1, s = 0, i = 0; i < RXFILTLEN; i++, v2++, c0i++)
s += (*v2) * (*c0i);
s >>= 16;
st -= s * s;
return st;
}
static void demod8bits(struct demodstate *s)
{
int16_t *samples;
unsigned int phinc = s->rxphinc;
unsigned int halfphinc = phinc >> 1;
unsigned int dcdinterv = phinc >> 2;
unsigned int phasecorr = phinc >> 4;
unsigned int totsamp = ((12 * phinc) >> 16) + RXFILTLEN;
unsigned int phase, nr;
int oldv, newv, midv, thv;
samples = alloca(totsamp * sizeof(samples[0]));
audioread(s->chan, samples, totsamp, s->rxphase >> 16);
phase = s->rxphase & 0xffff;
s->rxphase &= ~0xffff;
oldv = demfilter(s, samples, phase);
for (nr = 0; nr < 8; nr++) {
if (logcheck(260)) {
unsigned int i;
for (i = 0; i < 8; i++)
logprintf(260, "afsk: rxv[%1u]: %6d\n", i,
demfilter(s, samples, phase + ((i * phinc) >> 3)) >> 16);
}
phase += phinc;
newv = demfilter(s, samples, phase);
s->rxbits >>= 1;
if (!((newv > 0) ^ ((oldv > 0) | s->notdiff)))
s->rxbits |= 0x80;
else {
midv = demfilter(s, samples, phase - halfphinc);
if ((oldv > 0) ^ (midv > 0)) {
thv = demfilter(s, samples, phase - halfphinc - dcdinterv);
phase -= phasecorr;
} else {
thv = demfilter(s, samples, phase - halfphinc + dcdinterv);
phase += phasecorr;
}
if ((midv > 0) ^ (thv > 0))
s->dcdcnt++;
else
s->dcdcnt -= 2;
}
logprintf(258, "afsk: rx: oldv %10d midv %10d newv %10d\n", oldv, midv, newv);
oldv = newv;
s->dcdtim++;
}
s->rxphase += phase;
}
static void demoddemodulate(void *state)
{
struct demodstate *s = (struct demodstate *)state;
int dcd0 = -2, dcd1 = -2, dcd2 = -2;
unsigned char buf;
char buf2[9];
unsigned int i;
s->dcdcnt = -2;
s->dcdtim = 0;
s->rxphase = audiocurtime(s->chan) << 16;
for (;;) {
demod8bits(s);
buf = s->rxbits;
pktput(s->chan, &buf, 1);
if (logcheck(256)) {
for (i = 0; i < 8; i++)
buf2[i] = '0' + ((buf >> i) & 1);
buf2[8] = 0;
logprintf(256, "afskrx: %s\n", buf2);
}
#if 0
for (i = 0; i < 8; i++)
putchar('0' + ((buf >> i) & 1));
fflush(stdout);
#endif
if (s->dcdtim < 120)
continue;
dcd0 = dcd1;
dcd1 = dcd2;
dcd2 = s->dcdcnt;
s->dcdcnt = -2;
s->dcdtim = 0;
pktsetdcd(s->chan, (dcd0 + dcd1 + dcd2) > 0);
}
}
static inline double sinc(double x)
{
double arg = x * M_PI;
if (arg == 0)
return 1;
return sin(arg) / arg;
}
static inline double hamming(double x)
{
return 0.54-0.46*cos((2*M_PI)*x);
}
static void demodinit(void *state, unsigned int samplerate, unsigned int *bitrate)
{
struct demodstate *s = (struct demodstate *)state;
float f0r[RXFILTLEN*RXFILTOVER];
float f0i[RXFILTLEN*RXFILTOVER];
float f1r[RXFILTLEN*RXFILTOVER];
float f1i[RXFILTLEN*RXFILTOVER];
double ph0, ph1, w, tscale;
float max, max0, max1, max2, max3;
unsigned int i, j;
tscale = (float)s->bps / (float)samplerate / RXFILTOVER / WINDOWEXPAND;
ph0 = 2.0 * M_PI * (float)s->f0 / (float)samplerate / RXFILTOVER;
ph1 = 2.0 * M_PI * (float)s->f1 / (float)samplerate / RXFILTOVER;
for (i = 0; i < RXFILTLEN * RXFILTOVER; i++) {
w = i * tscale;
if (w > 1)
w = 0;
else
w = hamming(w);
f0r[i] = w * cos(ph0 * i);
f0i[i] = w * sin(ph0 * i);
f1r[i] = w * cos(ph1 * i);
f1i[i] = w * sin(ph1 * i);
}
/* determine maximum */
max = 0;
for (i = 0; i < RXFILTOVER; i++) {
max0 = max1 = max2 = max3 = 0;
for (j = i; j < RXFILTLEN * RXFILTOVER; j += RXFILTOVER) {
max0 += fabs(f0r[j]);
max1 += fabs(f0i[j]);
max2 += fabs(f1r[j]);
max3 += fabs(f1i[j]);
}
if (max0 > max)
max = max0;
if (max1 > max)
max = max1;
if (max2 > max)
max = max2;
if (max3 > max)
max = max3;
}
w = 32767 / max;
for (i = 0; i < RXFILTLEN * RXFILTOVER; i++) {
f0r[i] *= w;
f0i[i] *= w;
f1r[i] *= w;
f1i[i] *= w;
}
for (i = 0; i < RXFILTOVER; i++)
for (j = 0; j < RXFILTLEN; j++) {
s->f.f32.f0r[RXFILTOVER-1-i][j] = f0r[j*RXFILTOVER+i];
s->f.f32.f0i[RXFILTOVER-1-i][j] = f0i[j*RXFILTOVER+i];
s->f.f32.f1r[RXFILTOVER-1-i][j] = f1r[j*RXFILTOVER+i];
s->f.f32.f1i[RXFILTOVER-1-i][j] = f1i[j*RXFILTOVER+i];
}
s->srate = samplerate;
s->rxphinc = ((samplerate << 16) + s->bps / 2) / s->bps;
*bitrate = s->bps;
}
struct demodulator afskdemodulator = {
NULL,
"afsk",
demodparams,
demodconfig,
demodinit,
demoddemodulate,
free
};
/* --------------------------------------------------------------------- */
Reference in a new issue View git blame Copy permalink