3 * Copyright 2004 Free Software Foundation, Inc.
5 * This file is part of GNU Radio
7 * GNU Radio is free software; you can redistribute it and/or modify
8 * it under the terms of the GNU General Public License as published by
9 * the Free Software Foundation; either version 3, or (at your option)
12 * GNU Radio is distributed in the hope that it will be useful,
13 * but WITHOUT ANY WARRANTY; without even the implied warranty of
14 * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the
15 * GNU General Public License for more details.
17 * You should have received a copy of the GNU General Public License
18 * along with GNU Radio; see the file COPYING. If not, write to
19 * the Free Software Foundation, Inc., 51 Franklin Street,
20 * Boston, MA 02110-1301, USA.
27 #include <gr_simple_correlator.h>
28 #include <gr_simple_framer_sync.h>
29 #include <gr_io_signature.h>
32 #include <gr_count_bits.h>
37 static const int THRESHOLD = 3;
39 gr_simple_correlator_sptr
40 gr_make_simple_correlator (int payload_bytesize)
42 return gr_simple_correlator_sptr (new gr_simple_correlator (payload_bytesize));
45 gr_simple_correlator::gr_simple_correlator (int payload_bytesize)
46 : gr_block ("simple_correlator",
47 gr_make_io_signature (1, 1, sizeof (float)),
48 gr_make_io_signature (1, 1, sizeof (unsigned char))),
49 d_payload_bytesize (payload_bytesize),
50 d_state (ST_LOOKING), d_osi (0),
51 d_bblen ((payload_bytesize + GRSF_PAYLOAD_OVERHEAD) * GRSF_BITS_PER_BYTE),
52 d_bitbuf (new unsigned char [d_bblen]),
58 for (int i = 0; i < AVG_PERIOD; i++)
61 #ifdef DEBUG_SIMPLE_CORRELATOR
62 d_debug_fp = fopen("corr.log", "w");
68 gr_simple_correlator::~gr_simple_correlator ()
70 #ifdef DEBUG_SIMPLE_CORRELATOR
78 gr_simple_correlator::enter_looking ()
81 // fprintf (stderr, ">>> enter_looking\n");
83 for (int i = 0; i < OVERSAMPLE; i++)
90 for (int i = 0; i < AVG_PERIOD; i++)
95 gr_simple_correlator::enter_under_threshold ()
98 // fprintf (stderr, ">>> enter_under_threshold\n");
99 d_state = ST_UNDER_THRESHOLD;
100 d_transition_osi = d_osi;
104 gr_simple_correlator::enter_locked ()
107 int delta = sub_index (d_osi, d_transition_osi);
108 d_center_osi = add_index (d_transition_osi, delta/2);
109 d_center_osi = add_index (d_center_osi, 3); // FIXME
112 // fprintf (stderr, ">>> enter_locked d_center_osi = %d\n", d_center_osi);
114 d_avg = std::max(-1.0, std::min(1.0, d_accum * (1.0/AVG_PERIOD)));
115 // fprintf(stderr, ">>> enter_locked d_avg = %g\n", d_avg);
119 packit (unsigned char *pktbuf, const unsigned char *bitbuf, int bitcount)
121 for (int i = 0; i < bitcount; i += 8){
122 int t = bitbuf[i+0] & 0x1;
123 t = (t << 1) | (bitbuf[i+1] & 0x1);
124 t = (t << 1) | (bitbuf[i+2] & 0x1);
125 t = (t << 1) | (bitbuf[i+3] & 0x1);
126 t = (t << 1) | (bitbuf[i+4] & 0x1);
127 t = (t << 1) | (bitbuf[i+5] & 0x1);
128 t = (t << 1) | (bitbuf[i+6] & 0x1);
129 t = (t << 1) | (bitbuf[i+7] & 0x1);
135 gr_simple_correlator::update_avg(float x)
137 d_accum -= d_avgbuf[d_avbi];
138 d_avgbuf[d_avbi] = x;
140 d_avbi = (d_avbi + 1) & (AVG_PERIOD-1);
145 gr_simple_correlator::general_work (int noutput_items,
146 gr_vector_int &ninput_items,
147 gr_vector_const_void_star &input_items,
148 gr_vector_void_star &output_items)
150 const float *in = (const float *) input_items[0];
151 unsigned char *out = (unsigned char *) output_items[0];
155 int nin = ninput_items[0];
167 #ifdef DEBUG_SIMPLE_CORRELATOR
168 debug_data.raw_data = in[n];
169 debug_data.sampled = 0.0;
170 debug_data.enter_locked = 0.0;
176 if (d_osi == d_center_osi){
178 #ifdef DEBUG_SIMPLE_CORRELATOR
179 debug_data.sampled = 1.0;
181 decision = slice (in[n]);
183 d_bitbuf[d_bbi] = decision;
185 if (d_bbi >= d_bblen){
186 // printf ("got whole packet\n");
187 unsigned char pktbuf[d_bblen/GRSF_BITS_PER_BYTE];
188 packit (pktbuf, d_bitbuf, d_bbi);
189 printf ("seqno %3d\n", pktbuf[0]);
190 memcpy (out, &pktbuf[GRSF_PAYLOAD_OVERHEAD], d_payload_bytesize);
192 consume_each (n + 1);
193 return d_payload_bytesize;
199 case ST_UNDER_THRESHOLD:
201 decision = slice (in[n]);
202 d_shift_reg[d_osi] = (d_shift_reg[d_osi] << 1) | decision;
204 hamming_dist = gr_count_bits64 (d_shift_reg[d_osi] ^ GRSF_SYNC);
205 // printf ("%2d %d\n", hamming_dist, d_osi);
207 if (d_state == ST_LOOKING && hamming_dist <= THRESHOLD){
208 // We're seeing a good PN code, remember location
209 enter_under_threshold ();
211 else if (d_state == ST_UNDER_THRESHOLD && hamming_dist > THRESHOLD){
212 // no longer seeing good PN code, compute center of goodness
214 debug_data.enter_locked = 1.0;
222 #ifdef DEBUG_SIMPLE_CORRELATOR
223 fwrite(&debug_data, sizeof (debug_data), 1, d_debug_fp);
226 d_osi = add_index (d_osi, 1);