diff options
Diffstat (limited to 'src/cohpsk.c')
| -rw-r--r-- | src/cohpsk.c | 1941 |
1 files changed, 973 insertions, 968 deletions
diff --git a/src/cohpsk.c b/src/cohpsk.c index b7ce633..d5f7359 100644 --- a/src/cohpsk.c +++ b/src/cohpsk.c @@ -32,35 +32,31 @@ \*---------------------------------------------------------------------------*/ #include <assert.h> -#include <stdlib.h> +#include <math.h> #include <stdio.h> +#include <stdlib.h> #include <string.h> -#include <math.h> #include "codec2_cohpsk.h" #include "cohpsk_defs.h" #include "cohpsk_internal.h" -#include "fdmdv_internal.h" -#include "pilots_coh.h" #include "comp_prim.h" +#include "debug_alloc.h" +#include "fdmdv_internal.h" #include "kiss_fft.h" #include "linreg.h" +#include "pilots_coh.h" #include "rn_coh.h" #include "test_bits_coh.h" -#include "debug_alloc.h" - -static COMP qpsk_mod[] = { - { 1.0, 0.0}, - { 0.0, 1.0}, - { 0.0,-1.0}, - {-1.0, 0.0} -}; +static COMP qpsk_mod[] = {{1.0, 0.0}, {0.0, 1.0}, {0.0, -1.0}, {-1.0, 0.0}}; static int sampling_points[] = {0, 1, 6, 7}; -void corr_with_pilots(float *corr_out, float *mag_out, struct COHPSK *coh, int t, float f_fine); -void update_ct_symb_buf(COMP ct_symb_buf[][COHPSK_NC*COHPSK_ND], COMP ch_symb[][COHPSK_NC*COHPSK_ND]); +void corr_with_pilots(float *corr_out, float *mag_out, struct COHPSK *coh, + int t, float f_fine); +void update_ct_symb_buf(COMP ct_symb_buf[][COHPSK_NC * COHPSK_ND], + COMP ch_symb[][COHPSK_NC * COHPSK_ND]); /*---------------------------------------------------------------------------*\ @@ -68,7 +64,6 @@ void update_ct_symb_buf(COMP ct_symb_buf[][COHPSK_NC*COHPSK_ND], COMP ch_symb[][ \*---------------------------------------------------------------------------*/ - /*--------------------------------------------------------------------------* \ FUNCTION....: cohpsk_create @@ -81,130 +76,133 @@ void update_ct_symb_buf(COMP ct_symb_buf[][COHPSK_NC*COHPSK_ND], COMP ch_symb[][ \*---------------------------------------------------------------------------*/ -struct COHPSK *cohpsk_create(void) -{ - struct COHPSK *coh; - struct FDMDV *fdmdv; - int r,c,p,i; - float freq_hz, result; - float tau = 2.0f * M_PI; - - assert(COHPSK_NC == PILOTS_NC); - assert(COHPSK_NOM_SAMPLES_PER_FRAME == (COHPSK_M*NSYMROWPILOT)); - assert(COHPSK_MAX_SAMPLES_PER_FRAME == (COHPSK_M*NSYMROWPILOT+COHPSK_M/P)); - assert(COHPSK_NSYM == NSYM); /* as we want to use the tx sym mem on fdmdv */ - assert(COHPSK_NT == NT); - - coh = (struct COHPSK*)MALLOC(sizeof(struct COHPSK)); - if (coh == NULL) - return NULL; - - /* set up buffer of tx pilot symbols for coh demod on rx */ - - for(r=0; r<2*NPILOTSFRAME; ) { - for(p=0; p<NPILOTSFRAME; r++, p++) { - for(c=0; c<COHPSK_NC; c++) { - coh->pilot2[r][c] = pilots_coh[p][c]; - } - } +struct COHPSK *cohpsk_create(void) { + struct COHPSK *coh; + struct FDMDV *fdmdv; + int r, c, p, i; + float freq_hz, result; + float tau = 2.0f * M_PI; + + assert(COHPSK_NC == PILOTS_NC); + assert(COHPSK_NOM_SAMPLES_PER_FRAME == (COHPSK_M * NSYMROWPILOT)); + assert(COHPSK_MAX_SAMPLES_PER_FRAME == + (COHPSK_M * NSYMROWPILOT + COHPSK_M / P)); + assert(COHPSK_NSYM == NSYM); /* as we want to use the tx sym mem on fdmdv */ + assert(COHPSK_NT == NT); + + coh = (struct COHPSK *)MALLOC(sizeof(struct COHPSK)); + if (coh == NULL) return NULL; + + /* set up buffer of tx pilot symbols for coh demod on rx */ + + for (r = 0; r < 2 * NPILOTSFRAME;) { + for (p = 0; p < NPILOTSFRAME; r++, p++) { + for (c = 0; c < COHPSK_NC; c++) { + coh->pilot2[r][c] = pilots_coh[p][c]; + } } + } - /* Clear symbol buffer memory */ + /* Clear symbol buffer memory */ - for (r=0; r<NCT_SYMB_BUF; r++) { - for(c=0; c<COHPSK_NC*COHPSK_ND; c++) { - coh->ct_symb_buf[r][c].real = 0.0; - coh->ct_symb_buf[r][c].imag = 0.0; - } + for (r = 0; r < NCT_SYMB_BUF; r++) { + for (c = 0; c < COHPSK_NC * COHPSK_ND; c++) { + coh->ct_symb_buf[r][c].real = 0.0; + coh->ct_symb_buf[r][c].imag = 0.0; } + } - coh->ff_phase.real = 1.0; coh->ff_phase.imag = 0.0; - coh->sync = 0; - coh->frame = 0; - coh->ratio = 0.0; - coh->nin = COHPSK_M; + coh->ff_phase.real = 1.0; + coh->ff_phase.imag = 0.0; + coh->sync = 0; + coh->frame = 0; + coh->ratio = 0.0; + coh->nin = COHPSK_M; - /* clear sync window buffer */ + /* clear sync window buffer */ - for (i=0; i<NSW*NSYMROWPILOT*COHPSK_M; i++) { - coh->ch_fdm_frame_buf[i].real = 0.0; - coh->ch_fdm_frame_buf[i].imag = 0.0; - } + for (i = 0; i < NSW * NSYMROWPILOT * COHPSK_M; i++) { + coh->ch_fdm_frame_buf[i].real = 0.0; + coh->ch_fdm_frame_buf[i].imag = 0.0; + } - /* set up fdmdv states so we can use those modem functions */ + /* set up fdmdv states so we can use those modem functions */ - /* - * NC*ND -1 Realize that the function creates a sync carrier (+1), - * or one more carrier than asked for. We ignore any initialization - * inside of fdmdv and take care of that here, using the whole - * NC*ND number of carriers to be used in cohpsk. - */ - fdmdv = fdmdv_create((COHPSK_NC*COHPSK_ND) - 1); + /* + * NC*ND -1 Realize that the function creates a sync carrier (+1), + * or one more carrier than asked for. We ignore any initialization + * inside of fdmdv and take care of that here, using the whole + * NC*ND number of carriers to be used in cohpsk. + */ + fdmdv = fdmdv_create((COHPSK_NC * COHPSK_ND) - 1); - fdmdv->fsep = COHPSK_RS*(1.0 + COHPSK_EXCESS_BW); + fdmdv->fsep = COHPSK_RS * (1.0 + COHPSK_EXCESS_BW); - for(c=0; c<COHPSK_NC*COHPSK_ND; c++) { - fdmdv->phase_tx[c].real = 1.0; - fdmdv->phase_tx[c].imag = 0.0; + for (c = 0; c < COHPSK_NC * COHPSK_ND; c++) { + fdmdv->phase_tx[c].real = 1.0; + fdmdv->phase_tx[c].imag = 0.0; - /* note non-linear carrier spacing to help PAPR, works v well in conjunction with CLIP */ + /* note non-linear carrier spacing to help PAPR, works v well in conjunction + * with CLIP */ - freq_hz = fdmdv->fsep*( -(COHPSK_NC*COHPSK_ND)/2 - 0.5f + powf(c + 1.0f, 0.98f) ); - result = tau * freq_hz/COHPSK_FS; + freq_hz = fdmdv->fsep * + (-(COHPSK_NC * COHPSK_ND) / 2 - 0.5f + powf(c + 1.0f, 0.98f)); + result = tau * freq_hz / COHPSK_FS; - fdmdv->freq[c].real = cosf(result); - fdmdv->freq[c].imag = sinf(result); - fdmdv->freq_pol[c] = result; + fdmdv->freq[c].real = cosf(result); + fdmdv->freq[c].imag = sinf(result); + fdmdv->freq_pol[c] = result; - //printf("c: %d %f %f\n",c,freq_hz,fdmdv->freq_pol[c]); - for(i=0; i<COHPSK_NFILTER; i++) { - coh->rx_filter_memory[c][i].real = 0.0; - coh->rx_filter_memory[c][i].imag = 0.0; - } + // printf("c: %d %f %f\n",c,freq_hz,fdmdv->freq_pol[c]); + for (i = 0; i < COHPSK_NFILTER; i++) { + coh->rx_filter_memory[c][i].real = 0.0; + coh->rx_filter_memory[c][i].imag = 0.0; + } - /* optional per-carrier amplitude weighting for testing */ + /* optional per-carrier amplitude weighting for testing */ - coh->carrier_ampl[c] = 1.0; - } - - result = tau * FDMDV_FCENTRE/COHPSK_FS; - fdmdv->fbb_rect.real = cosf(result); - fdmdv->fbb_rect.imag = sinf(result); - fdmdv->fbb_pol = result; + coh->carrier_ampl[c] = 1.0; + } - coh->fdmdv = fdmdv; + result = tau * FDMDV_FCENTRE / COHPSK_FS; + fdmdv->fbb_rect.real = cosf(result); + fdmdv->fbb_rect.imag = sinf(result); + fdmdv->fbb_pol = result; - coh->sig_rms = coh->noise_rms = 0.0; + coh->fdmdv = fdmdv; - for(c=0; c<COHPSK_NC*COHPSK_ND; c++) { - for (r=0; r<NSYMROW; r++) { - coh->rx_symb[r][c].real = 0.0; - coh->rx_symb[r][c].imag = 0.0; - } + coh->sig_rms = coh->noise_rms = 0.0; + + for (c = 0; c < COHPSK_NC * COHPSK_ND; c++) { + for (r = 0; r < NSYMROW; r++) { + coh->rx_symb[r][c].real = 0.0; + coh->rx_symb[r][c].imag = 0.0; } + } - coh->verbose = 0; + coh->verbose = 0; - /* disable optional logging by default */ + /* disable optional logging by default */ - coh->rx_baseband_log = NULL; - coh->rx_baseband_log_col_index = 0; - coh->rx_filt_log = NULL; - coh->rx_filt_log_col_index = 0; - coh->ch_symb_log = NULL; - coh->ch_symb_log_r = 0; - coh->rx_timing_log = NULL; - coh->rx_timing_log_index = 0; + coh->rx_baseband_log = NULL; + coh->rx_baseband_log_col_index = 0; + coh->rx_filt_log = NULL; + coh->rx_filt_log_col_index = 0; + coh->ch_symb_log = NULL; + coh->ch_symb_log_r = 0; + coh->rx_timing_log = NULL; + coh->rx_timing_log_index = 0; - /* test frames */ + /* test frames */ - coh->ptest_bits_coh_tx = coh->ptest_bits_coh_rx[0] = coh->ptest_bits_coh_rx[1] = (int*)test_bits_coh; - coh->ptest_bits_coh_end = (int*)test_bits_coh + sizeof(test_bits_coh)/sizeof(int); + coh->ptest_bits_coh_tx = coh->ptest_bits_coh_rx[0] = + coh->ptest_bits_coh_rx[1] = (int *)test_bits_coh; + coh->ptest_bits_coh_end = + (int *)test_bits_coh + sizeof(test_bits_coh) / sizeof(int); - return coh; + return coh; } - /*---------------------------------------------------------------------------*\ FUNCTION....: cohpsk_destroy @@ -215,87 +213,85 @@ struct COHPSK *cohpsk_create(void) \*---------------------------------------------------------------------------*/ -void cohpsk_destroy(struct COHPSK *coh) -{ - fdmdv_destroy(coh->fdmdv); - assert(coh != NULL); - FREE(coh); +void cohpsk_destroy(struct COHPSK *coh) { + fdmdv_destroy(coh->fdmdv); + assert(coh != NULL); + FREE(coh); } - /*---------------------------------------------------------------------------*\ FUNCTION....: bits_to_qpsk_symbols() AUTHOR......: David Rowe DATE CREATED: March 2015 - Rate Rs modulator. Maps bits to parallel DQPSK symbols and inserts pilot symbols. + Rate Rs modulator. Maps bits to parallel DQPSK symbols and inserts pilot +symbols. \*---------------------------------------------------------------------------*/ -void bits_to_qpsk_symbols(COMP tx_symb[][COHPSK_NC*COHPSK_ND], int tx_bits[], int nbits) -{ - int i, r, c, p_r, data_r, d, diversity; - short bits; +void bits_to_qpsk_symbols(COMP tx_symb[][COHPSK_NC * COHPSK_ND], int tx_bits[], + int nbits) { + int i, r, c, p_r, data_r, d, diversity; + short bits; - /* check allowed number of bits supplied matches number of QPSK - symbols in the frame */ + /* check allowed number of bits supplied matches number of QPSK + symbols in the frame */ - assert( (NSYMROW*COHPSK_NC*2 == nbits) || (NSYMROW*COHPSK_NC*2*COHPSK_ND == nbits)); + assert((NSYMROW * COHPSK_NC * 2 == nbits) || + (NSYMROW * COHPSK_NC * 2 * COHPSK_ND == nbits)); - /* if we input twice as many bits we don't do diversity */ + /* if we input twice as many bits we don't do diversity */ - if (NSYMROW*COHPSK_NC*2 == nbits) { - diversity = 1; /* diversity mode */ - } - else { - diversity = 2; /* twice as many bits, non diversity mode */ - } + if (NSYMROW * COHPSK_NC * 2 == nbits) { + diversity = 1; /* diversity mode */ + } else { + diversity = 2; /* twice as many bits, non diversity mode */ + } - /* - Insert two rows of Nc pilots at beginning of data frame. + /* + Insert two rows of Nc pilots at beginning of data frame. - Organise QPSK symbols into a NSYMBROWS rows by PILOTS_NC*ND cols matrix, - each column is a carrier, time flows down the cols...... + Organise QPSK symbols into a NSYMBROWS rows by PILOTS_NC*ND cols matrix, + each column is a carrier, time flows down the cols...... - Note: the "& 0x1" prevents and non binary tx_bits[] screwing up - our lives. Call me defensive. + Note: the "& 0x1" prevents and non binary tx_bits[] screwing up + our lives. Call me defensive. - sqrtf(ND) term ensures the same energy/symbol for different - diversity factors. - */ + sqrtf(ND) term ensures the same energy/symbol for different + diversity factors. + */ - r = 0; - for(p_r=0; p_r<2; p_r++) { - for(c=0; c<COHPSK_NC*COHPSK_ND; c++) { - tx_symb[r][c].real = pilots_coh[p_r][c % COHPSK_NC]/sqrtf(COHPSK_ND); - tx_symb[r][c].imag = 0.0; - } - r++; + r = 0; + for (p_r = 0; p_r < 2; p_r++) { + for (c = 0; c < COHPSK_NC * COHPSK_ND; c++) { + tx_symb[r][c].real = pilots_coh[p_r][c % COHPSK_NC] / sqrtf(COHPSK_ND); + tx_symb[r][c].imag = 0.0; } - for(data_r=0; data_r<NSYMROW; data_r++, r++) { - for(c=0; c<COHPSK_NC*diversity; c++) { - i = c*NSYMROW + data_r; - bits = (tx_bits[2*i]&0x1)<<1 | (tx_bits[2*i+1]&0x1); - tx_symb[r][c] = fcmult(1.0/sqrtf(COHPSK_ND),qpsk_mod[bits]); - } + r++; + } + for (data_r = 0; data_r < NSYMROW; data_r++, r++) { + for (c = 0; c < COHPSK_NC * diversity; c++) { + i = c * NSYMROW + data_r; + bits = (tx_bits[2 * i] & 0x1) << 1 | (tx_bits[2 * i + 1] & 0x1); + tx_symb[r][c] = fcmult(1.0 / sqrtf(COHPSK_ND), qpsk_mod[bits]); } + } - assert(p_r == NPILOTSFRAME); - assert(r == NSYMROWPILOT); + assert(p_r == NPILOTSFRAME); + assert(r == NSYMROWPILOT); - /* if in diversity mode, copy symbols to upper carriers */ + /* if in diversity mode, copy symbols to upper carriers */ - for(d=1; d<1+COHPSK_ND-diversity; d++) { - for(r=0; r<NSYMROWPILOT; r++) { - for(c=0; c<COHPSK_NC; c++) { - tx_symb[r][c+COHPSK_NC*d] = tx_symb[r][c]; - } - } + for (d = 1; d < 1 + COHPSK_ND - diversity; d++) { + for (r = 0; r < NSYMROWPILOT; r++) { + for (c = 0; c < COHPSK_NC; c++) { + tx_symb[r][c + COHPSK_NC * d] = tx_symb[r][c]; + } } + } } - /*---------------------------------------------------------------------------*\ FUNCTION....: qpsk_symbols_to_bits() @@ -310,118 +306,118 @@ void bits_to_qpsk_symbols(COMP tx_symb[][COHPSK_NC*COHPSK_ND], int tx_bits[], in \*---------------------------------------------------------------------------*/ -void qpsk_symbols_to_bits(struct COHPSK *coh, float rx_bits[], COMP ct_symb_buf[][COHPSK_NC*COHPSK_ND]) -{ - int p, r, c, i, pc, d, n; - float x[NPILOTSFRAME+2], x1; - COMP y[NPILOTSFRAME+2], yfit; - COMP rx_symb_linear[NSYMROW*COHPSK_NC*COHPSK_ND]; - COMP m, b; - COMP __attribute__((unused)) corr, rot, pi_on_4, phi_rect, div_symb; - float mag, __attribute__((unused)) phi_, __attribute__((unused)) amp_; - float sum_x, sum_xx, noise_var; - float spi_4 = M_PI / 4.0f; - COMP s; - - pi_on_4.real = cosf(spi_4); pi_on_4.imag = sinf(spi_4); - - for(c=0; c<COHPSK_NC*COHPSK_ND; c++) { - - /* Set up lin reg model and interpolate phase. Works better than average for channels with - quickly changing phase, like HF. */ - - for(p=0; p<NPILOTSFRAME+2; p++) { - x[p] = sampling_points[p]; - pc = c % COHPSK_NC; - y[p] = fcmult(coh->pilot2[p][pc], ct_symb_buf[sampling_points[p]][c]); - } - - linreg(&m, &b, x, y, NPILOTSFRAME+2); - for(r=0; r<NSYMROW; r++) { - x1 = (float)(r+NPILOTSFRAME); - yfit = cadd(fcmult(x1,m),b); - coh->phi_[r][c] = atan2f(yfit.imag, yfit.real); - } - - /* amplitude estimation */ +void qpsk_symbols_to_bits(struct COHPSK *coh, float rx_bits[], + COMP ct_symb_buf[][COHPSK_NC * COHPSK_ND]) { + int p, r, c, i, pc, d, n; + float x[NPILOTSFRAME + 2], x1; + COMP y[NPILOTSFRAME + 2], yfit; + COMP rx_symb_linear[NSYMROW * COHPSK_NC * COHPSK_ND]; + COMP m, b; + COMP __attribute__((unused)) corr, rot, pi_on_4, phi_rect, div_symb; + float mag, __attribute__((unused)) phi_, __attribute__((unused)) amp_; + float sum_x, sum_xx, noise_var; + float spi_4 = M_PI / 4.0f; + COMP s; + + pi_on_4.real = cosf(spi_4); + pi_on_4.imag = sinf(spi_4); + + for (c = 0; c < COHPSK_NC * COHPSK_ND; c++) { + /* Set up lin reg model and interpolate phase. Works better than average + for channels with quickly changing phase, like HF. */ + + for (p = 0; p < NPILOTSFRAME + 2; p++) { + x[p] = sampling_points[p]; + pc = c % COHPSK_NC; + y[p] = fcmult(coh->pilot2[p][pc], ct_symb_buf[sampling_points[p]][c]); + } - mag = 0.0f; - for(p=0; p<NPILOTSFRAME+2; p++) { - mag += cabsolute(ct_symb_buf[sampling_points[p]][c]); - } - amp_ = mag/(NPILOTSFRAME+2); - for(r=0; r<NSYMROW; r++) { - coh->amp_[r][c] = amp_; - } + linreg(&m, &b, x, y, NPILOTSFRAME + 2); + for (r = 0; r < NSYMROW; r++) { + x1 = (float)(r + NPILOTSFRAME); + yfit = cadd(fcmult(x1, m), b); + coh->phi_[r][c] = atan2f(yfit.imag, yfit.real); } - /* now correct phase of data symbols */ + /* amplitude estimation */ - for(c=0; c<COHPSK_NC*COHPSK_ND; c++) { - for (r=0; r<NSYMROW; r++) { - phi_rect.real = cosf(coh->phi_[r][c]); phi_rect.imag = -sinf(coh->phi_[r][c]); - coh->rx_symb[r][c] = cmult(ct_symb_buf[NPILOTSFRAME + r][c], phi_rect); - i = c*NSYMROW + r; - rx_symb_linear[i] = coh->rx_symb[r][c]; - } + mag = 0.0f; + for (p = 0; p < NPILOTSFRAME + 2; p++) { + mag += cabsolute(ct_symb_buf[sampling_points[p]][c]); } - - /* and finally optional diversity combination, note output is soft decn a "1" is < 0 */ - - for(c=0; c<COHPSK_NC; c++) { - for(r=0; r<NSYMROW; r++) { - div_symb = coh->rx_symb[r][c]; - for (d=1; d<COHPSK_ND; d++) { - div_symb = cadd(div_symb, coh->rx_symb[r][c + COHPSK_NC*d]); - } - rot = cmult(div_symb, pi_on_4); - i = c*NSYMROW + r; - rx_bits[2*i+1] = rot.real; - rx_bits[2*i] = rot.imag; - - /* demodulate bits from upper and lower carriers separately for test purposes */ - - assert(COHPSK_ND == 2); - - i = c*NSYMROW + r; - rot = cmult(coh->rx_symb[r][c], pi_on_4); - coh->rx_bits_lower[2*i+1] = rot.real; - coh->rx_bits_lower[2*i] = rot.imag; - rot = cmult(coh->rx_symb[r][c + COHPSK_NC], pi_on_4); - coh->rx_bits_upper[2*i+1] = rot.real; - coh->rx_bits_upper[2*i] = rot.imag; - } + amp_ = mag / (NPILOTSFRAME + 2); + for (r = 0; r < NSYMROW; r++) { + coh->amp_[r][c] = amp_; } + } + /* now correct phase of data symbols */ - /* estimate RMS signal and noise */ + for (c = 0; c < COHPSK_NC * COHPSK_ND; c++) { + for (r = 0; r < NSYMROW; r++) { + phi_rect.real = cosf(coh->phi_[r][c]); + phi_rect.imag = -sinf(coh->phi_[r][c]); + coh->rx_symb[r][c] = cmult(ct_symb_buf[NPILOTSFRAME + r][c], phi_rect); + i = c * NSYMROW + r; + rx_symb_linear[i] = coh->rx_symb[r][c]; + } + } - mag = 0.0f; - for(i=0; i<NSYMROW*COHPSK_NC*COHPSK_ND; i++) - mag += cabsolute(rx_symb_linear[i]); - coh->sig_rms = mag/(NSYMROW*COHPSK_NC*COHPSK_ND); - - sum_x = 0.0f; - sum_xx = 0.0f; - n = 0; - for (i=0; i<NSYMROW*COHPSK_NC*COHPSK_ND; i++) { - s = rx_symb_linear[i]; - if (fabsf(s.real) > coh->sig_rms) { - sum_x += s.imag; - sum_xx += s.imag*s.imag; - n++; + /* and finally optional diversity combination, note output is soft decn a "1" + * is < 0 */ + + for (c = 0; c < COHPSK_NC; c++) { + for (r = 0; r < NSYMROW; r++) { + div_symb = coh->rx_symb[r][c]; + for (d = 1; d < COHPSK_ND; d++) { + div_symb = cadd(div_symb, coh->rx_symb[r][c + COHPSK_NC * d]); } + rot = cmult(div_symb, pi_on_4); + i = c * NSYMROW + r; + rx_bits[2 * i + 1] = rot.real; + rx_bits[2 * i] = rot.imag; + + /* demodulate bits from upper and lower carriers separately for test + * purposes */ + + assert(COHPSK_ND == 2); + + i = c * NSYMROW + r; + rot = cmult(coh->rx_symb[r][c], pi_on_4); + coh->rx_bits_lower[2 * i + 1] = rot.real; + coh->rx_bits_lower[2 * i] = rot.imag; + rot = cmult(coh->rx_symb[r][c + COHPSK_NC], pi_on_4); + coh->rx_bits_upper[2 * i + 1] = rot.real; + coh->rx_bits_upper[2 * i] = rot.imag; } - - noise_var = 0.0f; - if (n > 1) { - noise_var = (n*sum_xx - sum_x*sum_x)/(n*(n-1)); + } + + /* estimate RMS signal and noise */ + + mag = 0.0f; + for (i = 0; i < NSYMROW * COHPSK_NC * COHPSK_ND; i++) + mag += cabsolute(rx_symb_linear[i]); + coh->sig_rms = mag / (NSYMROW * COHPSK_NC * COHPSK_ND); + + sum_x = 0.0f; + sum_xx = 0.0f; + n = 0; + for (i = 0; i < NSYMROW * COHPSK_NC * COHPSK_ND; i++) { + s = rx_symb_linear[i]; + if (fabsf(s.real) > coh->sig_rms) { + sum_x += s.imag; + sum_xx += s.imag * s.imag; + n++; } - coh->noise_rms = sqrtf(noise_var); + } + noise_var = 0.0f; + if (n > 1) { + noise_var = (n * sum_xx - sum_x * sum_x) / (n * (n - 1)); + } + coh->noise_rms = sqrtf(noise_var); } - /*---------------------------------------------------------------------------*\ FUNCTION....: tx_filter_and_upconvert_coh() @@ -438,134 +434,132 @@ void qpsk_symbols_to_bits(struct COHPSK *coh, float rx_bits[], COMP ct_symb_buf[ void tx_filter_and_upconvert_coh(COMP tx_fdm[], int Nc, const COMP tx_symbols[], COMP tx_filter_memory[][COHPSK_NSYM], - COMP phase_tx[], COMP freq[], - COMP *fbb_phase, COMP fbb_rect) -{ - int c; - int i,j,k; - float acc; - COMP gain; - COMP tx_baseband; - COMP two = {2.0, 0.0}; - float mag; - - gain.real = sqrtf(2.0)/2.0; - gain.imag = 0.0; - - for(i=0; i<COHPSK_M; i++) { - tx_fdm[i].real = 0.0; - tx_fdm[i].imag = 0.0; + COMP phase_tx[], COMP freq[], COMP *fbb_phase, + COMP fbb_rect) { + int c; + int i, j, k; + float acc; + COMP gain; + COMP tx_baseband; + COMP two = {2.0, 0.0}; + float mag; + + gain.real = sqrtf(2.0) / 2.0; + gain.imag = 0.0; + + for (i = 0; i < COHPSK_M; i++) { + tx_fdm[i].real = 0.0; + tx_fdm[i].imag = 0.0; + } + + for (c = 0; c < Nc; c++) + tx_filter_memory[c][COHPSK_NSYM - 1] = cmult(tx_symbols[c], gain); + + /* + tx filter each symbol, generate M filtered output samples for + each symbol, which we then freq shift and sum with other + carriers. Efficient polyphase filter techniques used as + tx_filter_memory is sparse + */ + + for (c = 0; c < Nc; c++) { + for (i = 0; i < COHPSK_M; i++) { + /* filter real sample of symbol for carrier c */ + + acc = 0.0; + for (j = 0, k = COHPSK_M - i - 1; j < COHPSK_NSYM; j++, k += COHPSK_M) + acc += COHPSK_M * tx_filter_memory[c][j].real * gt_alpha5_root_coh[k]; + tx_baseband.real = acc; + + /* filter imag sample of symbol for carrier c */ + + acc = 0.0; + for (j = 0, k = COHPSK_M - i - 1; j < COHPSK_NSYM; j++, k += COHPSK_M) + acc += COHPSK_M * tx_filter_memory[c][j].imag * gt_alpha5_root_coh[k]; + tx_baseband.imag = acc; + // printf("%d %d %f %f\n", c, i, tx_baseband.real, tx_baseband.imag); + + /* freq shift and sum */ + + phase_tx[c] = cmult(phase_tx[c], freq[c]); + tx_fdm[i] = cadd(tx_fdm[i], cmult(tx_baseband, phase_tx[c])); + // printf("%d %d %f %f\n", c, i, phase_tx[c].real, phase_tx[c].imag); } + // exit(0); + } - for(c=0; c<Nc; c++) - tx_filter_memory[c][COHPSK_NSYM-1] = cmult(tx_symbols[c], gain); - - /* - tx filter each symbol, generate M filtered output samples for - each symbol, which we then freq shift and sum with other - carriers. Efficient polyphase filter techniques used as - tx_filter_memory is sparse - */ - - for(c=0; c<Nc; c++) { - for(i=0; i<COHPSK_M; i++) { - - /* filter real sample of symbol for carrier c */ - - acc = 0.0; - for(j=0,k=COHPSK_M-i-1; j<COHPSK_NSYM; j++,k+=COHPSK_M) - acc += COHPSK_M * tx_filter_memory[c][j].real * gt_alpha5_root_coh[k]; - tx_baseband.real = acc; - - /* filter imag sample of symbol for carrier c */ + /* shift whole thing up to carrier freq */ - acc = 0.0; - for(j=0,k=COHPSK_M-i-1; j<COHPSK_NSYM; j++,k+=COHPSK_M) - acc += COHPSK_M * tx_filter_memory[c][j].imag * gt_alpha5_root_coh[k]; - tx_baseband.imag = acc; - //printf("%d %d %f %f\n", c, i, tx_baseband.real, tx_baseband.imag); + for (i = 0; i < COHPSK_M; i++) { + *fbb_phase = cmult(*fbb_phase, fbb_rect); + tx_fdm[i] = cmult(tx_fdm[i], *fbb_phase); + } - /* freq shift and sum */ + /* + Scale such that total Carrier power C of real(tx_fdm) = Nc. This + excludes the power of the pilot tone. + We return the complex (single sided) signal to make frequency + shifting for the purpose of testing easier + */ - phase_tx[c] = cmult(phase_tx[c], freq[c]); - tx_fdm[i] = cadd(tx_fdm[i], cmult(tx_baseband, phase_tx[c])); - //printf("%d %d %f %f\n", c, i, phase_tx[c].real, phase_tx[c].imag); - } - //exit(0); - } - - /* shift whole thing up to carrier freq */ - - for (i=0; i<COHPSK_M; i++) { - *fbb_phase = cmult(*fbb_phase, fbb_rect); - tx_fdm[i] = cmult(tx_fdm[i], *fbb_phase); - } - - /* - Scale such that total Carrier power C of real(tx_fdm) = Nc. This - excludes the power of the pilot tone. - We return the complex (single sided) signal to make frequency - shifting for the purpose of testing easier - */ + for (i = 0; i < COHPSK_M; i++) tx_fdm[i] = cmult(two, tx_fdm[i]); - for (i=0; i<COHPSK_M; i++) - tx_fdm[i] = cmult(two, tx_fdm[i]); + /* normalise digital oscillators as the magnitude can drift over time */ - /* normalise digital oscillators as the magnitude can drift over time */ + for (c = 0; c < Nc; c++) { + mag = cabsolute(phase_tx[c]); + phase_tx[c].real /= mag; + phase_tx[c].imag /= mag; + } - for (c=0; c<Nc; c++) { - mag = cabsolute(phase_tx[c]); - phase_tx[c].real /= mag; - phase_tx[c].imag /= mag; - } + mag = cabsolute(*fbb_phase); + fbb_phase->real /= mag; + fbb_phase->imag /= mag; - mag = cabsolute(*fbb_phase); - fbb_phase->real /= mag; - fbb_phase->imag /= mag; + /* shift memory, inserting zeros at end */ - /* shift memory, inserting zeros at end */ + for (i = 0; i < COHPSK_NSYM - 1; i++) + for (c = 0; c < Nc; c++) + tx_filter_memory[c][i] = tx_filter_memory[c][i + 1]; - for(i=0; i<COHPSK_NSYM-1; i++) - for(c=0; c<Nc; c++) - tx_filter_memory[c][i] = tx_filter_memory[c][i+1]; - - for(c=0; c<Nc; c++) { - tx_filter_memory[c][COHPSK_NSYM-1].real = 0.0; - tx_filter_memory[c][COHPSK_NSYM-1].imag = 0.0; - } + for (c = 0; c < Nc; c++) { + tx_filter_memory[c][COHPSK_NSYM - 1].real = 0.0; + tx_filter_memory[c][COHPSK_NSYM - 1].imag = 0.0; + } } - - -void corr_with_pilots(float *corr_out, float *mag_out, struct COHPSK *coh, int t, float f_fine) -{ - COMP acorr, f_fine_rect[NPILOTSFRAME+2], f_corr; - float mag, corr, result; - float tau = 2.0f * M_PI; - int c, p, pc; - - for (p=0; p<NPILOTSFRAME+2; p++) { - result = f_fine * tau * (sampling_points[p]+1.0) / COHPSK_RS; - f_fine_rect[p].real = cosf(result); - f_fine_rect[p].imag = sinf(result); +void corr_with_pilots(float *corr_out, float *mag_out, struct COHPSK *coh, + int t, float f_fine) { + COMP acorr, f_fine_rect[NPILOTSFRAME + 2], f_corr; + float mag, corr, result; + float tau = 2.0f * M_PI; + int c, p, pc; + + for (p = 0; p < NPILOTSFRAME + 2; p++) { + result = f_fine * tau * (sampling_points[p] + 1.0) / COHPSK_RS; + f_fine_rect[p].real = cosf(result); + f_fine_rect[p].imag = sinf(result); + } + + corr = 0.0; + mag = 1E-12; + for (c = 0; c < COHPSK_NC * COHPSK_ND; c++) { + acorr.real = 0.0f; + acorr.imag = 0.0f; + pc = c % COHPSK_NC; + for (p = 0; p < NPILOTSFRAME + 2; p++) { + f_corr = + cmult(f_fine_rect[p], coh->ct_symb_buf[t + sampling_points[p]][c]); + acorr = cadd(acorr, fcmult(coh->pilot2[p][pc], f_corr)); + mag += cabsolute(f_corr); } + corr += cabsolute(acorr); + } - corr = 0.0; mag = 1E-12; - for (c=0; c<COHPSK_NC*COHPSK_ND; c++) { - acorr.real = 0.0f; acorr.imag = 0.0f; pc = c % COHPSK_NC; - for (p=0; p<NPILOTSFRAME+2; p++) { - f_corr = cmult(f_fine_rect[p], coh->ct_symb_buf[t+sampling_points[p]][c]); - acorr = cadd(acorr, fcmult(coh->pilot2[p][pc], f_corr)); - mag += cabsolute(f_corr); - } - corr += cabsolute(acorr); - } - - *corr_out = corr; - *mag_out = mag; + *corr_out = corr; + *mag_out = mag; } - /*---------------------------------------------------------------------------*\ FUNCTION....: frame_sync_fine_freq_est() @@ -578,105 +572,101 @@ void corr_with_pilots(float *corr_out, float *mag_out, struct COHPSK *coh, int t \*---------------------------------------------------------------------------*/ -void frame_sync_fine_freq_est(struct COHPSK *coh, COMP ch_symb[][COHPSK_NC*COHPSK_ND], int sync, int *next_sync) -{ - int t; - float f_fine, mag, max_corr, max_mag, corr, result; - float tau = 2.0f * M_PI; - - update_ct_symb_buf(coh->ct_symb_buf, ch_symb); - - /* sample pilots at start of this frame and start of next frame */ - - if (sync == 0) { - - /* sample correlation over 2D grid of time and fine freq points */ - - max_corr = 0.0; max_mag = 1E-12; - for (f_fine=-20; f_fine<=20; f_fine+=0.25) { - for (t=0; t<NSYMROWPILOT; t++) { - corr_with_pilots(&corr, &mag, coh, t, f_fine); - //printf(" f: %f t: %d corr: %f mag: %f\n", f_fine, t, corr, mag); - if (corr >= max_corr) { - max_corr = corr; - max_mag = mag; - coh->ct = t; - coh->f_fine_est = f_fine; - } - } +void frame_sync_fine_freq_est(struct COHPSK *coh, + COMP ch_symb[][COHPSK_NC * COHPSK_ND], int sync, + int *next_sync) { + int t; + float f_fine, mag, max_corr, max_mag, corr, result; + float tau = 2.0f * M_PI; + + update_ct_symb_buf(coh->ct_symb_buf, ch_symb); + + /* sample pilots at start of this frame and start of next frame */ + + if (sync == 0) { + /* sample correlation over 2D grid of time and fine freq points */ + + max_corr = 0.0; + max_mag = 1E-12; + for (f_fine = -20; f_fine <= 20; f_fine += 0.25) { + for (t = 0; t < NSYMROWPILOT; t++) { + corr_with_pilots(&corr, &mag, coh, t, f_fine); + // printf(" f: %f t: %d corr: %f mag: %f\n", f_fine, t, corr, mag); + if (corr >= max_corr) { + max_corr = corr; + max_mag = mag; + coh->ct = t; + coh->f_fine_est = f_fine; } + } + } - - result = coh->f_fine_est * tau / COHPSK_RS; - - coh->ff_rect.real = cosf(result); - coh->ff_rect.imag = -sinf(result); - if (coh->verbose) - fprintf(stderr, " [%d] fine freq f: %6.2f max_ratio: %f ct: %d\n", coh->frame, (double)coh->f_fine_est, (double)max_corr/(double)max_mag, coh->ct); - - if (max_corr/max_mag > 0.9) { - if (coh->verbose) - fprintf(stderr, " [%d] encouraging sync word!\n", coh->frame); - coh->sync_timer = 0; - *next_sync = 1; - } - else { - *next_sync = 0; - } - coh->ratio = max_corr/max_mag; + result = coh->f_fine_est * tau / COHPSK_RS; + + coh->ff_rect.real = cosf(result); + coh->ff_rect.imag = -sinf(result); + if (coh->verbose) + fprintf(stderr, " [%d] fine freq f: %6.2f max_ratio: %f ct: %d\n", + coh->frame, (double)coh->f_fine_est, + (double)max_corr / (double)max_mag, coh->ct); + + if (max_corr / max_mag > 0.9) { + if (coh->verbose) + fprintf(stderr, " [%d] encouraging sync word!\n", coh->frame); + coh->sync_timer = 0; + *next_sync = 1; + } else { + *next_sync = 0; } + coh->ratio = max_corr / max_mag; + } } +void update_ct_symb_buf(COMP ct_symb_buf[][COHPSK_NC * COHPSK_ND], + COMP ch_symb[][COHPSK_NC * COHPSK_ND]) { + int r, c, i; -void update_ct_symb_buf(COMP ct_symb_buf[][COHPSK_NC*COHPSK_ND], COMP ch_symb[][COHPSK_NC*COHPSK_ND]) -{ - int r, c, i; + /* update memory in symbol buffer */ - /* update memory in symbol buffer */ + for (r = 0; r < NCT_SYMB_BUF - NSYMROWPILOT; r++) { + for (c = 0; c < COHPSK_NC * COHPSK_ND; c++) + ct_symb_buf[r][c] = ct_symb_buf[r + NSYMROWPILOT][c]; + } - for(r=0; r<NCT_SYMB_BUF-NSYMROWPILOT; r++) { - for(c=0; c<COHPSK_NC*COHPSK_ND; c++) - ct_symb_buf[r][c] = ct_symb_buf[r+NSYMROWPILOT][c]; - } - - for(r=NCT_SYMB_BUF-NSYMROWPILOT, i=0; r<NCT_SYMB_BUF; r++, i++) { - for(c=0; c<COHPSK_NC*COHPSK_ND; c++) - ct_symb_buf[r][c] = ch_symb[i][c]; - } + for (r = NCT_SYMB_BUF - NSYMROWPILOT, i = 0; r < NCT_SYMB_BUF; r++, i++) { + for (c = 0; c < COHPSK_NC * COHPSK_ND; c++) + ct_symb_buf[r][c] = ch_symb[i][c]; + } } +int sync_state_machine(struct COHPSK *coh, int sync, int next_sync) { + float corr, mag; -int sync_state_machine(struct COHPSK *coh, int sync, int next_sync) -{ - float corr, mag; - - if (sync == 1) { - - /* check that sync is still good, fall out of sync on consecutive bad frames */ + if (sync == 1) { + /* check that sync is still good, fall out of sync on consecutive bad frames + */ - corr_with_pilots(&corr, &mag, coh, coh->ct, coh->f_fine_est); - coh->ratio = fabsf(corr)/mag; + corr_with_pilots(&corr, &mag, coh, coh->ct, coh->f_fine_est); + coh->ratio = fabsf(corr) / mag; - // printf("%f\n", cabsolute(corr)/mag); + // printf("%f\n", cabsolute(corr)/mag); - if (fabsf(corr)/mag < 0.8) - coh->sync_timer++; - else - coh->sync_timer = 0; + if (fabsf(corr) / mag < 0.8) + coh->sync_timer++; + else + coh->sync_timer = 0; - if (coh->sync_timer == 10) { - if (coh->verbose) - fprintf(stderr," [%d] lost sync ....\n", coh->frame); - next_sync = 0; - } + if (coh->sync_timer == 10) { + if (coh->verbose) fprintf(stderr, " [%d] lost sync ....\n", coh->frame); + next_sync = 0; } + } - sync = next_sync; + sync = next_sync; - return sync; + return sync; } - /*---------------------------------------------------------------------------*\ FUNCTION....: cohpsk_mod() @@ -696,24 +686,24 @@ int sync_state_machine(struct COHPSK *coh, int sync, int next_sync) \*---------------------------------------------------------------------------*/ -void cohpsk_mod(struct COHPSK *coh, COMP tx_fdm[], int tx_bits[], int nbits) -{ - struct FDMDV *fdmdv = coh->fdmdv; - COMP tx_symb[NSYMROWPILOT][COHPSK_NC*COHPSK_ND]; - COMP tx_onesym[COHPSK_NC*COHPSK_ND]; - int r,c; - - bits_to_qpsk_symbols(tx_symb, tx_bits, nbits); - - for(r=0; r<NSYMROWPILOT; r++) { - for(c=0; c<COHPSK_NC*COHPSK_ND; c++) - tx_onesym[c] = fcmult(coh->carrier_ampl[c], tx_symb[r][c]); - tx_filter_and_upconvert_coh(&tx_fdm[r*COHPSK_M], COHPSK_NC*COHPSK_ND , tx_onesym, fdmdv->tx_filter_memory, - fdmdv->phase_tx, fdmdv->freq, &fdmdv->fbb_phase_tx, fdmdv->fbb_rect); - } +void cohpsk_mod(struct COHPSK *coh, COMP tx_fdm[], int tx_bits[], int nbits) { + struct FDMDV *fdmdv = coh->fdmdv; + COMP tx_symb[NSYMROWPILOT][COHPSK_NC * COHPSK_ND]; + COMP tx_onesym[COHPSK_NC * COHPSK_ND]; + int r, c; + + bits_to_qpsk_symbols(tx_symb, tx_bits, nbits); + + for (r = 0; r < NSYMROWPILOT; r++) { + for (c = 0; c < COHPSK_NC * COHPSK_ND; c++) + tx_onesym[c] = fcmult(coh->carrier_ampl[c], tx_symb[r][c]); + tx_filter_and_upconvert_coh(&tx_fdm[r * COHPSK_M], COHPSK_NC * COHPSK_ND, + tx_onesym, fdmdv->tx_filter_memory, + fdmdv->phase_tx, fdmdv->freq, + &fdmdv->fbb_phase_tx, fdmdv->fbb_rect); + } } - /*---------------------------------------------------------------------------*\ FUNCTION....: cohpsk_clip() @@ -724,21 +714,20 @@ void cohpsk_mod(struct COHPSK *coh, COMP tx_fdm[], int tx_bits[], int nbits) \*---------------------------------------------------------------------------*/ -void cohpsk_clip(COMP tx_fdm[], float clip_thresh, int n) -{ - COMP sam; - float mag; - int i; - - for(i=0; i<n; i++) { - sam = tx_fdm[i]; - mag = cabsolute(sam); - if (mag > clip_thresh) { - sam = fcmult(clip_thresh/mag, sam); - } - tx_fdm[i] = sam; +void cohpsk_clip(COMP tx_fdm[], float clip_thresh, int n) { + COMP sam; + float mag; + int i; + + for (i = 0; i < n; i++) { + sam = tx_fdm[i]; + mag = cabsolute(sam); + if (mag > clip_thresh) { + sam = fcmult(clip_thresh / mag, sam); } - } + tx_fdm[i] = sam; + } +} /*---------------------------------------------------------------------------*\ @@ -752,42 +741,41 @@ void cohpsk_clip(COMP tx_fdm[], float clip_thresh, int n) \*---------------------------------------------------------------------------*/ -void fdm_downconvert_coh(COMP rx_baseband[][COHPSK_M+COHPSK_M/P], int Nc, COMP rx_fdm[], COMP phase_rx[], COMP freq[], int nin) -{ - int i,c; - float mag; +void fdm_downconvert_coh(COMP rx_baseband[][COHPSK_M + COHPSK_M / P], int Nc, + COMP rx_fdm[], COMP phase_rx[], COMP freq[], int nin) { + int i, c; + float mag; - /* maximum number of input samples to demod */ + /* maximum number of input samples to demod */ - assert(nin <= (COHPSK_M+COHPSK_M/P)); + assert(nin <= (COHPSK_M + COHPSK_M / P)); - /* downconvert */ + /* downconvert */ - for (c=0; c<Nc; c++) - for (i=0; i<nin; i++) { - phase_rx[c] = cmult(phase_rx[c], freq[c]); - rx_baseband[c][i] = cmult(rx_fdm[i], cconj(phase_rx[c])); - } + for (c = 0; c < Nc; c++) + for (i = 0; i < nin; i++) { + phase_rx[c] = cmult(phase_rx[c], freq[c]); + rx_baseband[c][i] = cmult(rx_fdm[i], cconj(phase_rx[c])); + } - /* normalise digital oscilators as the magnitude can drift over time */ + /* normalise digital oscilators as the magnitude can drift over time */ - for (c=0; c<Nc; c++) { - mag = cabsolute(phase_rx[c]); - phase_rx[c].real /= mag; - phase_rx[c].imag /= mag; - } + for (c = 0; c < Nc; c++) { + mag = cabsolute(phase_rx[c]); + phase_rx[c].real /= mag; + phase_rx[c].imag /= mag; + } } /* Determine if we can use vector ops below. */ -#if __GNUC__ > 4 || \ - (__GNUC__ == 4 && (__GNUC_MINOR__ > 6 || \ - (__GNUC_MINOR__ == 6 && \ - __GNUC_PATCHLEVEL__ > 0))) +#if __GNUC__ > 4 || \ + (__GNUC__ == 4 && \ + (__GNUC_MINOR__ > 6 || (__GNUC_MINOR__ == 6 && __GNUC_PATCHLEVEL__ > 0))) #define USE_VECTOR_OPS 1 #elif __clang_major__ > 3 || \ - (__clang_minor__ == 3 && (__clang_minor__ > 7 || \ - (__clang_minor__ == 7 && \ - __clang_patchlevel__ > 0))) + (__clang_minor__ == 3 && \ + (__clang_minor__ > 7 || \ + (__clang_minor__ == 7 && __clang_patchlevel__ > 0))) #define USE_VECTOR_OPS 1 #endif @@ -799,11 +787,11 @@ void fdm_downconvert_coh(COMP rx_baseband[][COHPSK_M+COHPSK_M/P], int Nc, COMP r typedef float32x4_t float4; #else /* Vector of 4 floating point numbers for use by the below function */ -typedef float float4 __attribute__ ((vector_size (16))); -#endif // __ARM_NEON +typedef float float4 __attribute__((vector_size(16))); +#endif // __ARM_NEON #endif /* USE_VECTOR_OPS */ - + /*---------------------------------------------------------------------------*\ FUNCTION....: rx_filter_coh() @@ -816,117 +804,122 @@ typedef float float4 __attribute__ ((vector_size (16))); \*---------------------------------------------------------------------------*/ -inline extern void rx_filter_coh(COMP rx_filt[COHPSK_NC*COHPSK_ND][P+1], int Nc, COMP rx_baseband[COHPSK_NC*COHPSK_ND][COHPSK_M+COHPSK_M/P], COMP rx_filter_memory[COHPSK_NC*COHPSK_ND][COHPSK_NFILTER], int nin) -{ - int c,i,j,k,l; - int n=COHPSK_M/P; - - /* rx filter each symbol, generate P filtered output samples for - each symbol. Note we keep filter memory at rate M, it's just - the filter output at rate P */ - - for(i=0, j=0; i<nin; i+=n,j++) - { - - /* latest input sample */ - - for(c=0; c<Nc; c++) - { - rx_filt[c][j].real = 0.0; - rx_filt[c][j].imag = 0.0; - - /* - This call is equivalent to the code below: - - for(k=COHPSK_NFILTER-n,l=i; k<COHPSK_NFILTER; k++,l++) - { - rx_filter_memory[c][k] = rx_baseband[c][l]; - } - */ - memcpy( - &rx_filter_memory[c][COHPSK_NFILTER-n], - &rx_baseband[c][i], - sizeof(COMP)*n); - - /* convolution (filtering) */ - +inline extern void rx_filter_coh( + COMP rx_filt[COHPSK_NC * COHPSK_ND][P + 1], int Nc, + COMP rx_baseband[COHPSK_NC * COHPSK_ND][COHPSK_M + COHPSK_M / P], + COMP rx_filter_memory[COHPSK_NC * COHPSK_ND][COHPSK_NFILTER], int nin) { + int c, i, j, k, l; + int n = COHPSK_M / P; + + /* rx filter each symbol, generate P filtered output samples for + each symbol. Note we keep filter memory at rate M, it's just + the filter output at rate P */ + + for (i = 0, j = 0; i < nin; i += n, j++) { + /* latest input sample */ + + for (c = 0; c < Nc; c++) { + rx_filt[c][j].real = 0.0; + rx_filt[c][j].imag = 0.0; + + /* + This call is equivalent to the code below: + + for(k=COHPSK_NFILTER-n,l=i; k<COHPSK_NFILTER; k++,l++) + { + rx_filter_memory[c][k] = rx_baseband[c][l]; + } + */ + memcpy(&rx_filter_memory[c][COHPSK_NFILTER - n], &rx_baseband[c][i], + sizeof(COMP) * n); + + /* convolution (filtering) */ + #if USE_VECTOR_OPS - /* assumes COHPSK_NFILTER is divisible by 2 */ + /* assumes COHPSK_NFILTER is divisible by 2 */ #ifdef __ARM_NEON - float4 resultVec = vdupq_n_f32(0); + float4 resultVec = vdupq_n_f32(0); #else - float4 resultVec = {0, 0, 0, 0}; -#endif // __ARM_NEON + float4 resultVec = {0, 0, 0, 0}; +#endif // __ARM_NEON - for(k=0, l=0; k<COHPSK_NFILTER; k += 2, l += 4) - { + for (k = 0, l = 0; k < COHPSK_NFILTER; k += 2, l += 4) { #ifdef __ARM_NEON - // Fetch gt_alpha5_root_coh and place it into a vector for later use. - // First half at index k, second half at index k + 1. - float4 alpha5Vec = vld1q_f32((const float32_t*)>_alpha5_root_coh_neon[l]); - - // Load two COMP elements (each containing two floats) into 4 element vector. - float4 filterMemVec = vld1q_f32((const float32_t *)&rx_filter_memory[c][k]); - - // Multiply each element of filterMemVec by alpha5Vec from above and add to the - // running total in resultVec. Odd indices are reals, even imag. - resultVec = vmlaq_f32(resultVec, alpha5Vec, filterMemVec); + // Fetch gt_alpha5_root_coh and place it into a vector for later use. + // First half at index k, second half at index k + 1. + float4 alpha5Vec = + vld1q_f32((const float32_t *)>_alpha5_root_coh_neon[l]); + + // Load two COMP elements (each containing two floats) into 4 element + // vector. + float4 filterMemVec = + vld1q_f32((const float32_t *)&rx_filter_memory[c][k]); + + // Multiply each element of filterMemVec by alpha5Vec from above and add + // to the running total in resultVec. Odd indices are reals, even imag. + resultVec = vmlaq_f32(resultVec, alpha5Vec, filterMemVec); #else - // Fetch gt_alpha5_root_coh and place it into a vector for later use. - // First half at index k, second half at index k + 1. - float4 alpha5Vec = { - gt_alpha5_root_coh_neon[l], gt_alpha5_root_coh_neon[l + 1], gt_alpha5_root_coh_neon[l + 2], gt_alpha5_root_coh_neon[l + 3], - }; - - // Load two COMP elements (each containing two floats) into 4 element vector. - float4 filterMemVec = { - rx_filter_memory[c][k].real, rx_filter_memory[c][k].imag, rx_filter_memory[c][k + 1].real, rx_filter_memory[c][k + 1].imag, - }; - - // Multiply each element of filterMemVec by alpha5Vec from above and add to the - // running total in resultVec. Odd indices are reals, even imag. - resultVec += alpha5Vec * filterMemVec; - -#endif // __ARM_NEON - } - - // Add total from resultVec to rx_filt. - rx_filt[c][j].real += resultVec[0] + resultVec[2]; - rx_filt[c][j].imag += resultVec[1] + resultVec[3]; + // Fetch gt_alpha5_root_coh and place it into a vector for later use. + // First half at index k, second half at index k + 1. + float4 alpha5Vec = { + gt_alpha5_root_coh_neon[l], + gt_alpha5_root_coh_neon[l + 1], + gt_alpha5_root_coh_neon[l + 2], + gt_alpha5_root_coh_neon[l + 3], + }; + + // Load two COMP elements (each containing two floats) into 4 element + // vector. + float4 filterMemVec = { + rx_filter_memory[c][k].real, + rx_filter_memory[c][k].imag, + rx_filter_memory[c][k + 1].real, + rx_filter_memory[c][k + 1].imag, + }; + + // Multiply each element of filterMemVec by alpha5Vec from above and add + // to the running total in resultVec. Odd indices are reals, even imag. + resultVec += alpha5Vec * filterMemVec; + +#endif // __ARM_NEON + } + + // Add total from resultVec to rx_filt. + rx_filt[c][j].real += resultVec[0] + resultVec[2]; + rx_filt[c][j].imag += resultVec[1] + resultVec[3]; #else - for(k=0; k<COHPSK_NFILTER; k++) - { - /* - Equivalent to this code: - - rx_filt[c][j] = cadd(rx_filt[c][j], fcmult(gt_alpha5_root_coh[k], rx_filter_memory[c][k])); - */ - rx_filt[c][j].real += gt_alpha5_root_coh[k] * rx_filter_memory[c][k].real; - rx_filt[c][j].imag += gt_alpha5_root_coh[k] * rx_filter_memory[c][k].imag; - } + for (k = 0; k < COHPSK_NFILTER; k++) { + /* + Equivalent to this code: + + rx_filt[c][j] = cadd(rx_filt[c][j], fcmult(gt_alpha5_root_coh[k], + rx_filter_memory[c][k])); + */ + rx_filt[c][j].real += + gt_alpha5_root_coh[k] * rx_filter_memory[c][k].real; + rx_filt[c][j].imag += + gt_alpha5_root_coh[k] * rx_filter_memory[c][k].imag; + } #endif /* USE_VECTOR_OPS */ - - /* - make room for next input sample. - - The below call is equivalent to this code: - - for(k=0,l=n; k<COHPSK_NFILTER-n; k++,l++) - { - rx_filter_memory[c][k] = rx_filter_memory[c][l]; - } - */ - memmove( - &rx_filter_memory[c][0], - &rx_filter_memory[c][n], - sizeof(COMP)*(COHPSK_NFILTER-n)); - } + + /* + make room for next input sample. + + The below call is equivalent to this code: + + for(k=0,l=n; k<COHPSK_NFILTER-n; k++,l++) + { + rx_filter_memory[c][k] = rx_filter_memory[c][l]; + } + */ + memmove(&rx_filter_memory[c][0], &rx_filter_memory[c][n], + sizeof(COMP) * (COHPSK_NFILTER - n)); } - - assert(j <= (P+1)); /* check for any over runs */ -} + } + assert(j <= (P + 1)); /* check for any over runs */ +} /*---------------------------------------------------------------------------*\ @@ -939,138 +932,149 @@ inline extern void rx_filter_coh(COMP rx_filt[COHPSK_NC*COHPSK_ND][P+1], int Nc, \*---------------------------------------------------------------------------*/ -void fdmdv_freq_shift_coh(COMP rx_fdm_fcorr[], COMP rx_fdm[], float foff, float Fs, - COMP *foff_phase_rect, int nin) -{ - COMP foff_rect; - float mag; - float tau = 2.0f * M_PI; - float result = tau * foff/Fs; - int i; - - foff_rect.real = cosf(result); - foff_rect.imag = sinf(result); - for(i=0; i<nin; i++) { - *foff_phase_rect = cmult(*foff_phase_rect, foff_rect); - rx_fdm_fcorr[i] = cmult(rx_fdm[i], *foff_phase_rect); - } - - /* normalise digital oscillator as the magnitude can drift over time */ - - mag = cabsolute(*foff_phase_rect); - foff_phase_rect->real /= mag; - foff_phase_rect->imag /= mag; +void fdmdv_freq_shift_coh(COMP rx_fdm_fcorr[], COMP rx_fdm[], float foff, + float Fs, COMP *foff_phase_rect, int nin) { + COMP foff_rect; + float mag; + float tau = 2.0f * M_PI; + float result = tau * foff / Fs; + int i; + + foff_rect.real = cosf(result); + foff_rect.imag = sinf(result); + for (i = 0; i < nin; i++) { + *foff_phase_rect = cmult(*foff_phase_rect, foff_rect); + rx_fdm_fcorr[i] = cmult(rx_fdm[i], *foff_phase_rect); + } + + /* normalise digital oscillator as the magnitude can drift over time */ + + mag = cabsolute(*foff_phase_rect); + foff_phase_rect->real /= mag; + foff_phase_rect->imag /= mag; } +void rate_Fs_rx_processing(struct COHPSK *coh, + COMP ch_symb[][COHPSK_NC * COHPSK_ND], + COMP ch_fdm_frame[], float *f_est, int nsymb, + int nin, int freq_track) { + struct FDMDV *fdmdv = coh->fdmdv; + int r, c, i, ch_fdm_frame_index; + COMP rx_fdm_frame_bb[COHPSK_M + COHPSK_M / P]; + COMP rx_baseband[COHPSK_NC * COHPSK_ND][COHPSK_M + COHPSK_M / P]; + COMP rx_filt[COHPSK_NC * COHPSK_ND][P + 1]; + float env[NT * P], rx_timing; + COMP rx_onesym[COHPSK_NC * COHPSK_ND]; + float beta, g; + COMP adiff, amod_strip, mod_strip; + + ch_fdm_frame_index = 0; + rx_timing = 0; + + for (r = 0; r < nsymb; r++) { + fdmdv_freq_shift_coh(rx_fdm_frame_bb, &ch_fdm_frame[ch_fdm_frame_index], + -(*f_est), COHPSK_FS, &fdmdv->fbb_phase_rx, nin); + ch_fdm_frame_index += nin; + fdm_downconvert_coh(rx_baseband, COHPSK_NC * COHPSK_ND, rx_fdm_frame_bb, + fdmdv->phase_rx, fdmdv->freq, nin); + rx_filter_coh(rx_filt, COHPSK_NC * COHPSK_ND, rx_baseband, + coh->rx_filter_memory, nin); + rx_timing = rx_est_timing(rx_onesym, fdmdv->Nc, rx_filt, + fdmdv->rx_filter_mem_timing, env, nin, COHPSK_M); + + for (c = 0; c < COHPSK_NC * COHPSK_ND; c++) { + ch_symb[r][c] = rx_onesym[c]; + } -void rate_Fs_rx_processing(struct COHPSK *coh, COMP ch_symb[][COHPSK_NC*COHPSK_ND], COMP ch_fdm_frame[], float *f_est, int nsymb, int nin, int freq_track) -{ - struct FDMDV *fdmdv = coh->fdmdv; - int r, c, i, ch_fdm_frame_index; - COMP rx_fdm_frame_bb[COHPSK_M+COHPSK_M/P]; - COMP rx_baseband[COHPSK_NC*COHPSK_ND][COHPSK_M+COHPSK_M/P]; - COMP rx_filt[COHPSK_NC*COHPSK_ND][P+1]; - float env[NT*P], rx_timing; - COMP rx_onesym[COHPSK_NC*COHPSK_ND]; - float beta, g; - COMP adiff, amod_strip, mod_strip; - - ch_fdm_frame_index = 0; - rx_timing = 0; - - for (r=0; r<nsymb; r++) { - fdmdv_freq_shift_coh(rx_fdm_frame_bb, &ch_fdm_frame[ch_fdm_frame_index], -(*f_est), COHPSK_FS, &fdmdv->fbb_phase_rx, nin); - ch_fdm_frame_index += nin; - fdm_downconvert_coh(rx_baseband, COHPSK_NC*COHPSK_ND, rx_fdm_frame_bb, fdmdv->phase_rx, fdmdv->freq, nin); - rx_filter_coh(rx_filt, COHPSK_NC*COHPSK_ND, rx_baseband, coh->rx_filter_memory, nin); - rx_timing = rx_est_timing(rx_onesym, fdmdv->Nc, rx_filt, fdmdv->rx_filter_mem_timing, env, nin, COHPSK_M); - - for(c=0; c<COHPSK_NC*COHPSK_ND; c++) { - ch_symb[r][c] = rx_onesym[c]; - } + /* freq tracking, see test_ftrack.m for unit test. Placed in + this function as it needs to work on a symbol by symbol + abasis rather than frame by frame. This means the control + loop operates at a sample rate of Rs = 50Hz for say 1 Hz/s + drift. */ + + if (freq_track) { + beta = 0.005; + g = 0.2; + + /* combine difference on phase from last symbol over Nc carriers */ + + mod_strip.real = 0.0; + mod_strip.imag = 0.0; + for (c = 0; c < fdmdv->Nc + 1; c++) { + // printf("rx_onesym[%d] %f %f prev_rx_symbols[%d] %f %f\n", c, + // rx_onesym[c].real, rx_onesym[c].imag, + // fdmdv->prev_rx_symbols[c].real, + // fdmdv->prev_rx_symbols[c].imag); + adiff = cmult(rx_onesym[c], cconj(fdmdv->prev_rx_symbols[c])); + fdmdv->prev_rx_symbols[c] = rx_onesym[c]; + + /* 4th power strips QPSK modulation, by multiplying phase by 4 + Using the abs value of the real coord was found to help + non-linear issues when noise power was large. */ + + amod_strip = cmult(adiff, adiff); + amod_strip = cmult(amod_strip, amod_strip); + amod_strip.real = fabsf(amod_strip.real); + mod_strip = cadd(mod_strip, amod_strip); + } + // printf("modstrip: %f %f\n", mod_strip.real, mod_strip.imag); - /* freq tracking, see test_ftrack.m for unit test. Placed in - this function as it needs to work on a symbol by symbol - abasis rather than frame by frame. This means the control - loop operates at a sample rate of Rs = 50Hz for say 1 Hz/s - drift. */ - - if (freq_track) { - beta = 0.005; - g = 0.2; - - /* combine difference on phase from last symbol over Nc carriers */ - - mod_strip.real = 0.0; mod_strip.imag = 0.0; - for(c=0; c<fdmdv->Nc+1; c++) { - //printf("rx_onesym[%d] %f %f prev_rx_symbols[%d] %f %f\n", c, rx_onesym[c].real, rx_onesym[c].imag, - // fdmdv->prev_rx_symbols[c].real, fdmdv->prev_rx_symbols[c].imag); - adiff = cmult(rx_onesym[c], cconj(fdmdv->prev_rx_symbols[c])); - fdmdv->prev_rx_symbols[c] = rx_onesym[c]; - - /* 4th power strips QPSK modulation, by multiplying phase by 4 - Using the abs value of the real coord was found to help - non-linear issues when noise power was large. */ - - amod_strip = cmult(adiff, adiff); - amod_strip = cmult(amod_strip, amod_strip); - amod_strip.real = fabsf(amod_strip.real); - mod_strip = cadd(mod_strip, amod_strip); - } - //printf("modstrip: %f %f\n", mod_strip.real, mod_strip.imag); - - /* loop filter made up of 1st order IIR plus integrator. Integerator - was found to be reqd */ - - fdmdv->foff_filt = (1.0f-beta)*fdmdv->foff_filt + beta*atan2f(mod_strip.imag, mod_strip.real); - //printf("foff_filt: %f angle: %f\n", fdmdv->foff_filt, atan2f(mod_strip.imag, mod_strip.real)); - *f_est += g*fdmdv->foff_filt; - } + /* loop filter made up of 1st order IIR plus integrator. Integerator + was found to be reqd */ - /* Optional logging used for testing against Octave version */ - - if (coh->rx_baseband_log) { - assert(nin <= (COHPSK_M+COHPSK_M/P)); - for(c=0; c<COHPSK_NC*COHPSK_ND; c++) { - for(i=0; i<nin; i++) { - coh->rx_baseband_log[c*coh->rx_baseband_log_col_sz + coh->rx_baseband_log_col_index + i] = rx_baseband[c][i]; - } - } - coh->rx_baseband_log_col_index += nin; - assert(coh->rx_baseband_log_col_index <= coh->rx_baseband_log_col_sz); - } + fdmdv->foff_filt = (1.0f - beta) * fdmdv->foff_filt + + beta * atan2f(mod_strip.imag, mod_strip.real); + // printf("foff_filt: %f angle: %f\n", fdmdv->foff_filt, + // atan2f(mod_strip.imag, mod_strip.real)); + *f_est += g * fdmdv->foff_filt; + } - if (coh->rx_filt_log) { - for(c=0; c<COHPSK_NC*COHPSK_ND; c++) { - for(i=0; i<nin/(COHPSK_M/P); i++) { - coh->rx_filt_log[c*coh->rx_filt_log_col_sz + coh->rx_filt_log_col_index + i] = rx_filt[c][i]; - } - } - coh->rx_filt_log_col_index += nin/(COHPSK_M/P); - } + /* Optional logging used for testing against Octave version */ - if (coh->ch_symb_log) { - for(c=0; c<COHPSK_NC*COHPSK_ND; c++) { - coh->ch_symb_log[coh->ch_symb_log_r*COHPSK_NC*COHPSK_ND + c] = ch_symb[r][c]; - } - coh->ch_symb_log_r++; + if (coh->rx_baseband_log) { + assert(nin <= (COHPSK_M + COHPSK_M / P)); + for (c = 0; c < COHPSK_NC * COHPSK_ND; c++) { + for (i = 0; i < nin; i++) { + coh->rx_baseband_log[c * coh->rx_baseband_log_col_sz + + coh->rx_baseband_log_col_index + i] = + rx_baseband[c][i]; } + } + coh->rx_baseband_log_col_index += nin; + assert(coh->rx_baseband_log_col_index <= coh->rx_baseband_log_col_sz); + } - if (coh->rx_timing_log) { - coh->rx_timing_log[coh->rx_timing_log_index] = rx_timing; - coh->rx_timing_log_index++; - //printf("rx_timing_log_index: %d\n", coh->rx_timing_log_index); + if (coh->rx_filt_log) { + for (c = 0; c < COHPSK_NC * COHPSK_ND; c++) { + for (i = 0; i < nin / (COHPSK_M / P); i++) { + coh->rx_filt_log[c * coh->rx_filt_log_col_sz + + coh->rx_filt_log_col_index + i] = rx_filt[c][i]; } + } + coh->rx_filt_log_col_index += nin / (COHPSK_M / P); + } - /* we only allow a timing shift on one symbol per frame */ + if (coh->ch_symb_log) { + for (c = 0; c < COHPSK_NC * COHPSK_ND; c++) { + coh->ch_symb_log[coh->ch_symb_log_r * COHPSK_NC * COHPSK_ND + c] = + ch_symb[r][c]; + } + coh->ch_symb_log_r++; + } - if (nin != COHPSK_M) - nin = COHPSK_M; + if (coh->rx_timing_log) { + coh->rx_timing_log[coh->rx_timing_log_index] = rx_timing; + coh->rx_timing_log_index++; + // printf("rx_timing_log_index: %d\n", coh->rx_timing_log_index); } - coh->rx_timing = rx_timing; -} + /* we only allow a timing shift on one symbol per frame */ + + if (nin != COHPSK_M) nin = COHPSK_M; + } + coh->rx_timing = rx_timing; +} /*---------------------------------------------------------------------------*\ @@ -1087,141 +1091,151 @@ void rate_Fs_rx_processing(struct COHPSK *coh, COMP ch_symb[][COHPSK_NC*COHPSK_N \*---------------------------------------------------------------------------*/ -void cohpsk_demod(struct COHPSK *coh, float rx_bits[], int *sync_good, COMP rx_fdm[], int *nin_frame) -{ - COMP ch_symb[NSW*NSYMROWPILOT][COHPSK_NC*COHPSK_ND]; - int i, j, sync, anext_sync, next_sync, nin, r, c; - float max_ratio, f_est; - - assert(*nin_frame <= COHPSK_MAX_SAMPLES_PER_FRAME); - - next_sync = sync = coh->sync; +void cohpsk_demod(struct COHPSK *coh, float rx_bits[], int *sync_good, + COMP rx_fdm[], int *nin_frame) { + COMP ch_symb[NSW * NSYMROWPILOT][COHPSK_NC * COHPSK_ND]; + int i, j, sync, anext_sync, next_sync, nin, r, c; + float max_ratio, f_est; - for (i=0; i<NSW*NSYMROWPILOT*COHPSK_M-*nin_frame; i++) - coh->ch_fdm_frame_buf[i] = coh->ch_fdm_frame_buf[i+*nin_frame]; - //printf("nin_frame: %d i: %d i+nin_frame: %d\n", *nin_frame, i, i+*nin_frame); - for (j=0; i<NSW*NSYMROWPILOT*COHPSK_M; i++,j++) - coh->ch_fdm_frame_buf[i] = rx_fdm[j]; - //printf("i: %d j: %d rx_fdm[0]: %f %f\n", i,j, rx_fdm[0].real, rx_fdm[0].imag); + assert(*nin_frame <= COHPSK_MAX_SAMPLES_PER_FRAME); - /* if out of sync do Initial Freq offset estimation using NSW frames to flush out filter memories */ + next_sync = sync = coh->sync; - if (sync == 0) { + for (i = 0; i < NSW * NSYMROWPILOT * COHPSK_M - *nin_frame; i++) + coh->ch_fdm_frame_buf[i] = coh->ch_fdm_frame_buf[i + *nin_frame]; + // printf("nin_frame: %d i: %d i+nin_frame: %d\n", *nin_frame, i, + // i+*nin_frame); + for (j = 0; i < NSW * NSYMROWPILOT * COHPSK_M; i++, j++) + coh->ch_fdm_frame_buf[i] = rx_fdm[j]; + // printf("i: %d j: %d rx_fdm[0]: %f %f\n", i,j, rx_fdm[0].real, + // rx_fdm[0].imag); - /* we can test +/- 20Hz, so we break this up into 3 tests to cover +/- 60Hz */ + /* if out of sync do Initial Freq offset estimation using NSW frames to flush + * out filter memories */ - max_ratio = 0.0; - f_est = 0.0; - for (coh->f_est = FDMDV_FCENTRE-40.0; coh->f_est <= FDMDV_FCENTRE+40.0; coh->f_est += 40.0) { - - if (coh->verbose) - fprintf(stderr, " [%d] acohpsk.f_est: %f +/- 20\n", coh->frame, (double)coh->f_est); - - /* we are out of sync so reset f_est and process two frames to clean out memories */ - - rate_Fs_rx_processing(coh, ch_symb, coh->ch_fdm_frame_buf, &coh->f_est, NSW*NSYMROWPILOT, COHPSK_M, 0); - for (i=0; i<NSW-1; i++) { - update_ct_symb_buf(coh->ct_symb_buf, &ch_symb[i*NSYMROWPILOT]); - } - frame_sync_fine_freq_est(coh, &ch_symb[(NSW-1)*NSYMROWPILOT], sync, &anext_sync); - - if (anext_sync == 1) { - //printf(" [%d] acohpsk.ratio: %f\n", f, coh->ratio); - if (coh->ratio > max_ratio) { - max_ratio = coh->ratio; - f_est = coh->f_est - coh->f_fine_est; - next_sync = anext_sync; - } - } - } - - if (next_sync == 1) { - - /* we've found a sync candidate! - re-process last NSW frames with adjusted f_est then check again */ - - coh->f_est = f_est; - - if (coh->verbose) - fprintf(stderr, " [%d] trying sync and f_est: %f\n", coh->frame, (double)coh->f_est); - - rate_Fs_rx_processing(coh, ch_symb, coh->ch_fdm_frame_buf, &coh->f_est, NSW*NSYMROWPILOT, COHPSK_M, 0); - for (i=0; i<NSW-1; i++) { - update_ct_symb_buf(coh->ct_symb_buf, &ch_symb[i*NSYMROWPILOT]); - } - /* - for(i=0; i<NSW*NSYMROWPILOT; i++) { - printf("%f %f\n", ch_symb[i][0].real, ch_symb[i][0].imag); - } - */ - /* - for(i=0; i<NCT_SYMB_BUF; i++) { - printf("%f %f\n", coh->ct_symb_buf[i][0].real, coh->ct_symb_buf[i][0].imag); - } - */ - frame_sync_fine_freq_est(coh, &ch_symb[(NSW-1)*NSYMROWPILOT], sync, &next_sync); - - if (fabsf(coh->f_fine_est) > 2.0) { - if (coh->verbose) - fprintf(stderr, " [%d] Hmm %f is a bit big :(\n", coh->frame, (double)coh->f_fine_est); - next_sync = 0; - } - } - - if (next_sync == 1) { - /* OK we are in sync! - demodulate first frame (demod completed below) */ + if (sync == 0) { + /* we can test +/- 20Hz, so we break this up into 3 tests to cover +/- 60Hz + */ - if (coh->verbose) - fprintf(stderr, " [%d] in sync! f_est: %f ratio: %f \n", coh->frame, (double)coh->f_est, (double)coh->ratio); - for(r=0; r<NSYMROWPILOT+2; r++) - for(c=0; c<COHPSK_NC*COHPSK_ND; c++) - coh->ct_symb_ff_buf[r][c] = coh->ct_symb_buf[coh->ct+r][c]; + max_ratio = 0.0; + f_est = 0.0; + for (coh->f_est = FDMDV_FCENTRE - 40.0; coh->f_est <= FDMDV_FCENTRE + 40.0; + coh->f_est += 40.0) { + if (coh->verbose) + fprintf(stderr, " [%d] acohpsk.f_est: %f +/- 20\n", coh->frame, + (double)coh->f_est); + + /* we are out of sync so reset f_est and process two frames to clean out + * memories */ + + rate_Fs_rx_processing(coh, ch_symb, coh->ch_fdm_frame_buf, &coh->f_est, + NSW * NSYMROWPILOT, COHPSK_M, 0); + for (i = 0; i < NSW - 1; i++) { + update_ct_symb_buf(coh->ct_symb_buf, &ch_symb[i * NSYMROWPILOT]); + } + frame_sync_fine_freq_est(coh, &ch_symb[(NSW - 1) * NSYMROWPILOT], sync, + &anext_sync); + + if (anext_sync == 1) { + // printf(" [%d] acohpsk.ratio: %f\n", f, coh->ratio); + if (coh->ratio > max_ratio) { + max_ratio = coh->ratio; + f_est = coh->f_est - coh->f_fine_est; + next_sync = anext_sync; } + } } - /* If in sync just do sample rate processing on latest frame */ + if (next_sync == 1) { + /* we've found a sync candidate! + re-process last NSW frames with adjusted f_est then check again */ - if (sync == 1) { - rate_Fs_rx_processing(coh, ch_symb, rx_fdm, &coh->f_est, NSYMROWPILOT, coh->nin, 1); - frame_sync_fine_freq_est(coh, ch_symb, sync, &next_sync); + coh->f_est = f_est; - for(r=0; r<2; r++) - for(c=0; c<COHPSK_NC*COHPSK_ND; c++) - coh->ct_symb_ff_buf[r][c] = coh->ct_symb_ff_buf[r+NSYMROWPILOT][c]; - for(; r<NSYMROWPILOT+2; r++) - for(c=0; c<COHPSK_NC*COHPSK_ND; c++) - coh->ct_symb_ff_buf[r][c] = coh->ct_symb_buf[coh->ct+r][c]; - } + if (coh->verbose) + fprintf(stderr, " [%d] trying sync and f_est: %f\n", coh->frame, + (double)coh->f_est); - /* if we are in sync complete demodulation with symbol rate processing */ + rate_Fs_rx_processing(coh, ch_symb, coh->ch_fdm_frame_buf, &coh->f_est, + NSW * NSYMROWPILOT, COHPSK_M, 0); + for (i = 0; i < NSW - 1; i++) { + update_ct_symb_buf(coh->ct_symb_buf, &ch_symb[i * NSYMROWPILOT]); + } + /* + for(i=0; i<NSW*NSYMROWPILOT; i++) { + printf("%f %f\n", ch_symb[i][0].real, ch_symb[i][0].imag); + } + */ + /* + for(i=0; i<NCT_SYMB_BUF; i++) { + printf("%f %f\n", coh->ct_symb_buf[i][0].real, + coh->ct_symb_buf[i][0].imag); + } + */ + frame_sync_fine_freq_est(coh, &ch_symb[(NSW - 1) * NSYMROWPILOT], sync, + &next_sync); - *sync_good = 0; - if ((next_sync == 1) || (sync == 1)) { - qpsk_symbols_to_bits(coh, rx_bits, coh->ct_symb_ff_buf); - *sync_good = 1; + if (fabsf(coh->f_fine_est) > 2.0) { + if (coh->verbose) + fprintf(stderr, " [%d] Hmm %f is a bit big :(\n", coh->frame, + (double)coh->f_fine_est); + next_sync = 0; + } } - sync = sync_state_machine(coh, sync, next_sync); - coh->sync = sync; - - /* work out how many samples we need for the next call to account - for differences in tx and rx sample clocks */ + if (next_sync == 1) { + /* OK we are in sync! + demodulate first frame (demod completed below) */ - nin = COHPSK_M; - if (sync == 1) { - if (coh->rx_timing > COHPSK_M/P) - nin = COHPSK_M + COHPSK_M/P; - if (coh->rx_timing < -COHPSK_M/P) - nin = COHPSK_M - COHPSK_M/P; + if (coh->verbose) + fprintf(stderr, " [%d] in sync! f_est: %f ratio: %f \n", coh->frame, + (double)coh->f_est, (double)coh->ratio); + for (r = 0; r < NSYMROWPILOT + 2; r++) + for (c = 0; c < COHPSK_NC * COHPSK_ND; c++) + coh->ct_symb_ff_buf[r][c] = coh->ct_symb_buf[coh->ct + r][c]; } - coh->nin = nin; - *nin_frame = (NSYMROWPILOT-1)*COHPSK_M + nin; - //if (coh->verbose) - // fprintf(stderr, "%f %d %d\n", coh->rx_timing, nin, *nin_frame); + } + + /* If in sync just do sample rate processing on latest frame */ + + if (sync == 1) { + rate_Fs_rx_processing(coh, ch_symb, rx_fdm, &coh->f_est, NSYMROWPILOT, + coh->nin, 1); + frame_sync_fine_freq_est(coh, ch_symb, sync, &next_sync); + + for (r = 0; r < 2; r++) + for (c = 0; c < COHPSK_NC * COHPSK_ND; c++) + coh->ct_symb_ff_buf[r][c] = coh->ct_symb_ff_buf[r + NSYMROWPILOT][c]; + for (; r < NSYMROWPILOT + 2; r++) + for (c = 0; c < COHPSK_NC * COHPSK_ND; c++) + coh->ct_symb_ff_buf[r][c] = coh->ct_symb_buf[coh->ct + r][c]; + } + + /* if we are in sync complete demodulation with symbol rate processing */ + + *sync_good = 0; + if ((next_sync == 1) || (sync == 1)) { + qpsk_symbols_to_bits(coh, rx_bits, coh->ct_symb_ff_buf); + *sync_good = 1; + } + + sync = sync_state_machine(coh, sync, next_sync); + coh->sync = sync; + + /* work out how many samples we need for the next call to account + for differences in tx and rx sample clocks */ + + nin = COHPSK_M; + if (sync == 1) { + if (coh->rx_timing > COHPSK_M / P) nin = COHPSK_M + COHPSK_M / P; + if (coh->rx_timing < -COHPSK_M / P) nin = COHPSK_M - COHPSK_M / P; + } + coh->nin = nin; + *nin_frame = (NSYMROWPILOT - 1) * COHPSK_M + nin; + // if (coh->verbose) + // fprintf(stderr, "%f %d %d\n", coh->rx_timing, nin, *nin_frame); } - /*---------------------------------------------------------------------------*\ FUNCTION....: cohpsk_fs_offset() @@ -1232,31 +1246,31 @@ void cohpsk_demod(struct COHPSK *coh, float rx_bits[], int *sync_good, COMP rx_f \*---------------------------------------------------------------------------*/ -int cohpsk_fs_offset(COMP out[], COMP in[], int n, float sample_rate_ppm) -{ - double f; - double tin = 0.0; - double step = 1.0 + sample_rate_ppm/1E6; - int t1, t2; - int tout = 0; - - while (tin < (double) n) { - t1 = (int) floor(tin); - t2 = (int) ceil(tin); - f = tin - (double) t1; - - out[tout].real = ((double)1.0-f)*(double)in[t1].real + f*(double)in[t2].real; - out[tout].imag = ((double)1.0-f)*(double)in[t1].imag + f*(double)in[t2].imag; - - tin += step; - tout++; - //printf("tin: %f tout: %d f: %f\n", tin, tout, f); - } - - return tout; +int cohpsk_fs_offset(COMP out[], COMP in[], int n, float sample_rate_ppm) { + double f; + double tin = 0.0; + double step = 1.0 + sample_rate_ppm / 1E6; + int t1, t2; + int tout = 0; + + while (tin < (double)n) { + t1 = (int)floor(tin); + t2 = (int)ceil(tin); + f = tin - (double)t1; + + out[tout].real = + ((double)1.0 - f) * (double)in[t1].real + f * (double)in[t2].real; + out[tout].imag = + ((double)1.0 - f) * (double)in[t1].imag + f * (double)in[t2].imag; + + tin += step; + tout++; + // printf("tin: %f tout: %d f: %f\n", tin, tout, f); + } + + return tout; } - /*---------------------------------------------------------------------------*\ FUNCTION....: cohpsk_get_demod_stats() @@ -1267,54 +1281,51 @@ int cohpsk_fs_offset(COMP out[], COMP in[], int n, float sample_rate_ppm) \*---------------------------------------------------------------------------*/ -void cohpsk_get_demod_stats(struct COHPSK *coh, struct MODEM_STATS *stats) -{ - float new_snr_est; - +void cohpsk_get_demod_stats(struct COHPSK *coh, struct MODEM_STATS *stats) { + float new_snr_est; + #ifndef __EMBEDDED__ - float spi_4 = M_PI/4.0f; - COMP pi_4; - pi_4.real = cosf(spi_4); - pi_4.imag = sinf(spi_4); + float spi_4 = M_PI / 4.0f; + COMP pi_4; + pi_4.real = cosf(spi_4); + pi_4.imag = sinf(spi_4); #endif - stats->Nc = COHPSK_NC*COHPSK_ND; - assert(stats->Nc <= MODEM_STATS_NC_MAX); - new_snr_est = 20.0f * log10f((coh->sig_rms+1E-6f)/(coh->noise_rms+1E-6f)) - 10.0f*log10f(3000.0f/700.0f); - stats->snr_est = 0.9f*stats->snr_est + 0.1f*new_snr_est; + stats->Nc = COHPSK_NC * COHPSK_ND; + assert(stats->Nc <= MODEM_STATS_NC_MAX); + new_snr_est = + 20.0f * log10f((coh->sig_rms + 1E-6f) / (coh->noise_rms + 1E-6f)) - + 10.0f * log10f(3000.0f / 700.0f); + stats->snr_est = 0.9f * stats->snr_est + 0.1f * new_snr_est; - //fprintf(stderr, "sig_rms: %f noise_rms: %f snr_est: %f\n", coh->sig_rms, coh->noise_rms, stats->snr_est); - stats->sync = coh->sync; - stats->foff = coh->f_est - FDMDV_FCENTRE; - stats->rx_timing = coh->rx_timing; - stats->clock_offset = 0.0f; /* TODO - implement clock offset estimation */ + // fprintf(stderr, "sig_rms: %f noise_rms: %f snr_est: %f\n", coh->sig_rms, + // coh->noise_rms, stats->snr_est); + stats->sync = coh->sync; + stats->foff = coh->f_est - FDMDV_FCENTRE; + stats->rx_timing = coh->rx_timing; + stats->clock_offset = 0.0f; /* TODO - implement clock offset estimation */ #ifndef __EMBEDDED__ - assert(NSYMROW <= MODEM_STATS_NR_MAX); - stats->nr = NSYMROW; - for(int c=0; c<COHPSK_NC*COHPSK_ND; c++) { - for (int r=0; r<NSYMROW; r++) { - stats->rx_symbols[r][c] = cmult(coh->rx_symb[r][c], pi_4); - } + assert(NSYMROW <= MODEM_STATS_NR_MAX); + stats->nr = NSYMROW; + for (int c = 0; c < COHPSK_NC * COHPSK_ND; c++) { + for (int r = 0; r < NSYMROW; r++) { + stats->rx_symbols[r][c] = cmult(coh->rx_symb[r][c], pi_4); } + } #endif } - -void cohpsk_set_verbose(struct COHPSK *coh, int verbose) -{ - assert(coh != NULL); - coh->verbose = verbose; +void cohpsk_set_verbose(struct COHPSK *coh, int verbose) { + assert(coh != NULL); + coh->verbose = verbose; } - -void cohpsk_set_frame(struct COHPSK *coh, int frame) -{ - assert(coh != NULL); - coh->frame = frame; +void cohpsk_set_frame(struct COHPSK *coh, int frame) { + assert(coh != NULL); + coh->frame = frame; } - /*---------------------------------------------------------------------------*\ FUNCTION....: cohpsk_get_test_bits() @@ -1325,16 +1336,14 @@ void cohpsk_set_frame(struct COHPSK *coh, int frame) \*---------------------------------------------------------------------------*/ -void cohpsk_get_test_bits(struct COHPSK *coh, int rx_bits[]) -{ - memcpy(rx_bits, coh->ptest_bits_coh_tx, sizeof(int)*COHPSK_BITS_PER_FRAME); - coh->ptest_bits_coh_tx += COHPSK_BITS_PER_FRAME; - if (coh->ptest_bits_coh_tx >=coh->ptest_bits_coh_end) { - coh->ptest_bits_coh_tx = (int*)test_bits_coh; - } +void cohpsk_get_test_bits(struct COHPSK *coh, int rx_bits[]) { + memcpy(rx_bits, coh->ptest_bits_coh_tx, sizeof(int) * COHPSK_BITS_PER_FRAME); + coh->ptest_bits_coh_tx += COHPSK_BITS_PER_FRAME; + if (coh->ptest_bits_coh_tx >= coh->ptest_bits_coh_end) { + coh->ptest_bits_coh_tx = (int *)test_bits_coh; + } } - /*---------------------------------------------------------------------------*\ FUNCTION....: cohpsk_put_test_bits() @@ -1350,84 +1359,80 @@ void cohpsk_get_test_bits(struct COHPSK *coh, int rx_bits[]) \*---------------------------------------------------------------------------*/ void cohpsk_put_test_bits(struct COHPSK *coh, int *state, short error_pattern[], - int *bit_errors, char rx_bits_char[], int channel) -{ - int i, next_state, anerror; - int rx_bits[COHPSK_BITS_PER_FRAME]; - - assert((channel == 0) || (channel == 1)); - int *ptest_bits_coh_rx = coh->ptest_bits_coh_rx[channel]; - - for(i=0; i<COHPSK_BITS_PER_FRAME; i++) { - rx_bits[i] = rx_bits_char[i]; - } - - *bit_errors = 0; - for(i=0; i<COHPSK_BITS_PER_FRAME; i++) { - anerror = (rx_bits[i] & 0x1) ^ ptest_bits_coh_rx[i]; - if ((anerror < 0) || (anerror > 1)) { - fprintf(stderr, "i: %d rx_bits: %d ptest_bits_coh_rx: %d\n", i, rx_bits[i], ptest_bits_coh_rx[i]); - } - *bit_errors += anerror; - error_pattern[i] = anerror; + int *bit_errors, char rx_bits_char[], int channel) { + int i, next_state, anerror; + int rx_bits[COHPSK_BITS_PER_FRAME]; + + assert((channel == 0) || (channel == 1)); + int *ptest_bits_coh_rx = coh->ptest_bits_coh_rx[channel]; + + for (i = 0; i < COHPSK_BITS_PER_FRAME; i++) { + rx_bits[i] = rx_bits_char[i]; + } + + *bit_errors = 0; + for (i = 0; i < COHPSK_BITS_PER_FRAME; i++) { + anerror = (rx_bits[i] & 0x1) ^ ptest_bits_coh_rx[i]; + if ((anerror < 0) || (anerror > 1)) { + fprintf(stderr, "i: %d rx_bits: %d ptest_bits_coh_rx: %d\n", i, + rx_bits[i], ptest_bits_coh_rx[i]); } + *bit_errors += anerror; + error_pattern[i] = anerror; + } - /* state logic */ + /* state logic */ - next_state = *state; + next_state = *state; - if (*state == 0) { - if (*bit_errors < 4) { - next_state = 1; - ptest_bits_coh_rx += COHPSK_BITS_PER_FRAME; - if (ptest_bits_coh_rx >= coh->ptest_bits_coh_end) { - ptest_bits_coh_rx = (int*)test_bits_coh; - } - } + if (*state == 0) { + if (*bit_errors < 4) { + next_state = 1; + ptest_bits_coh_rx += COHPSK_BITS_PER_FRAME; + if (ptest_bits_coh_rx >= coh->ptest_bits_coh_end) { + ptest_bits_coh_rx = (int *)test_bits_coh; + } } - - /* if 5 frames with large BER reset test frame sync */ - - if (*state > 0) { - if (*bit_errors > 8) { - if (*state == 6) - next_state = 0; - else - next_state = *state+1; - } - else - next_state = 1; + } + + /* if 5 frames with large BER reset test frame sync */ + + if (*state > 0) { + if (*bit_errors > 8) { + if (*state == 6) + next_state = 0; + else + next_state = *state + 1; + } else + next_state = 1; + } + + if (*state > 0) { + ptest_bits_coh_rx += COHPSK_BITS_PER_FRAME; + if (ptest_bits_coh_rx >= coh->ptest_bits_coh_end) { + ptest_bits_coh_rx = (int *)test_bits_coh; } + } - if (*state > 0) { - ptest_bits_coh_rx += COHPSK_BITS_PER_FRAME; - if (ptest_bits_coh_rx >= coh->ptest_bits_coh_end) { - ptest_bits_coh_rx = (int*)test_bits_coh; - } - } - - //fprintf(stderr, "state: %d next_state: %d bit_errors: %d\n", *state, next_state, *bit_errors); - - *state = next_state; - coh->ptest_bits_coh_rx[channel] = ptest_bits_coh_rx; -} - + // fprintf(stderr, "state: %d next_state: %d bit_errors: %d\n", *state, + // next_state, *bit_errors); -int cohpsk_error_pattern_size(void) { - return COHPSK_BITS_PER_FRAME; + *state = next_state; + coh->ptest_bits_coh_rx[channel] = ptest_bits_coh_rx; } +int cohpsk_error_pattern_size(void) { return COHPSK_BITS_PER_FRAME; } float *cohpsk_get_rx_bits_lower(struct COHPSK *coh) { - return coh->rx_bits_lower; + return coh->rx_bits_lower; } float *cohpsk_get_rx_bits_upper(struct COHPSK *coh) { - return coh->rx_bits_upper; + return coh->rx_bits_upper; } void cohpsk_set_carrier_ampl(struct COHPSK *coh, int c, float ampl) { - assert(c < COHPSK_NC*COHPSK_ND); - coh->carrier_ampl[c] = ampl; - fprintf(stderr, "cohpsk_set_carrier_ampl: %d %f\n", c, (double)ampl); + assert(c < COHPSK_NC * COHPSK_ND); + coh->carrier_ampl[c] = ampl; + fprintf(stderr, "cohpsk_set_carrier_ampl: %d %f\n", c, (double)ampl); } |