cellular-infrastructure
/
osmocom-analog
9
1
Fork 2
Code Issues 12 Pull Requests 2 Projects Releases Wiki Activity
osmocom-analog/src/imts/dsp.c

597 lines
18 KiB
C
Raw Permalink Blame History

/* MTS/IMTS signal processing
*
* (C) 2019 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/>.
*/
#define CHAN imts->sender.kanal
#include <stdio.h>
#include <stdint.h>
#include <stdlib.h>
#include <string.h>
#include <errno.h>
#include <math.h>
#include "../libsample/sample.h"
#include "../liblogging/logging.h"
#include <osmocom/core/timer.h>
#include "../libmobile/call.h"
#include "imts.h"
#include "dsp.h"
/* uncomment to debug decoder */
//#define DEBUG_DECODER
#define PI 3.1415927
/* signaling */
#define MAX_DEVIATION 7000.0 /* signaling tone plus some extra to calibrate */
#define MAX_MODULATION 3000.0 /* FIXME */
#define SPEECH_DEVIATION 2500.0 /* deviation of speech (with emphasis) */
#define TX_PEAK_TONE (5000.0 / SPEECH_DEVIATION) /* signaling tone level (5khz, no emphasis) */
#define RX_MIN_AMPL 0.25 /* FIXME: Minimum level to detect tone */
/* Note that 75 is half of the distance between two tones (2000 and 2150 Hz)
* An error of more than 50 causes too much toggeling between two tones,
* less would take too long to detect the tone and maybe not detect it, if
* it is too far off the expected frequency.
*/
#define RX_MIN_FREQ 50.0 /* minimum frequency error to detect tone */
#define MAX_DISPLAY (MAX_DEVIATION / SPEECH_DEVIATION)/* as much as MAX_DEVIATION */
/* Note that FILTER_BW / SUSTAIN and QUAL_TIME sum up and should not exceed minimum tone length */
#define RX_FILTER_BW 100.0 /* amplitude filter (causes delay) */
#define RX_SUSTAIN 0.010 /* how long a tone must sustain until detected (causes delay) */
#define RX_QUAL_TIME 0.005 /* how long a quality measurement lasts after detecting a tone */
/* carrier loss detection */
#define MUTE_TIME 0.1 /* time to mute after loosing signal */
#define DELAY_TIME 0.15 /* delay, so we don't hear the noise before squelch mutes (MTS mode) */
#define LOSS_TIME 12.0 /* duration of signal loss before release (FIXME: what was the actual duration ???) */
#define SIGNAL_DETECT 0.3 /* time to have a steady signal to detect phone (used by MTS) */
#define DISPLAY_INTERVAL 0.04
/* signaling tones to be modulated */
static double tones[NUM_SIG_TONES] = {
2150.0, /* guard */
2000.0, /* idle */
1800.0, /* seize */
1633.0, /* connect */
1336.0, /* disconnect */
600.0, /* MTS 600 Hz */
1500.0, /* MTS 1500 Hz */
};
/* signaling tones to be demodulated (a subset of tones above) */
static double tone_response[NUM_SIG_TONES] = {
0.71, /* guard */
1.08, /* idle */
1.01, /* seize */
1.04, /* connect */
0.71, /* disconnect */
0.71, /* MTS 600 Hz */
0.71, /* MTS 1500 Hz */
};
/* all tone, with signaling tones first */
static const char *tone_names[] = {
"GUARD tone",
"IDLE tone",
"SEIZE tone",
"CONNECT tone",
"DISCONNECT tone",
"600 Hz Tone",
"1500 Hz Tone",
"SILENCE",
"NOISE",
"DIALTONE",
};
#define DIALTONE1 350.0
#define DIALTONE2 440.0
/* table for fast sine generation */
static sample_t dsp_sine_tone[65536];
/* global init for audio processing */
void dsp_init(void)
{
int i;
double s;
LOGP(DDSP, LOGL_DEBUG, "Generating sine tables.\n");
for (i = 0; i < 65536; i++) {
s = sin((double)i / 65536.0 * 2.0 * PI);
dsp_sine_tone[i] = s * TX_PEAK_TONE;
}
}
/* Init transceiver instance. */
int dsp_init_transceiver(imts_t *imts, double squelch_db, int ptt)
{
int rc = -1;
LOGP_CHAN(DDSP, LOGL_DEBUG, "Init DSP for Transceiver.\n");
imts->sample_duration = 1.0 / (double)imts->sender.samplerate;
/* init squelch */
squelch_init(&imts->squelch, imts->sender.kanal, squelch_db, MUTE_TIME, LOSS_TIME);
/* set modulation parameters */
sender_set_fm(&imts->sender, MAX_DEVIATION, MAX_MODULATION, SPEECH_DEVIATION, MAX_DISPLAY);
/* init FM demodulator for tone detection */
if (imts->mode == MODE_IMTS) {
imts->demod_center = (tones[TONE_GUARD] + tones[TONE_DISCONNECT]) / 2.0;
imts->demod_bandwidth = tones[TONE_GUARD] - tones[TONE_DISCONNECT];
} else {
imts->demod_center = (tones[TONE_1500] + tones[TONE_600]) / 2.0;
imts->demod_bandwidth = tones[TONE_1500] - tones[TONE_600];
}
rc = fm_demod_init(&imts->demod, (double)imts->sender.samplerate, imts->demod_center, imts->demod_bandwidth);
if (rc < 0)
goto error;
/* use fourth order (2 iter) filter, since it is as fast as second order (1 iter) filter */
/* NOTE: CHANGE TONE RESPONSES ABOVE, IF YOU CHANGE FILTER */
iir_lowpass_init(&imts->demod_freq_lp, RX_FILTER_BW, (double)imts->sender.samplerate, 2);
iir_lowpass_init(&imts->demod_ampl_lp, RX_FILTER_BW, (double)imts->sender.samplerate, 2);
/* tones */
imts->tone_idle_phaseshift65536 = 65536.0 / ((double)imts->sender.samplerate / tones[TONE_IDLE]);
imts->tone_seize_phaseshift65536 = 65536.0 / ((double)imts->sender.samplerate / tones[TONE_SEIZE]);
imts->tone_600_phaseshift65536 = 65536.0 / ((double)imts->sender.samplerate / tones[TONE_600]);
imts->tone_1500_phaseshift65536 = 65536.0 / ((double)imts->sender.samplerate / tones[TONE_1500]);
imts->tone_dialtone_phaseshift65536[0] = 65536.0 / ((double)imts->sender.samplerate / DIALTONE1);
imts->tone_dialtone_phaseshift65536[1] = 65536.0 / ((double)imts->sender.samplerate / DIALTONE2);
/* demod init */
imts->demod_current_tone = TONE_SILENCE;
imts->demod_sig_tone = 0;
imts->demod_last_tone = TONE_SILENCE;
/* delay buffer */
if (ptt) {
LOGP_CHAN(DDSP, LOGL_DEBUG, "Push to talk: Adding delay buffer to remove noise when signal gets lost.\n");
imts->delay_max = (int)((double)imts->sender.samplerate * DELAY_TIME);
imts->delay_spl = calloc(imts->delay_max, sizeof(*imts->delay_spl));
if (!imts->delay_spl) {
LOGP(DDSP, LOGL_ERROR, "No mem for delay buffer!\n");
goto error;
}
}
imts->dmp_tone_level = display_measurements_add(&imts->sender.dispmeas, "Tone Level", "%.1f %%", DISPLAY_MEAS_LAST, DISPLAY_MEAS_LEFT, 0.0, 150.0, 100.0);
imts->dmp_tone_quality = display_measurements_add(&imts->sender.dispmeas, "Tone Quality", "%.1f %%", DISPLAY_MEAS_LAST, DISPLAY_MEAS_LEFT, 0.0, 150.0, 100.0);
return 0;
error:
dsp_cleanup_transceiver(imts);
return rc;
}
/* Cleanup transceiver instance. */
void dsp_cleanup_transceiver(imts_t *imts)
{
LOGP_CHAN(DDSP, LOGL_DEBUG, "Cleanup DSP for Transceiver.\n");
fm_demod_exit(&imts->demod);
if (imts->delay_spl) {
free(imts->delay_spl);
imts->delay_spl = NULL;
}
}
/* Generate audio stream from tone. Keep phase for next call of function. */
static int generate_tone(imts_t *imts, sample_t *samples, int length)
{
double phaseshift[2] = {0,0}, phase[2]; // make gcc happy
int i;
switch (imts->tone) {
case TONE_IDLE:
phaseshift[0] = imts->tone_idle_phaseshift65536;
break;
case TONE_SEIZE:
phaseshift[0] = imts->tone_seize_phaseshift65536;
break;
case TONE_600:
phaseshift[0] = imts->tone_600_phaseshift65536;
break;
case TONE_1500:
phaseshift[0] = imts->tone_1500_phaseshift65536;
break;
case TONE_DIALTONE:
phaseshift[0] = imts->tone_dialtone_phaseshift65536[0];
phaseshift[1] = imts->tone_dialtone_phaseshift65536[1];
break;
case TONE_SILENCE:
break;
default:
LOGP_CHAN(DDSP, LOGL_ERROR, "Software error, unsupported tone, please fix!\n");
return length;
}
/* don't send more than given length */
if (imts->tone_duration && length > imts->tone_duration)
length = imts->tone_duration;
phase[0] = imts->tone_phase65536[0];
phase[1] = imts->tone_phase65536[1];
switch (imts->tone) {
case TONE_SILENCE:
memset(samples, 0, length * sizeof(*samples));
phase[0] = phase[1] = 0;
break;
case TONE_DIALTONE:
for (i = 0; i < length; i++) {
*samples = dsp_sine_tone[(uint16_t)phase[0]];
*samples += dsp_sine_tone[(uint16_t)phase[1]];
*samples++ /= 4.0; /* not full volume */
phase[0] += phaseshift[0];
if (phase[0] >= 65536)
phase[0] -= 65536;
phase[1] += phaseshift[1];
if (phase[1] >= 65536)
phase[1] -= 65536;
}
break;
default:
for (i = 0; i < length; i++) {
*samples++ = dsp_sine_tone[(uint16_t)phase[0]];
phase[0] += phaseshift[0];
if (phase[0] >= 65536)
phase[0] -= 65536;
}
}
imts->tone_phase65536[0] = phase[0];
imts->tone_phase65536[1] = phase[1];
/* if tone has been sent completely, tell IMTS call process */
if (imts->tone_duration) {
imts->tone_duration -= length;
if (imts->tone_duration == 0)
imts_tone_sent(imts, imts->tone);
}
return length;
}
/* Provide stream of audio toward radio unit */
void sender_send(sender_t *sender, sample_t *samples, uint8_t *power, int length)
{
imts_t *imts = (imts_t *) sender;
int count, input_num;
memset(power, 1, length);
again:
switch (imts->dsp_mode) {
case DSP_MODE_OFF:
memset(power, 0, length);
memset(samples, 0, length * sizeof(*samples));
break;
case DSP_MODE_TONE:
memset(power, 1, length);
count = generate_tone(imts, samples, length);
samples += count;
length -= count;
if (length)
goto again;
break;
case DSP_MODE_AUDIO:
memset(power, 1, length);
input_num = samplerate_upsample_input_num(&sender->srstate, length);
{
int16_t spl[input_num];
jitter_load_samples(&sender->dejitter, (uint8_t *)spl, input_num, sizeof(*spl), jitter_conceal_s16, NULL);
int16_to_samples_speech(samples, spl, input_num);
}
samplerate_upsample(&sender->srstate, samples, input_num, samples, length);
if (imts->pre_emphasis)
pre_emphasis(&imts->estate, samples, length);
break;
}
}
static void tone_demod(imts_t *imts, sample_t *samples, int length)
{
sample_t frequency[length], amplitude[length];
sample_t I[length], Q[length];
double f, amp;
int i, t, tone = 0; // make GCC happy
/* demod frquency */
fm_demodulate_real(&imts->demod, frequency, length, samples, I, Q);
iir_process(&imts->demod_freq_lp, frequency, length);
/* demod amplitude
* peak amplitude is the length of I/Q vector
* since we filter out the unwanted modulation product, the vector is only half of length
*/
for (i = 0; i < length; i++)
amplitude[i] = sqrt(I[i] * I[i] + Q[i] * Q[i]) * 2.0;
iir_process(&imts->demod_ampl_lp, amplitude, length);
/* check result to detect tone or loss or change */
for (i = 0; i < length; i++) {
/* duration until change in tone */
imts->demod_duration += imts->sample_duration;
/* correct FSK amplitude */
amplitude[i] /= TX_PEAK_TONE;
/* see what we detect at this moment; tone is set */
if (amplitude[i] < RX_MIN_AMPL) {
/* silence */
tone = TONE_SILENCE;
} else {
/* tone */
f = frequency[i] + imts->demod_center;
if (imts->mode == MODE_IMTS) {
for (t = TONE_IDLE; t <= TONE_DISCONNECT; t++) {
/* check for frequency */
if (fabs(f - tones[t]) < RX_MIN_FREQ) {
tone = t;
break;
}
}
} else {
for (t = TONE_600; t <= TONE_1500; t++) {
/* check for frequency */
if (fabs(f - tones[t]) < RX_MIN_FREQ) {
tone = t;
break;
}
}
}
/* noise */
if (t == NUM_SIG_TONES)
tone = TONE_NOISE;
}
#ifdef DEBUG_DECODER
/* debug decoder */
if (tone < NUM_SIG_TONES) {
static int debug_interval = 0;
if (++debug_interval == 30) {
debug_interval = 0;
printf("decoder debug: diff=%s %.0f ", debug_amplitude((f - tones[t]) / RX_MIN_FREQ), fabs(f - tones[t]));
amp = amplitude[i] / tone_response[t];
printf("ampl=%s %.0f%%\n", debug_amplitude(amp), amp * 100.0);
}
}
#endif
/* display level of tones, or zero at noise/silence */
imts->display_interval +=imts->sample_duration;
if (imts->display_interval >= DISPLAY_INTERVAL) {
if (tone < NUM_SIG_TONES)
amp = amplitude[i] / tone_response[tone];
else
amp = 0.0;
display_measurements_update(imts->dmp_tone_level, amp * 100.0, 0.0);
imts->display_interval -= DISPLAY_INTERVAL;
}
/* check for tone change */
if (tone != imts->demod_current_tone) {
#ifdef DEBUG_DECODER
printf("decoder debug: %s detected, waiting to sustain\n", tone_names[tone]);
#endif
if (imts->demod_sig_tone) {
LOGP_CHAN(DDSP, LOGL_DEBUG, "Lost %s (duration %.0f ms)\n", tone_names[imts->demod_current_tone], imts->demod_duration * 1000.0);
imts_lost_tone(imts, imts->demod_current_tone, imts->demod_duration);
imts->demod_sig_tone = 0;
}
imts->demod_current_tone = tone;
imts->demod_sustain = 0.0;
} else if (imts->demod_sustain < RX_SUSTAIN) {
imts->demod_sustain += imts->sample_duration;
/* when sustained. also tone must change to prevent flapping; lost signaling tone can be detected again */
if (imts->demod_sustain >= RX_SUSTAIN && (imts->demod_current_tone != imts->demod_last_tone || !imts->demod_sig_tone)) {
if (imts->demod_current_tone < NUM_SIG_TONES) {
amp = amplitude[i] / tone_response[imts->demod_current_tone];
imts->demod_sig_tone = 1;
imts->demod_quality_time = 0.0;
imts->demod_quality_count = 0;
imts->demod_quality_value = 0.0;
} else {
amp = amplitude[i];
imts->demod_sig_tone = 0;
}
LOGP_CHAN(DDSP, LOGL_DEBUG, "Detected %s (level %.0f%%)\n", tone_names[imts->demod_current_tone], amp * 100);
imts_receive_tone(imts, imts->demod_current_tone, imts->demod_duration, amp);
imts->demod_last_tone = imts->demod_current_tone;
imts->demod_duration = imts->demod_sustain;
} else if (imts->demod_sig_tone && imts->demod_quality_time < RX_QUAL_TIME) {
f = frequency[i] + imts->demod_center;
imts->demod_quality_time += imts->sample_duration;
imts->demod_quality_count++;
imts->demod_quality_value += fabs((f - tones[imts->demod_current_tone])) / RX_MIN_FREQ;
if (imts->demod_quality_time >= RX_QUAL_TIME) {
double quality = 1.0 - imts->demod_quality_value / (double)imts->demod_quality_count * 2.0;
if (quality < 0)
quality = 0;
LOGP_CHAN(DDSP, LOGL_DEBUG, "Quality: %.0f%%\n", quality * 100.0);
display_measurements_update(imts->dmp_tone_quality, quality * 100.0, 0.0);
}
}
}
}
}
static void delay_audio(imts_t *imts, sample_t *samples, int count)
{
sample_t *spl, s;
int pos, max;
int i;
spl = imts->delay_spl;
pos = imts->delay_pos;
max = imts->delay_max;
/* feed audio though delay buffer */
for (i = 0; i < count; i++) {
s = samples[i];
samples[i] = spl[pos];
spl[pos] = s;
if (++pos == max)
pos = 0;
}
imts->delay_pos = pos;
}
/* Process received audio stream from radio unit. */
void sender_receive(sender_t *sender, sample_t *samples, int length, double rf_level_db)
{
imts_t *imts = (imts_t *) sender;
/* no processing if off */
if (imts->dsp_mode == DSP_MODE_OFF)
return;
/* process signal mute/loss, also for signalling tone */
switch (squelch(&imts->squelch, rf_level_db, (double)length * imts->sample_duration)) {
case SQUELCH_LOSS:
imts_loss_indication(imts, LOSS_TIME);
/* FALLTHRU */
case SQUELCH_MUTE:
if (imts->mode == MODE_MTS && !imts->is_mute) {
LOGP_CHAN(DDSP, LOGL_INFO, "Low RF level, muting.\n");
memset(imts->delay_spl, 0, sizeof(*samples) * imts->delay_max);
imts->is_mute = 1;
}
memset(samples, 0, sizeof(*samples) * length);
/* signal is gone */
imts->rf_signal = 0;
break;
default:
if (imts->is_mute) {
LOGP_CHAN(DDSP, LOGL_INFO, "High RF level, unmuting.\n");
imts->is_mute = 0;
}
/* detect signal, if it is steady for a while */
if (imts->rf_signal < SIGNAL_DETECT * (double)imts->sender.samplerate) {
/* only do this, if signal has not been detected yet */
imts->rf_signal += length;
if (imts->rf_signal >= SIGNAL_DETECT * (double)imts->sender.samplerate)
imts_signal_indication(imts);
}
break;
}
/* FM/AM demod */
tone_demod(imts, samples, length);
/* delay audio to prevent noise before squelch mutes (don't do that for signaling tones) */
if (imts->delay_spl)
delay_audio(imts, samples, length);
/* Forward audio to network (call process). */
if (imts->dsp_mode == DSP_MODE_AUDIO && imts->callref) {
sample_t *spl;
int pos;
int count;
int i;
if (imts->de_emphasis) {
dc_filter(&imts->estate, samples, length);
de_emphasis(&imts->estate, samples, length);
}
count = samplerate_downsample(&imts->sender.srstate, samples, length);
spl = imts->sender.rxbuf;
pos = imts->sender.rxbuf_pos;
for (i = 0; i < count; i++) {
spl[pos++] = samples[i];
if (pos == 160) {
call_up_audio(imts->callref, spl, 160);
pos = 0;
}
}
imts->sender.rxbuf_pos = pos;
} else
imts->sender.rxbuf_pos = 0;
}
static const char *imts_dsp_mode_name(enum dsp_mode mode)
{
static char invalid[16];
switch (mode) {
case DSP_MODE_OFF:
return "TRX OFF";
case DSP_MODE_TONE:
return "TONE";
case DSP_MODE_AUDIO:
return "AUDIO";
}
sprintf(invalid, "invalid(%d)", mode);
return invalid;
}
void imts_set_dsp_mode(imts_t *imts, enum dsp_mode mode, int tone, double duration, int reset_demod)
{
/* reset demod */
if (reset_demod) {
imts->demod_current_tone = TONE_SILENCE;
imts->demod_sig_tone = 0;
imts->demod_last_tone = TONE_SILENCE;
imts->demod_duration = 0.0;
}
if (mode == DSP_MODE_AUDIO && imts->dsp_mode != mode)
jitter_reset(&imts->sender.dejitter);
if (imts->dsp_mode != mode) {
LOGP_CHAN(DDSP, LOGL_DEBUG, "DSP mode %s -> %s\n", imts_dsp_mode_name(imts->dsp_mode), imts_dsp_mode_name(mode));
imts->dsp_mode = mode;
}
if (mode == DSP_MODE_TONE) {
if (duration)
LOGP_CHAN(DDSP, LOGL_DEBUG, "Start sending %s for %.3f seconds.\n", tone_names[tone], duration);
else
LOGP_CHAN(DDSP, LOGL_DEBUG, "Start sending %s continuously.\n", tone_names[tone]);
imts->tone = tone;
imts->tone_duration = duration * (double)imts->sender.samplerate;
}
}
/* Receive audio from call instance. */
void call_down_audio(void *decoder, void *decoder_priv, int callref, uint16_t sequence, uint8_t marker, uint32_t timestamp, uint32_t ssrc, uint8_t *payload, int payload_len)
{
sender_t *sender;
imts_t *imts;
for (sender = sender_head; sender; sender = sender->next) {
imts = (imts_t *) sender;
if (imts->callref == callref)
break;
}
if (!sender)
return;
if (imts->dsp_mode == DSP_MODE_AUDIO) {
jitter_frame_t *jf;
jf = jitter_frame_alloc(decoder, decoder_priv, payload, payload_len, marker, sequence, timestamp, ssrc);
if (jf)
jitter_save(&imts->sender.dejitter, jf);
}
}
void call_down_clock(void) {}
Reference in New Issue View Git Blame Copy Permalink