Implementation of NMT (Nordic Mobile Telephoe) network
This commit is contained in:
Andreas Eversberg
596
src/nmt/dsp.c
Normal file
596
src/nmt/dsp.c
Normal file
@@ -0,0 +1,596 @@
|
||||
/* NMT audio processing
|
||||
*
|
||||
* (C) 2016 by Andreas Eversberg <jolly@eversberg.eu>
|
||||
* All Rights Reserved
|
||||
*
|
||||
* 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 3 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, see <http://www.gnu.org/licenses/>.
|
||||
*/
|
||||
|
||||
#include <stdio.h>
|
||||
#include <stdint.h>
|
||||
#include <stdlib.h>
|
||||
#include <string.h>
|
||||
#include <errno.h>
|
||||
#include <math.h>
|
||||
#include "../common/debug.h"
|
||||
#include "../common/timer.h"
|
||||
#include "../common/call.h"
|
||||
#include "../common/goertzel.h"
|
||||
#include "nmt.h"
|
||||
#include "dsp.h"
|
||||
|
||||
#define PI M_PI
|
||||
|
||||
/* signalling */
|
||||
#define TX_PEAK_FSK 16384 /* peak amplitude of signalling FSK */
|
||||
#define TX_PEAK_SUPER 1638 /* peak amplitude of supervisory signal */
|
||||
#define BIT_RATE 1200 /* baud rate */
|
||||
#define STEPS_PER_BIT 10 /* step every 1/12000 sec */
|
||||
#define DIALTONE_HZ 425.0 /* dial tone frequency */
|
||||
#define TX_PEAK_DIALTONE 16000 /* dial tone peak */
|
||||
#define SUPER_DURATION 0.25 /* duration of supervisory signal measurement */
|
||||
#define SUPER_DETECT_COUNT 4 /* number of measures to detect supervisory signal */
|
||||
#define MUTE_DURATION 0.280 /* a tiny bit more than two frames */
|
||||
|
||||
/* two signalling tones */
|
||||
static double fsk_bits[2] = {
|
||||
1800.0,
|
||||
1200.0,
|
||||
};
|
||||
|
||||
/* two supervisory tones */
|
||||
static double super_freq[5] = {
|
||||
3955.0, /* 0-Signal 1 */
|
||||
3985.0, /* 0-Signal 2 */
|
||||
4015.0, /* 0-Signal 3 */
|
||||
4045.0, /* 0-Signal 4 */
|
||||
3900.0, /* noise level to check against */
|
||||
};
|
||||
|
||||
/* table for fast sine generation */
|
||||
int dsp_sine_super[256];
|
||||
int dsp_sine_dialtone[256];
|
||||
|
||||
/* global init for FSK */
|
||||
void dsp_init(void)
|
||||
{
|
||||
int i;
|
||||
double s;
|
||||
|
||||
PDEBUG(DFSK, DEBUG_DEBUG, "Generating sine table for supervisory signal.\n");
|
||||
for (i = 0; i < 256; i++) {
|
||||
s = sin((double)i / 256.0 * 2.0 * PI);
|
||||
dsp_sine_super[i] = (int)(s * TX_PEAK_SUPER);
|
||||
dsp_sine_dialtone[i] = (int)(s * TX_PEAK_DIALTONE);
|
||||
}
|
||||
}
|
||||
|
||||
/* Init FSK of transceiver */
|
||||
int dsp_init_sender(nmt_t *nmt)
|
||||
{
|
||||
double coeff;
|
||||
int16_t *spl;
|
||||
int i;
|
||||
|
||||
init_compander(&nmt->cstate, 8000, 3.0, 13.5);
|
||||
|
||||
if ((nmt->sender.samplerate % (BIT_RATE * STEPS_PER_BIT))) {
|
||||
PDEBUG(DFSK, DEBUG_ERROR, "Sample rate must be a multiple of %d bits per second.\n", BIT_RATE * STEPS_PER_BIT);
|
||||
return -EINVAL;
|
||||
}
|
||||
|
||||
/* this should not happen. it is implied by previous check */
|
||||
if (nmt->supervisory && nmt->sender.samplerate < 12000) {
|
||||
PDEBUG(DFSK, DEBUG_ERROR, "Sample rate must be at least 12000 Hz to process supervisory signal.\n");
|
||||
return -EINVAL;
|
||||
}
|
||||
|
||||
PDEBUG(DFSK, DEBUG_DEBUG, "Init DSP for Transceiver.\n");
|
||||
|
||||
/* allocate sample for 2 bits with 2 polarities */
|
||||
nmt->samples_per_bit = nmt->sender.samplerate / BIT_RATE;
|
||||
PDEBUG(DFSK, DEBUG_DEBUG, "Using %d samples per bit duration.\n", nmt->samples_per_bit);
|
||||
nmt->fsk_filter_step = nmt->samples_per_bit / STEPS_PER_BIT;
|
||||
PDEBUG(DFSK, DEBUG_DEBUG, "Using %d samples per filter step.\n", nmt->fsk_filter_step);
|
||||
nmt->fsk_sine[0][0] = calloc(4, nmt->samples_per_bit * sizeof(int16_t));
|
||||
nmt->fsk_sine[0][1] = nmt->fsk_sine[0][0] + nmt->samples_per_bit;
|
||||
nmt->fsk_sine[1][0] = nmt->fsk_sine[0][1] + nmt->samples_per_bit;
|
||||
nmt->fsk_sine[1][1] = nmt->fsk_sine[1][0] + nmt->samples_per_bit;
|
||||
if (!nmt->fsk_sine[0][0]) {
|
||||
PDEBUG(DFSK, DEBUG_ERROR, "No memory!\n");
|
||||
return -ENOMEM;
|
||||
}
|
||||
|
||||
/* generate sines */
|
||||
for (i = 0; i < nmt->samples_per_bit; i++) {
|
||||
nmt->fsk_sine[0][0][i] = TX_PEAK_FSK * sin(3.0 * PI * (double)i / (double)nmt->samples_per_bit); /* 1.5 waves */
|
||||
nmt->fsk_sine[0][1][i] = TX_PEAK_FSK * sin(2.0 * PI * (double)i / (double)nmt->samples_per_bit); /* 1 wave */
|
||||
nmt->fsk_sine[1][0][i] = -nmt->fsk_sine[0][0][i];
|
||||
nmt->fsk_sine[1][1][i] = -nmt->fsk_sine[0][1][i];
|
||||
}
|
||||
|
||||
/* allocate ring buffers, one bit duration */
|
||||
spl = calloc(1, nmt->samples_per_bit * sizeof(*spl));
|
||||
if (!spl) {
|
||||
PDEBUG(DFSK, DEBUG_ERROR, "No memory!\n");
|
||||
return -ENOMEM;
|
||||
}
|
||||
nmt->fsk_filter_spl = spl;
|
||||
nmt->fsk_filter_bit = -1;
|
||||
|
||||
/* allocate transmit buffer for a complete frame */
|
||||
spl = calloc(166, nmt->samples_per_bit * sizeof(*spl));
|
||||
if (!spl) {
|
||||
PDEBUG(DFSK, DEBUG_ERROR, "No memory!\n");
|
||||
return -ENOMEM;
|
||||
}
|
||||
nmt->frame_spl = spl;
|
||||
|
||||
/* allocate ring buffer for supervisory signal detection */
|
||||
nmt->super_samples = (int)((double)nmt->sender.samplerate * SUPER_DURATION + 0.5);
|
||||
spl = calloc(166, nmt->super_samples * sizeof(*spl));
|
||||
if (!spl) {
|
||||
PDEBUG(DFSK, DEBUG_ERROR, "No memory!\n");
|
||||
return -ENOMEM;
|
||||
}
|
||||
nmt->super_filter_spl = spl;
|
||||
|
||||
/* count symbols */
|
||||
for (i = 0; i < 2; i++) {
|
||||
coeff = 2.0 * cos(2.0 * PI * fsk_bits[i] / (double)nmt->sender.samplerate);
|
||||
nmt->fsk_coeff[i] = coeff * 32768.0;
|
||||
PDEBUG(DFSK, DEBUG_DEBUG, "coeff[%d] = %d\n", i, (int)nmt->fsk_coeff[i]);
|
||||
}
|
||||
|
||||
/* count supervidory tones */
|
||||
for (i = 0; i < 5; i++) {
|
||||
coeff = 2.0 * cos(2.0 * PI * super_freq[i] / (double)nmt->sender.samplerate);
|
||||
nmt->super_coeff[i] = coeff * 32768.0;
|
||||
PDEBUG(DFSK, DEBUG_DEBUG, "supervisory coeff[%d] = %d\n", i, (int)nmt->super_coeff[i]);
|
||||
|
||||
if (i < 4) {
|
||||
nmt->super_phaseshift256[i] = 256.0 / ((double)nmt->sender.samplerate / super_freq[i]);
|
||||
PDEBUG(DFSK, DEBUG_DEBUG, "phaseshift_super[%d] = %.4f\n", i, nmt->super_phaseshift256[i]);
|
||||
}
|
||||
}
|
||||
super_reset(nmt);
|
||||
|
||||
/* dial tone */
|
||||
nmt->dial_phaseshift256 = 256.0 / ((double)nmt->sender.samplerate / DIALTONE_HZ);
|
||||
|
||||
/* dtmf */
|
||||
dtmf_init(&nmt->dtmf, 8000);
|
||||
|
||||
return 0;
|
||||
}
|
||||
|
||||
/* Cleanup transceiver instance. */
|
||||
void dsp_cleanup_sender(nmt_t *nmt)
|
||||
{
|
||||
PDEBUG(DFSK, DEBUG_DEBUG, "Cleanup DSP for 'Sender'.\n");
|
||||
|
||||
if (nmt->frame_spl) {
|
||||
free(nmt->frame_spl);
|
||||
nmt->frame_spl = NULL;
|
||||
}
|
||||
if (nmt->fsk_filter_spl) {
|
||||
free(nmt->fsk_filter_spl);
|
||||
nmt->fsk_filter_spl = NULL;
|
||||
}
|
||||
if (nmt->super_filter_spl) {
|
||||
free(nmt->super_filter_spl);
|
||||
nmt->super_filter_spl = NULL;
|
||||
}
|
||||
}
|
||||
|
||||
/* Check for SYNC bits, then collect data bits */
|
||||
static void fsk_receive_bit(nmt_t *nmt, int bit, double quality, double level)
|
||||
{
|
||||
double frames_elapsed;
|
||||
|
||||
// printf("bit=%d quality=%.4f\n", bit, quality);
|
||||
if (!nmt->fsk_filter_in_sync) {
|
||||
nmt->fsk_filter_sync = (nmt->fsk_filter_sync << 1) | bit;
|
||||
|
||||
/* check if pattern 1010111100010010 matches */
|
||||
if (nmt->fsk_filter_sync != 0xaf12)
|
||||
return;
|
||||
// printf("sync\n");
|
||||
|
||||
/* sync time */
|
||||
nmt->rx_sample_count_last = nmt->rx_sample_count_current;
|
||||
nmt->rx_sample_count_current = nmt->rx_sample_count - nmt->samples_per_bit * 26;
|
||||
|
||||
/* rest sync register */
|
||||
nmt->fsk_filter_sync = 0;
|
||||
nmt->fsk_filter_in_sync = 1;
|
||||
nmt->fsk_filter_count = 0;
|
||||
nmt->fsk_filter_levelsum = 0;
|
||||
nmt->fsk_filter_qualitysum = 0;
|
||||
|
||||
/* set muting of receive path */
|
||||
nmt->fsk_filter_mute = (int)((double)nmt->sender.samplerate * MUTE_DURATION);
|
||||
return;
|
||||
}
|
||||
|
||||
/* read bits */
|
||||
nmt->fsk_filter_frame[nmt->fsk_filter_count++] = bit + '0';
|
||||
nmt->fsk_filter_levelsum += level;
|
||||
nmt->fsk_filter_qualitysum += quality;
|
||||
if (nmt->fsk_filter_count != 140)
|
||||
return;
|
||||
|
||||
/* end of frame */
|
||||
nmt->fsk_filter_frame[140] = '\0';
|
||||
nmt->fsk_filter_in_sync = 0;
|
||||
|
||||
/* send telegramm */
|
||||
frames_elapsed = (double)(nmt->rx_sample_count_current - nmt->rx_sample_count_last) / (double)(nmt->samples_per_bit * 166);
|
||||
nmt_receive_frame(nmt, nmt->fsk_filter_frame, nmt->fsk_filter_qualitysum / 140.0, nmt->fsk_filter_levelsum / 140.0, frames_elapsed);
|
||||
}
|
||||
|
||||
char *show_level(int value)
|
||||
{
|
||||
static char text[22];
|
||||
|
||||
value /= 5;
|
||||
if (value < 0)
|
||||
value = 0;
|
||||
if (value > 20)
|
||||
value = 20;
|
||||
strcpy(text, " ");
|
||||
text[value] = '*';
|
||||
|
||||
return text;
|
||||
}
|
||||
|
||||
//#define DEBUG_MODULATOR
|
||||
//#define DEBUG_FILTER
|
||||
//#define DEBUG_QUALITY
|
||||
|
||||
/* Filter one chunk of audio an detect tone, quality and loss of signal.
|
||||
* The chunk is a window of 10ms. This window slides over audio stream
|
||||
* and is processed every 1ms. (one step) */
|
||||
static inline void fsk_decode_step(nmt_t *nmt, int pos)
|
||||
{
|
||||
double level, result[2], softbit, quality;
|
||||
int max;
|
||||
int16_t *spl;
|
||||
int bit;
|
||||
|
||||
max = nmt->samples_per_bit;
|
||||
spl = nmt->fsk_filter_spl;
|
||||
|
||||
/* count time in samples*/
|
||||
nmt->rx_sample_count += nmt->fsk_filter_step;
|
||||
|
||||
level = audio_level(spl, max);
|
||||
// level = 0.63662 / 2.0;
|
||||
|
||||
audio_goertzel(spl, max, pos, nmt->fsk_coeff, result, 2);
|
||||
|
||||
/* calculate soft bit from both frequencies */
|
||||
softbit = (result[1] / level - result[0] / level + 1.0) / 2.0;
|
||||
/* scale it, since both filters overlap by some percent */
|
||||
#define MIN_QUALITY 0.33
|
||||
softbit = (softbit - MIN_QUALITY) / (1.0 - MIN_QUALITY - MIN_QUALITY);
|
||||
if (softbit > 1)
|
||||
softbit = 1;
|
||||
if (softbit < 0)
|
||||
softbit = 0;
|
||||
#ifdef DEBUG_FILTER
|
||||
// printf("|%s", show_level(result[0]/level*100));
|
||||
// printf("|%s| low=%.3f high=%.3f level=%d\n", show_level(result[1]/level*100), result[0]/level, result[1]/level, (int)level);
|
||||
printf("|%s| softbit=%.3f\n", show_level(softbit * 100), softbit);
|
||||
#endif
|
||||
if (softbit > 0.5)
|
||||
bit = 1;
|
||||
else
|
||||
bit = 0;
|
||||
|
||||
if (nmt->fsk_filter_bit != bit) {
|
||||
#ifdef DEBUG_FILTER
|
||||
puts("bit change");
|
||||
#endif
|
||||
/* if we have a bit change, reset sample counter to one half bit duration */
|
||||
nmt->fsk_filter_bit = bit;
|
||||
nmt->fsk_filter_sample = 5;
|
||||
} else if (--nmt->fsk_filter_sample == 0) {
|
||||
#ifdef DEBUG_FILTER
|
||||
puts("sample");
|
||||
#endif
|
||||
/* if sample counter bit reaches 0, we reset sample counter to one bit duration */
|
||||
// quality = result[bit] / level;
|
||||
if (softbit > 0.5)
|
||||
quality = softbit * 2.0 - 1.0;
|
||||
else
|
||||
quality = 1.0 - softbit * 2.0;
|
||||
#ifdef DEBUG_QUALITY
|
||||
printf("|%s| quality=%.2f ", show_level(softbit * 100), quality);
|
||||
printf("|%s|\n", show_level(quality * 100));
|
||||
#endif
|
||||
/* adjust level, so a peak level becomes 100% */
|
||||
fsk_receive_bit(nmt, bit, quality, level / 0.63662);
|
||||
nmt->fsk_filter_sample = 10;
|
||||
}
|
||||
}
|
||||
|
||||
/* compare supervisory signal against noise floor on 3900 Hz */
|
||||
static void super_decode(nmt_t *nmt, int16_t *samples, int length)
|
||||
{
|
||||
int coeff[2];
|
||||
double result[2], quality;
|
||||
|
||||
coeff[0] = nmt->super_coeff[nmt->supervisory - 1];
|
||||
coeff[1] = nmt->super_coeff[4]; /* noise floor detection */
|
||||
audio_goertzel(samples, length, 0, coeff, result, 2);
|
||||
|
||||
#if 0
|
||||
/* normalize levels */
|
||||
result[0] *= 32768.0 / (double)TX_PEAK_SUPER / 0.63662;
|
||||
result[1] *= 32768.0 / (double)TX_PEAK_SUPER / 0.63662;
|
||||
printf("signal=%.4f noise=%.4f\n", result[0], result[1]);
|
||||
#endif
|
||||
|
||||
quality = (result[0] - result[1]) / result[0];
|
||||
if (quality < 0)
|
||||
quality = 0;
|
||||
|
||||
if (nmt->sender.loopback)
|
||||
PDEBUG(DFSK, DEBUG_NOTICE, "Supervisory level %.2f%% quality %.0f%%\n", result[0] / 0.63662 * 100.0, quality * 100.0);
|
||||
if (quality > 0.5) {
|
||||
if (nmt->super_detected == 0) {
|
||||
nmt->super_detect_count++;
|
||||
if (nmt->super_detect_count == SUPER_DETECT_COUNT) {
|
||||
nmt->super_detected = 1;
|
||||
nmt->super_detect_count = 0;
|
||||
PDEBUG(DFSK, DEBUG_DEBUG, "Supervisory signal detected with level=%.0f%%, quality=%.0f%%.\n", result[0] / 0.63662 * 100.0, quality * 100.0);
|
||||
nmt_rx_super(nmt, 1, quality);
|
||||
}
|
||||
} else
|
||||
nmt->super_detect_count = 0;
|
||||
} else {
|
||||
if (nmt->super_detected == 1) {
|
||||
nmt->super_detect_count++;
|
||||
if (nmt->super_detect_count == SUPER_DETECT_COUNT) {
|
||||
nmt->super_detected = 0;
|
||||
nmt->super_detect_count = 0;
|
||||
PDEBUG(DFSK, DEBUG_DEBUG, "Supervisory signal lost.\n");
|
||||
nmt_rx_super(nmt, 0, 0.0);
|
||||
}
|
||||
} else
|
||||
nmt->super_detect_count = 0;
|
||||
}
|
||||
}
|
||||
|
||||
/* Reset supervisory detection states, so ongoing tone will be detected again. */
|
||||
void super_reset(nmt_t *nmt)
|
||||
{
|
||||
PDEBUG(DFSK, DEBUG_DEBUG, "Supervisory detector reset.\n");
|
||||
nmt->super_detected = 0;
|
||||
nmt->super_detect_count = 0;
|
||||
}
|
||||
|
||||
/* Process received audio stream from radio unit. */
|
||||
void sender_receive(sender_t *sender, int16_t *samples, int length)
|
||||
{
|
||||
nmt_t *nmt = (nmt_t *) sender;
|
||||
int16_t *spl;
|
||||
int max, pos, step;
|
||||
int i;
|
||||
|
||||
/* write received samples to decode buffer */
|
||||
max = nmt->super_samples;
|
||||
spl = nmt->super_filter_spl;
|
||||
pos = nmt->super_filter_pos;
|
||||
for (i = 0; i < length; i++) {
|
||||
spl[pos++] = samples[i];
|
||||
if (pos == max) {
|
||||
pos = 0;
|
||||
if (nmt->supervisory)
|
||||
super_decode(nmt, spl, max);
|
||||
}
|
||||
}
|
||||
nmt->super_filter_pos = pos;
|
||||
|
||||
/* write received samples to decode buffer */
|
||||
max = nmt->samples_per_bit;
|
||||
pos = nmt->fsk_filter_pos;
|
||||
step = nmt->fsk_filter_step;
|
||||
spl = nmt->fsk_filter_spl;
|
||||
for (i = 0; i < length; i++) {
|
||||
#ifdef DEBUG_MODULATOR
|
||||
printf("|%s|\n", show_level((int)((samples[i] / (double)TX_PEAK_FSK) * 50)+50));
|
||||
#endif
|
||||
spl[pos++] = samples[i];
|
||||
if (nmt->fsk_filter_mute) {
|
||||
samples[i] = 0;
|
||||
nmt->fsk_filter_mute--;
|
||||
}
|
||||
if (pos == max)
|
||||
pos = 0;
|
||||
/* if filter step has been reched */
|
||||
if (!(pos % step)) {
|
||||
fsk_decode_step(nmt, pos);
|
||||
}
|
||||
}
|
||||
nmt->fsk_filter_pos = pos;
|
||||
|
||||
if ((nmt->dsp_mode == DSP_MODE_AUDIO || nmt->dsp_mode == DSP_MODE_DTMF)
|
||||
&& nmt->sender.callref) {
|
||||
int16_t down[length]; /* more than enough */
|
||||
int count;
|
||||
|
||||
count = samplerate_downsample(&nmt->sender.srstate, samples, length, down);
|
||||
if (nmt->compander)
|
||||
expand_audio(&nmt->cstate, down, count);
|
||||
if (nmt->dsp_mode == DSP_MODE_DTMF)
|
||||
dtmf_tone(&nmt->dtmf, down, count);
|
||||
spl = nmt->sender.rxbuf;
|
||||
pos = nmt->sender.rxbuf_pos;
|
||||
for (i = 0; i < count; i++) {
|
||||
spl[pos++] = down[i];
|
||||
if (pos == 160) {
|
||||
call_tx_audio(nmt->sender.callref, spl, 160);
|
||||
pos = 0;
|
||||
}
|
||||
}
|
||||
nmt->sender.rxbuf_pos = pos;
|
||||
} else
|
||||
nmt->sender.rxbuf_pos = 0;
|
||||
}
|
||||
|
||||
static int fsk_frame(nmt_t *nmt, int16_t *samples, int length)
|
||||
{
|
||||
int16_t *spl;
|
||||
const char *frame;
|
||||
int i;
|
||||
int bit, polarity;
|
||||
int count, max;
|
||||
|
||||
next_frame:
|
||||
if (!nmt->frame) {
|
||||
/* request frame */
|
||||
frame = nmt_get_frame(nmt);
|
||||
if (!frame) {
|
||||
PDEBUG(DFSK, DEBUG_DEBUG, "Stop sending frames.\n");
|
||||
return length;
|
||||
}
|
||||
nmt->frame = 1;
|
||||
nmt->frame_pos = 0;
|
||||
spl = nmt->frame_spl;
|
||||
/* render frame */
|
||||
polarity = nmt->fsk_polarity;
|
||||
for (i = 0; i < 166; i++) {
|
||||
bit = (frame[i] == '1');
|
||||
memcpy(spl, nmt->fsk_sine[polarity][bit], nmt->samples_per_bit * sizeof(*spl));
|
||||
spl += nmt->samples_per_bit;
|
||||
/* flip polarity when we have 1.5 sine waves */
|
||||
if (bit == 0)
|
||||
polarity = 1 - polarity;
|
||||
}
|
||||
nmt->fsk_polarity = polarity;
|
||||
}
|
||||
|
||||
/* send audio from frame */
|
||||
max = nmt->samples_per_bit * 166;
|
||||
count = max - nmt->frame_pos;
|
||||
if (count > length)
|
||||
count = length;
|
||||
spl = nmt->frame_spl + nmt->frame_pos;
|
||||
for (i = 0; i < count; i++) {
|
||||
*samples++ = *spl++;
|
||||
}
|
||||
length -= count;
|
||||
nmt->frame_pos += count;
|
||||
/* check for end of telegramm */
|
||||
if (nmt->frame_pos == max) {
|
||||
nmt->frame = 0;
|
||||
/* we need more ? */
|
||||
if (length)
|
||||
goto next_frame;
|
||||
}
|
||||
|
||||
return length;
|
||||
}
|
||||
|
||||
/* Generate audio stream with supervisory signal. Keep phase for next call of function. */
|
||||
static void super_encode(nmt_t *nmt, int16_t *samples, int length)
|
||||
{
|
||||
double phaseshift, phase;
|
||||
int32_t sample;
|
||||
int i;
|
||||
|
||||
phaseshift = nmt->super_phaseshift256[nmt->supervisory - 1];
|
||||
phase = nmt->super_phase256;
|
||||
|
||||
for (i = 0; i < length; i++) {
|
||||
sample = *samples;
|
||||
sample += dsp_sine_super[((uint8_t)phase) & 0xff];
|
||||
if (sample > 32767)
|
||||
sample = 32767;
|
||||
else if (sample < -32767)
|
||||
sample = -32767;
|
||||
*samples++ = sample;
|
||||
phase += phaseshift;
|
||||
if (phase >= 256)
|
||||
phase -= 256;
|
||||
}
|
||||
|
||||
nmt->super_phase256 = phase;
|
||||
}
|
||||
|
||||
/* Generate audio stream from dial tone. Keep phase for next call of function. */
|
||||
static void dial_tone(nmt_t *nmt, int16_t *samples, int length)
|
||||
{
|
||||
double phaseshift, phase;
|
||||
int i;
|
||||
|
||||
phaseshift = nmt->dial_phaseshift256;
|
||||
phase = nmt->dial_phase256;
|
||||
|
||||
for (i = 0; i < length; i++) {
|
||||
*samples++ = dsp_sine_dialtone[((uint8_t)phase) & 0xff];
|
||||
phase += phaseshift;
|
||||
if (phase >= 256)
|
||||
phase -= 256;
|
||||
}
|
||||
|
||||
nmt->dial_phase256 = phase;
|
||||
}
|
||||
|
||||
/* Provide stream of audio toward radio unit */
|
||||
void sender_send(sender_t *sender, int16_t *samples, int length)
|
||||
{
|
||||
nmt_t *nmt = (nmt_t *) sender;
|
||||
int len;
|
||||
|
||||
again:
|
||||
switch (nmt->dsp_mode) {
|
||||
case DSP_MODE_AUDIO:
|
||||
case DSP_MODE_DTMF:
|
||||
jitter_load(&nmt->sender.audio, samples, length);
|
||||
if (nmt->supervisory)
|
||||
super_encode(nmt, samples, length);
|
||||
break;
|
||||
case DSP_MODE_DIALTONE:
|
||||
dial_tone(nmt, samples, length);
|
||||
break;
|
||||
case DSP_MODE_SILENCE:
|
||||
memset(samples, 0, length * sizeof(*samples));
|
||||
break;
|
||||
case DSP_MODE_FRAME:
|
||||
/* Encode frame into audio stream. If frames have
|
||||
* stopped, process again for rest of stream. */
|
||||
len = fsk_frame(nmt, samples, length);
|
||||
/* special case: add supervisory signal to frame at loop test */
|
||||
if (nmt->sender.loopback && nmt->supervisory)
|
||||
super_encode(nmt, samples, length);
|
||||
if (len) {
|
||||
samples += length - len;
|
||||
length = len;
|
||||
goto again;
|
||||
}
|
||||
break;
|
||||
}
|
||||
}
|
||||
|
||||
void nmt_set_dsp_mode(nmt_t *nmt, enum dsp_mode mode)
|
||||
{
|
||||
/* reset telegramm */
|
||||
if (mode == DSP_MODE_FRAME && nmt->dsp_mode != mode)
|
||||
nmt->frame = 0;
|
||||
nmt->dsp_mode = mode;
|
||||
}
|
||||
|
||||
Reference in New Issue
Block a user