From 3492bd70b13c8e36649016f8f014ba16c0d865a7 Mon Sep 17 00:00:00 2001 From: Max Date: Tue, 4 Jan 2022 19:06:28 -0500 Subject: [PATCH] new algo to eliminate +- 2400 Hz tuning ambiguity --- .../lib/gardner_costas_cc_impl.cc | 103 ++++++++++++++++-- .../lib/gardner_costas_cc_impl.h | 4 + 2 files changed, 96 insertions(+), 11 deletions(-) diff --git a/op25/gr-op25_repeater/lib/gardner_costas_cc_impl.cc b/op25/gr-op25_repeater/lib/gardner_costas_cc_impl.cc index 2d770bf..dc0fe4a 100644 --- a/op25/gr-op25_repeater/lib/gardner_costas_cc_impl.cc +++ b/op25/gr-op25_repeater/lib/gardner_costas_cc_impl.cc @@ -69,6 +69,48 @@ static inline std::complex sgn(std::complexc) { d_event_type = c; \ } +static int tuning_score(gr_complex buf[], int bufp, int bufl, int sync_l, int sps) { + int n_scan_samples = sps * 6; + float atan_prev = 0; + float score = 0; + int p = bufp - (n_scan_samples + 1); + if (p < 0) + p += bufl; + int n_tests = 0; + int n_plus = 0; + // a series of N consecutive samples treated as N-1 connected line segments + // is evaluated to find the angle, in radians, at the junction of each pair + // of lines. + // when a frequency tuning error is present the algebraic sum of the angles + // will contain a significant positive or negative bias. + for (int i = 0; i < n_scan_samples; i++) { + gr_complex sample1 = buf[ (p+i) % bufl ]; + gr_complex sample2 = buf[ (p+i+1) % bufl ]; + gr_complex diff = sample2 - sample1; + float atan = gr::fast_atan2f(diff.real(), diff.imag()); + if (i == 0) { + atan_prev = atan; + continue; + } + float atan_diff = atan - atan_prev; + if (atan_diff > M_PI) + atan_diff -= M_TWOPI; + if (atan_diff < -M_PI) + atan_diff += M_TWOPI; + atan_prev = atan; + score += atan_diff; + n_tests += 1; + if (atan_diff > 0) + n_plus += 1; + } + float f1 = (score > 0) ? n_plus : n_tests - n_plus; + float f2 = n_tests; + int pct = (int) (100*f1/f2 + 0.5); + if (score < 0) + pct *= -1; + return pct; +} + static inline bool is_future(struct timeval*t) { struct timeval current_t; gettimeofday(¤t_t,0); @@ -95,7 +137,7 @@ void gardner_costas_cc_impl::dump_samples(int error_amt) { if (is_future(&d_next_sample_time)) return; gettimeofday(&d_next_sample_time,0); - d_next_sample_time.tv_sec += 5; + d_next_sample_time.tv_sec += 1; sprintf(filename, "sample-%ld-%d.dat", unique_id(), d_sample_file_id); d_sample_file_id ++; d_sample_file_id = d_sample_file_id % N_FILES; @@ -114,6 +156,7 @@ void gardner_costas_cc_impl::dump_samples(int error_amt) { uint8_t gardner_costas_cc_impl::slicer(float sym) { uint8_t dibit = 0; + int sps = (int) (d_omega + 0.5); static const float PI_4 = M_PI / 4.0; static const float d_slice_levels[4] = {(float)-2.0*PI_4, (float)0.0*PI_4, (float)2.0*PI_4, (float)4.0*PI_4}; if (d_slice_levels[3] < 0) { @@ -135,44 +178,74 @@ uint8_t gardner_costas_cc_impl::slicer(float sym) { if(check_frame_sync((nid_accum & P25_FRAME_SYNC_MASK) ^ P25_FRAME_SYNC_MAGIC, 0, 48)) { // fprintf(stderr, "P25P1 Framing detect\n"); UPDATE_COUNT(' ') + d_sync_valid_until = d_sample_count + 2400 * sps; dump_samples(0); } else if(check_frame_sync((nid_accum & P25P2_FRAME_SYNC_MASK) ^ P25P2_FRAME_SYNC_MAGIC, 0, 40)) { // fprintf(stderr, "P25P2 Framing detect\n"); UPDATE_COUNT(' ') + d_sync_valid_until = d_sample_count + 2400 * sps; dump_samples(0); } if (d_is_tdma) { if(check_frame_sync((nid_accum & P25_FRAME_SYNC_MASK) ^ 0x000104015155LL, 0, 40)) { fprintf(stderr, "TDMA: channel %d tuning error -1200\n", -1); UPDATE_COUNT('-') + d_sync_valid_until = d_sample_count + 2400 * sps; dump_samples(-1200); } else if(check_frame_sync((nid_accum & P25_FRAME_SYNC_MASK) ^ 0xfefbfeaeaaLL, 0, 40)) { fprintf(stderr, "TDMA: channel %d tuning error +1200\n", -1); UPDATE_COUNT('+') + d_sync_valid_until = d_sample_count + 2400 * sps; dump_samples(1200); } else if(check_frame_sync((nid_accum & P25_FRAME_SYNC_MASK) ^ 0xa8a2a80800LL, 0, 40)) { - fprintf(stderr, "TDMA: channel %d tuning error +/- 2400\n", -1); - UPDATE_COUNT('|') - dump_samples(2400); + int score = tuning_score(d_prev_sample, d_prev_sample_p, d_n_prev_sample, 20, sps); + int error = 24; + if (score == -100) { + error = 2400; + UPDATE_COUNT('>') + fprintf(stderr, "TDMA: channel %d tuning error %d\n", -1, error); + } else if (score == 100) { + error = -2400; + UPDATE_COUNT('<') + fprintf(stderr, "TDMA: channel %d tuning error %d\n", -1, error); + } else { + fprintf(stderr, "TDMA: channel %d tuning error +/-2400, confidence %d\n", -1, score); + } + d_sync_valid_until = d_sample_count + 2400 * sps; + dump_samples(error); } } else { if(check_frame_sync((nid_accum & P25_FRAME_SYNC_MASK) ^ 0x001050551155LL, 0, 48)) { // fprintf(stderr, "tuning error -1200\n"); UPDATE_COUNT('-') + d_sync_valid_until = d_sample_count + 2400 * sps; dump_samples(-1200); } else if(check_frame_sync((nid_accum & P25_FRAME_SYNC_MASK) ^ 0xFFEFAFAAEEAALL, 0, 48)) { // fprintf(stderr, "tuning error +1200\n"); UPDATE_COUNT('+') + d_sync_valid_until = d_sample_count + 2400 * sps; dump_samples(1200); } else if(check_frame_sync((nid_accum & P25_FRAME_SYNC_MASK) ^ 0xAA8A0A008800LL, 0, 48)) { -// fprintf(stderr, "tuning error +/- 2400\n"); - UPDATE_COUNT('|') - dump_samples(2400); + int score = tuning_score(d_prev_sample, d_prev_sample_p, d_n_prev_sample, 24, sps); + int error = 24; + if (score == -100) { + error = 2400; + UPDATE_COUNT('>') + fprintf(stderr, "channel %d block id %ld tuning error %d\n", -1, (long)unique_id(), error); + } else if (score == 100) { + error = -2400; + UPDATE_COUNT('<') + fprintf(stderr, "channel %d block id %ld tuning error %d\n", -1, (long)unique_id(), error); + } else { + fprintf(stderr, "channel %d block id %ld tuning error +/-2400, confidence %d\n", -1, (long)unique_id(), score); + } + d_sync_valid_until = d_sample_count + 2400 * sps; + dump_samples(error); } } if (d_event_type == ' ' || d_event_count < 5) { @@ -182,8 +255,10 @@ uint8_t gardner_costas_cc_impl::slicer(float sym) { d_update_request = -1; else if (d_event_type == '-' && d_fm < 0) d_update_request = 1; - else if (d_event_type == '|') - d_update_request = (d_fm < 0) ? 2 : -2; + else if (d_event_type == '<') + d_update_request = -2; + else if (d_event_type == '>') + d_update_request = 2; else d_update_request = 0; } return dibit; @@ -222,7 +297,8 @@ uint8_t gardner_costas_cc_impl::slicer(float sym) { d_fm(0), d_fm_accum(0), d_fm_count(0), d_muted(false), d_is_tdma(false), d_enable_sync_plot(false), d_prev_sample(NULL), d_n_prev_sample(0), d_prev_sample_p(0), - d_sample_file_id(0) + d_sample_file_id(0), + d_sample_count(0), d_sync_valid_until(0) { set_omega(samples_per_symbol); set_relative_rate (1.0 / d_omega); @@ -257,10 +333,14 @@ void gardner_costas_cc_impl::set_omega (float omega) { } float gardner_costas_cc_impl::get_freq_error (void) { + if (!recent_sync()) + return 0.0; return (d_freq); } int gardner_costas_cc_impl::get_error_band (void) { + if (!recent_sync()) + return 0; return (d_update_request); } @@ -338,7 +418,7 @@ gardner_costas_cc_impl::general_work (int noutput_items, gr_complex interp_samp, interp_samp_mid, diffdec; float error_real, error_imag, symbol_error; - if (d_enable_sync_plot && d_prev_sample == NULL) { + if (d_prev_sample == NULL) { d_n_prev_sample = (int) (d_omega + 0.5); // sps d_n_prev_sample *= (d_is_tdma) ? 32 : 25; // enough for p25p1 or p25p2 sync d_prev_sample = (gr_complex *) calloc(d_n_prev_sample, sizeof(gr_complex)); @@ -371,6 +451,7 @@ gardner_costas_cc_impl::general_work (int noutput_items, d_dl_index = d_dl_index % d_twice_sps; i++; + d_sample_count++; gr_complex df = symbol * conj(d_prev); float fmd = atan2f(df.imag(), df.real()); d_fm_accum += fmd; diff --git a/op25/gr-op25_repeater/lib/gardner_costas_cc_impl.h b/op25/gr-op25_repeater/lib/gardner_costas_cc_impl.h index e731ed4..aa9e8b7 100644 --- a/op25/gr-op25_repeater/lib/gardner_costas_cc_impl.h +++ b/op25/gr-op25_repeater/lib/gardner_costas_cc_impl.h @@ -131,9 +131,13 @@ protected: struct timeval d_next_sample_time; int d_sample_file_id; + unsigned int d_sample_count; + unsigned int d_sync_valid_until; + float phase_error_detector_qpsk(gr_complex sample); void phase_error_tracking(gr_complex sample); void dump_samples(int); + bool recent_sync(void) { return d_sample_count <= d_sync_valid_until; } }; } // namespace op25_repeater