260 lines
6.7 KiB
C
260 lines
6.7 KiB
C
/*
|
|
* generic FSK modulator & demodulator
|
|
*
|
|
* Copyright (c) 1999,2000 Fabrice Bellard.
|
|
*
|
|
* This code is released under the GNU General Public License version
|
|
* 2. Please read the file COPYING to know the exact terms of the
|
|
* license.
|
|
*/
|
|
#include "lm.h"
|
|
|
|
void FSK_mod_init(FSK_mod_state *s)
|
|
{
|
|
int b;
|
|
|
|
s->omega[0] = (PHASE_BASE * s->f_lo) / s->sample_rate;
|
|
s->omega[1] = (PHASE_BASE * s->f_hi) / s->sample_rate;
|
|
s->baud_incr = (s->baud_rate * 0x10000) / s->sample_rate;
|
|
s->phase = 0;
|
|
s->baud_frac = 0;
|
|
b = 0;
|
|
s->current_bit = b;
|
|
}
|
|
|
|
void FSK_mod(FSK_mod_state *s, s16 *samples, unsigned int nb)
|
|
{
|
|
int phase,baud_frac,b,i;
|
|
|
|
phase = s->phase;
|
|
baud_frac = s->baud_frac;
|
|
b = s->current_bit;
|
|
|
|
for(i=0;i<nb;i++) {
|
|
baud_frac += s->baud_incr;
|
|
if (baud_frac >= 0x10000) {
|
|
baud_frac -= 0x10000;
|
|
b = s->get_bit(s->opaque);
|
|
}
|
|
samples[i] = dsp_cos(phase);
|
|
phase += s->omega[b];
|
|
}
|
|
s->phase = phase;
|
|
s->baud_frac = baud_frac;
|
|
s->current_bit = b;
|
|
}
|
|
|
|
void FSK_demod_init(FSK_demod_state *s)
|
|
{
|
|
float phase;
|
|
int i, a;
|
|
|
|
s->baud_incr = (s->baud_rate * 0x10000) / s->sample_rate;
|
|
s->baud_pll = 0;
|
|
s->baud_pll_adj = s->baud_incr / 4;
|
|
|
|
s->filter_size = s->sample_rate / s->baud_rate;
|
|
|
|
memset(s->filter_buf, 0, sizeof(s->filter_buf));
|
|
s->buf_ptr = s->filter_size;
|
|
s->lastsample = 0;
|
|
|
|
/* compute the filters */
|
|
for(i=0;i<s->filter_size;i++) {
|
|
phase = 2 * M_PI * s->f_lo * i / (float)s->sample_rate;
|
|
s->filter_lo_i[i] = (int) (cos(phase) * COS_BASE);
|
|
s->filter_lo_q[i] = (int) (sin(phase) * COS_BASE);
|
|
|
|
phase = 2 * M_PI * s->f_hi * i / (float)s->sample_rate;
|
|
s->filter_hi_i[i] = (int) (cos(phase) * COS_BASE);
|
|
s->filter_hi_q[i] = (int) (sin(phase) * COS_BASE);
|
|
}
|
|
|
|
s->shift = -2;
|
|
a = s->filter_size;
|
|
while (a != 0) {
|
|
s->shift++;
|
|
a /= 2;
|
|
}
|
|
printf("shift=%d\n", s->shift);
|
|
}
|
|
|
|
void FSK_demod(FSK_demod_state *s, const s16 *samples, unsigned int nb)
|
|
{
|
|
int buf_ptr, corr, newsample, baud_pll, i;
|
|
int sum;
|
|
|
|
baud_pll = s->baud_pll;
|
|
buf_ptr = s->buf_ptr;
|
|
|
|
for(i=0;i<nb;i++) {
|
|
/* add a new sample in the demodulation filter */
|
|
s->filter_buf[buf_ptr++] = samples[i] >> s->shift;
|
|
if (buf_ptr == FSK_FILTER_BUF_SIZE) {
|
|
memmove(s->filter_buf,
|
|
s->filter_buf + FSK_FILTER_BUF_SIZE - s->filter_size,
|
|
s->filter_size * sizeof(s16));
|
|
buf_ptr = s->filter_size;
|
|
}
|
|
|
|
/* non coherent FSK demodulation - not optimal, but it seems
|
|
very difficult to do another way */
|
|
corr = dsp_dot_prod(s->filter_buf + buf_ptr - s->filter_size,
|
|
s->filter_hi_i, s->filter_size, 0);
|
|
corr = corr >> COS_BITS;
|
|
sum = corr * corr;
|
|
|
|
corr = dsp_dot_prod(s->filter_buf + buf_ptr - s->filter_size,
|
|
s->filter_hi_q, s->filter_size, 0);
|
|
corr = corr >> COS_BITS;
|
|
sum += corr * corr;
|
|
|
|
corr = dsp_dot_prod(s->filter_buf + buf_ptr - s->filter_size,
|
|
s->filter_lo_i, s->filter_size, 0);
|
|
corr = corr >> COS_BITS;
|
|
sum -= corr * corr;
|
|
|
|
corr = dsp_dot_prod(s->filter_buf + buf_ptr - s->filter_size,
|
|
s->filter_lo_q, s->filter_size, 0);
|
|
corr = corr >> COS_BITS;
|
|
sum -= corr * corr;
|
|
|
|
lm_dump_sample(CHANNEL_SAMPLESYNC, sum / 32768.0);
|
|
// printf("sum=%0.3f\n", sum / 65536.0);
|
|
newsample = sum > 0;
|
|
|
|
/* baud PLL synchronisation : when we see a transition of
|
|
frequency, we tend to modify the baud phase so that it is
|
|
in the middle of two bits */
|
|
if (s->lastsample != newsample) {
|
|
s->lastsample = newsample;
|
|
// printf("pll=%0.3f (%d)\n", baud_pll / 65536.0, newsample);
|
|
if (baud_pll < 0x8000)
|
|
baud_pll += s->baud_pll_adj;
|
|
else
|
|
baud_pll -= s->baud_pll_adj;
|
|
}
|
|
|
|
baud_pll += s->baud_incr;
|
|
|
|
if (baud_pll >= 0x10000) {
|
|
baud_pll -= 0x10000;
|
|
// printf("baud=%f (%d)\n", baud_pll / 65536.0, s->lastsample);
|
|
s->put_bit(s->opaque, s->lastsample);
|
|
}
|
|
}
|
|
|
|
s->baud_pll = baud_pll;
|
|
s->buf_ptr = buf_ptr;
|
|
}
|
|
|
|
/* test for FSK using V21 or V23 */
|
|
|
|
#define NB_SAMPLES 40
|
|
|
|
#define MAXDELAY 32
|
|
|
|
static int tx_bits[MAXDELAY], tx_ptr = 0, rx_ptr = 0;
|
|
static int tx_blank = 32;
|
|
|
|
/* transmit random bits with a sync header (31 ones, 1 zero) */
|
|
static int test_get_bit(void *opaque)
|
|
{
|
|
int bit;
|
|
|
|
if (tx_blank != 0) {
|
|
/* send 1 at the beginning for synchronization */
|
|
bit = (tx_blank > 1);
|
|
tx_blank--;
|
|
} else {
|
|
bit = random() % 2;
|
|
tx_bits[tx_ptr] = bit;
|
|
if (++tx_ptr == MAXDELAY)
|
|
tx_ptr = 0;
|
|
}
|
|
return bit;
|
|
}
|
|
|
|
static int nb_bits = 0, errors = 0, sync_count = 0, got_sync = 0;
|
|
|
|
static void test_put_bit(void *opaque, int bit)
|
|
{
|
|
int tbit;
|
|
|
|
if (!got_sync) {
|
|
|
|
if (bit) {
|
|
sync_count++;
|
|
} else {
|
|
if (sync_count > 16)
|
|
got_sync = 1;
|
|
sync_count = 0;
|
|
}
|
|
} else {
|
|
tbit = tx_bits[rx_ptr];
|
|
if (++rx_ptr == MAXDELAY)
|
|
rx_ptr = 0;
|
|
if (bit != tbit) {
|
|
errors++;
|
|
}
|
|
nb_bits++;
|
|
}
|
|
}
|
|
|
|
void FSK_test(int do_v23)
|
|
{
|
|
FSK_mod_state tx;
|
|
FSK_demod_state rx;
|
|
int err, calling;
|
|
struct LineModelState *line_state;
|
|
s16 buf[NB_SAMPLES];
|
|
s16 buf1[NB_SAMPLES];
|
|
s16 buf2[NB_SAMPLES];
|
|
s16 buf3[NB_SAMPLES];
|
|
FILE *f1;
|
|
|
|
err = lm_display_init();
|
|
if (err < 0) {
|
|
fprintf(stderr, "Could not init X display\n");
|
|
exit(1);
|
|
}
|
|
|
|
line_state = line_model_init();
|
|
|
|
f1 = fopen("cal.sw", "wb");
|
|
if (f1 == NULL) {
|
|
perror("cal.sw");
|
|
exit(1);
|
|
}
|
|
|
|
calling = 0;
|
|
if (do_v23) {
|
|
V23_mod_init(&tx, calling, test_get_bit, NULL);
|
|
V23_demod_init(&rx, 1 - calling, test_put_bit, NULL);
|
|
} else {
|
|
V21_mod_init(&tx, calling, test_get_bit, NULL);
|
|
V21_demod_init(&rx, 1 - calling, test_put_bit, NULL);
|
|
}
|
|
|
|
nb_bits = 0;
|
|
errors = 0;
|
|
for(;;) {
|
|
if (lm_display_poll_event())
|
|
break;
|
|
|
|
FSK_mod(&tx, buf, NB_SAMPLES);
|
|
memset(buf3, 0, sizeof(buf3));
|
|
|
|
line_model(line_state, buf1, buf, buf2, buf3, NB_SAMPLES);
|
|
|
|
fwrite(buf, 1, NB_SAMPLES * 2, f1);
|
|
|
|
FSK_demod(&rx, buf1, NB_SAMPLES);
|
|
}
|
|
|
|
fclose(f1);
|
|
|
|
printf("errors=%d nb_bits=%d Pe=%f\n",
|
|
errors, nb_bits, (float) errors / (float)nb_bits);
|
|
}
|