94 lines
2.6 KiB
C
94 lines
2.6 KiB
C
/* C-Netz audio spectrum inversion (Sprachverschleierung)
|
|
*
|
|
* (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 <stdint.h>
|
|
#include <math.h>
|
|
#include "scrambler.h"
|
|
|
|
#define PI M_PI
|
|
|
|
/* FTZ 171 TR 60 Clause 6.2
|
|
* Carrier frequency, that is the spectrum that is mirrored */
|
|
#define CARRIER_HZ 3300.0
|
|
#define FILTER_BELOW 300.0
|
|
#define FILTER_TURNS 2
|
|
|
|
/* FTZ 171 TR 60 Clause 6.3
|
|
* How much must the carrier frequency be lower than a 1000 HZ tone that passes the inversion.
|
|
* The filter must be tuned to get that loss. */
|
|
#define TEST_1000HZ_DB 55.0
|
|
|
|
/* sine wave for carrier to modulate to */
|
|
static double carrier[256];
|
|
|
|
void scrambler_init(void)
|
|
{
|
|
int i;
|
|
|
|
for (i = 0; i < 256; i++) {
|
|
carrier[i] = sin((double)i / 256.0 * 2 * PI);
|
|
}
|
|
}
|
|
|
|
void scrambler_setup(scrambler_t *scrambler, int samplerate)
|
|
{
|
|
biquad_init(&scrambler->bq, CARRIER_HZ - FILTER_BELOW, samplerate);
|
|
scrambler->carrier_phaseshift256 = 256.0 / ((double)samplerate / CARRIER_HZ);
|
|
}
|
|
|
|
/* Modulate samples to carriere that is twice the mirror frequency.
|
|
* Then we got spectrum above carrier and mirrored spectrum below carrier.
|
|
* Afterwards we cut off carrier frequency and frequencies above carrier.
|
|
*/
|
|
void scrambler(scrambler_t *scrambler, int16_t *samples, int length)
|
|
{
|
|
double spl[length];
|
|
int32_t sample;
|
|
double phaseshift, phase;
|
|
int i;
|
|
|
|
phaseshift = scrambler->carrier_phaseshift256;
|
|
phase = scrambler->carrier_phase256;
|
|
|
|
for (i = 0; i < length; i++) {
|
|
/* modulate samples to carrier */
|
|
spl[i] = (double)samples[i] / 32768.0 * carrier[((uint8_t)phase) & 0xff];
|
|
phase += phaseshift;
|
|
if (phase >= 256.0)
|
|
phase -= 256.0;
|
|
}
|
|
|
|
scrambler->carrier_phase256 = phase;
|
|
|
|
/* cut off carrier frequency and modulation above carrier frequency */
|
|
biquad_process(&scrambler->bq, spl, length, FILTER_TURNS);
|
|
|
|
for (i = 0; i < length; i++) {
|
|
/* store result */
|
|
sample = spl[i] * 2.0 * 32768.0;
|
|
if (sample > 32767)
|
|
sample = 32767;
|
|
else if (sample < -32768)
|
|
sample = -32768;
|
|
*samples++ = sample;
|
|
}
|
|
}
|
|
|
|
|