fsk_minitel/fsk_minitel.c
2014-12-29 15:41:16 +01:00

628 lines
15 KiB
C

#include <stdio.h>
#include <string.h>
#include <stdlib.h>
#include <limits.h>
#include <math.h>
#define __cplusplus__strings__
#ifndef M_PI
#define M_PI 3.14159265358979324
#endif
#include <sndfile.h>
//#if defined(WITH_SPANDSP_INTERNALS)
#define SPANDSP_EXPOSE_INTERNAL_STRUCTURES
//#endif
#include "minitel_fsk.h"
#include "spandsp.h"
#include "spandsp-sim.h"
int16_t tone_amp[160];
int tone_pos=0;
typedef struct
{
double x, y;
} tComplex;
tComplex complexAdd(const tComplex* a, const tComplex* b)
{
tComplex c;
c.x = a->x + b->x;
c.y = a->y + b->y;
return c;
}
tComplex complexMul(const tComplex* a, const tComplex* b)
{
tComplex c;
c.x = a->x * b->x - a->y * b->y;
c.y = a->x * b->y + a->y * b->x;
return c;
}
void dft(tComplex out[], const tComplex in[], size_t n, int direction)
{
size_t k, i;
for (k = 0; k < n; k++)
{
tComplex r = { 0, 0 }, e;
for (i = 0; i < n; i++)
{
e.x = cos(-2 * direction * M_PI / n * ((double)k - n / 2) * ((double)i - n / 2));
e.y = sin(-2 * direction * M_PI / n * ((double)k - n / 2) * ((double)i - n / 2));
e = complexMul(&e, &in[i]);
r = complexAdd(&r, &e);
}
out[k] = r;
}
}
#define FILTER_LENGTH 64
void TxInit(tTx* pTx,
uint SampleRate,
uint (*pTxGetDataCallBack)(tTx*, uint8*))
{
memset(pTx, 0, sizeof(*pTx));
pTx->State = stSendingOnes;
pTx->SampleRate = SampleRate;
pTx->OnesFreq = 1300;
pTx->ZeroesFreq = 2100;
pTx->BitRate = 1200;
pTx->pTxGetDataCallBack = pTxGetDataCallBack;
pTx->SampleCnt = 0;
pTx->BitSampleCnt = pTx->SampleRate;
pTx->Data = 0;
pTx->DataLeft = 0;
pTx->Phase = 0.0;
pTx->PhaseIncrement = 2 * M_PI * pTx->OnesFreq / pTx->SampleRate;
pTx->queue = queue_init(NULL, 128, QUEUE_READ_ATOMIC | QUEUE_WRITE_ATOMIC);
}
int16 TxGetSample(tTx* pTx)
{
int16 sample;
float sample1, sample2, mixed;
if (pTx->State == stSendingOnes &&
pTx->SampleCnt >= pTx->SampleRate)
{
// Sent 1 second worth of 1's, can now send data
pTx->State = stSendingData;
}
if (pTx->State == stSendingData &&
pTx->BitSampleCnt >= pTx->SampleRate)
{
// Another data bit can now be sent
uint8 d;
pTx->BitSampleCnt -= pTx->SampleRate;
if (!pTx->DataLeft)
{
// Get the next data byte (if any)
if (pTx->pTxGetDataCallBack(pTx, &d) != 0)
{
pTx->Data = d & 0x7F;
pTx->Data |= (__builtin_popcount( d & 0x7F ) & 1) << 7; // insert parity
pTx->Data |= 1 << 8; // insert the stop bit
pTx->Data <<= 1; // insert the start bit
pTx->DataLeft = 10;
}
else
{
pTx->Data = 0x3FF; // no data, send 10 1's
pTx->DataLeft = 10;
}
}
// Extract the next data bit to send
d = pTx->Data & 1;
pTx->Data >>= 1;
pTx->DataLeft--;
// Choose the appropriate frequency for 0 and 1
if (d)
{
pTx->PhaseIncrement = 2 * M_PI * pTx->OnesFreq / pTx->SampleRate;
}
else
{
pTx->PhaseIncrement = 2 * M_PI * pTx->ZeroesFreq / pTx->SampleRate;
}
}
// Generate the next sample, advance the generator's phase
sample = (int16)(16000 * cos(pTx->Phase));
pTx->Phase += pTx->PhaseIncrement;
if (pTx->Phase >= 2 * M_PI)
{
pTx->Phase -= 2 * M_PI;
}
if (pTx->State == stSendingData)
{
pTx->BitSampleCnt += pTx->BitRate;
}
pTx->SampleCnt++;
/*Mix tone*/
sample1 = sample / 32768.0f;
sample2 = tone_amp[tone_pos%sizeof(tone_amp)] / 32768.0f;
mixed = sample1+sample2;
mixed *= 0.75;
if (mixed > 1.0f) { mixed = 1.0f ;printf("clip\n"); }
if (mixed < -1.0f){mixed = -1.0f; printf("clip\n"); }
tone_pos++;
return (uint16_t)(mixed * 32768.0f);
}
void RxInit(tRx* pRx,
uint SampleRate,
void (*pRxGetDataCallBack)(struct tRx*, uint8))
{
tComplex tmp[FILTER_LENGTH];
uint i;
memset(pRx, 0, sizeof(*pRx));
pRx->State = stCarrierLost;
pRx->SampleRate = SampleRate;
pRx->OnesFreq = 1300;
pRx->ZeroesFreq = 2100;
pRx->MidFreq = (pRx->OnesFreq + pRx->ZeroesFreq) / 2;
pRx->BitRate = 1200;
pRx->pRxGetDataCallBack = pRxGetDataCallBack;
pRx->SampleCnt = 0;
pRx->BitSampleCnt = 0;
pRx->Data = 0x3FF;
pRx->Phase = 0.0;
pRx->PhaseIncrement = 2 * M_PI * pRx->MidFreq / pRx->SampleRate;
pRx->LastAngle = 0.0;
pRx->LastDelta = 0;
pRx->Deltas = 0;
pRx->CarrierAngle = 0;
pRx->CarrierCnt = 0;
pRx->LongAvgPower = 0.0;
pRx->ShortAvgPower = 0.0;
pRx->queue = queue_init(rx->queue, 128, QUEUE_READ_ATOMIC | QUEUE_WRITE_ATOMIC);
for (i = 0; i < FILTER_LENGTH; i++)
{
pRx->Delay[i] = 0.0;
}
for (i = 0; i < FILTER_LENGTH; i++)
{
if (i == 0) // w < 0 (min w)
{
pRx->Filter[i].x = 0;
pRx->Filter[i].y = 0;
}
else if (i < FILTER_LENGTH / 2) // w < 0
{
pRx->Filter[i].x = 0;
pRx->Filter[i].y = 0;
}
else if (i == FILTER_LENGTH / 2) // w = 0
{
pRx->Filter[i].x = 0;
pRx->Filter[i].y = 0;
}
else if (i > FILTER_LENGTH / 2) // w > 0
{
pRx->Filter[i].x = 0;
pRx->Filter[i].y = -1;
// Extra filter to combat channel noise
if (i - FILTER_LENGTH / 2 < 875UL * FILTER_LENGTH / pRx->SampleRate ||
i - FILTER_LENGTH / 2 > (2350UL * FILTER_LENGTH + pRx->SampleRate - 1) / pRx->SampleRate)
{
pRx->Filter[i].y = 0;
}
}
}
memcpy(tmp, pRx->Filter, sizeof(tmp));
dft(pRx->Filter, tmp, FILTER_LENGTH, -1);
}
#define RX_VERBOSE 0
void RxGetSample(tRx* pRx, int16 Sample)
{
tComplex s = { 0.0, 0.0 }, ss;
double angle;
uint i;
int delta;
double pwr;
// Insert the sample into the delay line
memmove(&pRx->Delay[0], &pRx->Delay[1], sizeof(pRx->Delay) - sizeof(pRx->Delay[0]));
pRx->Delay[FILTER_LENGTH - 1] = Sample;
// Get the next analytic signal sample by applying Hilbert transform/filter
for (i = 0; i < FILTER_LENGTH; i++)
{
s.x += pRx->Delay[i] * pRx->Filter[FILTER_LENGTH - 1 - i].x;
s.y += pRx->Delay[i] * pRx->Filter[FILTER_LENGTH - 1 - i].y;
}
// Frequency shift by MidFreq down
ss.x = cos(-pRx->Phase);
ss.y = sin(-pRx->Phase);
s = complexMul(&s, &ss);
pRx->Phase += pRx->PhaseIncrement;
if (pRx->Phase >= 2 * M_PI)
{
pRx->Phase -= 2 * M_PI;
}
// Calculate signal power
pwr = (s.x * s.x + s.y * s.y) / 32768 / 32768;
pRx->LongAvgPower *= 1 - pRx->BitRate / (pRx->SampleRate * 8.0 * 8);
pRx->LongAvgPower += pwr;
pRx->ShortAvgPower *= 1 - pRx->BitRate / (pRx->SampleRate * 8.0);
pRx->ShortAvgPower += pwr;
#if 0
printf("LongAvgPower:%f ShortAvgPower:%f\n", pRx->LongAvgPower, pRx->ShortAvgPower);
#endif
// Disconnect if the signal power changes abruptly.
if (pRx->State != stCarrierLost &&
pRx->LongAvgPower > pRx->ShortAvgPower * 8 * 8)
{
// N.B. The receiver may have received a few extra (garbage) bytes
// while demodulating the abruptly changed signal.
// Prefixing data with its size or using a more advanced protocol
// may be a good solution to this little problem.
pRx->State = stCarrierLost;
pRx->BitSampleCnt = 0;
pRx->Data = 0x3FF;
pRx->Phase = 0.0;
pRx->LastAngle = 0.0;
pRx->LastDelta = 0;
pRx->Deltas = 0;
pRx->CarrierAngle = 0;
pRx->CarrierCnt = 0;
}
// Get the phase angle from the analytic signal sample
angle = (fpclassify(s.x) == FP_ZERO && fpclassify(s.y) == FP_ZERO) ?
0.0 : 180 / M_PI * atan2(s.y, s.x);
// Calculate the phase angle change and force it to the -PI to +PI range
delta = (int)(360.5 + angle - pRx->LastAngle) % 360;
if (delta > 180) delta -= 360;
if (pRx->State == stCarrierLost)
{
// Accumulate the phase angle change to see if we're receiving 1's
pRx->CarrierAngle += delta;
pRx->CarrierCnt++;
// Check whether or not the phase corresponds to 1's
if (delta < 0)
{
if (pRx->CarrierCnt >= pRx->SampleRate / pRx->OnesFreq * 8)
{
double ph = (double)pRx->CarrierAngle / pRx->CarrierCnt;
#if RX_VERBOSE
printf("ca:%5ld, cc:%4ld, ca/cc:%4ld\n",
(long)pRx->CarrierAngle,
(long)pRx->CarrierCnt,
(long)(pRx->CarrierAngle / pRx->CarrierCnt));
#endif
// Frequency tolerance is +/-16 Hz per the V.23 spec
if (ph < (pRx->OnesFreq - 17.0 - pRx->MidFreq) * 360.0 / pRx->SampleRate ||
ph > (pRx->OnesFreq + 17.0 - pRx->MidFreq) * 360.0 / pRx->SampleRate)
{
goto BadCarrier;
}
}
}
else
{
BadCarrier:
// Phase doesn't correspond to 1's
pRx->CarrierAngle = 0.0;
pRx->CarrierCnt = 0;
}
if (pRx->CarrierCnt >= pRx->SampleRate / 2 + pRx->SampleRate / 4)
{
// 0.75 seconds worth of 1's have been detected, ready to receive data
// Adjust MidFreq to compensate for the DAC and ADC sample rate difference
double f1 = (double)pRx->CarrierAngle / pRx->CarrierCnt / 360 * pRx->SampleRate + pRx->MidFreq;
pRx->MidFreq = (uint)(pRx->MidFreq * f1 / pRx->OnesFreq);
pRx->PhaseIncrement = 2 * M_PI * pRx->MidFreq / pRx->SampleRate;
#if RX_VERBOSE
printf("f1:%u, new MidFreq:%u\n", (uint)f1, pRx->MidFreq);
#endif
pRx->State = stCarrierDetected;
}
}
else
{
// Detect frequency changes (transitions between 0's and 1's)
int freqChange = ((int32)pRx->LastDelta * delta < 0 || pRx->LastDelta && !delta);
int reAddDelta = 0;
#if RX_VERBOSE
printf("%6lu: delta:%4d freqChange:%d BitSampleCnt:%u\n",
(ulong)pRx->SampleCnt,
delta,
freqChange,
pRx->BitSampleCnt);
#endif
// Synchronize with 1<->0 transitions
if (freqChange)
{
if (pRx->BitSampleCnt >= pRx->SampleRate / 2)
{
pRx->BitSampleCnt = pRx->SampleRate;
pRx->Deltas -= delta;
reAddDelta = 1;
}
else
{
pRx->BitSampleCnt = 0;
pRx->Deltas = 0;
}
}
// Accumulate analytic signal phase angle changes
// (positive for 0, negative for 1)
pRx->Deltas += delta;
if (pRx->BitSampleCnt >= pRx->SampleRate)
{
// Another data bit has accumulated
pRx->BitSampleCnt -= pRx->SampleRate;
#if RX_VERBOSE
printf("bit: %u\n", pRx->Deltas < 0);
#endif
pRx->Data >>= 1;
pRx->Data |= (pRx->Deltas < 0) << 9;
pRx->Deltas = delta * reAddDelta;
if ((pRx->Data & 0x201) == 0x200)
{
// Start and stop bits have been detected
if (pRx->State == stCarrierDetected)
{
pRx->State = stReceivingData;
}
pRx->Data = (pRx->Data >> 1) & 0x7F;
pRx->pRxGetDataCallBack(pRx, (uint8)pRx->Data);
#if RX_VERBOSE
printf("byte: 0x%02X ('%c')\n",
pRx->Data,
(pRx->Data >= 0x20 && pRx->Data <= 0x7F) ? pRx->Data : '?');
#endif
pRx->Data = 0x3FF;
}
}
pRx->BitSampleCnt += pRx->BitRate;
}
pRx->LastAngle = angle;
pRx->LastDelta = delta;
pRx->SampleCnt++;
}
typedef struct
{
tTx Tx;
FILE* DataFile;
int CountDown;
} tTxTest;
uint TxGetDataCallBack(tTx* pTx, uint8* pTxData)
{
tTxTest* pTxTest = (tTxTest*)pTx;
uchar c;
if (pTxTest->CountDown)
{
pTxTest->CountDown--;
return 0;
}
/*
if (fread(&c, 1, 1, pTxTest->DataFile) != 1)
{
pTxTest->CountDown = 20;
return 0;
}
*/
if( ! queue_empty(pTx->queue)){
c = queue_read_byte(pTx->queue);
pTxTest->CountDown = 20;
}
*pTxData = c;
return 1;
}
int testTx(uint SampleRate,
double NoiseLevel,
const char* DataFileName,
const char* AudioFileName)
{
FILE *fData = NULL;
SNDFILE *fAudio = NULL;
//FILE *fAudio = NULL;
tone_gen_descriptor_t t_desc;
tone_gen_state_t t_state;
int err = EXIT_FAILURE;
tTxTest txTest;
if ((fData = fopen(DataFileName, "rb")) == NULL)
{
printf("Can't open file \"%s\"\n", DataFileName);
goto Exit;
}
tone_gen_descriptor_init(&t_desc, 1300, -12.0, 0, 0,20, 0, 0, 0, 0);
tone_gen_init(&t_state, &t_desc);
tone_gen(&t_state, tone_amp, sizeof(tone_amp));
if ((fAudio = sf_open_telephony_write(AudioFileName, 1)) == NULL)
// if ((fAudio = fopen(AudioFileName, "wb")) == NULL)
{
printf("Can't create file \"%s\"\n", AudioFileName);
goto Exit;
}
txTest.DataFile = fData;
txTest.CountDown = 0;
TxInit(&txTest.Tx,
SampleRate,
&TxGetDataCallBack);
do
{
int16 sample = TxGetSample(&txTest.Tx);
if (txTest.CountDown > 1 && txTest.CountDown <= 10)
{
#if 0 // Enable this code to test disconnecting.
// Finish with silence.
#endif
}
//sample += (rand() - (int)RAND_MAX / 2) * NoiseLevel * 16000 / (RAND_MAX / 2);
sf_writef_short(fAudio, &sample, 1);
//fwrite(&sample, 1, sizeof(sample), fAudio);
} while (txTest.CountDown != 1); // Drain all data-containing samples
err = EXIT_SUCCESS;
Exit:
if (fData != NULL) fclose(fData);
if (fAudio != NULL)
sf_close(fAudio);
// fclose(fAudio);
return err;
}
typedef struct
{
tRx Rx;
FILE* DataFile;
} tRxTest;
void RxGetDataCallBack(tRx* pRx, uint8 RxData)
{
tRxTest* pRxTest = (tRxTest*)pRx;
uchar c = RxData;
if (queue_free_space(pRx->queue)){
queue_write_byte(pRx->queue, (uint16_t)Rxdata);
}
//fwrite(&c, 1, 1, pRxTest->DataFile);
}
int testRx(uint SampleRate,
const char* AudioFileName,
const char* DataFileName)
{
uint lastState;
int err = EXIT_FAILURE;
tRxTest rxTest;
RxInit(&rxTest.Rx,
8000,//SampleRate,
&RxGetDataCallBack);
for (;;)
{
int16 sample;
if (fread(&sample, 1, sizeof(sample), fAudio) != sizeof(sample))
{
if (rxTest.Rx.State != stCarrierLost) goto NoCarrier;
break;
}
lastState = rxTest.Rx.State;
RxGetSample(&rxTest.Rx, sample);
if (rxTest.Rx.State != lastState && rxTest.Rx.State == stCarrierDetected)
{
printf("\nCONNECT %u\n\n", rxTest.Rx.BitRate);
}
if (rxTest.Rx.State != lastState && rxTest.Rx.State == stCarrierLost)
{
NoCarrier:
printf("\n\nNO CARRIER\n");
break;
}
}
err = EXIT_SUCCESS;
Exit:
if (fAudio != NULL) fclose(fAudio);
if (fData != NULL) fclose(fData);
return err;
}
/*
int main(int argc, char* argv[])
{
uint sampleRate;
double noiseLevel;
if (argc < 2 ||
!strcasecmp(argv[1], "-help") ||
!strcasecmp(argv[1], "/help") ||
!strcasecmp(argv[1], "-?") ||
!strcasecmp(argv[1], "/?"))
{
Usage:
printf("Usage:\n\n"
" %s tx <sample rate> <noise level> <data input file> <PCM output file>\n"
" %s rx <sample rate> <PCM input file> <data output file>\n",
argv[0],
argv[0]);
return 0;
}
if (!strcasecmp(argv[1], "tx") &&
argc == 6 &&
sscanf(argv[2], "%u", &sampleRate) == 1 &&
sscanf(argv[3], "%lf", &noiseLevel) == 1)
{
return testTx(sampleRate, noiseLevel, argv[4], argv[5]);
}
else if (!strcasecmp(argv[1], "rx") &&
argc == 5 &&
sscanf(argv[2], "%u", &sampleRate) == 1)
{
return testRx(sampleRate, argv[3], argv[4]);
}
else
{
goto Usage;
}
}
*/