4428 lines
102 KiB
C
4428 lines
102 KiB
C
/*
|
||
Copyright (C) 2019-2020 Andrei Kopanchuk UZ7HO
|
||
|
||
This file is part of QtSoundModem
|
||
|
||
QtSoundModem 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.
|
||
|
||
QtSoundModem 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 QtSoundModem. If not, see http://www.gnu.org/licenses
|
||
|
||
*/
|
||
|
||
// UZ7HO Soundmodem Port by John Wiseman G8BPQ
|
||
|
||
#include "UZ7HOStuff.h"
|
||
|
||
extern int blnBusyStatus;
|
||
extern word MEMRecovery[5];
|
||
|
||
void make_rx_frame_FX25(int snd_ch, int rcvr_nr, int emph, string * data);
|
||
string * memory_ARQ(TStringList * buf, string * data);
|
||
void CreateStringList(TStringList * List);
|
||
void analiz_frame(int snd_ch, string * frame, char * code, boolean fecflag);
|
||
void KISS_on_data_out(int port, string * frame, int TX);
|
||
void updateDCD(int Chan, boolean State);
|
||
void Frame_Optimize(TAX25Port * AX25Sess, TStringList * buf);
|
||
void RX2TX(int snd_ch);
|
||
int fx25_decode_rs(Byte * data, int * eras_pos, int no_eras, int pad, int rs_size);
|
||
void il2p_rec_bit(int chan, int subchan, int slice, int dbit);
|
||
|
||
float GuessCentreFreq(int i);
|
||
void ProcessRXFrames(int snd_ch);
|
||
|
||
extern struct il2p_context_s *il2p_context[4][16][3];
|
||
|
||
|
||
/*
|
||
|
||
unit ax25_demod;
|
||
|
||
interface
|
||
|
||
uses math,sysutils,Graphics,classes;
|
||
|
||
procedure detector_init;
|
||
procedure detector_free;
|
||
procedure Mux3(snd_ch,rcvr_nr,emph: byte; src1,core: array of single; var dest,prevI,prevQ: array of single; tap,buf_size: word);
|
||
procedure Mux3_PSK(snd_ch,rcvr_nr,emph: byte; src1,core: array of single; var destI,destQ,prevI,prevQ: array of single; tap,buf_size: word);
|
||
procedure make_core_intr(snd_ch: byte);
|
||
procedure make_core_LPF(snd_ch: byte; width: single);
|
||
procedure make_core_BPF(snd_ch: byte; freq,width: single);
|
||
procedure make_core_TXBPF(snd_ch: byte; freq,width: single);
|
||
procedure init_BPF(freq1,freq2: single; tap: word; samplerate: single; var buf: array of single);
|
||
procedure FIR_filter(src: array of single; buf_size,tap: word; core: array of single; var dest,prev: array of single);
|
||
procedure Demodulator(snd_ch,rcvr_nr: byte; src_buf: array of single; last: boolean);
|
||
function memory_ARQ(buf: TStringList; data: string): string;
|
||
|
||
type TSurvivor = record
|
||
BitEstimates: int64;
|
||
Pathdistance: integer;
|
||
}
|
||
|
||
type TMChannel = record
|
||
prev_LPF1I_buf : array [0..4095] of single;
|
||
prev_LPF1Q_buf : array [0..4095] of single;
|
||
prev_dLPFI_buf : array [0..4095] of single;
|
||
prev_dLPFQ_buf : array [0..4095] of single;
|
||
prev_AFCI_buf : array [0..4095] of single;
|
||
prev_AFCQ_buf : array [0..4095] of single;
|
||
AngleCorr : single;
|
||
MUX_osc : single;
|
||
AFC_IZ1 : single;
|
||
AFC_IZ2 : single;
|
||
AFC_QZ1 : single;
|
||
AFC_QZ2 : single;
|
||
AFC_bit_buf1I : array [0..1023] of single;
|
||
AFC_bit_buf1Q : array [0..1023] of single;
|
||
AFC_bit_buf2 : array [0..1023] of single;
|
||
AFC_IIZ1 : single;
|
||
AFC_QQZ1 : single;
|
||
}
|
||
*/
|
||
|
||
|
||
#define sbc 175
|
||
|
||
single ch_offset[4] = { -sbc * 1.5,-sbc * 0.5,sbc*0.5,sbc*1.5 };
|
||
|
||
|
||
|
||
float PI125 = 0.125f * M_PI;
|
||
float PI375 = 0.375f * M_PI;
|
||
float PI625 = 0.625f * M_PI;
|
||
float PI875 = 0.875f * M_PI;
|
||
float PI5 = 0.5f * M_PI;
|
||
float PI25 = 0.25f * M_PI;
|
||
float PI75 = 0.75f * M_PI;
|
||
|
||
unsigned char modem_mode[5] ={0,0,0,0};
|
||
|
||
unsigned short bpf[5] = { 500, 500, 500, 500,500 };
|
||
unsigned short lpf[5] = { 150, 150, 150, 150, 150 };
|
||
|
||
float BIT_AFC = 32;
|
||
float slottime_tick[5] = { 0 };
|
||
float resptime_tick[5] = { 0 };
|
||
int dcd_threshold = 128;
|
||
int rxOffset = 0;
|
||
int chanOffset[4] = { 0,0,0,0 };
|
||
|
||
float DCD_LastPkPos[5] = { 0 };
|
||
float DCD_LastPerc[5] = { 0 };
|
||
int dcd_bit_cnt[5] = { 0 };
|
||
Byte DCD_status[5] = { 0 };
|
||
float DCD_persist[5] = { 0 };
|
||
int dcd_bit_sync[5] = { 0 };
|
||
Byte dcd_hdr_cnt[5] = { 0 };
|
||
longword DCD_header[5] = { 0 };
|
||
int dcd_on_hdr[5] = { 0 };
|
||
|
||
extern int centreFreq[4];
|
||
|
||
float lastangle[4]; // pevious value for differential modes
|
||
|
||
|
||
unsigned short n_INTR[5] = { 1,1,1,1,1 };
|
||
unsigned short INTR_tap[5] = { 16, 16,16,16,16 };
|
||
unsigned short BPF_tap[5] = { 256, 256,256,256,256 }; // 256 default
|
||
unsigned short LPF_tap[5] = { 128, 128,128,128,128 }; // 128
|
||
|
||
|
||
|
||
short rx_freq[5] = { 1700, 1700,1700,1700,1700 };
|
||
short rx_shift[5] = { 200, 200, 200, 200, 200 };
|
||
short rx_baudrate[5] = { 300, 300, 300, 300, 300 };
|
||
short rcvr_offset[5] = { 30, 30, 30, 30,30 };
|
||
|
||
// rx_freq is configured freq. We shouldn't change it so need a sparate variable
|
||
// for the actual demod freq when using multiple decoders
|
||
|
||
short active_rx_freq[5] = { 1700, 1700,1700,1700,1700 };
|
||
|
||
int fx25_mode[4] = { 0, 0, 0, 0 };
|
||
int il2p_mode[4] = { 0, 0, 0, 0 };
|
||
int il2p_crc[4] = { 0, 0, 0, 0 };
|
||
|
||
int pnt_change[5] = { 0 };
|
||
float src_buf[5][2048];
|
||
|
||
float INTR_core[5][2048];
|
||
float AFC_core[5][2048];
|
||
float LPF_core[5][2048];
|
||
|
||
int new_tx_port[4] = { 0,0,0,0 };
|
||
UCHAR RCVR[5] = { 0 };
|
||
|
||
// We allow two (or more!) ports to be assigned to the same soundcard channel
|
||
|
||
int soundChannel[5] = { 0 }; // 0 = Unused 1 = Left 2 = Right 3 = Mono
|
||
int modemtoSoundLR[4] = { 0 };
|
||
|
||
struct TDetector_t DET[nr_emph + 1][16];
|
||
|
||
// Chan, Decoder, Emph
|
||
|
||
float Phases[4][16][nr_emph + 1][4096];
|
||
float Mags[4][16][nr_emph + 1][4096];
|
||
int nPhases[4][16][nr_emph + 1];
|
||
|
||
TStringList detect_list_l[5];
|
||
TStringList detect_list[5];
|
||
TStringList detect_list_c[5];
|
||
|
||
int lastDCDState[4] = { 0,0,0,0 };
|
||
/*
|
||
|
||
implementation
|
||
|
||
uses sm_main,ax25,ax25_l2,ax25_mod,ax25_agw,rsunit,kiss_mode;
|
||
*/
|
||
|
||
void detector_init()
|
||
{
|
||
int i, k, j;
|
||
|
||
for (k = 0; k < 16; k++)
|
||
{
|
||
for (i = 1; i <= 4; i++)
|
||
{
|
||
for (j = 0; j <= nr_emph; j++)
|
||
{
|
||
struct TDetector_t * pDET = &DET[j][k];
|
||
|
||
pDET->fx25[i].status = FX25_TAG;
|
||
pDET->AngleCorr[i] = 0;
|
||
pDET->last_sample[i] = 0;
|
||
pDET->sample_cnt[i] = 0;
|
||
pDET->last_bit[i] = 0;
|
||
pDET->PkAmp[i] = 0;
|
||
pDET->PkAmpMax[i] = 0;
|
||
pDET->newpkpos[i] = 0;
|
||
pDET->ones[i] = 0;
|
||
pDET->zeros[i] = 0;
|
||
pDET->MinAmp[i] = 0;
|
||
pDET->MaxAmp[i] = 0;
|
||
pDET->MUX3_osc[i] = 0;
|
||
pDET->Preemphasis6[i] = 0;
|
||
pDET->Preemphasis12[i] = 0;
|
||
pDET->PSK_AGC[i] = 0;
|
||
pDET->AGC[i] = 0;
|
||
pDET->AGC1[i] = 0;
|
||
pDET->AGC2[i] = 0;
|
||
pDET->AGC3[i] = 0;
|
||
pDET->AGC_max[i] = 0;
|
||
pDET->AGC_min[i] = 0;
|
||
pDET->AFC_IZ1[i] = 0;
|
||
pDET->AFC_IZ2[i] = 0;
|
||
pDET->AFC_QZ1[i] = 0;
|
||
pDET->AFC_QZ2[i] = 0;
|
||
pDET->AFC_dF[i] = 0;
|
||
pDET->AFC_cnt[i] = 0;
|
||
pDET->PSK_IZ1[i] = 0;
|
||
pDET->PSK_QZ1[i] = 0;
|
||
pDET->PkAmpI[i] = 0;
|
||
pDET->PkAmpQ[i] = 0;
|
||
pDET->last_rx_bit[i] = 0;
|
||
pDET->bit_stream[i] = 0;
|
||
pDET->byte_rx[i] = 0;
|
||
pDET->bit_stuff_cnt[i] = 0;
|
||
pDET->bit_cnt[i] = 0;
|
||
pDET->bit_osc[i] = 0;
|
||
pDET->frame_status[i] = 0;
|
||
initString(&pDET->FEC_rx_data[i]);
|
||
initString(&pDET->rx_data[i]);
|
||
initString(&pDET->raw_bits[i]);
|
||
initTStringList(&pDET->mem_ARQ_buf[i]);
|
||
initTStringList(&pDET->mem_ARQ_F_buf[i]);
|
||
pDET->rx_decoded = 0;
|
||
pDET->emph_decoded = 0;
|
||
}
|
||
}
|
||
}
|
||
|
||
for (i = 1; i <= 4; i++)
|
||
{
|
||
initTStringList(&detect_list[i]);
|
||
initTStringList(&detect_list_l[i]);
|
||
initTStringList(&detect_list_c[i]);
|
||
}
|
||
}
|
||
|
||
|
||
/*
|
||
procedure detector_free;
|
||
var
|
||
i,k,j: word;
|
||
{
|
||
for i = 1 to 4 do
|
||
{
|
||
detect_list[i].Free;
|
||
detect_list_l[i].Free;
|
||
detect_list_c[i].Free;
|
||
}
|
||
for k = 0 to 16 do
|
||
for i = 1 to 4 do
|
||
for j = 0 to nr_emph do
|
||
{
|
||
DET[j,k].mem_ARQ_buf[i].Free;
|
||
DET[j,k].mem_ARQ_F_buf[i].Free;
|
||
}
|
||
}
|
||
*/
|
||
|
||
void FIR_filter(float * src, unsigned short buf_size, unsigned short tap, float * core, float * dest, float * prev)
|
||
{
|
||
float accum = 0.0f;
|
||
float fp1;
|
||
|
||
int eax, ebx;
|
||
float * edi;
|
||
|
||
fmove(&prev[buf_size], &prev[0], tap * 4);
|
||
fmove(&src[0], &prev[tap], buf_size * 4);
|
||
|
||
eax = 0;
|
||
|
||
// ; shl ecx, 2;
|
||
// ; shl edx, 2;
|
||
|
||
cfir_i:
|
||
edi = prev;
|
||
|
||
edi += eax;
|
||
|
||
ebx = 0;
|
||
accum = 0.0f;
|
||
|
||
cfir_k:
|
||
|
||
// FLD pushes operand onto stack, so old value goes to fp1
|
||
|
||
fp1 = accum;
|
||
accum = edi[ebx];
|
||
accum *= core[ebx];
|
||
accum += fp1;
|
||
|
||
ebx++;
|
||
if (ebx != tap)
|
||
goto cfir_k;
|
||
|
||
dest[eax] = accum;
|
||
|
||
eax++;
|
||
|
||
if (eax != buf_size)
|
||
goto cfir_i;
|
||
|
||
}
|
||
|
||
|
||
float get_persist(int snd_ch, int persist)
|
||
{
|
||
single x, x1 ;
|
||
|
||
x = 256 / persist;
|
||
|
||
x1 = round(x*x) * rand() / RAND_MAX;
|
||
|
||
return x1 * 0.5 * slottime[snd_ch];
|
||
}
|
||
|
||
void chk_dcd1(int snd_ch, int buf_size)
|
||
{
|
||
// This seems to schedule all TX, but is only called when a frame has been processed
|
||
// ? does this work as Andy passes aborted frames to decoder
|
||
|
||
Byte port;
|
||
int i;
|
||
single tick;
|
||
word active;
|
||
boolean ind_dcd;
|
||
boolean dcd_sync;
|
||
longint n;
|
||
|
||
TAX25Port * AX25Sess;
|
||
|
||
dcd[snd_ch] = 1;
|
||
|
||
ind_dcd = 0;
|
||
|
||
tick = 1000 / RX_Samplerate;
|
||
|
||
if (modem_mode[snd_ch] == MODE_ARDOP)
|
||
{
|
||
dcd_bit_sync[snd_ch] = blnBusyStatus;
|
||
}
|
||
else if (modem_mode[snd_ch] == MODE_RUH)
|
||
{
|
||
dcd_bit_sync[snd_ch] = blnBusyStatus;
|
||
}
|
||
else
|
||
{
|
||
if (dcd_bit_cnt[snd_ch] > 0)
|
||
dcd_bit_sync[snd_ch] = 0;
|
||
else
|
||
dcd_bit_sync[snd_ch] = 1;
|
||
|
||
if (dcd_on_hdr[snd_ch])
|
||
dcd_bit_sync[snd_ch] = 1;
|
||
|
||
if (modem_mode[snd_ch] == MODE_MPSK && DET[0][0].frame_status[snd_ch] == FRAME_LOAD)
|
||
dcd_bit_sync[snd_ch] = 1;
|
||
}
|
||
|
||
if (lastDCDState[snd_ch] != dcd_bit_sync[snd_ch])
|
||
{
|
||
updateDCD(snd_ch, dcd_bit_sync[snd_ch]);
|
||
updateDCD(snd_ch, dcd_bit_sync[snd_ch]);
|
||
lastDCDState[snd_ch] = dcd_bit_sync[snd_ch];
|
||
}
|
||
|
||
if (resptime_tick[snd_ch] < resptime[snd_ch])
|
||
resptime_tick[snd_ch] = resptime_tick[snd_ch] + tick * buf_size;
|
||
|
||
slottime_tick[snd_ch] = slottime_tick[snd_ch] + tick * buf_size;
|
||
|
||
if (dcd_bit_sync[snd_ch]) // reset the slottime timer
|
||
{
|
||
slottime_tick[snd_ch] = 0;
|
||
DCD_status[snd_ch] = DCD_WAIT_SLOT;
|
||
}
|
||
|
||
switch (DCD_status[snd_ch])
|
||
{
|
||
case DCD_WAIT_SLOT:
|
||
|
||
if (slottime_tick[snd_ch] >= slottime[snd_ch])
|
||
{
|
||
DCD_status[snd_ch] = DCD_WAIT_PERSIST;
|
||
DCD_persist[snd_ch] = get_persist(snd_ch, persist[snd_ch]);
|
||
}
|
||
break;
|
||
|
||
case DCD_WAIT_PERSIST:
|
||
|
||
if (slottime_tick[snd_ch] >= slottime[snd_ch] + DCD_persist[snd_ch])
|
||
{
|
||
dcd[snd_ch] = FALSE;
|
||
slottime_tick[snd_ch] = 0;
|
||
DCD_status[snd_ch] = DCD_WAIT_SLOT;
|
||
}
|
||
break;
|
||
}
|
||
|
||
active = 0;
|
||
|
||
for (i = 0; i < port_num; i++)
|
||
{
|
||
if (AX25Port[snd_ch][i].status != STAT_NO_LINK)
|
||
active++;
|
||
|
||
if (active < 2)
|
||
resptime_tick[snd_ch] = resptime[snd_ch];
|
||
|
||
if (TX_rotate)
|
||
{
|
||
for (int n = 0; n < 4; n++)
|
||
{
|
||
if (snd_status[n] == SND_TX)
|
||
dcd[snd_ch] = TRUE;
|
||
}
|
||
}
|
||
|
||
if (snd_ch == 1)
|
||
snd_ch = 1;
|
||
|
||
if (!dcd[snd_ch] && resptime_tick[snd_ch] >= resptime[snd_ch])
|
||
{
|
||
int n = 0;
|
||
|
||
port = new_tx_port[snd_ch];
|
||
do
|
||
{
|
||
AX25Sess = &AX25Port[snd_ch][port];
|
||
|
||
if (AX25Sess->frame_buf.Count > 0)
|
||
Frame_Optimize(AX25Sess, &AX25Sess->frame_buf);
|
||
|
||
if (AX25Sess->frame_buf.Count > 0)
|
||
{
|
||
for (n = 0; n < AX25Sess->frame_buf.Count; n++)
|
||
{
|
||
Add(&all_frame_buf[snd_ch], duplicateString(Strings(&AX25Sess->frame_buf, n)));
|
||
}
|
||
|
||
Clear(&AX25Sess->frame_buf);
|
||
}
|
||
|
||
port++;
|
||
|
||
if (port >= port_num)
|
||
port = 0;
|
||
|
||
if (all_frame_buf[snd_ch].Count > 0)
|
||
new_tx_port[snd_ch] = port;
|
||
|
||
n++;
|
||
|
||
} while (all_frame_buf[snd_ch].Count == 0 && n < port_num);
|
||
|
||
// Add KISS frames
|
||
|
||
if (KISSServ)
|
||
{
|
||
// KISS monitor outgoing AGW frames
|
||
|
||
if (all_frame_buf[snd_ch].Count > 0)
|
||
{
|
||
for (int n = 0; n < all_frame_buf[snd_ch].Count; n++)
|
||
{
|
||
KISS_on_data_out(snd_ch, Strings(&all_frame_buf[snd_ch], n), 1); // Mon TX
|
||
}
|
||
}
|
||
|
||
// Add outgoing KISS frames to TX Q
|
||
|
||
if (KISS.buffer[snd_ch].Count > 0)
|
||
{
|
||
for (int k = 0; k < KISS.buffer[snd_ch].Count; k++)
|
||
{
|
||
if (AGWServ)
|
||
AGW_Raw_monitor(snd_ch, Strings(&KISS.buffer[snd_ch], n));
|
||
|
||
// Need to add copy as clear will free original
|
||
|
||
Add(&all_frame_buf[snd_ch], duplicateString(Strings(&KISS.buffer[snd_ch], k)));
|
||
}
|
||
Clear(&KISS.buffer[snd_ch]);
|
||
}
|
||
}
|
||
|
||
if (all_frame_buf[snd_ch].Count > 0 && snd_status[snd_ch] == SND_IDLE)
|
||
{
|
||
resptime_tick[snd_ch] = 0;
|
||
RX2TX(snd_ch); // Do TX
|
||
return;
|
||
}
|
||
}
|
||
}
|
||
}
|
||
|
||
|
||
string * get_pkt_data(string * stream)
|
||
{
|
||
Byte bitstuff_cnt;
|
||
Byte bits_cnt;
|
||
word i;
|
||
string * s = newString();
|
||
|
||
Byte bit;
|
||
Byte raw_bit;
|
||
Byte sym;
|
||
|
||
bits_cnt = 0;
|
||
bitstuff_cnt = 0;
|
||
sym = 0;
|
||
|
||
if (stream->Length > 0)
|
||
{
|
||
for (i = 0; i < stream->Length; i++)
|
||
{
|
||
if (stream->Data[i] == '1')
|
||
bit = RX_BIT1;
|
||
else
|
||
bit = RX_BIT0;
|
||
|
||
if (bitstuff_cnt < 5)
|
||
{
|
||
sym = (sym >> 1) | bit;
|
||
bits_cnt++;
|
||
}
|
||
|
||
if (bitstuff_cnt == 5 || bit == RX_BIT0)
|
||
bitstuff_cnt = 0;
|
||
|
||
if (bit == RX_BIT1)
|
||
bitstuff_cnt++;
|
||
|
||
if (bits_cnt == 8)
|
||
{
|
||
stringAdd(s, &sym, 1);
|
||
sym = 0;
|
||
bits_cnt = 0;
|
||
}
|
||
}
|
||
}
|
||
|
||
return s;
|
||
}
|
||
|
||
string * get_pkt_data2(string * stream, Byte last_nrzi_bit)
|
||
{
|
||
Byte bitstuff_cnt;
|
||
Byte bits_cnt;
|
||
word i;
|
||
string * s = newString();
|
||
|
||
Byte pkt[350];
|
||
|
||
Byte bit;
|
||
Byte raw_bit;
|
||
Byte sym;
|
||
int n = 0;
|
||
|
||
bits_cnt = 0;
|
||
bitstuff_cnt = 0;
|
||
sym = 0;
|
||
|
||
if (stream->Length > 0)
|
||
{
|
||
for (i = 0; i < stream->Length; i++)
|
||
{
|
||
if (stream->Data[i] == '1') raw_bit = RX_BIT1; else raw_bit = RX_BIT0;
|
||
if (raw_bit == last_nrzi_bit) bit = RX_BIT1; else bit = RX_BIT0;
|
||
|
||
last_nrzi_bit = raw_bit;
|
||
|
||
if (bitstuff_cnt < 5)
|
||
{
|
||
sym = (sym >> 1) | bit;
|
||
bits_cnt++;
|
||
}
|
||
|
||
if (bitstuff_cnt == 5 || bit == RX_BIT0)
|
||
bitstuff_cnt = 0;
|
||
|
||
if (bit == RX_BIT1)
|
||
bitstuff_cnt++;
|
||
|
||
if (bits_cnt == 8)
|
||
{
|
||
if (n < 330)
|
||
pkt[n++] = sym;
|
||
|
||
sym = 0;
|
||
bits_cnt = 0;
|
||
}
|
||
}
|
||
}
|
||
|
||
stringAdd(s, pkt, n);
|
||
return s;
|
||
}
|
||
|
||
string * get_NRZI_data(string * stream, UCHAR last_nrzi_bit)
|
||
{
|
||
longword len;
|
||
word i;
|
||
string * s = NULL;
|
||
Byte raw_bit;
|
||
|
||
len = stream->Length;
|
||
|
||
if (len > 65535)
|
||
len = 65535;
|
||
|
||
if (len > 0)
|
||
{
|
||
s = newString();
|
||
|
||
setlength(s, len);
|
||
|
||
for (i = 0; i < len; i++)
|
||
{
|
||
if (stream->Data[i] == '1')
|
||
raw_bit = RX_BIT1;
|
||
else
|
||
raw_bit = RX_BIT0;
|
||
|
||
if (raw_bit == last_nrzi_bit)
|
||
s->Data[i] = '1';
|
||
else
|
||
s->Data[i] = '0';
|
||
|
||
last_nrzi_bit = raw_bit;
|
||
}
|
||
}
|
||
return s;
|
||
}
|
||
/*
|
||
|
||
function invert_NRZI_data(stream: string; last_nrzi_bit: byte): string;
|
||
var
|
||
len: longword;
|
||
i: word;
|
||
s: string;
|
||
{
|
||
s = '';
|
||
len = length(stream);
|
||
if len>65535 then len = 65535;
|
||
if len>0 then
|
||
{
|
||
setlength(s,len);
|
||
for i = 1 to len do
|
||
if last_nrzi_bit=RX_BIT0 then
|
||
{
|
||
if stream[i]='1' then s[i] = '0' else s[i] = '1';
|
||
end
|
||
else s[i] = stream[i];
|
||
}
|
||
result = s;
|
||
}
|
||
*/
|
||
|
||
void make_rx_frame(int snd_ch, int rcvr_nr, int emph, Byte last_nrzi_bit, string * raw_data, string * raw_data1)
|
||
{
|
||
int swap_i, swap_k;
|
||
string * data;
|
||
string * nrzi_data;
|
||
longword raw_len;
|
||
word len, crc1, crc2;
|
||
int arq_mem = 0;
|
||
string s;
|
||
int i, k, n;
|
||
unsigned char * raw;
|
||
unsigned char * raw1;
|
||
char Mode[16] = "";
|
||
|
||
struct TDetector_t * pDET = &DET[emph][rcvr_nr];
|
||
|
||
// Decode RAW-stream
|
||
|
||
raw_len = raw_data->Length;
|
||
|
||
if (raw_len < 80)
|
||
return;
|
||
|
||
mydelete(raw_data, raw_len - 6, 7); // Does this remove trailing flag
|
||
raw_len = raw_data->Length;
|
||
|
||
nrzi_data = get_NRZI_data(raw_data, last_nrzi_bit);
|
||
|
||
if (nrzi_data == NULL)
|
||
return;
|
||
|
||
// data = newString();
|
||
data = get_pkt_data(nrzi_data);
|
||
|
||
len = data->Length;
|
||
|
||
if (len < pkt_raw_min_len)
|
||
{
|
||
freeString(nrzi_data);
|
||
freeString(data);
|
||
return;
|
||
}
|
||
|
||
crc1 = get_fcs(data->Data, len - 2);
|
||
crc2 = (data->Data[len - 1] << 8) | data->Data[len - 2];
|
||
|
||
// MEM recovery
|
||
|
||
arq_mem = FALSE;
|
||
|
||
if (raw_len > 2970)
|
||
freeString(nrzi_data);
|
||
else
|
||
{
|
||
Add(&pDET->mem_ARQ_buf[snd_ch], nrzi_data);
|
||
|
||
if (pDET->mem_ARQ_buf[snd_ch].Count > MEMRecovery[snd_ch])
|
||
Delete(&pDET->mem_ARQ_buf[snd_ch], 0);
|
||
|
||
if (crc1 != crc2)
|
||
{
|
||
freeString(data);
|
||
data = get_pkt_data(memory_ARQ(&pDET->mem_ARQ_buf[snd_ch], nrzi_data));
|
||
crc1 = get_fcs(data->Data, len - 2);
|
||
arq_mem = TRUE;
|
||
}
|
||
}
|
||
|
||
if (crc1 == crc2)
|
||
{
|
||
if (arq_mem)
|
||
{
|
||
Debugprintf("Good CRC after Memory ARQ correction %x Len %d chan %d rcvr %d emph %d", crc1, len, snd_ch, rcvr_nr, emph);
|
||
stat_r_mem++;
|
||
|
||
pDET->emph_decoded = 2; //MEM
|
||
pDET->rx_decoded = decodedMEM;
|
||
}
|
||
else
|
||
{
|
||
Debugprintf("Good CRC %x Len %d chan %d rcvr %d emph %d", crc1, len, snd_ch, rcvr_nr, emph);
|
||
|
||
pDET->rx_decoded = decodedNormal;
|
||
pDET->emph_decoded = 4; //Normal
|
||
}
|
||
|
||
|
||
if (detect_list[snd_ch].Count > 0 &&
|
||
my_indexof(&detect_list[snd_ch], data) >= 0)
|
||
{
|
||
// Already have a copy of this frame
|
||
|
||
freeString(data);
|
||
Debugprintf("Discarding copy rcvr %d emph %d", rcvr_nr, emph);
|
||
return;
|
||
}
|
||
|
||
string * xx = newString();
|
||
memset(xx->Data, 0, 16);
|
||
|
||
sprintf(Mode, "AX25 %d", centreFreq[snd_ch]);
|
||
|
||
|
||
Add(&detect_list_c[snd_ch], xx);
|
||
Add(&detect_list[snd_ch], data);
|
||
|
||
if (arq_mem)
|
||
stringAdd(xx, "MEM", 3);
|
||
else
|
||
stringAdd(xx, "", 0);
|
||
|
||
sprintf(Mode, "AX25 %d", centreFreq[snd_ch]);
|
||
|
||
stringAdd(xx, Mode, strlen(Mode));
|
||
|
||
|
||
return;
|
||
|
||
}
|
||
|
||
// Single bit recovery
|
||
|
||
freeString(data); // finished with original
|
||
|
||
if (recovery[snd_ch] == 0 || raw_len > 2970)
|
||
return;
|
||
|
||
raw = raw_data->Data;
|
||
raw1 = raw_data1->Data;
|
||
|
||
for (i = 0; i < raw_len; i++)
|
||
{
|
||
if (raw[i] != raw1[i])
|
||
{
|
||
//change bit
|
||
raw[i] ^= 1;
|
||
|
||
// get new data
|
||
|
||
data = get_pkt_data2(raw_data, last_nrzi_bit);
|
||
|
||
//restore bit
|
||
|
||
raw[i] ^= 1;
|
||
|
||
len = data->Length;
|
||
|
||
if (len > pkt_raw_min_len)
|
||
{
|
||
crc1 = get_fcs(data->Data, len - 2);
|
||
crc2 = (data->Data[len - 1] << 8) | data->Data[len - 2];
|
||
|
||
if (crc1 == crc2)
|
||
{
|
||
Debugprintf("Good CRC after single bit correction %x Len %d chan %d rcvr %d emph %d", crc1, len, snd_ch, rcvr_nr, emph);
|
||
|
||
if (detect_list[snd_ch].Count > 0 &&
|
||
my_indexof(&detect_list[snd_ch], data) >=- 0)
|
||
{
|
||
// Already have a copy of this frame
|
||
|
||
Debugprintf("Discarding copy rcvr %d, emph %d", rcvr_nr, emph);
|
||
freeString(data);
|
||
return;
|
||
}
|
||
string * xx = newString();
|
||
memset(xx->Data, 0, 16);
|
||
|
||
Add(&detect_list_c[snd_ch], xx);
|
||
Add(&detect_list[snd_ch], data);
|
||
stringAdd(xx, "SINGLE", 3);
|
||
|
||
pDET->rx_decoded = decodedSingle;
|
||
pDET->emph_decoded = 1; //SINGLE
|
||
|
||
return;
|
||
}
|
||
}
|
||
freeString(data); // finished with original
|
||
}
|
||
}
|
||
}
|
||
|
||
|
||
|
||
int lastcrc = 0;
|
||
|
||
|
||
void make_rx_frame_PSK(int snd_ch, int rcvr_nr, int emph, string * data)
|
||
{
|
||
word len, crc1, crc2;
|
||
|
||
len = data->Length;
|
||
|
||
if (len < pkt_raw_min_len)
|
||
return;
|
||
|
||
crc1 = get_fcs(data->Data, len - 2);
|
||
crc2 = (data->Data[len - 1] << 8) | data->Data[len - 2];
|
||
|
||
if (crc1 == crc2)
|
||
{
|
||
struct TDetector_t * pDET = &DET[emph][rcvr_nr];
|
||
|
||
Debugprintf("Good CRC %x Len %d chan %d rcvr %d emph %d", crc1, len, snd_ch, rcvr_nr, emph);
|
||
|
||
pDET->rx_decoded = decodedNormal;
|
||
pDET->emph_decoded = 4; //Normal
|
||
|
||
if (detect_list[snd_ch].Count > 0 &&
|
||
my_indexof(&detect_list[snd_ch], data) >= 0)
|
||
{
|
||
// Already have a copy of this frame
|
||
|
||
Debugprintf("Discarding copy rcvr %d emph %d", rcvr_nr, emph);
|
||
return;
|
||
}
|
||
|
||
string * xx = newString();
|
||
|
||
memset(xx->Data, 0, 16);
|
||
Add(&detect_list_c[snd_ch], xx);
|
||
xx = duplicateString(data);
|
||
Add(&detect_list[snd_ch], xx);
|
||
}
|
||
}
|
||
|
||
|
||
/*
|
||
|
||
function memory_ARQ_FEC(buf: TStringList; data: string): string;
|
||
var
|
||
len,i,k: integer;
|
||
s,temp: string;
|
||
new_blk,temp_blk: TStringList;
|
||
n,err: byte;
|
||
done: boolean;
|
||
{
|
||
s = '';
|
||
if data='' then { result = s; exit; }
|
||
new_blk = TStringList.Create;
|
||
temp_blk = TStringList.Create;
|
||
temp = data;
|
||
len = length(data);
|
||
// Split new data;
|
||
repeat
|
||
n = ord(temp[1]) and $7F;
|
||
err = ord(temp[1]) and $80;
|
||
if err=0 then new_blk.Add(copy(temp,2,n)) else new_blk.Add('');
|
||
delete(temp,1,n+1);
|
||
until temp='';
|
||
// Search blocks
|
||
if (buf.Count>0) and (new_blk.Count>0) then
|
||
{
|
||
i = 0;
|
||
repeat
|
||
// If length is the same
|
||
if length(buf.Strings[i])=len then
|
||
{
|
||
temp = buf.Strings[i];
|
||
// If last 4 bytes is the same
|
||
if copy(temp,len-3,4)=copy(data,len-3,4) then
|
||
{
|
||
temp_blk.Clear;
|
||
repeat
|
||
n = ord(temp[1]) and $7F;
|
||
err = ord(temp[1]) and $80;
|
||
if err=0 then temp_blk.Add(copy(temp,2,n)) else temp_blk.Add('');
|
||
delete(temp,1,n+1);
|
||
until temp='';
|
||
// Add new parts
|
||
if new_blk.Count=temp_blk.Count then
|
||
{
|
||
done = TRUE;
|
||
for k = 0 to new_blk.Count-1 do
|
||
{
|
||
if (new_blk.Strings[k]='') and (temp_blk.Strings[k]<>'') then
|
||
new_blk.Strings[k] = temp_blk.Strings[k];
|
||
// Check if no empty data
|
||
if new_blk.Strings[k]='' then done = FALSE;
|
||
}
|
||
}
|
||
}
|
||
}
|
||
inc(i);
|
||
until (i=buf.Count) or done;
|
||
if done then for k = 0 to new_blk.Count-1 do s = s+new_blk.Strings[k];
|
||
}
|
||
result = s;
|
||
new_blk.Free;
|
||
temp_blk.Free
|
||
}
|
||
|
||
procedure add_to_ARQ_FEC(buf: TStringList; data: string);
|
||
{
|
||
if buf.Count=50 then buf.Delete(0);
|
||
buf.Add(data);
|
||
}
|
||
*/
|
||
|
||
void make_rx_frame_FEC(int snd_ch, int rcvr_nr, string * data, string * fec_data, word nErr)
|
||
{
|
||
}
|
||
|
||
/*var
|
||
len,crc1,crc2: word;
|
||
s: string;
|
||
i,k,n: word;
|
||
{
|
||
len = length(data);
|
||
if len<17 then exit;
|
||
crc1 = get_fcs(data,len-2);
|
||
crc2 = (ord(data[len]) shl 8) or ord(data[len-1]);
|
||
if crc1=crc2 then
|
||
{
|
||
if detect_list[snd_ch].Count>0 then
|
||
{
|
||
//if detect_list[snd_ch].IndexOf(data)<0 then
|
||
if my_indexof(detect_list[snd_ch],data)<0 then
|
||
{
|
||
detect_list[snd_ch].Add(data);
|
||
detect_list_c[snd_ch].Add('Err: '+inttostr(nErr));
|
||
}
|
||
end
|
||
else
|
||
{
|
||
detect_list[snd_ch].Add(data);
|
||
detect_list_c[snd_ch].Add('Err: '+inttostr(nErr));
|
||
}
|
||
add_to_ARQ_FEC(DET[0,rcvr_nr].mem_ARQ_F_buf[snd_ch],fec_data);
|
||
}
|
||
if crc1<>crc2 then
|
||
{
|
||
data = memory_ARQ_FEC(DET[0,rcvr_nr].mem_ARQ_F_buf[snd_ch],fec_data);
|
||
add_to_ARQ_FEC(DET[0,rcvr_nr].mem_ARQ_F_buf[snd_ch],fec_data);
|
||
if data<>'' then
|
||
{
|
||
len = length(data);
|
||
crc1 = get_fcs(data,len-2);
|
||
crc2 = (ord(data[len]) shl 8) or ord(data[len-1]);
|
||
if crc1=crc2 then
|
||
{
|
||
if detect_list[snd_ch].Count>0 then
|
||
{
|
||
//if detect_list[snd_ch].IndexOf(data)<0 then
|
||
if my_indexof(detect_list[snd_ch],data)<0 then
|
||
{
|
||
detect_list[snd_ch].Add(data);
|
||
detect_list_c[snd_ch].Add('MEM Err: '+inttostr(nErr));
|
||
}
|
||
end
|
||
else
|
||
{
|
||
detect_list[snd_ch].Add(data);
|
||
detect_list_c[snd_ch].Add('MEM Err: '+inttostr(nErr));
|
||
}
|
||
}
|
||
}
|
||
}
|
||
}
|
||
|
||
*/
|
||
//////////////////////////// PLL-Peak-detector ////////////////////////////
|
||
|
||
void Mux3(int snd_ch, int rcvr_nr, int emph, float * src1, float * core, float *dest, float * prevI, float * prevQ, int tap, int buf_size)
|
||
{
|
||
float pi2 = 2 * pi;
|
||
|
||
int i;
|
||
float x;
|
||
float acc1, acc2, acc3, mag;
|
||
int tap4;
|
||
int tap_cnt;
|
||
unsigned int ii, kk;
|
||
|
||
float Preemphasis6, Preemphasis12, MUX3_osc, AGC;
|
||
float AFC_IZ1, AFC_QZ1, AFC_IZ2, AFC_QZ2;
|
||
|
||
// looks like this is an LPF
|
||
|
||
// Get local copy of this detectors variables
|
||
|
||
struct TDetector_t * pDET = &DET[emph][rcvr_nr];
|
||
|
||
Preemphasis6 = pDET->Preemphasis6[snd_ch];
|
||
Preemphasis12 = pDET->Preemphasis12[snd_ch];
|
||
MUX3_osc = pDET->MUX3_osc[snd_ch];
|
||
AGC = pDET->AGC[snd_ch];
|
||
AFC_IZ2 = pDET->AFC_IZ2[snd_ch];
|
||
AFC_QZ2 = pDET->AFC_QZ2[snd_ch];
|
||
AFC_QZ1 = pDET->AFC_QZ1[snd_ch];
|
||
AFC_IZ1 = pDET->AFC_IZ1[snd_ch];
|
||
//
|
||
tap4 = tap * 4;
|
||
x = active_rx_freq[snd_ch] * pi2 / RX_Samplerate;
|
||
|
||
fmove(&prevI[buf_size], &prevI[0], tap4);
|
||
fmove(&prevQ[buf_size], &prevQ[0], tap4);
|
||
tap_cnt = tap;
|
||
|
||
if (prevI[128] != prevI[128])
|
||
prevI[128] = 0;
|
||
|
||
for (i = 0; i < buf_size; i++)
|
||
{
|
||
// Pre-emphasis 6dB
|
||
if (emph > 0)
|
||
{
|
||
acc1 = Preemphasis6 - src1[i];
|
||
Preemphasis6 = src1[i];
|
||
src1[i] = acc1;
|
||
}
|
||
// Pre-emphasis 12dB
|
||
if (emph > 1)
|
||
{
|
||
acc1 = Preemphasis12 - src1[i];
|
||
Preemphasis12 = src1[i];
|
||
src1[i] = acc1;
|
||
}
|
||
//
|
||
MUX3_osc = MUX3_osc + x;
|
||
|
||
if (MUX3_osc > pi2)
|
||
MUX3_osc = MUX3_osc - pi2;
|
||
|
||
if (src1[i] != src1[i])
|
||
src1[i] = 0;
|
||
|
||
if (prevI[128] != prevI[128])
|
||
prevI[128] = 0;
|
||
|
||
|
||
prevI[tap_cnt] = src1[i] * sinf(MUX3_osc);
|
||
prevQ[tap_cnt] = src1[i] * cosf(MUX3_osc);
|
||
|
||
if (prevI[128] != prevI[128])
|
||
prevI[tap_cnt] = src1[i] * sinf(MUX3_osc);
|
||
|
||
if (prevI[128] != prevI[128])
|
||
prevI[128] = 0;
|
||
/*
|
||
|
||
mag = sqrtf(prevI[tap_cnt] * prevI[tap_cnt] + prevQ[tap_cnt] * prevQ[tap_cnt]);
|
||
DET[emph][rcvr_nr].AGC1[snd_ch] = 0.5*DET[emph][rcvr_nr].AGC1[snd_ch] + 0.5*mag;
|
||
AGC = 0.5*AGC + 0.5*DET[emph][rcvr_nr].AGC1[snd_ch];
|
||
if (AGC > 1)
|
||
begin
|
||
prevI[tap_cnt] = prevI[tap_cnt] / AGC;
|
||
prevQ[tap_cnt] = prevQ[tap_cnt] / AGC;
|
||
end
|
||
*/
|
||
|
||
|
||
// Fast AGC
|
||
|
||
mag = sqrtf(prevI[tap_cnt] * prevI[tap_cnt] + prevQ[tap_cnt] * prevQ[tap_cnt]);
|
||
|
||
AGC = 0.5 * AGC + 0.5 *mag;
|
||
|
||
if (AGC > 1)
|
||
{
|
||
prevI[tap_cnt] = prevI[tap_cnt] / AGC;
|
||
prevQ[tap_cnt] = prevQ[tap_cnt] / AGC;
|
||
}
|
||
|
||
ii = i << 2;
|
||
kk = tap << 2;
|
||
|
||
// C version of delphi asm code below
|
||
{
|
||
float accum = 0.0f;
|
||
float fp1;
|
||
|
||
int ebx;
|
||
float * edi;
|
||
|
||
edi = &prevI[i];
|
||
ebx = 0;
|
||
accum = 0.0f;
|
||
|
||
fsk_k1:
|
||
|
||
// FLD pushes operand onto stack, so old value goes to fp1
|
||
|
||
fp1 = accum;
|
||
accum = edi[ebx];
|
||
if (accum != accum)
|
||
accum = 0;
|
||
|
||
accum *= core[ebx];
|
||
if (accum != accum)
|
||
accum = 0;
|
||
accum += fp1;
|
||
if (accum != accum)
|
||
accum = 0;
|
||
|
||
ebx++;
|
||
if (ebx != tap)
|
||
goto fsk_k1;
|
||
|
||
acc1 = accum;
|
||
|
||
if (acc1 != acc1)
|
||
acc1 = 0;
|
||
|
||
edi = &prevQ[i];
|
||
|
||
ebx = 0;
|
||
accum = 0.0f;
|
||
|
||
fsk_k2:
|
||
|
||
fp1 = accum;
|
||
accum = edi[ebx];
|
||
accum *= core[ebx];
|
||
accum += fp1;
|
||
|
||
ebx++;
|
||
if (ebx != tap)
|
||
goto fsk_k2;
|
||
|
||
acc2 = accum;
|
||
}
|
||
|
||
if (acc1 != acc1)
|
||
acc1 = 0;
|
||
|
||
|
||
tap_cnt++;
|
||
|
||
/// PLL-Detector ///
|
||
|
||
|
||
dest[i] = (acc1 - AFC_IZ2)*AFC_QZ1 - (acc2 - AFC_QZ2)*AFC_IZ1;
|
||
|
||
// Check for NAN
|
||
|
||
if (dest[i] != dest[i])
|
||
dest[i] = 0.0f;
|
||
|
||
AFC_IZ2 = AFC_IZ1;
|
||
AFC_QZ2 = AFC_QZ1;
|
||
AFC_IZ1 = acc1;
|
||
AFC_QZ1 = acc2;
|
||
}
|
||
|
||
pDET->Preemphasis6[snd_ch] = Preemphasis6;
|
||
pDET->Preemphasis12[snd_ch] = Preemphasis12;
|
||
pDET->MUX3_osc[snd_ch] = MUX3_osc;
|
||
pDET->AGC[snd_ch] = AGC;
|
||
pDET->AFC_IZ2[snd_ch] = AFC_IZ2;
|
||
pDET->AFC_QZ2[snd_ch] = AFC_QZ2;
|
||
pDET->AFC_QZ1[snd_ch] = AFC_QZ1;
|
||
pDET->AFC_IZ1[snd_ch] = AFC_IZ1;
|
||
}
|
||
|
||
|
||
|
||
void Mux3_PSK(int snd_ch, int rcvr_nr, int emph, float * src1, float * core, float *destI, float *destQ, float * prevI, float * prevQ, int tap, int buf_size)
|
||
{
|
||
float pi2 = 2 * pi;
|
||
|
||
int i;
|
||
float x;
|
||
float acc1, acc2, mag;
|
||
int tap4;
|
||
int prev_cnt, tap_cnt;
|
||
|
||
float Preemphasis6, Preemphasis12, MUX3_osc;
|
||
|
||
// looks like this is an LPF
|
||
|
||
// Get local copy of this detectors variables
|
||
|
||
struct TDetector_t * pDET = &DET[emph][rcvr_nr];
|
||
|
||
Preemphasis6 = pDET->Preemphasis6[snd_ch];
|
||
Preemphasis12 = pDET->Preemphasis12[snd_ch];
|
||
MUX3_osc = pDET->MUX3_osc[snd_ch];
|
||
|
||
tap4 = tap * 4;
|
||
|
||
x = active_rx_freq[snd_ch] * pi2 / RX_Samplerate;
|
||
|
||
fmove(&prevI[buf_size], &prevI[0], tap4);
|
||
fmove(&prevQ[buf_size], &prevQ[0], tap4);
|
||
|
||
tap_cnt = tap;
|
||
|
||
if (prevI[128] != prevI[128])
|
||
prevI[128] = 0;
|
||
|
||
for (i = 0; i < buf_size; i++)
|
||
{
|
||
// Pre-emphasis 6dB
|
||
if (emph > 0)
|
||
{
|
||
acc1 = Preemphasis6 - src1[i];
|
||
Preemphasis6 = src1[i];
|
||
src1[i] = acc1;
|
||
}
|
||
// Pre-emphasis 12dB
|
||
if (emph > 1)
|
||
{
|
||
acc1 = Preemphasis12 - src1[i];
|
||
Preemphasis12 = src1[i];
|
||
src1[i] = acc1;
|
||
}
|
||
|
||
MUX3_osc = MUX3_osc + x;
|
||
|
||
if (MUX3_osc > pi2)
|
||
MUX3_osc = MUX3_osc - pi2;
|
||
|
||
prevI[tap_cnt] = src1[i] * sinf(MUX3_osc);
|
||
prevQ[tap_cnt] = src1[i] * cosf(MUX3_osc);
|
||
|
||
|
||
// C version of delphi asm code
|
||
{
|
||
float accum = 0.0f;
|
||
float fp1;
|
||
|
||
int ebx;
|
||
float * edi;
|
||
|
||
edi = &prevI[i];
|
||
ebx = 0;
|
||
accum = 0.0f;
|
||
|
||
fsk_k1:
|
||
|
||
// FLD pushes operand onto stack, so old value goes to fp1
|
||
|
||
fp1 = accum;
|
||
accum = edi[ebx];
|
||
accum *= core[ebx];
|
||
accum += fp1;
|
||
|
||
ebx++;
|
||
|
||
if (ebx != tap)
|
||
goto fsk_k1;
|
||
|
||
acc1 = accum;
|
||
|
||
edi = &prevQ[i];
|
||
|
||
ebx = 0;
|
||
accum = 0.0f;
|
||
|
||
fsk_k2:
|
||
|
||
fp1 = accum;
|
||
accum = edi[ebx];
|
||
accum *= core[ebx];
|
||
accum += fp1;
|
||
|
||
ebx++;
|
||
if (ebx != tap)
|
||
goto fsk_k2;
|
||
|
||
acc2 = accum;
|
||
}
|
||
|
||
if (acc1 != acc1)
|
||
acc1 = 0;
|
||
|
||
tap_cnt++;
|
||
|
||
destI[i] = acc1;
|
||
destQ[i] = acc2;
|
||
}
|
||
|
||
pDET->Preemphasis6[snd_ch] = Preemphasis6;
|
||
pDET->Preemphasis12[snd_ch] = Preemphasis12;
|
||
pDET->MUX3_osc[snd_ch] = MUX3_osc;
|
||
|
||
}
|
||
|
||
int stats[2] = { 0 };
|
||
|
||
#define dcd_corr 0.11111f
|
||
|
||
void decode_stream_MPSK(int snd_ch, int rcvr_nr, float * src, int buf_size, int last)
|
||
{
|
||
|
||
#ifndef XXXX
|
||
|
||
// Until ASM is converted
|
||
|
||
return;
|
||
}
|
||
#else
|
||
|
||
float pi2 = 2 * pi;
|
||
|
||
#define NR_FEC_CH 3
|
||
|
||
float agc_fast = 0.01f;
|
||
float agc_fast1 = 1 - agc_fast;
|
||
float agc_slow = agc_fast / 4;
|
||
float agc_slow1 = 1 - agc_slow;
|
||
|
||
word dcnt, dsize;
|
||
word i, k, j, j1, j2, j3;
|
||
single x, x1;
|
||
single amp, acc1, acc2;
|
||
single sumI, sumQ, sumIQ, muxI, muxQ;
|
||
word tap_cnt, tap_cnt1;
|
||
single afc_lim;
|
||
word i_tap, tap;
|
||
single AFC, k1, k2, freq;
|
||
single maxval, div_bit_afc, baudrate;
|
||
word max_cnt;
|
||
single AmpI, AmpQ, angle, muxI1, muxQ1, muxI2, muxQ2, sumIQ1, sumIQ2;
|
||
single AFC_acc1, AFC_acc2;
|
||
single BIT_acc1, BIT_acc2;
|
||
integer AFC_newpkpos;
|
||
//
|
||
single threshol;
|
||
single tr;
|
||
Byte fec_ch, bit;
|
||
longword ii, kk;
|
||
single * core, *prevI, *prevQ;
|
||
//
|
||
unsigned long long bit64 = 0;
|
||
boolean hdr_ok;
|
||
Byte fec_code;
|
||
string fec_data_blk;
|
||
string line1;
|
||
|
||
Byte line[512];
|
||
int linelen = 0;
|
||
|
||
integer nErr;
|
||
word crc1, crc2, size;
|
||
|
||
Byte hdr_byte[15] = "";
|
||
|
||
tr = dcd_threshold * dcd_corr;
|
||
|
||
if (last)
|
||
{
|
||
if (dcd_hdr_cnt[snd_ch] == 0)
|
||
dcd_on_hdr[snd_ch] = 0;
|
||
|
||
dcd_bit_cnt[snd_ch] = 0;
|
||
}
|
||
|
||
baudrate = 400;
|
||
div_bit_afc = 1.0f / roundf(BIT_AFC*(RX_Samplerate / 11025));
|
||
x1 = baudrate / RX_Samplerate;
|
||
max_cnt = roundf(RX_Samplerate / baudrate);
|
||
//
|
||
|
||
afc_lim = rx_baudrate[snd_ch] * 0.1f;
|
||
dsize = buf_size / n_INTR[snd_ch];
|
||
tap = LPF_tap[snd_ch];
|
||
i_tap = INTR_tap[snd_ch];
|
||
freq = active_rx_freq[snd_ch];
|
||
|
||
|
||
for (fec_ch = 0; fec_ch <= NR_FEC_CH; fec_ch++)
|
||
{
|
||
struct TMChannel_t * pMChan = &DET[0][rcvr_nr].MChannel[snd_ch][fec_ch];
|
||
|
||
fmove(&pMChan->prev_dLPFI_buf[buf_size], &pMChan->prev_dLPFI_buf[0], i_tap * 4);
|
||
fmove(&pMChan->prev_dLPFQ_buf[buf_size], &pMChan->prev_dLPFQ_buf[0], i_tap * 4);
|
||
fmove(&pMChan->prev_LPF1I_buf[dsize], &pMChan->prev_LPF1I_buf[0], tap * 4);
|
||
fmove(&pMChan->prev_LPF1Q_buf[dsize], &pMChan->prev_LPF1Q_buf[0], tap * 4);
|
||
fmove(&pMChan->prev_AFCI_buf[dsize], &pMChan->prev_AFCI_buf[0], tap * 4);
|
||
fmove(&pMChan->prev_AFCQ_buf[dsize], &pMChan->prev_AFCQ_buf[0], tap * 4);
|
||
}
|
||
|
||
tap_cnt = i_tap;
|
||
tap_cnt1 = tap;
|
||
dcnt = 0;
|
||
k = 0;
|
||
|
||
for (i = 0; i < buf_size; i++)
|
||
{
|
||
for (fec_ch = 0; fec_ch <= NR_FEC_CH; fec_ch++)
|
||
{
|
||
struct TDetector_t * pDET = &DET[0][rcvr_nr];
|
||
|
||
x = (freq + pDET->AFC_dF[snd_ch] + ch_offset[fec_ch])*pi2 / RX_Samplerate;
|
||
|
||
struct TMChannel_t * pMChan = &pDET->MChannel[snd_ch][fec_ch];
|
||
{
|
||
pMChan->MUX_osc = pMChan->MUX_osc + x;
|
||
|
||
if (pMChan->MUX_osc > pi2)
|
||
pMChan->MUX_osc = pMChan->MUX_osc - pi2;
|
||
|
||
pMChan->prev_dLPFI_buf[tap_cnt] = src[i] * sinf(pMChan->MUX_osc);
|
||
pMChan->prev_dLPFQ_buf[tap_cnt] = src[i] * cosf(pMChan->MUX_osc);
|
||
prevI = pMChan->prev_dLPFI_buf;
|
||
prevQ = pMChan->prev_dLPFQ_buf;
|
||
core = INTR_core[snd_ch];
|
||
// Decimation filter
|
||
ii = i << 2;
|
||
kk = i_tap << 2;
|
||
|
||
_asm
|
||
{
|
||
push eax;
|
||
push ebx;
|
||
push edi;
|
||
push esi;
|
||
mov edi, prevI;
|
||
mov esi, core;
|
||
add edi, ii;
|
||
mov eax, kk;
|
||
xor ebx, ebx;
|
||
fldz;
|
||
lk1:
|
||
fld dword ptr[edi + ebx];
|
||
fmul dword ptr[esi + ebx];
|
||
fadd;
|
||
add ebx, 4;
|
||
cmp ebx, eax;
|
||
jne lk1;
|
||
fstp dword ptr acc1;
|
||
wait;
|
||
mov edi, prevQ;
|
||
add edi, ii;
|
||
xor ebx, ebx;
|
||
fldz;
|
||
lk2:
|
||
fld dword ptr[edi + ebx];
|
||
fmul dword ptr[esi + ebx];
|
||
fadd;
|
||
add ebx, 4;
|
||
cmp ebx, eax;
|
||
jne lk2;
|
||
fstp dword ptr acc2;
|
||
wait;
|
||
pop esi;
|
||
pop edi;
|
||
pop ebx;
|
||
pop eax;
|
||
}
|
||
}
|
||
|
||
if (fec_ch == NR_FEC_CH)
|
||
tap_cnt++;
|
||
|
||
// Decimation
|
||
|
||
if (dcnt == 0)
|
||
{
|
||
{
|
||
pMChan->prev_LPF1I_buf[tap_cnt1] = acc1;
|
||
pMChan->prev_LPF1Q_buf[tap_cnt1] = acc2;
|
||
pMChan->prev_AFCI_buf[tap_cnt1] = acc1;
|
||
pMChan->prev_AFCQ_buf[tap_cnt1] = acc2;
|
||
// Bit-filter
|
||
prevI = pMChan->prev_LPF1I_buf;
|
||
prevQ = pMChan->prev_LPF1Q_buf;
|
||
core = LPF_core[snd_ch];
|
||
ii = k << 2;
|
||
kk = tap << 2;
|
||
|
||
__asm
|
||
{
|
||
push eax;
|
||
push ebx;
|
||
push edi;
|
||
push esi;
|
||
mov edi, prevI;
|
||
mov esi, core;
|
||
add edi, ii;
|
||
mov eax, kk;
|
||
xor ebx, ebx;
|
||
fldz;
|
||
xk1:
|
||
fld dword ptr[edi + ebx];
|
||
fmul dword ptr[esi + ebx];
|
||
fadd;
|
||
add ebx, 4;
|
||
cmp ebx, eax;
|
||
jne xk1;
|
||
fstp dword ptr BIT_acc1;
|
||
wait;
|
||
mov edi, prevQ;
|
||
add edi, ii;
|
||
xor ebx, ebx;
|
||
fldz;
|
||
xk2:
|
||
fld dword ptr[edi + ebx];
|
||
fmul dword ptr[esi + ebx];
|
||
fadd;
|
||
add ebx, 4;
|
||
cmp ebx, eax;
|
||
jne xk2;
|
||
fstp dword ptr BIT_acc2;
|
||
wait;
|
||
pop esi;
|
||
pop edi;
|
||
pop ebx;
|
||
pop eax;
|
||
}
|
||
|
||
// AFC-filter
|
||
prevI = pMChan->prev_AFCI_buf;
|
||
prevQ = pMChan->prev_AFCQ_buf;
|
||
core = AFC_core[snd_ch];
|
||
ii = k << 2;
|
||
kk = tap << 2;
|
||
_asm
|
||
{
|
||
push eax;
|
||
push ebx;
|
||
push edi;
|
||
push esi;
|
||
mov edi, prevI;
|
||
mov esi, core;
|
||
add edi, ii;
|
||
mov eax, kk;
|
||
xor ebx, ebx;
|
||
fldz;
|
||
xxk1:
|
||
fld dword ptr[edi + ebx];
|
||
fmul dword ptr[esi + ebx];
|
||
fadd;
|
||
add ebx, 4;
|
||
cmp ebx, eax;
|
||
jne xxk1;
|
||
fstp dword ptr AFC_acc1;
|
||
wait;
|
||
mov edi, prevQ;
|
||
add edi, ii;
|
||
xor ebx, ebx;
|
||
fldz;
|
||
xxk2:
|
||
fld dword ptr[edi + ebx];
|
||
fmul dword ptr[esi + ebx];
|
||
fadd;
|
||
add ebx, 4;
|
||
cmp ebx, eax;
|
||
jne xxk2;
|
||
fstp dword ptr AFC_acc2;
|
||
wait;
|
||
pop esi;
|
||
pop edi;
|
||
pop ebx;
|
||
pop eax;
|
||
}
|
||
}
|
||
|
||
// AGC
|
||
|
||
amp = sqrtf(BIT_acc1*BIT_acc1 + BIT_acc2 * BIT_acc2);
|
||
if (amp > pDET->PSK_AGC[snd_ch])
|
||
pDET->PSK_AGC[snd_ch] = pDET->PSK_AGC[snd_ch] * agc_fast1 + amp*agc_fast;
|
||
else
|
||
pDET->PSK_AGC[snd_ch] = pDET->PSK_AGC[snd_ch] * agc_slow1 + amp*agc_slow;
|
||
|
||
if (pDET->PSK_AGC[snd_ch] > 1)
|
||
{
|
||
BIT_acc1 = BIT_acc1 / pDET->PSK_AGC[snd_ch];
|
||
BIT_acc2 = BIT_acc2 / pDET->PSK_AGC[snd_ch];
|
||
AFC_acc1 = AFC_acc1 / pDET->PSK_AGC[snd_ch];
|
||
AFC_acc2 = AFC_acc2 / pDET->PSK_AGC[snd_ch];
|
||
amp = amp / pDET->PSK_AGC[snd_ch];
|
||
}
|
||
|
||
// AFC Correction
|
||
|
||
|
||
sumIQ = (AFC_acc1 - pMChan->AFC_IZ2)*pMChan->AFC_QZ1 - (AFC_acc2 - pMChan->AFC_QZ2)*pMChan->AFC_IZ1;
|
||
pMChan->AFC_IZ2 = pMChan->AFC_IZ1;
|
||
pMChan->AFC_QZ2 = pMChan->AFC_QZ1;
|
||
pMChan->AFC_IZ1 = AFC_acc1;
|
||
pMChan->AFC_QZ1 = AFC_acc2;
|
||
|
||
pDET->AFC_dF[snd_ch] = pDET->AFC_dF[snd_ch] - sumIQ * 0.07f; // AFC_LPF=1
|
||
|
||
if (pDET->AFC_dF[snd_ch] > afc_lim)
|
||
pDET->AFC_dF[snd_ch] = afc_lim;
|
||
|
||
if (pDET->AFC_dF[snd_ch] < -afc_lim)
|
||
pDET->AFC_dF[snd_ch] = -afc_lim;
|
||
|
||
|
||
pMChan->AFC_bit_buf1I[pDET->AFC_cnt[snd_ch]] = BIT_acc1;
|
||
pMChan->AFC_bit_buf1Q[pDET->AFC_cnt[snd_ch]] = BIT_acc2;
|
||
pMChan->AFC_bit_buf2[pDET->AFC_cnt[snd_ch]] = amp;
|
||
|
||
if (fec_ch == NR_FEC_CH)
|
||
{
|
||
pDET->AFC_cnt[snd_ch]++;
|
||
pDET->AFC_bit_osc[snd_ch] = pDET->AFC_bit_osc[snd_ch] + x1;
|
||
|
||
if (pDET->AFC_bit_osc[snd_ch] >= 1)
|
||
{
|
||
// Find the maximum in the synchronization buffer
|
||
|
||
for (j = 0; j <= NR_FEC_CH; j++)
|
||
{
|
||
struct TMChannel_t * pMChan = &pDET->MChannel[snd_ch][j];
|
||
|
||
maxval = 0;
|
||
|
||
for (j1 = 0; j1 < pDET->AFC_cnt[snd_ch]; j1++)
|
||
{
|
||
amp = pMChan->AFC_bit_buf2[j1];
|
||
|
||
pDET->AFC_bit_buf[snd_ch][j1] = pDET->AFC_bit_buf[snd_ch][j1] * 0.95 + amp*0.05;
|
||
|
||
if (pDET->AFC_bit_buf[snd_ch][j1] > maxval)
|
||
{
|
||
{
|
||
AFC_newpkpos = j1;
|
||
maxval = pDET->AFC_bit_buf[snd_ch][j1];
|
||
}
|
||
}
|
||
|
||
k1 = 1.0f *AFC_newpkpos / (pDET->AFC_cnt[snd_ch] - 1);
|
||
k2 = pila(k1) - 1;
|
||
|
||
|
||
|
||
//AFC = div_bit_afc*k2;
|
||
AFC = div_bit_afc * k2*0.25; //for 4 carriers
|
||
if (k1 > 0.5)
|
||
pDET->AFC_bit_osc[snd_ch] = pDET->AFC_bit_osc[snd_ch] + AFC;
|
||
else
|
||
pDET->AFC_bit_osc[snd_ch] = pDET->AFC_bit_osc[snd_ch] - AFC;
|
||
|
||
//DCD feature
|
||
|
||
if (last)
|
||
{
|
||
DCD_LastPkPos[snd_ch] = DCD_LastPkPos[snd_ch] * 0.96f + AFC_newpkpos * 0.04f;
|
||
DCD_LastPerc[snd_ch] = DCD_LastPerc[snd_ch] * 0.96f + abs(AFC_newpkpos - DCD_LastPkPos[snd_ch])*0.04f;
|
||
if (DCD_LastPerc[snd_ch] >= tr || DCD_LastPerc[snd_ch] < 0.00001f)
|
||
dcd_bit_cnt[snd_ch] = dcd_bit_cnt[snd_ch] + 1;
|
||
else
|
||
dcd_bit_cnt[snd_ch] = dcd_bit_cnt[snd_ch] - 1;
|
||
}
|
||
|
||
}
|
||
// Bit-detector
|
||
|
||
AmpI = pMChan->AFC_bit_buf1I[AFC_newpkpos];
|
||
AmpQ = pMChan->AFC_bit_buf1Q[AFC_newpkpos];
|
||
muxI1 = AmpI * pMChan->AFC_IIZ1;
|
||
muxI2 = AmpQ * pMChan->AFC_IIZ1;
|
||
muxQ1 = AmpQ * pMChan->AFC_QQZ1;
|
||
muxQ2 = AmpI * pMChan->AFC_QQZ1;
|
||
sumIQ1 = muxI1 + muxQ1;
|
||
sumIQ2 = muxI2 - muxQ2;
|
||
angle = atan2f(sumIQ2, sumIQ1);
|
||
pMChan->AFC_IIZ1 = AmpI;
|
||
pMChan->AFC_QQZ1 = AmpQ;
|
||
|
||
// Phase corrector
|
||
|
||
if (fabsf(angle) < PI5)
|
||
pMChan->AngleCorr = pMChan->AngleCorr * 0.9f - angle * 0.1f;
|
||
else
|
||
{
|
||
if (angle > 0)
|
||
pMChan->AngleCorr = pMChan->AngleCorr * 0.9f + (pi - angle)*0.1f;
|
||
else
|
||
pMChan->AngleCorr = pMChan->AngleCorr * 0.9f + (-pi - angle)*0.1f;
|
||
}
|
||
angle = angle + pMChan->AngleCorr;
|
||
|
||
|
||
if (fabsf(angle) < PI5)
|
||
bit = RX_BIT1;
|
||
else
|
||
bit = RX_BIT0;
|
||
|
||
// DCD on flag
|
||
|
||
if (last)
|
||
{
|
||
if (dcd_hdr_cnt[snd_ch] > 0)
|
||
dcd_hdr_cnt[snd_ch]--;
|
||
|
||
DCD_header[snd_ch] = (DCD_header[snd_ch] >> 1) | (bit << 24);
|
||
|
||
if ((DCD_header[snd_ch] & 0xFFFF0000) == 0x7E7E0000 ||
|
||
(DCD_header[snd_ch] & 0xFFFFFF00) == 0x7E000000 ||
|
||
(DCD_header[snd_ch] & 0xFFFFFF00) == 0x00000000)
|
||
{
|
||
dcd_hdr_cnt[snd_ch] = 48;
|
||
dcd_on_hdr[snd_ch] = TRUE;
|
||
}
|
||
}
|
||
|
||
// header stream
|
||
bit64 = bit;
|
||
bit64 <<= 56;
|
||
|
||
pDET->FEC_header1[snd_ch][1] = (pDET->FEC_header1[snd_ch][1] >> 1) | (pDET->FEC_header1[snd_ch][0] << 63);
|
||
pDET->FEC_header1[snd_ch][0] = (pDET->FEC_header1[snd_ch][0] >> 1) | bit64;
|
||
|
||
// copy body
|
||
if (pDET->frame_status[snd_ch] == FRAME_LOAD)
|
||
{
|
||
pDET->bit_stream[snd_ch] = (pDET->bit_stream[snd_ch] >> 1) + bit;
|
||
pDET->bit_cnt[snd_ch]++;
|
||
if (pDET->bit_cnt[snd_ch] == 8)
|
||
{
|
||
pDET->bit_cnt[snd_ch] = 0;
|
||
pDET->FEC_len_cnt[snd_ch]++;
|
||
|
||
stringAdd(&pDET->FEC_rx_data[snd_ch], &pDET->bit_stream[snd_ch], 1);
|
||
|
||
if (pDET->FEC_len_cnt[snd_ch] == pDET->FEC_len[snd_ch])
|
||
{
|
||
// descrambler
|
||
scrambler(pDET->FEC_rx_data[snd_ch].Data, pDET->FEC_rx_data[snd_ch].Length);
|
||
// deinterleave
|
||
pDET->FEC_blk_int[snd_ch] = ((pDET->FEC_len[snd_ch] - 1) / 16) + 1;
|
||
|
||
linelen = pDET->FEC_rx_data[snd_ch].Length;
|
||
|
||
memcpy(line, pDET->FEC_rx_data[snd_ch].Data, linelen);
|
||
|
||
j3 = 1;
|
||
|
||
for (j1 = 0; j1 < 16; j1++)
|
||
{
|
||
for (j2 = 0; j2 < pDET->FEC_blk_int[snd_ch]; j2++)
|
||
{
|
||
if ((j2 * 16 + j1) <= pDET->FEC_len[snd_ch] && j3 <= pDET->FEC_len[snd_ch])
|
||
{
|
||
pDET->FEC_rx_data[snd_ch].Data[j2 * 16 + j1] = line[j3];
|
||
j3++;
|
||
}
|
||
}
|
||
}
|
||
|
||
// RS-decode
|
||
|
||
/*
|
||
|
||
line = pDET->FEC_rx_data[snd_ch];
|
||
pDET->FEC_rx_data[snd_ch].Length = 0;
|
||
do
|
||
{
|
||
line1 = copy(line, 1, 16);
|
||
size = length(line1);
|
||
FillChar(xEncoded, SizeOf(xEncoded), 0);
|
||
FillChar(xDecoded, SizeOf(xDecoded), 0);
|
||
move(line1[1], xEncoded[0], size);
|
||
RS.InitBuffers;
|
||
nErr = RS.DecodeRS(xEncoded, xDecoded);
|
||
line1 = '';
|
||
for j1 = MaxErrors * 2 to size - 1 do line1 = line1 + chr(xDecoded[j1]);
|
||
pDET->FEC_rx_data[snd_ch] = FEC_rx_data[snd_ch] + line1;
|
||
if nErr >= 0 then FEC_err[snd_ch] = FEC_err[snd_ch] + nErr;
|
||
// For MEM-ARQ
|
||
fec_code = length(line1) and $7F;
|
||
if nErr < 0 then fec_code = fec_code or $80;
|
||
fec_data_blk = fec_data_blk + chr(fec_code) + line1;
|
||
delete(line, 1, 16);
|
||
} while(line.Count);
|
||
*/
|
||
|
||
|
||
make_rx_frame_FEC(snd_ch, rcvr_nr, &pDET->FEC_rx_data[snd_ch], &fec_data_blk, pDET->FEC_err[snd_ch]);
|
||
pDET->FEC_rx_data[snd_ch].Length = 0;
|
||
pDET->frame_status[snd_ch] = FRAME_WAIT;
|
||
pDET->FEC_header1[snd_ch][0] = 0;
|
||
pDET->FEC_header1[snd_ch][1] = 0;
|
||
}
|
||
}
|
||
}
|
||
|
||
hdr_ok = FALSE;
|
||
|
||
// I think FEC_header1[0] and FEC_header1[1] form the 128 bit header
|
||
// We look for a pair of flags, but allow a few bits to be wrong
|
||
// as FEC may fix them
|
||
|
||
if (pDET->frame_status[snd_ch] == FRAME_WAIT)
|
||
{
|
||
j1 = (pDET->FEC_header1[snd_ch][1] >> 16) ^ 0x7E7E;
|
||
/*_asm
|
||
{
|
||
push ax;
|
||
push bx;
|
||
push cx;
|
||
mov ax, 15;
|
||
mov bx, j1;
|
||
zloop:
|
||
mov cx, bx;
|
||
and cx, 1;
|
||
cmp cx, 1;
|
||
jne is_zero;
|
||
inc ah; // count damaged bits
|
||
is_zero:
|
||
shr bx, 1;
|
||
dec al;
|
||
jnz zloop;
|
||
cmp ah, 5; // greater than 4 bits
|
||
jnb greater;
|
||
mov hdr_ok, TRUE;
|
||
greater:
|
||
pop cx;
|
||
pop bx;
|
||
pop ax;
|
||
}
|
||
*/
|
||
}
|
||
//if (FEC_header1[snd_ch][1] shr 24 and $FF=$7E) and (frame_status[snd_ch]=FRAME_WAIT) then
|
||
|
||
if (hdr_ok)
|
||
{
|
||
// Have up to 4 bits wrong in 7E7E pattern
|
||
|
||
// Extract header, check crc then try RS
|
||
|
||
hdr_ok = FALSE;
|
||
|
||
// if ((pDET->FEC_header1[snd_ch][1] & 0xffff0000) == 0x7E7E0000)
|
||
// {
|
||
|
||
hdr_byte[13] = (pDET->FEC_header1[snd_ch][1] >> 24) & 0xFF;
|
||
hdr_byte[14] = (pDET->FEC_header1[snd_ch][1] >> 16) & 0xFF;
|
||
|
||
if (hdr_byte[13] == 0x7E && hdr_byte[14] == 0x7E)
|
||
{
|
||
hdr_byte[1] = (pDET->FEC_header1[snd_ch][0] >> 56) & 0xFF;
|
||
hdr_byte[2] = (pDET->FEC_header1[snd_ch][0] >> 48) & 0xFF;
|
||
hdr_byte[3] = (pDET->FEC_header1[snd_ch][0] >> 40) & 0xFF;
|
||
hdr_byte[4] = (pDET->FEC_header1[snd_ch][0] >> 32) & 0xFF;
|
||
hdr_byte[5] = (pDET->FEC_header1[snd_ch][0] >> 24) & 0xFF;
|
||
hdr_byte[6] = (pDET->FEC_header1[snd_ch][0] >> 16) & 0xFF;
|
||
hdr_byte[7] = (pDET->FEC_header1[snd_ch][0] >> 8) & 0xFF;
|
||
hdr_byte[8] = pDET->FEC_header1[snd_ch][0] & 0xFF;
|
||
hdr_byte[9] = (pDET->FEC_header1[snd_ch][1] >> 56) & 0xFF;
|
||
hdr_byte[10] = (pDET->FEC_header1[snd_ch][1] >> 48) & 0xFF;
|
||
hdr_byte[11] = (pDET->FEC_header1[snd_ch][1] >> 40) & 0xFF;
|
||
hdr_byte[12] = (pDET->FEC_header1[snd_ch][1] >> 32) & 0xFF;
|
||
|
||
pDET->FEC_len[snd_ch] = hdr_byte[12] << 8 + hdr_byte[11];
|
||
line[0] = 0x7E;
|
||
line[1] = 0x7E;
|
||
line[2] = hdr_byte[12];
|
||
line[3] = hdr_byte[11];
|
||
|
||
crc1 = (hdr_byte[10] << 8) + hdr_byte[9];
|
||
crc2 = get_fcs(line, 4);
|
||
|
||
if (crc1 == crc2)
|
||
hdr_ok = TRUE;
|
||
|
||
Debugprintf("Len %d CRC %x %x", pDET->FEC_len[snd_ch], crc1, crc2);
|
||
}
|
||
/* if (!hdr_ok)
|
||
{
|
||
linelen = 0;
|
||
for (j1 = 14; j1 > 0; j1-)
|
||
line[linelen++] = hdr_byte[j1);
|
||
|
||
|
||
|
||
FillChar(xEncoded, SizeOf(xEncoded), 0);
|
||
FillChar(xDecoded, SizeOf(xDecoded), 0);
|
||
line = copy(&line, 7, 8) + copy(&line, 1, 6);
|
||
move(&line[1], xEncoded[0], 14);
|
||
RS.InitBuffers;
|
||
nErr = RS.DecodeRS(xEncoded, xDecoded);
|
||
if (nErr > -1)
|
||
{
|
||
line.Length = 0;
|
||
|
||
for (j1 = 8; j1 < 13; j1++)
|
||
stringAdd(&line, &xDecoded[j1], 1);
|
||
|
||
if (line[1] == 0x7E && line[2] == 0x7E)
|
||
{
|
||
FEC_len[snd_ch] = ord(line[3]) shl 8 + ord(line[4]);
|
||
crc1 = (line[5] << 8) + line[6]);
|
||
line = copy(line, 1, 4);
|
||
crc2 = get_fcs(line, 4);
|
||
if (crc1 == crc2)
|
||
hdr_ok = TRUE;
|
||
}
|
||
}
|
||
}
|
||
*/
|
||
if (hdr_ok)
|
||
{
|
||
pDET->FEC_len[snd_ch] = pDET->FEC_len[snd_ch] & 1023; //limit of length
|
||
if (pDET->FEC_len[snd_ch] > 0)
|
||
{
|
||
pDET->frame_status[snd_ch] = FRAME_LOAD;
|
||
pDET->FEC_len_cnt[snd_ch] = 0;
|
||
pDET->bit_cnt[snd_ch] = 0;
|
||
pDET->FEC_err[snd_ch] = 0;
|
||
pDET->FEC_rx_data[snd_ch].Length = 0;
|
||
fec_data_blk.Length = 0;
|
||
}
|
||
}
|
||
}
|
||
}
|
||
// Finalize
|
||
if (pDET->AFC_cnt[snd_ch] <= max_cnt)
|
||
for (j = pDET->AFC_cnt[snd_ch]; j <= max_cnt + 5; j++)
|
||
pDET->AFC_bit_buf[snd_ch][j] = 0.95f*pDET->AFC_bit_buf[snd_ch][j];
|
||
|
||
pDET->AFC_cnt[snd_ch] = 0;
|
||
pDET->AFC_bit_osc[snd_ch] = pDET->AFC_bit_osc[snd_ch] - 1;
|
||
}
|
||
}
|
||
if (fec_ch == NR_FEC_CH)
|
||
{
|
||
tap_cnt1++;
|
||
k++;
|
||
}
|
||
}
|
||
}
|
||
dcnt = (dcnt + 1) % n_INTR[snd_ch];
|
||
}
|
||
}
|
||
|
||
#endif
|
||
|
||
void make_rx_frame_FX25(int snd_ch, int rcvr_nr, int emph, string * data)
|
||
{
|
||
struct TDetector_t * pDET = &DET[emph][rcvr_nr];
|
||
|
||
word len, crc1, crc2;
|
||
|
||
len = data->Length;
|
||
|
||
if (len < pkt_raw_min_len)
|
||
{
|
||
free(data);
|
||
return;
|
||
}
|
||
|
||
crc1 = get_fcs(data->Data, len - 2);
|
||
crc2 = (data->Data[len - 1] << 8) | data->Data[len - 2];
|
||
|
||
if (crc1 != crc2)
|
||
{
|
||
freeString(data);
|
||
return;
|
||
}
|
||
Debugprintf("FEC Good CRC %x Len %d chan %d rcvr %d emph %d", crc1, len, snd_ch, rcvr_nr, emph);
|
||
|
||
pDET->rx_decoded = decodedFEC;
|
||
|
||
if (detect_list[snd_ch].Count > 0)
|
||
{
|
||
//if detect_list[snd_ch].IndexOf(data)<0 then
|
||
|
||
if (my_indexof(&detect_list[snd_ch], data) < 0)
|
||
{
|
||
string * xx = newString();
|
||
xx->Length = sprintf(xx->Data, "FX25 %d", centreFreq[snd_ch]);
|
||
|
||
|
||
Add(&detect_list_c[snd_ch], xx);
|
||
Add(&detect_list[snd_ch], data);
|
||
|
||
stringAdd(xx, "", 0);
|
||
}
|
||
else
|
||
{
|
||
// Should check if previous decode was Single or MEM and if so replace
|
||
|
||
Debugprintf("Discarding copy rcvr %d", rcvr_nr);
|
||
freeString(data);
|
||
}
|
||
}
|
||
else
|
||
{
|
||
string * xx = newString();
|
||
xx->Length = sprintf(xx->Data, "FX25 %d", centreFreq[snd_ch]);
|
||
|
||
Add(&detect_list_c[snd_ch], xx);
|
||
Add(&detect_list[snd_ch], data);
|
||
|
||
if (rcvr_nr == 0)
|
||
pDET->emph_decoded = 3; //FX.25
|
||
}
|
||
|
||
}
|
||
|
||
|
||
string * decode_FX25_data(TFX25 fx25)
|
||
{
|
||
integer eras_pos = 0, i, j, len, rs_res;
|
||
Byte a, k;
|
||
Byte bit, byte_rx, bit_stuff_cnt, bit_cnt = 0, frame_status, bit_stream;
|
||
|
||
string * data = newString();
|
||
|
||
int done;
|
||
Byte rs_block[256];
|
||
int RSOK;
|
||
|
||
bit_stream = 0;
|
||
len = fx25.size - fx25.rs_size;
|
||
frame_status = FRAME_WAIT;
|
||
|
||
done = 0;
|
||
|
||
// RS FEC
|
||
|
||
memset(rs_block, 0, 255);
|
||
memcpy(rs_block, fx25.data.Data, len);
|
||
memcpy(&rs_block[255 - fx25.rs_size], &fx25.data.Data[len], fx25.rs_size);
|
||
|
||
rs_res = fx25_decode_rs(rs_block, &eras_pos, 0, 0, fx25.rs_size);
|
||
|
||
if (rs_res == -1)
|
||
{
|
||
Debugprintf("RS Correction Failed");
|
||
return data;
|
||
}
|
||
|
||
if (rs_res == 0)
|
||
Debugprintf("RS No Errors");
|
||
else
|
||
Debugprintf("RS %d Errors Corrected", rs_res);
|
||
|
||
// need to do ax.25 decode of bit stream
|
||
|
||
i = 0;
|
||
|
||
while (i < len)
|
||
{
|
||
a = rs_block[i];
|
||
i++;
|
||
for (k = 0; k < 8; k++)
|
||
{
|
||
bit = a << 7;
|
||
a = a >> 1;
|
||
|
||
bit_stream = (bit_stream >> 1) | bit;
|
||
|
||
if ((bit_stream & FRAME_FLAG) == FRAME_FLAG && frame_status == FRAME_LOAD)
|
||
{
|
||
frame_status = FRAME_WAIT;
|
||
|
||
if (bit_cnt == 6 && data->Length)
|
||
return data;
|
||
}
|
||
|
||
if (frame_status == FRAME_LOAD)
|
||
{
|
||
if (bit_stuff_cnt == 5)
|
||
bit_stuff_cnt = 0;
|
||
else
|
||
{
|
||
if (bit == RX_BIT1)
|
||
bit_stuff_cnt++;
|
||
else
|
||
bit_stuff_cnt = 0;
|
||
|
||
byte_rx = (byte_rx >> 1) | bit;
|
||
bit_cnt++;
|
||
}
|
||
|
||
if (bit_cnt == 8)
|
||
{
|
||
stringAdd(data, &byte_rx, 1);
|
||
bit_cnt = 0;
|
||
}
|
||
}
|
||
|
||
if ((bit_stream && FRAME_FLAG == FRAME_FLAG) && frame_status == FRAME_WAIT)
|
||
{
|
||
frame_status = FRAME_LOAD;
|
||
bit_cnt = 0;
|
||
bit_stuff_cnt = 0;
|
||
data->Length = 0;
|
||
}
|
||
}
|
||
}
|
||
return data;
|
||
}
|
||
|
||
int FX25_corr[4] = {1, 1, 1, 1};
|
||
|
||
#define tags_nr 11
|
||
#define tt 8
|
||
|
||
unsigned long long tags[tags_nr] =
|
||
{
|
||
0xB74DB7DF8A532F3E, 0x26FF60A600CC8FDE, 0xC7DC0508F3D9B09E, 0x8F056EB4369660EE,
|
||
0x6E260B1AC5835FAE, 0xFF94DC634F1CFF4E, 0x1EB7B9CDBC09C00E, 0xDBF869BD2DBB1776,
|
||
0x3ADB0C13DEAE2836, 0xAB69DB6A543188D6, 0x4A4ABEC4A724B796
|
||
};
|
||
|
||
int sizes[tags_nr] = { 255, 144, 80, 48, 255, 160, 96, 64, 255, 192, 128 };
|
||
int rs_sizes[tags_nr] = { 16, 16, 16, 16, 32, 32, 32, 32, 64, 64, 64 };
|
||
|
||
/*
|
||
unsigned char get_corr_arm(unsigned long long n)
|
||
{
|
||
unsigned char max_corr;
|
||
unsigned char result = 255;
|
||
int i = 0;
|
||
|
||
while (i < tags_nr)
|
||
{
|
||
if (__builtin_popcountll(n ^ tags[i] <= tt))
|
||
return i;
|
||
}
|
||
|
||
return 255;
|
||
}
|
||
*/
|
||
|
||
char errors;
|
||
|
||
|
||
unsigned char get_corr(unsigned long long val)
|
||
{
|
||
unsigned long v;
|
||
unsigned long long n;
|
||
int i = 0;
|
||
|
||
while (i < tags_nr)
|
||
{
|
||
n = val ^ tags[i];
|
||
|
||
v = n;
|
||
|
||
v = v - ((v >> 1) & 0x55555555); // reuse input as temporary
|
||
v = (v & 0x33333333) + ((v >> 2) & 0x33333333); // temp
|
||
errors = ((v + (v >> 4) & 0xF0F0F0F) * 0x1010101) >> 24; // count
|
||
|
||
if (errors > tt)
|
||
{
|
||
i++;
|
||
continue;
|
||
}
|
||
|
||
v = n >> 32;
|
||
|
||
v = v - ((v >> 1) & 0x55555555); // reuse input as temporary
|
||
v = (v & 0x33333333) + ((v >> 2) & 0x33333333); // temp
|
||
errors += ((v + (v >> 4) & 0xF0F0F0F) * 0x1010101) >> 24; // count
|
||
|
||
if (errors <= tt)
|
||
return i;
|
||
|
||
i++;
|
||
}
|
||
return 255;
|
||
}
|
||
|
||
|
||
|
||
void decode_stream_FSK(int last, int snd_ch, int rcvr_nr, int emph, float * src_buf, float * bit_buf, int buf_size, string * data)
|
||
{
|
||
int i, k, j, n;
|
||
UCHAR bit;
|
||
UCHAR raw_bit;
|
||
UCHAR raw_bit1;
|
||
UCHAR raw_bit2;
|
||
float AFC, x, amp, k1, k2;
|
||
float baudrate;
|
||
float div_bit_afc;
|
||
word max_cnt;
|
||
float threshold;
|
||
float tr;
|
||
float Freq;
|
||
|
||
Byte sample_cnt;
|
||
float PkAmp, PkAmpMax = 0, MaxAmp, MinAmp, AverageAmp;
|
||
int newpkpos;
|
||
float bit_osc;
|
||
Byte last_rx_bit, bit_stream, frame_status;
|
||
|
||
TFX25 fx25;
|
||
unsigned long long tag64;
|
||
boolean rx_fx25_mode;
|
||
|
||
// get saved values to local variables to speed up access
|
||
|
||
struct TDetector_t * pDET = &DET[emph][rcvr_nr];
|
||
|
||
last_rx_bit = pDET->last_rx_bit[snd_ch];
|
||
sample_cnt = pDET->sample_cnt[snd_ch];
|
||
PkAmp = pDET->PkAmp[snd_ch];
|
||
PkAmpMax = pDET->PkAmpMax[snd_ch];
|
||
newpkpos = pDET->newpkpos[snd_ch];
|
||
bit_osc = pDET->bit_osc[snd_ch];
|
||
MaxAmp = pDET->MaxAmp[snd_ch];
|
||
MinAmp = pDET->MinAmp[snd_ch];
|
||
AverageAmp = pDET->AverageAmp[snd_ch];
|
||
bit_stream = pDET->bit_stream[snd_ch];
|
||
frame_status = pDET->frame_status[snd_ch];
|
||
|
||
fx25 = pDET->fx25[snd_ch];
|
||
|
||
if (fx25_mode[snd_ch] == FX25_MODE_NONE)
|
||
{
|
||
rx_fx25_mode = FALSE;
|
||
fx25.status = FX25_TAG;
|
||
}
|
||
else
|
||
rx_fx25_mode = TRUE;
|
||
|
||
|
||
tr = dcd_threshold * dcd_corr;
|
||
|
||
if (last)
|
||
{
|
||
// Update DCD status
|
||
|
||
if (dcd_hdr_cnt[snd_ch] == 0)
|
||
dcd_on_hdr[snd_ch] = 0;
|
||
|
||
dcd_bit_cnt[snd_ch] = 0;
|
||
}
|
||
|
||
// src_buf is input samples processed in some way.
|
||
// not sure what bit_buf is, but guess bits extracted from samples
|
||
// but then why floats ??
|
||
|
||
baudrate = 300;
|
||
|
||
div_bit_afc = 1.0f / roundf(BIT_AFC*(RX_Samplerate / 11025.0f));
|
||
|
||
x = baudrate / RX_Samplerate;
|
||
|
||
// I guess max_cnt is samples per bit
|
||
|
||
//was - why + 1 then round??
|
||
max_cnt = roundf(RX_Samplerate / baudrate) + 1;
|
||
|
||
max_cnt = (RX_Samplerate / baudrate);
|
||
|
||
for (i = 0; i < buf_size; i++)
|
||
{
|
||
// Seems to be accumulating squares of all samples in the input for one bit period
|
||
|
||
bit_buf[sample_cnt] = 0.95*bit_buf[sample_cnt] + 0.05*src_buf[i] * src_buf[i];
|
||
|
||
// Check for NAN
|
||
|
||
if (bit_buf[sample_cnt] != bit_buf[sample_cnt])
|
||
bit_buf[sample_cnt] = 0.0f;
|
||
|
||
// <20><><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD> <20><><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD> <20> <20><><EFBFBD><EFBFBD><EFBFBD><EFBFBD> <20><><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD>
|
||
// Find the maximum in the synchronization buffer
|
||
|
||
if (bit_buf[sample_cnt] > PkAmpMax)
|
||
{
|
||
PkAmp = src_buf[i];
|
||
PkAmpMax = bit_buf[sample_cnt];
|
||
newpkpos = sample_cnt;
|
||
}
|
||
sample_cnt++;
|
||
|
||
bit_osc = bit_osc + x;
|
||
|
||
// This seems to be how it does samples to bits
|
||
|
||
|
||
if (bit_osc >= 0.99f) // Allow for rounding errors
|
||
{
|
||
if (sample_cnt <= max_cnt)
|
||
for (k = sample_cnt; k <= max_cnt; k++)
|
||
bit_buf[k] = 0.95f*bit_buf[k];
|
||
|
||
k1 = (1.0f * newpkpos) / (sample_cnt - 1);
|
||
k2 = pila(k1) - 1;
|
||
AFC = div_bit_afc * k2;
|
||
|
||
if (k1 > 0.5f)
|
||
bit_osc = bit_osc + AFC;
|
||
else
|
||
bit_osc = bit_osc - AFC;
|
||
|
||
PkAmpMax = 0;
|
||
sample_cnt = 0;
|
||
|
||
// Not sure about this, but if bit_buf gets to NaN it stays there
|
||
|
||
bit_osc = bit_osc - 1;
|
||
//DCD feature
|
||
if (last)
|
||
{
|
||
DCD_LastPkPos[snd_ch] = DCD_LastPkPos[snd_ch] * 0.96f + newpkpos * 0.04f;
|
||
DCD_LastPerc[snd_ch] = DCD_LastPerc[snd_ch] * 0.96f + fabsf(newpkpos - DCD_LastPkPos[snd_ch])*0.04f;
|
||
|
||
if (DCD_LastPerc[snd_ch] >= tr || DCD_LastPerc[snd_ch] < 0.00001f)
|
||
dcd_bit_cnt[snd_ch]++;
|
||
else
|
||
dcd_bit_cnt[snd_ch]--;
|
||
}
|
||
|
||
amp = PkAmp;
|
||
|
||
if (amp > 0)
|
||
raw_bit1 = RX_BIT1;
|
||
else
|
||
raw_bit1 = RX_BIT0;
|
||
//
|
||
if (amp > 0)
|
||
MaxAmp = MaxAmp * 0.9f + amp*0.1f; //0.9
|
||
else
|
||
MinAmp = MinAmp * 0.9f + amp*0.1f;
|
||
|
||
amp = amp - (MaxAmp + MinAmp)*0.5f;
|
||
|
||
// Bit-detector
|
||
|
||
AverageAmp = AverageAmp * 0.5f + amp*0.5f;
|
||
threshold = 0.5f * AverageAmp;
|
||
|
||
if (amp > threshold)
|
||
raw_bit = RX_BIT1;
|
||
else
|
||
raw_bit = RX_BIT0;
|
||
|
||
// 0.75
|
||
|
||
if (amp > 0.75*AverageAmp)
|
||
raw_bit2 = RX_BIT1;
|
||
else
|
||
raw_bit2 = RX_BIT0;
|
||
|
||
if (raw_bit != raw_bit2)
|
||
raw_bit1 = raw_bit2;
|
||
|
||
// look for il2p before nrzi
|
||
|
||
if (il2p_mode[snd_ch])
|
||
{
|
||
il2p_rec_bit(snd_ch, rcvr_nr, emph, raw_bit);
|
||
if (il2p_mode[snd_ch] == IL2P_MODE_ONLY) // Dont try HDLC decode
|
||
continue;
|
||
}
|
||
//NRZI
|
||
|
||
if (raw_bit == last_rx_bit)
|
||
bit = RX_BIT1;
|
||
else
|
||
bit = RX_BIT0;
|
||
|
||
last_rx_bit = raw_bit;
|
||
//
|
||
bit_stream = (bit_stream >> 1) | bit;
|
||
|
||
// DCD on flag
|
||
|
||
if (last)
|
||
{
|
||
if (dcd_hdr_cnt[snd_ch] > 0)
|
||
dcd_hdr_cnt[snd_ch]--;
|
||
|
||
DCD_header[snd_ch] = (DCD_header[snd_ch] >> 1) | (bit << 24);
|
||
|
||
if (((DCD_header[snd_ch] & 0xFFFF0000) == 0x7E7E0000) ||
|
||
((DCD_header[snd_ch] & 0xFFFFFF00) == 0x7E000000) ||
|
||
((DCD_header[snd_ch] & 0xFFFFFF00) == 0x00000000))
|
||
{
|
||
dcd_hdr_cnt[snd_ch] = 48;
|
||
dcd_on_hdr[snd_ch] = 1;
|
||
}
|
||
}
|
||
|
||
// FX25 process
|
||
|
||
if (rx_fx25_mode)
|
||
{
|
||
if (fx25.status == FX25_LOAD)
|
||
{
|
||
//if last then DCD_on_hdr[snd_ch]:=true;
|
||
|
||
fx25.byte_rx = (fx25.byte_rx >> 1) | bit;
|
||
fx25.bit_cnt++;
|
||
|
||
if (fx25.bit_cnt == 8)
|
||
{
|
||
fx25.bit_cnt = 0;
|
||
stringAdd(&fx25.data, &fx25.byte_rx, 1);
|
||
fx25.size_cnt++;
|
||
if (fx25.size == fx25.size_cnt)
|
||
{
|
||
fx25.status = FX25_TAG;
|
||
make_rx_frame_FX25(snd_ch, rcvr_nr, emph, decode_FX25_data(fx25));
|
||
//if last and (DCD_hdr_cnt[snd_ch]=0) then DCD_on_hdr[snd_ch]:=false;
|
||
}
|
||
}
|
||
}
|
||
else
|
||
{
|
||
fx25.size = 0;
|
||
|
||
fx25.tag = (fx25.tag >> 1);
|
||
if (bit)
|
||
fx25.tag |= 0x8000000000000000;
|
||
|
||
tag64 = fx25.tag & 0XFFFFFFFFFFFFFFFE;
|
||
|
||
// FX25 tag correlation
|
||
|
||
if (FX25_corr[snd_ch])
|
||
{
|
||
unsigned char res;
|
||
|
||
res = get_corr(tag64);
|
||
|
||
if (res < tags_nr)
|
||
{
|
||
Debugprintf("Got FEC Tag %d Errors %d", res, errors);
|
||
fx25.size = sizes[res];
|
||
fx25.rs_size = rs_sizes[res];
|
||
}
|
||
}
|
||
else
|
||
{
|
||
if (tag64 == 0xB74DB7DF8A532F3E)
|
||
{
|
||
fx25.size = 255;
|
||
fx25.rs_size = 16;
|
||
}
|
||
if (tag64 == 0x26FF60A600CC8FDE)
|
||
{
|
||
fx25.size = 144;
|
||
fx25.rs_size = 16;
|
||
}
|
||
if (tag64 == 0xC7DC0508F3D9B09E)
|
||
{
|
||
fx25.size = 80;
|
||
fx25.rs_size = 16;
|
||
}
|
||
if (tag64 == 0x8F056EB4369660EE)
|
||
{
|
||
fx25.size = 48;
|
||
fx25.rs_size = 16;
|
||
}
|
||
if (tag64 == 0x6E260B1AC5835FAE)
|
||
{
|
||
fx25.size = 255;
|
||
fx25.rs_size = 32;
|
||
}
|
||
if (tag64 == 0xFF94DC634F1CFF4E)
|
||
{
|
||
fx25.size = 160;
|
||
fx25.rs_size = 32;
|
||
}
|
||
if (tag64 == 0x1EB7B9CDBC09C00E)
|
||
{
|
||
fx25.size = 96;
|
||
fx25.rs_size = 32;
|
||
}
|
||
if (tag64 == 0xDBF869BD2DBB1776)
|
||
{
|
||
fx25.size = 64;
|
||
fx25.rs_size = 32;
|
||
}
|
||
if (tag64 == 0x3ADB0C13DEAE2836)
|
||
{
|
||
fx25.size = 255;
|
||
fx25.rs_size = 64;
|
||
}
|
||
if (tag64 == 0xAB69DB6A543188D6)
|
||
{
|
||
fx25.size = 192;
|
||
fx25.rs_size = 64;
|
||
}
|
||
if (tag64 == 0x4A4ABEC4A724B796)
|
||
{
|
||
fx25.size = 128;
|
||
fx25.rs_size = 64;
|
||
}
|
||
}
|
||
if (fx25.size != 0)
|
||
{
|
||
fx25.status = FX25_LOAD;
|
||
fx25.data.Length = 0;
|
||
fx25.bit_cnt = 0;
|
||
fx25.size_cnt = 0;
|
||
centreFreq[snd_ch] = GuessCentreFreq(snd_ch);
|
||
}
|
||
}
|
||
}
|
||
//
|
||
|
||
|
||
if (bit_stream == 0xFF || bit_stream == 0x7F || bit_stream == 0xFE)
|
||
{
|
||
// All have 7 or more 1 bits
|
||
|
||
if (frame_status == FRAME_LOAD)
|
||
{
|
||
// Have started receiving frame
|
||
|
||
// Debugprintf("Frame Abort len= %d bits", pDET->raw_bits[snd_ch].Length);
|
||
|
||
frame_status = FRAME_WAIT;
|
||
|
||
// Raw stream init
|
||
|
||
pDET->raw_bits[snd_ch].Length = 0;
|
||
pDET->raw_bits1[snd_ch].Length = 0;
|
||
pDET->last_nrzi_bit[snd_ch] = raw_bit;
|
||
|
||
// dcd_hdr_cnt[snd_ch] = 48;
|
||
// dcd_on_hdr[snd_ch] = 1;
|
||
|
||
|
||
if (last)
|
||
chk_dcd1(snd_ch, buf_size);
|
||
}
|
||
continue;
|
||
}
|
||
|
||
if (((bit_stream & FRAME_FLAG) == FRAME_FLAG) && (frame_status == FRAME_LOAD))
|
||
{
|
||
frame_status = FRAME_WAIT;
|
||
|
||
if (pDET->raw_bits[snd_ch].Length == 7) // Another flag
|
||
{
|
||
// Raw stream init
|
||
|
||
pDET->raw_bits[snd_ch].Length = 0;
|
||
pDET->raw_bits1[snd_ch].Length = 0;
|
||
pDET->last_nrzi_bit[snd_ch] = raw_bit;
|
||
}
|
||
|
||
if (pDET->raw_bits[snd_ch].Length > 7)
|
||
{
|
||
//b Debugprintf("Got Frame len = %d AFC %f", pDET->raw_bits[snd_ch].Length, AFC);
|
||
centreFreq[snd_ch] = GuessCentreFreq(snd_ch);
|
||
make_rx_frame(snd_ch, rcvr_nr, emph, pDET->last_nrzi_bit[snd_ch], &pDET->raw_bits[snd_ch], &pDET->raw_bits1[snd_ch]);
|
||
}
|
||
}
|
||
|
||
|
||
if (frame_status == FRAME_LOAD)
|
||
{
|
||
//Raw stream
|
||
|
||
if (pDET->raw_bits[snd_ch].Length < 36873)
|
||
{
|
||
if (raw_bit == RX_BIT1)
|
||
stringAdd(&pDET->raw_bits[snd_ch], "1", 1);
|
||
else
|
||
stringAdd(&pDET->raw_bits[snd_ch], "0", 1);
|
||
}
|
||
|
||
if (pDET->raw_bits1[snd_ch].Length < 36873)
|
||
{
|
||
if (raw_bit1 == RX_BIT1)
|
||
stringAdd(&pDET->raw_bits1[snd_ch], "1", 1);
|
||
else
|
||
stringAdd(&pDET->raw_bits1[snd_ch], "0", 1);
|
||
}
|
||
//
|
||
}
|
||
|
||
if (((bit_stream & FRAME_FLAG) == FRAME_FLAG) && (frame_status == FRAME_WAIT))
|
||
{
|
||
frame_status = FRAME_LOAD;
|
||
|
||
// Raw stream init
|
||
|
||
pDET->raw_bits[snd_ch].Length = 0;
|
||
pDET->raw_bits1[snd_ch].Length = 0;
|
||
pDET->last_nrzi_bit[snd_ch] = raw_bit;
|
||
|
||
// Debugprintf("New Frame");
|
||
}
|
||
|
||
}
|
||
}
|
||
|
||
pDET->sample_cnt[snd_ch] = sample_cnt;
|
||
pDET->PkAmp[snd_ch] = PkAmp;
|
||
pDET->PkAmpMax[snd_ch] = PkAmpMax;
|
||
pDET->newpkpos[snd_ch] = newpkpos;
|
||
pDET->bit_osc[snd_ch] = bit_osc;
|
||
pDET->MaxAmp[snd_ch] = MaxAmp;
|
||
pDET->MinAmp[snd_ch] = MinAmp;
|
||
pDET->AverageAmp[snd_ch] = AverageAmp;
|
||
pDET->bit_stream[snd_ch] = bit_stream;
|
||
pDET->frame_status[snd_ch] = frame_status;
|
||
pDET->last_rx_bit[snd_ch] = last_rx_bit;
|
||
pDET->fx25[snd_ch] = fx25;
|
||
}
|
||
|
||
|
||
void decode_stream_BPSK(int last, int snd_ch, int rcvr_nr, int emph, float * srcI, float * srcQ, float * bit_buf, int buf_size, string * data)
|
||
{
|
||
float agc_fast = 0.01f;
|
||
float agc_fast1 = 1 - agc_fast;
|
||
float agc_slow = agc_fast / 4;
|
||
float agc_slow1 = 1 - agc_slow;
|
||
|
||
int i, k, j, n;
|
||
Byte dibit, bit;
|
||
single afc, x, amp, k1, k2;
|
||
single baudrate;
|
||
single div_bit_afc;
|
||
word max_cnt;
|
||
single threshold;
|
||
single tr;
|
||
single KCorr, AngleCorr, angle, muxI1, muxQ1, muxI2, muxQ2, sumIQ1, sumIQ2;
|
||
Byte newpkpos, sample_cnt;
|
||
single PkAmpI, PkAmpQ, PkAmpMax, PSK_AGC;
|
||
single PSK_IZ1, PSK_QZ1;
|
||
single bit_osc;
|
||
Byte bit_stuff_cnt, last_rx_bit, frame_status, bit_cnt, bit_stream, byte_rx;
|
||
|
||
// get saved values to local variables to speed up access
|
||
|
||
struct TDetector_t * pDET = &DET[emph][rcvr_nr];
|
||
|
||
// global -> local
|
||
|
||
AngleCorr = pDET->AngleCorr[snd_ch];
|
||
bit_stuff_cnt = pDET->bit_stuff_cnt[snd_ch];
|
||
sample_cnt = pDET->sample_cnt[snd_ch];
|
||
PSK_AGC = pDET->PSK_AGC[snd_ch];
|
||
PkAmpI = pDET->PkAmpI[snd_ch];
|
||
PkAmpQ = pDET->PkAmpQ[snd_ch];
|
||
PkAmpMax = pDET->PkAmpMax[snd_ch];
|
||
newpkpos = pDET->newpkpos[snd_ch];
|
||
PSK_IZ1 = pDET->PSK_IZ1[snd_ch];
|
||
PSK_QZ1 = pDET->PSK_QZ1[snd_ch];
|
||
bit_osc = pDET->bit_osc[snd_ch];
|
||
frame_status = pDET->frame_status[snd_ch];
|
||
bit_cnt = pDET->bit_cnt[snd_ch];
|
||
bit_stream = pDET->bit_stream[snd_ch];
|
||
byte_rx = pDET->byte_rx[snd_ch];
|
||
|
||
//
|
||
tr = dcd_threshold * dcd_corr;
|
||
|
||
if (last)
|
||
{
|
||
// Update DCD status
|
||
|
||
if (dcd_hdr_cnt[snd_ch] == 0)
|
||
dcd_on_hdr[snd_ch] = 0;
|
||
|
||
dcd_bit_cnt[snd_ch] = 0;
|
||
}
|
||
|
||
baudrate = 300;
|
||
div_bit_afc = 1.0f / round(BIT_AFC*(RX_Samplerate / 11025));
|
||
x = baudrate / RX_Samplerate;
|
||
|
||
// max_cnt = round(RX_Samplerate / baudrate) + 1;
|
||
max_cnt = round(RX_Samplerate / baudrate) + 1;
|
||
|
||
for (i = 0; i < buf_size - 1; i++)
|
||
{
|
||
// AGC
|
||
amp = sqrt(srcI[i] * srcI[i] + srcQ[i] * srcQ[i]);
|
||
|
||
if (amp > PSK_AGC)
|
||
|
||
PSK_AGC = PSK_AGC * agc_fast1 + amp*agc_fast;
|
||
else
|
||
PSK_AGC = PSK_AGC * agc_slow1 + amp*agc_slow;
|
||
|
||
if (PSK_AGC > 1)
|
||
|
||
{
|
||
srcI[i] = srcI[i] / PSK_AGC;
|
||
srcQ[i] = srcQ[i] / PSK_AGC;
|
||
amp = amp / PSK_AGC; // <20><><EFBFBD><EFBFBD><EFBFBD><EFBFBD> SQRT
|
||
}
|
||
//
|
||
bit_buf[sample_cnt] = 0.95*bit_buf[sample_cnt] + 0.05*amp;
|
||
// <20><><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD> <20><><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD> <20> <20><><EFBFBD><EFBFBD><EFBFBD><EFBFBD> <20><><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD>
|
||
if (bit_buf[sample_cnt] > PkAmpMax)
|
||
{
|
||
PkAmpI = srcI[i];
|
||
PkAmpQ = srcQ[i];
|
||
PkAmpMax = bit_buf[sample_cnt];
|
||
newpkpos = sample_cnt;
|
||
}
|
||
|
||
sample_cnt++;
|
||
|
||
bit_osc = bit_osc + x;
|
||
|
||
if (bit_osc >= 1)
|
||
{
|
||
if (sample_cnt <= max_cnt)
|
||
for (k = sample_cnt; k <= max_cnt; k++)
|
||
bit_buf[k] = 0.95*bit_buf[k];
|
||
|
||
k1 = (1.0f * newpkpos) / (sample_cnt - 1);
|
||
k2 = pila(k1) - 1;
|
||
|
||
afc = div_bit_afc * k2;
|
||
|
||
if (k1 > 0.5f)
|
||
bit_osc = bit_osc + afc;
|
||
else
|
||
bit_osc = bit_osc - afc;
|
||
|
||
PkAmpMax = 0;
|
||
sample_cnt = 0;
|
||
bit_osc = bit_osc - 1;
|
||
|
||
//DCD feature
|
||
if (last)
|
||
{
|
||
DCD_LastPkPos[snd_ch] = DCD_LastPkPos[snd_ch] * 0.96f + newpkpos * 0.04f;
|
||
DCD_LastPerc[snd_ch] = DCD_LastPerc[snd_ch] * 0.96f + fabsf(newpkpos - DCD_LastPkPos[snd_ch])*0.04f;
|
||
|
||
if (DCD_LastPerc[snd_ch] >= tr || DCD_LastPerc[snd_ch] < 0.00001f)
|
||
dcd_bit_cnt[snd_ch] = dcd_bit_cnt[snd_ch] + 1;
|
||
else
|
||
dcd_bit_cnt[snd_ch] = dcd_bit_cnt[snd_ch] - 1;
|
||
}
|
||
|
||
// Bit-detector
|
||
|
||
muxI1 = PkAmpI * PSK_IZ1;
|
||
muxI2 = PkAmpQ * PSK_IZ1;
|
||
muxQ1 = PkAmpQ * PSK_QZ1;
|
||
muxQ2 = PkAmpI * PSK_QZ1;
|
||
sumIQ1 = muxI1 + muxQ1;
|
||
sumIQ2 = muxI2 - muxQ2;
|
||
angle = atan2f(sumIQ2, sumIQ1);
|
||
PSK_IZ1 = PkAmpI;
|
||
PSK_QZ1 = PkAmpQ;
|
||
|
||
float Mag = sqrtf(powf(PSK_IZ1, 2) + powf(PSK_QZ1, 2));
|
||
|
||
// Phase corrector
|
||
|
||
if (fabsf(angle) < PI5)
|
||
AngleCorr = AngleCorr * 0.95f - angle * 0.05f;
|
||
else
|
||
{
|
||
if (angle > 0)
|
||
AngleCorr = AngleCorr * 0.95f + (pi - angle)*0.05f;
|
||
else
|
||
AngleCorr = AngleCorr * 0.95f - (pi + angle)*0.05f;
|
||
}
|
||
|
||
angle = angle + AngleCorr;
|
||
//
|
||
|
||
if (fabsf(angle) < PI5)
|
||
bit = RX_BIT1;
|
||
else
|
||
bit = RX_BIT0;
|
||
//
|
||
|
||
// is this the best place to store phase for constellation?
|
||
// only for ilp2 for now
|
||
|
||
if (il2p_mode[snd_ch])
|
||
{
|
||
struct il2p_context_s * il2p = il2p_context[snd_ch][rcvr_nr][emph];
|
||
|
||
if (il2p && il2p->state > IL2P_SEARCHING)
|
||
{
|
||
Phases[snd_ch][rcvr_nr][emph][nPhases[snd_ch][rcvr_nr][emph]] = angle;
|
||
Mags[snd_ch][rcvr_nr][emph][nPhases[snd_ch][rcvr_nr][emph]++] = Mag;
|
||
if (nPhases[snd_ch][rcvr_nr][emph] > 4090)
|
||
nPhases[snd_ch][rcvr_nr][emph]--;
|
||
}
|
||
il2p_rec_bit(snd_ch, rcvr_nr, emph, bit);
|
||
if (il2p_mode[snd_ch] == IL2P_MODE_ONLY) // Dont try HDLC decode
|
||
continue;
|
||
}
|
||
if (bit)
|
||
stats[1]++;
|
||
else
|
||
stats[0]++;
|
||
|
||
bit_stream = (bit_stream >> 1) | bit;
|
||
|
||
// DCD on flag
|
||
|
||
if (last)
|
||
{
|
||
if (dcd_hdr_cnt[snd_ch] > 0)
|
||
dcd_hdr_cnt[snd_ch]--;
|
||
|
||
DCD_header[snd_ch] = (DCD_header[snd_ch] >> 1) | (bit << 24);
|
||
|
||
if (((DCD_header[snd_ch] & 0xFFFF0000) == 0x7E7E0000) ||
|
||
((DCD_header[snd_ch] & 0xFFFFFF00) == 0x7E000000) ||
|
||
((DCD_header[snd_ch] & 0xFFFFFF00) == 0x00000000))
|
||
{
|
||
dcd_hdr_cnt[snd_ch] = 48;
|
||
dcd_on_hdr[snd_ch] = 1;
|
||
}
|
||
}
|
||
|
||
|
||
// I think Andy looks for both flag and abort here. I think it would be
|
||
// clearer to detect abort separately
|
||
|
||
// This may not be optimun but should work
|
||
|
||
if (bit_stream == 0xFF || bit_stream == 0x7F || bit_stream == 0xFE)
|
||
{
|
||
// All have 7 or more 1 bits
|
||
|
||
if (frame_status == FRAME_LOAD)
|
||
{
|
||
// Have started receiving frame
|
||
|
||
// Debugprintf("Frame Abort len= %d bytes", data->Length);
|
||
|
||
frame_status = FRAME_WAIT;
|
||
|
||
// Raw stream init
|
||
|
||
bit_cnt = 0;
|
||
bit_stuff_cnt = 0;
|
||
data->Length = 0;
|
||
}
|
||
continue;
|
||
}
|
||
|
||
|
||
if ((bit_stream & FRAME_FLAG) == FRAME_FLAG && frame_status == FRAME_LOAD)
|
||
{
|
||
frame_status = FRAME_WAIT;
|
||
// if (bit_cnt == 6)
|
||
make_rx_frame_PSK(snd_ch, rcvr_nr, emph, data);
|
||
}
|
||
|
||
if (frame_status == FRAME_LOAD)
|
||
{
|
||
if (bit_stuff_cnt == 5)
|
||
bit_stuff_cnt = 0;
|
||
else
|
||
{
|
||
if (bit == RX_BIT1)
|
||
bit_stuff_cnt++;
|
||
else
|
||
bit_stuff_cnt = 0;
|
||
|
||
byte_rx = (byte_rx >> 1) + bit;
|
||
bit_cnt++;
|
||
}
|
||
|
||
if (bit_cnt == 8)
|
||
{
|
||
if (data->Length < 4097)
|
||
stringAdd(data, &byte_rx, 1);
|
||
bit_cnt = 0;
|
||
}
|
||
}
|
||
if ((bit_stream & FRAME_FLAG) == FRAME_FLAG && frame_status == FRAME_WAIT)
|
||
{
|
||
frame_status = FRAME_LOAD;
|
||
bit_cnt = 0;
|
||
bit_stuff_cnt = 0;
|
||
data->Length = 0;
|
||
}
|
||
}
|
||
}
|
||
|
||
pDET->sample_cnt[snd_ch] = sample_cnt;
|
||
pDET->PSK_AGC[snd_ch] = PSK_AGC;
|
||
pDET->PkAmpI[snd_ch] = PkAmpI;
|
||
pDET->PkAmpQ[snd_ch] = PkAmpQ;
|
||
pDET->PkAmpMax[snd_ch] = PkAmpMax;
|
||
pDET->newpkpos[snd_ch] = newpkpos;
|
||
pDET->PSK_IZ1[snd_ch] = PSK_IZ1;
|
||
pDET->PSK_QZ1[snd_ch] = PSK_QZ1;
|
||
pDET->bit_osc[snd_ch] = bit_osc;
|
||
pDET->frame_status[snd_ch] = frame_status;
|
||
pDET->bit_cnt[snd_ch] = bit_cnt;
|
||
pDET->bit_stream[snd_ch] = bit_stream;
|
||
pDET->byte_rx[snd_ch] = byte_rx;
|
||
pDET->bit_stuff_cnt[snd_ch] = bit_stuff_cnt;
|
||
pDET->AngleCorr[snd_ch] = AngleCorr;
|
||
}
|
||
|
||
|
||
void decode_stream_QPSK(int last, int snd_ch, int rcvr_nr, int emph, float * srcI, float * srcQ, float * bit_buf, int buf_size, string * data)
|
||
{
|
||
float agc_fast = 0.01f;
|
||
float agc_fast1 = 1 - agc_fast;
|
||
float agc_slow = agc_fast / 4;
|
||
float agc_slow1 = 1 - agc_slow;
|
||
|
||
int i, k, j, n;
|
||
Byte dibit = 0, bit;
|
||
single afc, x, amp, k1, k2;
|
||
single baudrate;
|
||
single div_bit_afc;
|
||
word max_cnt;
|
||
single threshold;
|
||
single tr;
|
||
single KCorr = 0, AngleCorr, angle, muxI1, muxQ1, muxI2, muxQ2, sumIQ1, sumIQ2;
|
||
Byte newpkpos, sample_cnt;
|
||
single PkAmpI, PkAmpQ, PkAmpMax, PSK_AGC;
|
||
single PSK_IZ1, PSK_QZ1;
|
||
single bit_osc;
|
||
Byte bit_stuff_cnt, last_rx_bit, frame_status, bit_cnt, bit_stream, byte_rx;
|
||
|
||
// get saved values to local variables to speed up access
|
||
|
||
struct TDetector_t * pDET = &DET[emph][rcvr_nr];
|
||
|
||
bit_stuff_cnt = pDET->bit_stuff_cnt[snd_ch];
|
||
last_rx_bit = pDET->last_rx_bit[snd_ch];
|
||
sample_cnt = pDET->sample_cnt[snd_ch];
|
||
PSK_AGC = pDET->PSK_AGC[snd_ch];
|
||
PkAmpI = pDET->PkAmpI[snd_ch];
|
||
PkAmpQ = pDET->PkAmpQ[snd_ch];
|
||
PkAmpMax = pDET->PkAmpMax[snd_ch];
|
||
newpkpos = pDET->newpkpos[snd_ch];
|
||
PSK_IZ1 = pDET->PSK_IZ1[snd_ch];
|
||
PSK_QZ1 = pDET->PSK_QZ1[snd_ch];
|
||
bit_osc = pDET->bit_osc[snd_ch];
|
||
frame_status = pDET->frame_status[snd_ch];
|
||
bit_cnt = pDET->bit_cnt[snd_ch];
|
||
bit_stream = pDET->bit_stream[snd_ch];
|
||
byte_rx = pDET->byte_rx[snd_ch];
|
||
AngleCorr = pDET->AngleCorr[snd_ch];
|
||
|
||
tr = dcd_threshold * dcd_corr;
|
||
|
||
if (last)
|
||
{
|
||
// Update DCD status
|
||
|
||
if (dcd_hdr_cnt[snd_ch] == 0)
|
||
dcd_on_hdr[snd_ch] = 0;
|
||
|
||
dcd_bit_cnt[snd_ch] = 0;
|
||
}
|
||
|
||
// I think this works because of upsampling - 1200 = 4x 300 and we upsampled by 4
|
||
|
||
baudrate = 300;
|
||
|
||
div_bit_afc = 1.0f / roundf(BIT_AFC*(RX_Samplerate / 11025.0f));
|
||
|
||
x = baudrate / RX_Samplerate;
|
||
|
||
max_cnt = roundf(RX_Samplerate / baudrate) + 1;
|
||
|
||
for (i = 0; i < buf_size; i++)
|
||
{
|
||
// AGC
|
||
amp = sqrt(srcI[i] * srcI[i] + srcQ[i] * srcQ[i]);
|
||
|
||
if (amp > PSK_AGC)
|
||
PSK_AGC = PSK_AGC * agc_fast1 + amp * agc_fast;
|
||
else
|
||
PSK_AGC = PSK_AGC * agc_slow1 + amp * agc_slow;
|
||
|
||
if (PSK_AGC > 1)
|
||
{
|
||
srcI[i] = srcI[i] / PSK_AGC;
|
||
srcQ[i] = srcQ[i] / PSK_AGC;
|
||
|
||
amp = amp / PSK_AGC; // <20><><EFBFBD><EFBFBD><EFBFBD><EFBFBD> SQRT
|
||
}
|
||
|
||
bit_buf[sample_cnt] = 0.95 * bit_buf[sample_cnt] + 0.05 * amp;
|
||
|
||
// Check for NAN
|
||
|
||
if (bit_buf[sample_cnt] != bit_buf[sample_cnt])
|
||
bit_buf[sample_cnt] = 0.0f;
|
||
|
||
// Find the maximum in the synchronization buffer
|
||
|
||
if (bit_buf[sample_cnt] > PkAmpMax)
|
||
{
|
||
PkAmpI = srcI[i];
|
||
PkAmpQ = srcQ[i];
|
||
PkAmpMax = bit_buf[sample_cnt];
|
||
newpkpos = sample_cnt;
|
||
}
|
||
|
||
sample_cnt++;
|
||
|
||
bit_osc = bit_osc + x;
|
||
|
||
// This seems to be how it does samples to bits
|
||
|
||
if (bit_osc >= 1)
|
||
{
|
||
if (sample_cnt <= max_cnt)
|
||
for (k = sample_cnt; k <= max_cnt; k++)
|
||
bit_buf[k] = 0.95*bit_buf[k];
|
||
|
||
k1 = (1.0f * newpkpos) / (sample_cnt - 1);
|
||
k2 = pila(k1) - 1;
|
||
afc = div_bit_afc * k2;
|
||
|
||
if (k1 > 0.5)
|
||
bit_osc = bit_osc + afc;
|
||
else
|
||
bit_osc = bit_osc - afc;
|
||
|
||
PkAmpMax = 0;
|
||
sample_cnt = 0;
|
||
bit_osc = bit_osc - 1;
|
||
//DCD feature
|
||
|
||
if (last)
|
||
{
|
||
DCD_LastPkPos[snd_ch] = DCD_LastPkPos[snd_ch] * 0.96 + newpkpos * 0.04;
|
||
DCD_LastPerc[snd_ch] = DCD_LastPerc[snd_ch] * 0.96 + fabsf(newpkpos - DCD_LastPkPos[snd_ch])*0.04;
|
||
|
||
if (DCD_LastPerc[snd_ch] >= tr || DCD_LastPerc[snd_ch] < 0.00001f)
|
||
dcd_bit_cnt[snd_ch] = dcd_bit_cnt[snd_ch] + 1;
|
||
else
|
||
dcd_bit_cnt[snd_ch] = dcd_bit_cnt[snd_ch] - 1;
|
||
}
|
||
|
||
// Bit-detector
|
||
|
||
muxI1 = PkAmpI * PSK_IZ1;
|
||
muxI2 = PkAmpQ * PSK_IZ1;
|
||
muxQ1 = PkAmpQ * PSK_QZ1;
|
||
muxQ2 = PkAmpI * PSK_QZ1;
|
||
sumIQ1 = muxI1 + muxQ1;
|
||
sumIQ2 = muxI2 - muxQ2;
|
||
angle = atan2f(sumIQ2, sumIQ1);
|
||
PSK_IZ1 = PkAmpI;
|
||
PSK_QZ1 = PkAmpQ;
|
||
|
||
float Mag = sqrtf(powf(PSK_IZ1, 2) + powf(PSK_QZ1, 2));
|
||
|
||
if (angle > pi || angle < -pi)
|
||
angle = angle;
|
||
|
||
if (modem_mode[snd_ch] == MODE_PI4QPSK)
|
||
{
|
||
// Phase corrector
|
||
|
||
// I'm pretty sure we send 4 phases starting 45 degrees from 0 so .25, .75 - .25 - .75
|
||
|
||
if (angle >= 0 && angle <= PI5)
|
||
KCorr = angle - PI25;
|
||
|
||
if (angle > PI5)
|
||
KCorr = angle - PI75;
|
||
|
||
if (angle < -PI5)
|
||
KCorr = angle + PI75;
|
||
|
||
if (angle < 0 && angle >= -PI5)
|
||
KCorr = angle + PI25;
|
||
|
||
AngleCorr = AngleCorr * 0.95f - KCorr * 0.05f;
|
||
angle = angle + AngleCorr;
|
||
|
||
if (angle >= 0 && angle <= PI5)
|
||
{
|
||
dibit = qpsk_set[snd_ch].rx[0]; // 00 - 0
|
||
qpsk_set[snd_ch].count[0]++;
|
||
}
|
||
else if (angle > PI5)
|
||
{
|
||
dibit = qpsk_set[snd_ch].rx[1]; // 01 - PI/2
|
||
qpsk_set[snd_ch].count[1]++;
|
||
}
|
||
else if (angle < -PI5)
|
||
{
|
||
dibit = qpsk_set[snd_ch].rx[2]; // 10 - PI
|
||
qpsk_set[snd_ch].count[2]++;
|
||
}
|
||
else if (angle < 0 && angle >= -PI5)
|
||
{
|
||
dibit = qpsk_set[snd_ch].rx[3]; // 11 - -PI/2
|
||
qpsk_set[snd_ch].count[3]++;
|
||
}
|
||
}
|
||
else
|
||
{
|
||
// "Normal" QPSK
|
||
|
||
// Phase corrector
|
||
|
||
// I think this sends 0 90 180 270
|
||
|
||
if (fabsf(angle) < PI25)
|
||
KCorr = angle;
|
||
|
||
if (angle >= PI25 && angle <= PI75)
|
||
KCorr = angle - PI5;
|
||
|
||
if (angle <= -PI25 && angle >= -PI75)
|
||
KCorr = angle + PI5;
|
||
|
||
if (fabsf(angle) > PI75)
|
||
{
|
||
if (angle > 0)
|
||
KCorr = -M_PI + angle;
|
||
else
|
||
KCorr = M_PI + angle;
|
||
}
|
||
|
||
AngleCorr = AngleCorr * 0.95 - KCorr * 0.05;
|
||
angle = angle + AngleCorr;
|
||
|
||
if (fabsf(angle) < PI25)
|
||
dibit = qpsk_set[snd_ch].rx[0]; // 00 - 0
|
||
else if (angle >= PI25 && angle <= PI75)
|
||
dibit = qpsk_set[snd_ch].rx[1]; // 01 - PI/2
|
||
else if (fabsf(angle) > PI75)
|
||
dibit = qpsk_set[snd_ch].rx[2]; // 10 - PI
|
||
else if (angle <= -PI25 && angle >= -PI75)
|
||
dibit = qpsk_set[snd_ch].rx[3]; // 11 - -PI/2
|
||
}
|
||
|
||
for (j = 0; j < 2; j++)
|
||
{
|
||
dibit = dibit << 1;
|
||
|
||
// is this the best place to store phase for constellation?
|
||
// only for ilp2 for now
|
||
|
||
if (il2p_mode[snd_ch])
|
||
{
|
||
struct il2p_context_s * il2p = il2p_context[snd_ch][rcvr_nr][emph];
|
||
|
||
if (il2p && il2p->state > IL2P_SEARCHING)
|
||
{
|
||
Phases[snd_ch][rcvr_nr][emph][nPhases[snd_ch][rcvr_nr][emph]] = angle;
|
||
Mags[snd_ch][rcvr_nr][emph][nPhases[snd_ch][rcvr_nr][emph]++] = Mag;
|
||
if (nPhases[snd_ch][rcvr_nr][emph] > 4090)
|
||
nPhases[snd_ch][rcvr_nr][emph]--;
|
||
}
|
||
|
||
il2p_rec_bit(snd_ch, rcvr_nr, emph, (dibit & RX_BIT1));
|
||
if (il2p_mode[snd_ch] == IL2P_MODE_ONLY) // Dont try HDLC decode
|
||
continue;
|
||
}
|
||
|
||
// NRZI
|
||
|
||
if (last_rx_bit == (dibit & RX_BIT1))
|
||
bit = RX_BIT1;
|
||
else
|
||
bit = RX_BIT0;
|
||
|
||
last_rx_bit = dibit & RX_BIT1;
|
||
|
||
bit_stream = (bit_stream >> 1) | bit;
|
||
|
||
// DCD on flag
|
||
|
||
if (last)
|
||
{
|
||
if (dcd_hdr_cnt[snd_ch] > 0)
|
||
dcd_hdr_cnt[snd_ch]--;
|
||
|
||
DCD_header[snd_ch] = (DCD_header[snd_ch] >> 1) | (bit << 24);
|
||
|
||
if (((DCD_header[snd_ch] & 0xFFFF0000) == 0x7E7E0000) ||
|
||
((DCD_header[snd_ch] & 0xFFFFFF00) == 0x7E000000) ||
|
||
((DCD_header[snd_ch] & 0xFFFFFF00) == 0x00000000))
|
||
{
|
||
dcd_hdr_cnt[snd_ch] = 48;
|
||
dcd_on_hdr[snd_ch] = 1;
|
||
}
|
||
}
|
||
|
||
|
||
// I think Andy looks for both flag and abort here. I think it would be
|
||
// clearer to detect abort separately
|
||
|
||
// This may not be optimun but should work
|
||
|
||
if (bit_stream == 0xFF || bit_stream == 0x7F || bit_stream == 0xFE)
|
||
{
|
||
// All have 7 or more 1 bits
|
||
|
||
if (frame_status == FRAME_LOAD)
|
||
{
|
||
// Have started receiving frame
|
||
|
||
// Debugprintf("Frame Abort len= %d bytes", data->Length);
|
||
|
||
frame_status = FRAME_WAIT;
|
||
|
||
// Raw stream init
|
||
|
||
bit_cnt = 0;
|
||
bit_stuff_cnt = 0;
|
||
data->Length = 0;
|
||
}
|
||
continue;
|
||
}
|
||
|
||
if ((bit_stream & FRAME_FLAG) == FRAME_FLAG && frame_status == FRAME_LOAD)
|
||
{
|
||
frame_status = FRAME_WAIT;
|
||
// if (bit_cnt == 6)
|
||
make_rx_frame_PSK(snd_ch, rcvr_nr, emph, data);
|
||
}
|
||
|
||
if (frame_status == FRAME_LOAD)
|
||
{
|
||
if (bit_stuff_cnt == 5)
|
||
bit_stuff_cnt = 0;
|
||
else
|
||
{
|
||
if (bit == RX_BIT1)
|
||
bit_stuff_cnt++;
|
||
else
|
||
bit_stuff_cnt = 0;
|
||
|
||
byte_rx = (byte_rx >> 1) + bit;
|
||
bit_cnt++;
|
||
|
||
}
|
||
|
||
if (bit_cnt == 8)
|
||
{
|
||
if (data->Length < 4097)
|
||
stringAdd(data, &byte_rx, 1);
|
||
bit_cnt = 0;
|
||
}
|
||
}
|
||
if ((bit_stream & FRAME_FLAG) == FRAME_FLAG && frame_status == FRAME_WAIT)
|
||
{
|
||
frame_status = FRAME_LOAD;
|
||
bit_cnt = 0;
|
||
bit_stuff_cnt = 0;
|
||
data->Length = 0;
|
||
}
|
||
}
|
||
}
|
||
}
|
||
|
||
pDET->sample_cnt[snd_ch] = sample_cnt;
|
||
pDET->PSK_AGC[snd_ch] = PSK_AGC;
|
||
pDET->PkAmpI[snd_ch] = PkAmpI;
|
||
pDET->PkAmpQ[snd_ch] = PkAmpQ;
|
||
pDET->PkAmpMax[snd_ch] = PkAmpMax;
|
||
pDET->newpkpos[snd_ch] = newpkpos;
|
||
pDET->PSK_IZ1[snd_ch] = PSK_IZ1;
|
||
pDET->PSK_QZ1[snd_ch] = PSK_QZ1;
|
||
pDET->bit_osc[snd_ch] = bit_osc;
|
||
pDET->frame_status[snd_ch] = frame_status;
|
||
pDET->bit_cnt[snd_ch] = bit_cnt;
|
||
pDET->bit_stream[snd_ch] = bit_stream;
|
||
pDET->byte_rx[snd_ch] = byte_rx;
|
||
pDET->last_rx_bit[snd_ch] = last_rx_bit;
|
||
pDET->bit_stuff_cnt[snd_ch] = bit_stuff_cnt;
|
||
pDET->AngleCorr[snd_ch] = AngleCorr;
|
||
}
|
||
|
||
void decode_stream_8PSK(int last, int snd_ch, int rcvr_nr, int emph, float * srcI, float * srcQ, float * bit_buf, int buf_size, string * data)
|
||
{
|
||
float agc_fast = 0.01f;
|
||
float agc_fast1 = 1 - agc_fast;
|
||
float agc_slow = agc_fast / 4;
|
||
float agc_slow1 = 1 - agc_slow;
|
||
|
||
int i, k, j, n;
|
||
Byte tribit = 0, bit;
|
||
single afc, x, amp, k1, k2;
|
||
single baudrate;
|
||
single div_bit_afc;
|
||
word max_cnt;
|
||
single threshold;
|
||
single tr;
|
||
single KCorr = 0, AngleCorr, angle, muxI1, muxQ1, muxI2, muxQ2, sumIQ1, sumIQ2;
|
||
Byte newpkpos, sample_cnt;
|
||
single PkAmpI, PkAmpQ, PkAmpMax, PSK_AGC;
|
||
single PSK_IZ1, PSK_QZ1;
|
||
single bit_osc;
|
||
Byte bit_stuff_cnt, last_rx_bit, frame_status, bit_cnt, bit_stream, byte_rx;
|
||
|
||
// get saved values to local variables to speed up access
|
||
|
||
struct TDetector_t * pDET = &DET[emph][rcvr_nr];
|
||
|
||
bit_stuff_cnt = pDET->bit_stuff_cnt[snd_ch];
|
||
last_rx_bit = pDET->last_rx_bit[snd_ch];
|
||
sample_cnt = pDET->sample_cnt[snd_ch];
|
||
PSK_AGC = pDET->PSK_AGC[snd_ch];
|
||
PkAmpI = pDET->PkAmpI[snd_ch];
|
||
PkAmpQ = pDET->PkAmpQ[snd_ch];
|
||
PkAmpMax = pDET->PkAmpMax[snd_ch];
|
||
newpkpos = pDET->newpkpos[snd_ch];
|
||
PSK_IZ1 = pDET->PSK_IZ1[snd_ch];
|
||
PSK_QZ1 = pDET->PSK_QZ1[snd_ch];
|
||
bit_osc = pDET->bit_osc[snd_ch];
|
||
frame_status = pDET->frame_status[snd_ch];
|
||
bit_cnt = pDET->bit_cnt[snd_ch];
|
||
bit_stream = pDET->bit_stream[snd_ch];
|
||
byte_rx = pDET->byte_rx[snd_ch];
|
||
AngleCorr = pDET->AngleCorr[snd_ch];
|
||
|
||
tr = dcd_threshold * dcd_corr;
|
||
|
||
if (last)
|
||
{
|
||
// Update DCD status
|
||
|
||
if (dcd_hdr_cnt[snd_ch] == 0)
|
||
dcd_on_hdr[snd_ch] = 0;
|
||
|
||
dcd_bit_cnt[snd_ch] = 0;
|
||
}
|
||
|
||
// Not sure how this works
|
||
|
||
if (tx_baudrate[snd_ch] == 300)
|
||
baudrate = 300;
|
||
else
|
||
baudrate = 1600 / 6;
|
||
|
||
div_bit_afc = 1.0 / round(BIT_AFC*(RX_Samplerate / 11025));
|
||
x = baudrate / RX_Samplerate;
|
||
max_cnt = round(RX_Samplerate / baudrate) + 1;
|
||
for (i = 0; i < buf_size; i++)
|
||
{
|
||
// AGC
|
||
amp = sqrt(srcI[i] * srcI[i] + srcQ[i] * srcQ[i]);
|
||
if (amp > PSK_AGC)
|
||
PSK_AGC = PSK_AGC * agc_fast1 + amp*agc_fast;
|
||
else
|
||
PSK_AGC = PSK_AGC * agc_slow1 + amp*agc_slow;
|
||
|
||
if (PSK_AGC > 1)
|
||
{
|
||
srcI[i] = srcI[i] / PSK_AGC;
|
||
srcQ[i] = srcQ[i] / PSK_AGC;
|
||
amp = amp / PSK_AGC; // <20><><EFBFBD><EFBFBD><EFBFBD><EFBFBD> SQRT
|
||
}
|
||
|
||
bit_buf[sample_cnt] = 0.95*bit_buf[sample_cnt] + 0.05*amp;
|
||
|
||
// <20><><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD> <20><><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD> <20> <20><><EFBFBD><EFBFBD><EFBFBD><EFBFBD> <20><><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD>
|
||
if (bit_buf[sample_cnt] > PkAmpMax)
|
||
{
|
||
PkAmpI = srcI[i];
|
||
PkAmpQ = srcQ[i];
|
||
PkAmpMax = bit_buf[sample_cnt];
|
||
newpkpos = sample_cnt;
|
||
}
|
||
//
|
||
|
||
sample_cnt++;
|
||
|
||
bit_osc = bit_osc + x;
|
||
|
||
if (bit_osc >= 1)
|
||
{
|
||
if (sample_cnt <= max_cnt)
|
||
for (k = sample_cnt; k <= max_cnt; k++)
|
||
bit_buf[k] = 0.95*bit_buf[k];
|
||
|
||
k1 = (1.0f * newpkpos) / (sample_cnt - 1);
|
||
k2 = pila(k1) - 1;
|
||
|
||
afc = div_bit_afc * k2;
|
||
if (k1 > 0.5)
|
||
bit_osc = bit_osc + afc;
|
||
else
|
||
bit_osc = bit_osc - afc;
|
||
|
||
PkAmpMax = 0;
|
||
sample_cnt = 0;
|
||
bit_osc = bit_osc - 1;
|
||
//DCD feature
|
||
if (last)
|
||
{
|
||
DCD_LastPkPos[snd_ch] = DCD_LastPkPos[snd_ch] * 0.96 + newpkpos * 0.04;
|
||
DCD_LastPerc[snd_ch] = DCD_LastPerc[snd_ch] * 0.96 + abs(newpkpos - DCD_LastPkPos[snd_ch])*0.04;
|
||
if (DCD_LastPerc[snd_ch] >= tr || DCD_LastPerc[snd_ch] < 0.00001)
|
||
dcd_bit_cnt[snd_ch] = dcd_bit_cnt[snd_ch] + 1;
|
||
else
|
||
dcd_bit_cnt[snd_ch] = dcd_bit_cnt[snd_ch] - 1;
|
||
}
|
||
// Bit-detector
|
||
muxI1 = PkAmpI * PSK_IZ1;
|
||
muxI2 = PkAmpQ * PSK_IZ1;
|
||
muxQ1 = PkAmpQ * PSK_QZ1;
|
||
muxQ2 = PkAmpI * PSK_QZ1;
|
||
sumIQ1 = muxI1 + muxQ1;
|
||
sumIQ2 = muxI2 - muxQ2;
|
||
angle = atan2f(sumIQ2, sumIQ1);
|
||
PSK_IZ1 = PkAmpI;
|
||
PSK_QZ1 = PkAmpQ;
|
||
|
||
float Mag = sqrtf(powf(PSK_IZ1, 2) + powf(PSK_QZ1, 2));
|
||
|
||
// Phase corrector
|
||
|
||
if (fabsf(angle) < PI125)
|
||
KCorr = angle;
|
||
|
||
if (angle >= PI125 && angle <= PI375)
|
||
KCorr = angle - PI25;
|
||
if (angle >= PI375 && angle < PI625)
|
||
KCorr = angle - PI5;
|
||
if (angle >= PI625 && angle <= PI875)
|
||
KCorr = angle - PI75;
|
||
if (angle <= -PI125 && angle > -PI375)
|
||
KCorr = angle + PI25;
|
||
if (angle <= -PI375 && angle > -PI625)
|
||
KCorr = angle + PI5;
|
||
if (angle <= -PI625 && angle >= -PI875)
|
||
KCorr = angle + PI75;
|
||
|
||
if (fabsf(angle) > PI875)
|
||
{
|
||
if (angle > 0)
|
||
KCorr = angle - pi;
|
||
else
|
||
KCorr = angle + pi;
|
||
}
|
||
|
||
AngleCorr = AngleCorr * 0.95 - KCorr * 0.05;
|
||
angle = angle + AngleCorr;
|
||
|
||
if (fabsf(angle) < PI125)
|
||
tribit = 1;
|
||
if (angle >= PI125 && angle < PI375)
|
||
tribit = 0;
|
||
if (angle >= PI375 && angle < PI625)
|
||
tribit = 2;
|
||
if (angle >= PI625 && angle <= PI875)
|
||
tribit = 3;
|
||
if (fabsf(angle) > PI875)
|
||
tribit = 7;
|
||
if (angle <= -PI625 && angle >= -PI875)
|
||
tribit = 6;
|
||
if (angle <= -PI375 && angle > -PI625)
|
||
tribit = 4;
|
||
if (angle <= -PI125 && angle > -PI375)
|
||
tribit = 5;
|
||
|
||
tribit = tribit << 4;
|
||
|
||
for (j = 0; j < 3; j++)
|
||
{
|
||
tribit = tribit << 1;
|
||
|
||
// look for il2p before nrzi
|
||
|
||
// is this the best place to store phase for constellation?
|
||
// only for ilp2 for now
|
||
|
||
if (il2p_mode[snd_ch])
|
||
{
|
||
struct il2p_context_s * il2p = il2p_context[snd_ch][rcvr_nr][emph];
|
||
|
||
if (il2p && il2p->state > IL2P_SEARCHING)
|
||
{
|
||
Phases[snd_ch][rcvr_nr][emph][nPhases[snd_ch][rcvr_nr][emph]] = angle;
|
||
Mags[snd_ch][rcvr_nr][emph][nPhases[snd_ch][rcvr_nr][emph]++] = Mag;
|
||
if (nPhases[snd_ch][rcvr_nr][emph] > 4090)
|
||
nPhases[snd_ch][rcvr_nr][emph]--;
|
||
}
|
||
|
||
il2p_rec_bit(snd_ch, rcvr_nr, emph, tribit & RX_BIT1);
|
||
if (il2p_mode[snd_ch] == IL2P_MODE_ONLY) // Dont try HDLC decode
|
||
continue;
|
||
}
|
||
|
||
//NRZI
|
||
|
||
if (last_rx_bit == (tribit & RX_BIT1))
|
||
bit = RX_BIT1;
|
||
else
|
||
bit = RX_BIT0;
|
||
|
||
last_rx_bit = tribit & RX_BIT1;
|
||
//
|
||
bit_stream = (bit_stream >> 1) | bit;
|
||
|
||
// DCD on flag
|
||
|
||
if (last)
|
||
{
|
||
if (dcd_hdr_cnt[snd_ch] > 0)
|
||
dcd_hdr_cnt[snd_ch]--;
|
||
|
||
DCD_header[snd_ch] = (DCD_header[snd_ch] >> 1) | (bit << 24);
|
||
|
||
if (((DCD_header[snd_ch] & 0xFFFF0000) == 0x7E7E0000) ||
|
||
((DCD_header[snd_ch] & 0xFFFFFF00) == 0x7E000000) ||
|
||
((DCD_header[snd_ch] & 0xFFFFFF00) == 0x00000000))
|
||
{
|
||
dcd_hdr_cnt[snd_ch] = 48;
|
||
dcd_on_hdr[snd_ch] = 1;
|
||
}
|
||
}
|
||
|
||
|
||
// I think Andy looks for both flag and abort here. I think it would be
|
||
// clearer to detect abort separately
|
||
|
||
// This may not be optimun but should work
|
||
|
||
if (bit_stream == 0xFF || bit_stream == 0x7F || bit_stream == 0xFE)
|
||
{
|
||
// All have 7 or more 1 bits
|
||
|
||
if (frame_status == FRAME_LOAD)
|
||
{
|
||
// Have started receiving frame
|
||
|
||
// Debugprintf("Frame Abort len= %d bytes", data->Length);
|
||
|
||
frame_status = FRAME_WAIT;
|
||
|
||
// Raw stream init
|
||
|
||
bit_cnt = 0;
|
||
bit_stuff_cnt = 0;
|
||
data->Length = 0;
|
||
}
|
||
continue;
|
||
}
|
||
|
||
|
||
if ((bit_stream & FRAME_FLAG) == FRAME_FLAG && frame_status == FRAME_LOAD)
|
||
{
|
||
frame_status = FRAME_WAIT;
|
||
if (bit_cnt == 6)
|
||
make_rx_frame_PSK(snd_ch, rcvr_nr, emph, data);
|
||
}
|
||
if (frame_status == FRAME_LOAD)
|
||
{
|
||
if (bit_stuff_cnt == 5)
|
||
bit_stuff_cnt = 0;
|
||
else
|
||
{
|
||
if (bit == RX_BIT1)
|
||
bit_stuff_cnt++;
|
||
else
|
||
bit_stuff_cnt = 0;
|
||
|
||
byte_rx = (byte_rx >> 1) + bit;
|
||
bit_cnt++;
|
||
}
|
||
if (bit_cnt == 8)
|
||
{
|
||
if (data->Length < 4097)
|
||
stringAdd(data, &byte_rx, 1);
|
||
bit_cnt = 0;
|
||
}
|
||
}
|
||
|
||
if ((bit_stream & FRAME_FLAG) == FRAME_FLAG && frame_status == FRAME_WAIT)
|
||
{
|
||
frame_status = FRAME_LOAD;
|
||
bit_cnt = 0;
|
||
bit_stuff_cnt = 0;
|
||
data->Length = 0;
|
||
}
|
||
}
|
||
}
|
||
}
|
||
|
||
pDET->sample_cnt[snd_ch] = sample_cnt;
|
||
pDET->PSK_AGC[snd_ch] = PSK_AGC;
|
||
pDET->PkAmpI[snd_ch] = PkAmpI;
|
||
pDET->PkAmpQ[snd_ch] = PkAmpQ;
|
||
pDET->PkAmpMax[snd_ch] = PkAmpMax;
|
||
pDET->newpkpos[snd_ch] = newpkpos;
|
||
pDET->PSK_IZ1[snd_ch] = PSK_IZ1;
|
||
pDET->PSK_QZ1[snd_ch] = PSK_QZ1;
|
||
pDET->bit_osc[snd_ch] = bit_osc;
|
||
pDET->frame_status[snd_ch] = frame_status;
|
||
pDET->bit_cnt[snd_ch] = bit_cnt;
|
||
pDET->bit_stream[snd_ch] = bit_stream;
|
||
pDET->byte_rx[snd_ch] = byte_rx;
|
||
pDET->last_rx_bit[snd_ch] = last_rx_bit;
|
||
pDET->bit_stuff_cnt[snd_ch] = bit_stuff_cnt;
|
||
pDET->AngleCorr[snd_ch] = AngleCorr;
|
||
|
||
}
|
||
|
||
/*
|
||
|
||
////////////////////////////////////////////////////////
|
||
|
||
function blackman(i,tap: word): single;
|
||
var
|
||
a0,a1,a2,a: single;
|
||
{
|
||
a = 0.16;
|
||
a0 = (1-a)/2;
|
||
a1 = 1/2;
|
||
a2 = a/2;
|
||
result = a0-a1*cos(2*pi*i/(tap-1))+a2*cos(4*pi*i/(tap-1));
|
||
}
|
||
|
||
function nuttal(i,tap: word): single;
|
||
var
|
||
a0,a1,a2,a3: single;
|
||
{
|
||
a0 = 0.355768;
|
||
a1 = 0.487396;
|
||
a2 = 0.144232;
|
||
a3 = 0.012604;
|
||
result = a0-a1*cos(2*pi*i/(tap-1))+a2*cos(4*pi*i/(tap-1))-a3*cos(6*pi*i/(tap-1));
|
||
}
|
||
|
||
function flattop(i,tap: word): single;
|
||
var
|
||
a0,a1,a2,a3,a4: single;
|
||
{
|
||
a0 = 1;
|
||
a1 = 1.93;
|
||
a2 = 1.29;
|
||
a3 = 0.388;
|
||
a4 = 0.032;
|
||
result = a0-a1*cos(2*pi*i/(tap-1))+a2*cos(4*pi*i/(tap-1))-a3*cos(6*pi*i/(tap-1))+a4*cos(8*pi*i/(tap-1));
|
||
}
|
||
*/
|
||
|
||
|
||
void init_BPF(float freq1, float freq2, unsigned short tap, float samplerate, float * buf)
|
||
{
|
||
unsigned short tap1, i;
|
||
float tap12, ham, acc1, acc2;
|
||
float bpf_l[2048];
|
||
float bpf_h[2048];
|
||
float itap12, pi2, x1, x2;
|
||
|
||
acc1 = 0;
|
||
acc2 = 0;
|
||
tap1 = tap - 1;
|
||
tap12 = tap1 / 2;
|
||
pi2 = 2 * pi;
|
||
x1 = pi2 * freq1 / samplerate;
|
||
x2 = pi2 * freq2 / samplerate;
|
||
for (i = 0; i <= tap1; i++)
|
||
{
|
||
// float x = (pi2 * i) / tap1;
|
||
// x = cosf(x);
|
||
// ham = 0.5 - 0.5 * x;
|
||
|
||
ham = 0.5 - 0.5 * cosf((pi2 * i) / tap1); //old
|
||
|
||
if (ham != ham) // check for NaN
|
||
ham = 0.0f;
|
||
|
||
itap12 = i - tap12;
|
||
|
||
if (itap12 == 0)
|
||
{
|
||
bpf_l[i] = x1;
|
||
bpf_h[i] = x2;
|
||
}
|
||
else
|
||
{
|
||
bpf_l[i] = sinf(x1*itap12) / itap12;
|
||
bpf_h[i] = sinf(x2*itap12) / itap12;
|
||
}
|
||
|
||
bpf_l[i] = bpf_l[i] * ham;
|
||
bpf_h[i] = bpf_h[i] * ham;
|
||
acc1 = acc1 + bpf_l[i];
|
||
acc2 = acc2 + bpf_h[i];
|
||
}
|
||
|
||
for (i = 0; i <= tap1; i++)
|
||
{
|
||
bpf_l[i] = bpf_l[i] / acc1;
|
||
bpf_h[i] = -(bpf_h[i] / acc2);
|
||
};
|
||
|
||
bpf_h[tap / 2] = bpf_h[tap / 2] + 1;
|
||
|
||
for (i = 0; i <= tap; i++)
|
||
{
|
||
buf[i] = -(bpf_l[i] + bpf_h[i]);
|
||
}
|
||
buf[tap / 2] = buf[tap / 2] + 1;
|
||
}
|
||
|
||
|
||
|
||
void init_LPF(float width, unsigned short tap, float samplerate, float * buf)
|
||
{
|
||
float acc1, ham;
|
||
unsigned short tap1, i;
|
||
float itap12, tap12, x1, pi2;
|
||
|
||
acc1 = 0;
|
||
tap1 = tap - 1;
|
||
tap12 = tap1 / 2;
|
||
pi2 = 2 * pi;
|
||
x1 = pi2 * width / samplerate;
|
||
|
||
for (i = 0; i <= tap1; i++)
|
||
{
|
||
ham = 0.53836f - 0.46164f * cosf(pi2 * i / tap1); //old
|
||
|
||
if (ham != ham) // check for NaN
|
||
ham = 0.0f;
|
||
|
||
//ham = 0.5-0.5*cos(pi2*i/tap1);
|
||
//ham = 0.5*(1-cos(pi2*i/tap1)); //hann
|
||
|
||
//ham = blackman(i,tap); //blackman
|
||
//ham = nuttal(i,tap);
|
||
|
||
itap12 = i - tap12;
|
||
|
||
if (itap12 == 0)
|
||
buf[i] = x1;
|
||
else
|
||
buf[i] = sinf(x1*itap12) / itap12;
|
||
|
||
buf[i] = buf[i] * ham;
|
||
acc1 = acc1 + buf[i];
|
||
}
|
||
for (i = 0; i <= tap1; i++)
|
||
buf[i] = buf[i] / acc1;
|
||
}
|
||
|
||
void make_core_INTR(UCHAR snd_ch)
|
||
{
|
||
float width;
|
||
|
||
width = roundf(RX_Samplerate / 2);
|
||
|
||
n_INTR[snd_ch] = 1;
|
||
|
||
switch (speed[snd_ch])
|
||
{
|
||
case SPEED_300:
|
||
|
||
width = roundf(RX_Samplerate / 2);
|
||
n_INTR[snd_ch] = 1;
|
||
break;
|
||
|
||
case SPEED_P300:
|
||
|
||
width = roundf(RX_Samplerate / 2);
|
||
n_INTR[snd_ch] = 1;
|
||
break;
|
||
|
||
|
||
case SPEED_Q300:
|
||
case SPEED_8PSK300:
|
||
|
||
width = roundf(RX_Samplerate / 2);
|
||
n_INTR[snd_ch] = 1;
|
||
break;
|
||
|
||
case SPEED_600:
|
||
|
||
width = roundf(RX_Samplerate / 4);
|
||
n_INTR[snd_ch] = 2;
|
||
break;
|
||
|
||
case SPEED_P600:
|
||
|
||
width = roundf(RX_Samplerate / 4);
|
||
n_INTR[snd_ch] = 2;
|
||
break;
|
||
|
||
case SPEED_1200:
|
||
width = roundf(RX_Samplerate / 8);
|
||
n_INTR[snd_ch] = 4;
|
||
break;
|
||
|
||
case SPEED_P1200:
|
||
width = roundf(RX_Samplerate / 8);
|
||
n_INTR[snd_ch] = 4;
|
||
break;
|
||
|
||
// case SPEED_Q1200:
|
||
// width = roundf(RX_Samplerate / 8);
|
||
// n_INTR[snd_ch] = 4;
|
||
// break;
|
||
|
||
case SPEED_Q2400:
|
||
width = 300;
|
||
n_INTR[snd_ch] = 4;
|
||
break; //8
|
||
|
||
case SPEED_DW2400:
|
||
|
||
width = 300;
|
||
n_INTR[snd_ch] = 4;
|
||
break;
|
||
|
||
case SPEED_2400V26B:
|
||
|
||
width = 300;
|
||
n_INTR[snd_ch] = 4;
|
||
break;
|
||
|
||
case SPEED_MP400:
|
||
|
||
width = round(RX_Samplerate / 8);
|
||
n_INTR[snd_ch] = 4;
|
||
break;
|
||
|
||
case SPEED_Q3600:
|
||
width = 300;
|
||
n_INTR[snd_ch] = 6;//12
|
||
break;
|
||
|
||
case SPEED_8P4800:
|
||
|
||
width = 100;
|
||
n_INTR[snd_ch] = 6;
|
||
break;
|
||
|
||
case SPEED_2400:
|
||
|
||
width = round(RX_Samplerate / 16);
|
||
n_INTR[snd_ch] = 8;
|
||
break;
|
||
|
||
case SPEED_P2400:
|
||
width = round(RX_Samplerate / 16);
|
||
n_INTR[snd_ch] = 8;
|
||
break;
|
||
|
||
case SPEED_Q4800:
|
||
width = 300;
|
||
n_INTR[snd_ch] = 8;//16
|
||
break;
|
||
}
|
||
|
||
|
||
init_LPF(width, INTR_tap[snd_ch], RX_Samplerate, INTR_core[snd_ch]);
|
||
}
|
||
|
||
void make_core_LPF(UCHAR snd_ch, short width)
|
||
{
|
||
if (modem_mode[snd_ch] == MODE_MPSK)
|
||
{
|
||
init_LPF(width, LPF_tap[snd_ch], RX_Samplerate / n_INTR[snd_ch], LPF_core[snd_ch]);
|
||
init_LPF(rx_baudrate[snd_ch], LPF_tap[snd_ch], RX_Samplerate / n_INTR[snd_ch], AFC_core[snd_ch]);
|
||
}
|
||
else
|
||
init_LPF(width, LPF_tap[snd_ch], RX_Samplerate, LPF_core[snd_ch]);
|
||
}
|
||
|
||
|
||
void make_core_BPF(UCHAR snd_ch, short freq, short width)
|
||
{
|
||
float old_freq, width2, rx_samplerate2, freq1, freq2;
|
||
|
||
UCHAR i;
|
||
|
||
freq = freq + rxOffset + chanOffset[snd_ch];
|
||
|
||
// I want to run decoders lowest to highest to simplify my display,
|
||
// so filters must be calculated in same order
|
||
|
||
int offset = -(RCVR[snd_ch] * rcvr_offset[snd_ch]); // lowest
|
||
|
||
rx_samplerate2 = 0.5 * RX_Samplerate;
|
||
width2 = 0.5 * width;
|
||
old_freq = freq;
|
||
|
||
for (i = 0; i <= RCVR[snd_ch] << 1; i++)
|
||
{
|
||
freq = old_freq + offset;
|
||
|
||
freq1 = freq - width2;
|
||
freq2 = freq + width2;
|
||
if (freq1 < 1)
|
||
freq1 = 1;
|
||
|
||
if (freq2 < 1)
|
||
freq2 = 1;
|
||
|
||
if (freq1 > rx_samplerate2)
|
||
freq1 = rx_samplerate2;
|
||
|
||
if (freq2 > rx_samplerate2)
|
||
freq2 = rx_samplerate2;
|
||
|
||
init_BPF(freq1, freq2, BPF_tap[snd_ch], RX_Samplerate, &DET[0][i].BPF_core[snd_ch][0]);
|
||
|
||
offset += rcvr_offset[snd_ch];
|
||
}
|
||
}
|
||
|
||
|
||
|
||
void make_core_TXBPF(UCHAR snd_ch, float freq, float width)
|
||
{
|
||
float freq1, freq2;
|
||
|
||
freq1 = freq - width / 2;
|
||
freq2 = freq + width / 2;
|
||
|
||
if (freq1 < 1)
|
||
freq1 = 1;
|
||
|
||
if (freq2 < 1)
|
||
freq2 = 1;
|
||
|
||
if (freq1 > TX_Samplerate / 2)
|
||
freq1 = TX_Samplerate / 2;
|
||
|
||
if (freq2 > TX_Samplerate / 2)
|
||
freq2 = TX_Samplerate / 2;
|
||
|
||
init_BPF(freq1, freq2, tx_BPF_tap[snd_ch], TX_Samplerate, tx_BPF_core[snd_ch]);
|
||
}
|
||
|
||
|
||
|
||
|
||
void interpolation(int snd_ch, int rcvr_nr, int emph, float * dest_buf, float * src_buf, int buf_size)
|
||
{
|
||
int n_intr1, buf_size1, k, i, j;
|
||
float buf[8192];
|
||
|
||
buf_size1 = buf_size - 1;
|
||
n_intr1 = n_INTR[snd_ch] - 1;
|
||
k = 0;
|
||
|
||
for (i = 0; i <= buf_size1; i++)
|
||
{
|
||
for (j = 0; j <= n_intr1; j++)
|
||
{
|
||
buf[k] = src_buf[i];
|
||
k++;
|
||
}
|
||
}
|
||
FIR_filter(buf, buf_size *n_INTR[snd_ch], INTR_tap[snd_ch], INTR_core[snd_ch], dest_buf, DET[emph][rcvr_nr].prev_INTR_buf[snd_ch]);
|
||
}
|
||
|
||
void interpolation_PSK(int snd_ch, int rcvr_nr, int emph, float * destI, float * destQ, float * srcI, float * srcQ, int buf_size)
|
||
{
|
||
word n_intr1, buf_size1, k, i, j;
|
||
single bufI[8192], bufQ[8192];
|
||
|
||
buf_size1 = buf_size - 1;
|
||
n_intr1 = n_INTR[snd_ch] - 1;
|
||
|
||
k = 0;
|
||
|
||
for (i = 0; i <= buf_size1; i++)
|
||
{
|
||
for (j = 0; j <= n_intr1; j++)
|
||
{
|
||
bufI[k] = srcI[i];
|
||
bufQ[k] = srcQ[i];
|
||
k++;
|
||
}
|
||
}
|
||
|
||
FIR_filter(bufI, buf_size*n_INTR[snd_ch], INTR_tap[snd_ch], INTR_core[snd_ch], destI, DET[emph][rcvr_nr].prev_INTRI_buf[snd_ch]);
|
||
FIR_filter(bufQ, buf_size*n_INTR[snd_ch], INTR_tap[snd_ch], INTR_core[snd_ch], destQ, DET[emph][rcvr_nr].prev_INTRQ_buf[snd_ch]);
|
||
}
|
||
|
||
|
||
void FSK_Demodulator(int snd_ch, int rcvr_nr, int emph, int last)
|
||
{
|
||
// filtered samples in src_BPF_buf, output in src_Loop_buf
|
||
|
||
Mux3(snd_ch,rcvr_nr,emph, &DET[0][rcvr_nr].src_BPF_buf[snd_ch][0], &LPF_core[snd_ch][0], &DET[emph][rcvr_nr].src_Loop_buf[snd_ch][0],
|
||
&DET[emph][rcvr_nr].prev_LPF1I_buf[snd_ch][0], &DET[emph][rcvr_nr].prev_LPF1Q_buf[snd_ch][0], LPF_tap[snd_ch], rx_bufsize);
|
||
|
||
if (n_INTR[snd_ch] > 1)
|
||
{
|
||
interpolation(snd_ch, rcvr_nr, emph, DET[emph][rcvr_nr].src_INTR_buf[snd_ch], DET[emph][rcvr_nr].src_Loop_buf[snd_ch], rx_bufsize);
|
||
decode_stream_FSK(last, snd_ch, rcvr_nr, emph, &DET[emph][rcvr_nr].src_INTR_buf[snd_ch][0], &DET[emph][rcvr_nr].bit_buf[snd_ch][0], rx_bufsize*n_INTR[snd_ch], &DET[emph][rcvr_nr].rx_data[snd_ch]);
|
||
}
|
||
else
|
||
decode_stream_FSK(last,snd_ch,rcvr_nr,emph,DET[emph][rcvr_nr].src_Loop_buf[snd_ch], &DET[emph][rcvr_nr].bit_buf[snd_ch][0], rx_bufsize, &DET[emph][rcvr_nr].rx_data[snd_ch]);
|
||
}
|
||
|
||
void BPSK_Demodulator(int snd_ch, int rcvr_nr, int emph, int last)
|
||
{
|
||
Mux3_PSK(snd_ch, rcvr_nr, emph,
|
||
DET[0][rcvr_nr].src_BPF_buf[snd_ch],
|
||
LPF_core[snd_ch],
|
||
DET[emph][rcvr_nr].src_LPF1I_buf[snd_ch],
|
||
DET[emph][rcvr_nr].src_LPF1Q_buf[snd_ch],
|
||
DET[emph][rcvr_nr].prev_LPF1I_buf[snd_ch],
|
||
DET[emph][rcvr_nr].prev_LPF1Q_buf[snd_ch],
|
||
LPF_tap[snd_ch], rx_bufsize);
|
||
|
||
if (n_INTR[snd_ch] > 1)
|
||
{
|
||
interpolation_PSK(snd_ch, rcvr_nr, emph,
|
||
DET[emph][rcvr_nr].src_INTRI_buf[snd_ch],
|
||
DET[emph][rcvr_nr].src_INTRQ_buf[snd_ch],
|
||
DET[emph][rcvr_nr].src_LPF1I_buf[snd_ch],
|
||
DET[emph][rcvr_nr].src_LPF1Q_buf[snd_ch], rx_bufsize);
|
||
|
||
decode_stream_BPSK(last, snd_ch, rcvr_nr, emph,
|
||
DET[emph][rcvr_nr].src_INTRI_buf[snd_ch],
|
||
DET[emph][rcvr_nr].src_INTRQ_buf[snd_ch],
|
||
DET[emph][rcvr_nr].bit_buf[snd_ch],
|
||
rx_bufsize*n_INTR[snd_ch],
|
||
&DET[emph][rcvr_nr].rx_data[snd_ch]);
|
||
|
||
}
|
||
else
|
||
decode_stream_BPSK(last, snd_ch, rcvr_nr, emph,
|
||
DET[emph][rcvr_nr].src_LPF1I_buf[snd_ch],
|
||
DET[emph][rcvr_nr].src_LPF1Q_buf[snd_ch],
|
||
DET[emph][rcvr_nr].bit_buf[snd_ch],
|
||
rx_bufsize,
|
||
&DET[emph][rcvr_nr].rx_data[snd_ch]);
|
||
}
|
||
|
||
void QPSK_Demodulator(int snd_ch, int rcvr_nr, int emph, int last)
|
||
{
|
||
Mux3_PSK(snd_ch, rcvr_nr, emph,
|
||
DET[0][rcvr_nr].src_BPF_buf[snd_ch],
|
||
LPF_core[snd_ch],
|
||
DET[emph][rcvr_nr].src_LPF1I_buf[snd_ch],
|
||
DET[emph][rcvr_nr].src_LPF1Q_buf[snd_ch],
|
||
DET[emph][rcvr_nr].prev_LPF1I_buf[snd_ch],
|
||
DET[emph][rcvr_nr].prev_LPF1Q_buf[snd_ch],
|
||
LPF_tap[snd_ch], rx_bufsize);
|
||
|
||
if (n_INTR[snd_ch] > 1)
|
||
{
|
||
interpolation_PSK(snd_ch, rcvr_nr, emph,
|
||
DET[emph][rcvr_nr].src_INTRI_buf[snd_ch],
|
||
DET[emph][rcvr_nr].src_INTRQ_buf[snd_ch],
|
||
DET[emph][rcvr_nr].src_LPF1I_buf[snd_ch],
|
||
DET[emph][rcvr_nr].src_LPF1Q_buf[snd_ch], rx_bufsize);
|
||
|
||
decode_stream_QPSK(last, snd_ch, rcvr_nr, emph,
|
||
DET[emph][rcvr_nr].src_INTRI_buf[snd_ch],
|
||
DET[emph][rcvr_nr].src_INTRQ_buf[snd_ch],
|
||
DET[emph][rcvr_nr].bit_buf[snd_ch],
|
||
rx_bufsize*n_INTR[snd_ch],
|
||
&DET[emph][rcvr_nr].rx_data[snd_ch]);
|
||
|
||
}
|
||
else decode_stream_QPSK(last, snd_ch, rcvr_nr, emph,
|
||
DET[emph][rcvr_nr].src_LPF1I_buf[snd_ch],
|
||
DET[emph][rcvr_nr].src_LPF1Q_buf[snd_ch],
|
||
DET[emph][rcvr_nr].bit_buf[snd_ch],
|
||
rx_bufsize,
|
||
&DET[emph][rcvr_nr].rx_data[snd_ch]);
|
||
|
||
}
|
||
|
||
|
||
|
||
|
||
void PSK8_Demodulator(int snd_ch, int rcvr_nr, int emph, boolean last)
|
||
{
|
||
Mux3_PSK(snd_ch, rcvr_nr, emph,
|
||
DET[0][rcvr_nr].src_BPF_buf[snd_ch],
|
||
LPF_core[snd_ch],
|
||
DET[emph][rcvr_nr].src_LPF1I_buf[snd_ch],
|
||
DET[emph][rcvr_nr].src_LPF1Q_buf[snd_ch],
|
||
DET[emph][rcvr_nr].prev_LPF1I_buf[snd_ch],
|
||
DET[emph][rcvr_nr].prev_LPF1Q_buf[snd_ch],
|
||
LPF_tap[snd_ch], rx_bufsize);
|
||
|
||
if (n_INTR[snd_ch] > 1)
|
||
{
|
||
interpolation_PSK(snd_ch, rcvr_nr, emph,
|
||
DET[emph][rcvr_nr].src_INTRI_buf[snd_ch],
|
||
DET[emph][rcvr_nr].src_INTRQ_buf[snd_ch],
|
||
DET[emph][rcvr_nr].src_LPF1I_buf[snd_ch],
|
||
DET[emph][rcvr_nr].src_LPF1Q_buf[snd_ch], rx_bufsize);
|
||
|
||
decode_stream_8PSK(last, snd_ch, rcvr_nr, emph,
|
||
DET[emph][rcvr_nr].src_INTRI_buf[snd_ch],
|
||
DET[emph][rcvr_nr].src_INTRQ_buf[snd_ch],
|
||
DET[emph][rcvr_nr].bit_buf[snd_ch],
|
||
rx_bufsize*n_INTR[snd_ch],
|
||
&DET[emph][rcvr_nr].rx_data[snd_ch]);
|
||
|
||
}
|
||
else
|
||
decode_stream_8PSK(last, snd_ch, rcvr_nr, emph,
|
||
DET[emph][rcvr_nr].src_LPF1I_buf[snd_ch],
|
||
DET[emph][rcvr_nr].src_LPF1Q_buf[snd_ch],
|
||
DET[emph][rcvr_nr].bit_buf[snd_ch],
|
||
rx_bufsize,
|
||
&DET[emph][rcvr_nr].rx_data[snd_ch]);
|
||
}
|
||
|
||
|
||
void Demodulator(int snd_ch, int rcvr_nr, float * src_buf, int last, int xcenter)
|
||
{
|
||
// called once per decoder (current one in rcvr_nr)
|
||
|
||
int i, k;
|
||
string rec_code;
|
||
UCHAR emph;
|
||
int found;
|
||
string * s_emph;
|
||
struct TDetector_t * pDET = &DET[0][rcvr_nr];
|
||
|
||
// looks like this filters to src_BPF_buf
|
||
|
||
FIR_filter(src_buf, rx_bufsize, BPF_tap[snd_ch], pDET->BPF_core[snd_ch], pDET->src_BPF_buf[snd_ch], pDET->prev_BPF_buf[snd_ch]);
|
||
|
||
// AFSK demodulator
|
||
|
||
if (modem_mode[snd_ch] == MODE_FSK)
|
||
{
|
||
if (emph_all[snd_ch])
|
||
{
|
||
for (emph = 1; emph <= nr_emph; emph++)
|
||
FSK_Demodulator(snd_ch, rcvr_nr, emph, FALSE);
|
||
|
||
FSK_Demodulator(snd_ch, rcvr_nr, 0, last);
|
||
}
|
||
else
|
||
FSK_Demodulator(snd_ch, rcvr_nr, emph_db[snd_ch], last);
|
||
}
|
||
|
||
// BPSK demodulator
|
||
if (modem_mode[snd_ch] == MODE_BPSK)
|
||
{
|
||
if (emph_all[snd_ch])
|
||
{
|
||
for (emph = 1; emph <= nr_emph; emph++)
|
||
BPSK_Demodulator(snd_ch, rcvr_nr, emph, FALSE);
|
||
|
||
BPSK_Demodulator(snd_ch, rcvr_nr, 0, last);
|
||
}
|
||
else
|
||
BPSK_Demodulator(snd_ch, rcvr_nr, emph_db[snd_ch], last);
|
||
|
||
}
|
||
|
||
// QPSK demodulator
|
||
if (modem_mode[snd_ch] == MODE_QPSK || modem_mode[snd_ch] == MODE_PI4QPSK)
|
||
{
|
||
if (emph_all[snd_ch])
|
||
{
|
||
for (emph = 1; emph <= nr_emph; emph++)
|
||
QPSK_Demodulator(snd_ch, rcvr_nr, emph, FALSE);
|
||
|
||
QPSK_Demodulator(snd_ch, rcvr_nr, 0, last);
|
||
}
|
||
else
|
||
QPSK_Demodulator(snd_ch, rcvr_nr, emph_db[snd_ch], last);
|
||
}
|
||
|
||
// 8PSK demodulator
|
||
|
||
if (modem_mode[snd_ch]==MODE_8PSK)
|
||
{
|
||
if (emph_all[snd_ch])
|
||
{
|
||
for (emph = 1; emph <= nr_emph; emph++)
|
||
PSK8_Demodulator(snd_ch, rcvr_nr, emph, FALSE);
|
||
|
||
PSK8_Demodulator(snd_ch, rcvr_nr, 0, last);
|
||
}
|
||
else
|
||
PSK8_Demodulator(snd_ch,rcvr_nr,emph_db[snd_ch],last);
|
||
}
|
||
|
||
// MPSK demodulator
|
||
|
||
if (modem_mode[snd_ch] == MODE_MPSK)
|
||
{
|
||
decode_stream_MPSK(snd_ch, rcvr_nr, DET[0][rcvr_nr].src_BPF_buf[snd_ch], rx_bufsize, last);
|
||
}
|
||
|
||
|
||
// I think this handles multiple decoders and passes packet on to next level
|
||
|
||
// Packet manager
|
||
if (last)
|
||
ProcessRXFrames(snd_ch);
|
||
}
|
||
|
||
void ProcessRXFrames(int snd_ch)
|
||
{
|
||
boolean fecflag = 0;
|
||
char indicators[5] = "-$#F+"; // None, Single, MEM, FEC, Normal
|
||
|
||
// Work out which decoder and which emph settings worked.
|
||
|
||
if (snd_ch < 0 || snd_ch > 3)
|
||
return;
|
||
|
||
if (detect_list[snd_ch].Count > 0) // no point if nothing decoded
|
||
{
|
||
char decoded[32] = "";
|
||
char indicators[5] = "-$#F+"; // None, Single, MEM, FEC, Normal
|
||
char s_emph[4] = "";
|
||
int emph[4] = { 0 };
|
||
char report[32] = "";
|
||
int il2perrors = 255;
|
||
|
||
// The is one DET for each Decoder for each Emph setting
|
||
|
||
struct TDetector_t * pDET;
|
||
int i = 0, j, found;
|
||
int maxemph = nr_emph;
|
||
|
||
for (i = 0; i <= nr_emph; i++)
|
||
{
|
||
for (j = 0; j <= RCVR[snd_ch] * 2; j++)
|
||
{
|
||
pDET = &DET[i][j];
|
||
|
||
if (pDET->rx_decoded > decoded[j]) // Better than other one (| is higher than F)
|
||
decoded[j] = pDET->rx_decoded;
|
||
|
||
if (pDET->emph_decoded > emph[i])
|
||
emph[i] = pDET->emph_decoded;
|
||
|
||
if (il2perrors > pDET->errors)
|
||
il2perrors = pDET->errors;
|
||
|
||
pDET->rx_decoded = 0;
|
||
pDET->emph_decoded = 0; // Ready for next time
|
||
pDET->errors = 255;
|
||
}
|
||
if (emph_all[snd_ch] == 0)
|
||
break;
|
||
}
|
||
|
||
decoded[j] = 0;
|
||
|
||
for (j--; j >= 0; j--)
|
||
decoded[j] = indicators[decoded[j]];
|
||
|
||
if (emph_all[snd_ch])
|
||
{
|
||
for (i = 0; i <= nr_emph; i++)
|
||
{
|
||
s_emph[i] = indicators[emph[i]];
|
||
}
|
||
sprintf(report, "%s][%s", s_emph, decoded);
|
||
}
|
||
|
||
else
|
||
strcpy(report, decoded);
|
||
|
||
if (detect_list_c[snd_ch].Items[0]->Length)
|
||
{
|
||
if (il2perrors < 255 && il2perrors > 0)
|
||
sprintf(detect_list_c[snd_ch].Items[0]->Data, "%s-%d", detect_list_c[snd_ch].Items[0]->Data, il2perrors);
|
||
|
||
strcat(report, "][");
|
||
strcat(report, detect_list_c[snd_ch].Items[0]->Data);
|
||
}
|
||
|
||
if (detect_list[snd_ch].Count > 0)
|
||
{
|
||
for (i = 0; i < detect_list[snd_ch].Count; i++)
|
||
{
|
||
found = 0;
|
||
|
||
// if (detect_list_l[snd_ch].Count > 0)
|
||
// if (my_indexof(&detect_list_l[snd_ch], detect_list[snd_ch].Items[i]) > -1)
|
||
// found = 1;
|
||
|
||
if (found == 0)
|
||
{
|
||
if (modem_mode[snd_ch] == MODE_MPSK)
|
||
{
|
||
// analiz_frame(snd_ch, detect_list[snd_ch].Items[i]->Data, [snd_ch].Items[i]->Data + ' dF: ' + FloatToStrF(DET[0, 0].AFC_dF[snd_ch], ffFixed, 0, 1));
|
||
}
|
||
else
|
||
{
|
||
analiz_frame(snd_ch, detect_list[snd_ch].Items[i], report, fecflag);
|
||
}
|
||
}
|
||
}
|
||
|
||
// Cancel FX25 decode
|
||
|
||
if (fx25_mode[snd_ch] != FX25_MODE_NONE)
|
||
{
|
||
int e;
|
||
|
||
for (i = 0; i < 16; i++)
|
||
for (e = 0; e <= nr_emph; e++)
|
||
DET[e][i].fx25[snd_ch].status = FX25_TAG;
|
||
}
|
||
}
|
||
|
||
// Assign(&detect_list_l[snd_ch], &detect_list[snd_ch]); // Duplicate detect_list to detect_list_l
|
||
|
||
Clear(&detect_list[snd_ch]);
|
||
Clear(&detect_list_c[snd_ch]);
|
||
}
|
||
chk_dcd1(snd_ch, rx_bufsize);
|
||
}
|
||
|
||
string * memory_ARQ(TStringList * buf, string * data)
|
||
{
|
||
unsigned char crc[32];
|
||
string * s;
|
||
string * frame;
|
||
word k, len, i;
|
||
|
||
Byte zeros, ones;
|
||
TStringList need_frames;
|
||
|
||
s = data;
|
||
|
||
CreateStringList(&need_frames);
|
||
len = data->Length;
|
||
|
||
memcpy(crc, &data->Data[data->Length - 18], 18);
|
||
|
||
if (buf->Count > 0)
|
||
{
|
||
for (i = 0; i < buf->Count; i++)
|
||
{
|
||
if (buf->Items[i]->Length == len)
|
||
if (memcmp(&buf->Items[i]->Data[len - 18], crc, 18) == 0)
|
||
Add(&need_frames, buf->Items[i]);
|
||
}
|
||
}
|
||
|
||
if (need_frames.Count > 2)
|
||
{
|
||
for (i = 0; i < len - 18; i++)
|
||
{
|
||
zeros = 0;
|
||
ones = 0;
|
||
|
||
for (k = 0; k < need_frames.Count; k++)
|
||
{
|
||
frame = need_frames.Items[k];
|
||
if (frame->Data[i] == '1') ones++; else zeros++;
|
||
}
|
||
if (ones > zeros) s->Data[i] = '1'; else s->Data[i] = '0';
|
||
}
|
||
}
|
||
|
||
// Clear(&need_frames);
|
||
return s;
|
||
}
|
||
|
||
|