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>
35 static const int THRESHOLD = 3;
37 gr_simple_correlator_sptr
38 gr_make_simple_correlator (int payload_bytesize)
40 return gr_simple_correlator_sptr (new gr_simple_correlator (payload_bytesize));
43 gr_simple_correlator::gr_simple_correlator (int payload_bytesize)
44 : gr_block ("simple_correlator",
45 gr_make_io_signature (1, 1, sizeof (float)),
46 gr_make_io_signature (1, 1, sizeof (unsigned char))),
47 d_payload_bytesize (payload_bytesize),
48 d_state (ST_LOOKING), d_osi (0),
49 d_bblen ((payload_bytesize + GRSF_PAYLOAD_OVERHEAD) * GRSF_BITS_PER_BYTE),
50 d_bitbuf (new unsigned char [d_bblen]),
56 for (int i = 0; i < AVG_PERIOD; i++)
59 #ifdef DEBUG_SIMPLE_CORRELATOR
60 d_debug_fp = fopen("corr.log", "w");
66 gr_simple_correlator::~gr_simple_correlator ()
68 #ifdef DEBUG_SIMPLE_CORRELATOR
76 gr_simple_correlator::enter_looking ()
79 // fprintf (stderr, ">>> enter_looking\n");
81 for (int i = 0; i < OVERSAMPLE; i++)
88 for (int i = 0; i < AVG_PERIOD; i++)
93 gr_simple_correlator::enter_under_threshold ()
96 // fprintf (stderr, ">>> enter_under_threshold\n");
97 d_state = ST_UNDER_THRESHOLD;
98 d_transition_osi = d_osi;
102 gr_simple_correlator::enter_locked ()
105 int delta = sub_index (d_osi, d_transition_osi);
106 d_center_osi = add_index (d_transition_osi, delta/2);
107 d_center_osi = add_index (d_center_osi, 3); // FIXME
110 // fprintf (stderr, ">>> enter_locked d_center_osi = %d\n", d_center_osi);
112 d_avg = std::max(-1.0, std::min(1.0, d_accum * (1.0/AVG_PERIOD)));
113 // fprintf(stderr, ">>> enter_locked d_avg = %g\n", d_avg);
117 packit (unsigned char *pktbuf, const unsigned char *bitbuf, int bitcount)
119 for (int i = 0; i < bitcount; i += 8){
120 int t = bitbuf[i+0] & 0x1;
121 t = (t << 1) | (bitbuf[i+1] & 0x1);
122 t = (t << 1) | (bitbuf[i+2] & 0x1);
123 t = (t << 1) | (bitbuf[i+3] & 0x1);
124 t = (t << 1) | (bitbuf[i+4] & 0x1);
125 t = (t << 1) | (bitbuf[i+5] & 0x1);
126 t = (t << 1) | (bitbuf[i+6] & 0x1);
127 t = (t << 1) | (bitbuf[i+7] & 0x1);
133 gr_simple_correlator::update_avg(float x)
135 d_accum -= d_avgbuf[d_avbi];
136 d_avgbuf[d_avbi] = x;
138 d_avbi = (d_avbi + 1) & (AVG_PERIOD-1);
143 gr_simple_correlator::general_work (int noutput_items,
144 gr_vector_int &ninput_items,
145 gr_vector_const_void_star &input_items,
146 gr_vector_void_star &output_items)
148 const float *in = (const float *) input_items[0];
149 unsigned char *out = (unsigned char *) output_items[0];
153 int nin = ninput_items[0];
165 #ifdef DEBUG_SIMPLE_CORRELATOR
166 debug_data.raw_data = in[n];
167 debug_data.sampled = 0.0;
168 debug_data.enter_locked = 0.0;
174 if (d_osi == d_center_osi){
176 #ifdef DEBUG_SIMPLE_CORRELATOR
177 debug_data.sampled = 1.0;
179 decision = slice (in[n]);
181 d_bitbuf[d_bbi] = decision;
183 if (d_bbi >= d_bblen){
184 // printf ("got whole packet\n");
185 unsigned char pktbuf[d_bblen/GRSF_BITS_PER_BYTE];
186 packit (pktbuf, d_bitbuf, d_bbi);
187 printf ("seqno %3d\n", pktbuf[0]);
188 memcpy (out, &pktbuf[GRSF_PAYLOAD_OVERHEAD], d_payload_bytesize);
190 consume_each (n + 1);
191 return d_payload_bytesize;
197 case ST_UNDER_THRESHOLD:
199 decision = slice (in[n]);
200 d_shift_reg[d_osi] = (d_shift_reg[d_osi] << 1) | decision;
202 hamming_dist = gr_count_bits64 (d_shift_reg[d_osi] ^ GRSF_SYNC);
203 // printf ("%2d %d\n", hamming_dist, d_osi);
205 if (d_state == ST_LOOKING && hamming_dist <= THRESHOLD){
206 // We're seeing a good PN code, remember location
207 enter_under_threshold ();
209 else if (d_state == ST_UNDER_THRESHOLD && hamming_dist > THRESHOLD){
210 // no longer seeing good PN code, compute center of goodness
212 debug_data.enter_locked = 1.0;
220 #ifdef DEBUG_SIMPLE_CORRELATOR
221 fwrite(&debug_data, sizeof (debug_data), 1, d_debug_fp);
224 d_osi = add_index (d_osi, 1);