1 /* -*- c++ -*- */
2 /*
3 * Copyright 2004,2010,2013 Free Software Foundation, Inc.
4 *
5 * This file is part of GNU Radio
6 *
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)
10 * any later version.
11 *
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.
16 *
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.
21 */
22
23 #ifdef HAVE_CONFIG_H
24 #include "config.h"
25 #endif
26
27 #include "simple_correlator_impl.h"
28 #include <gnuradio/blocks/count_bits.h>
29 #include <gnuradio/digital/simple_framer_sync.h>
30 #include <gnuradio/io_signature.h>
31 #include <assert.h>
32 #include <string.h>
33 #include <cstdio>
34 #include <stdexcept>
35
36 namespace gr {
37 namespace digital {
38
39 static const int THRESHOLD = 3;
40
make(int payload_bytesize)41 simple_correlator::sptr simple_correlator::make(int payload_bytesize)
42 {
43 return gnuradio::get_initial_sptr(new simple_correlator_impl(payload_bytesize));
44 }
45
simple_correlator_impl(int payload_bytesize)46 simple_correlator_impl::simple_correlator_impl(int payload_bytesize)
47 : block("simple_correlator",
48 io_signature::make(1, 1, sizeof(float)),
49 io_signature::make(1, 1, sizeof(unsigned char))),
50 d_payload_bytesize(payload_bytesize),
51 d_state(ST_LOOKING),
52 d_osi(0),
53 d_transition_osi(0),
54 d_center_osi(0),
55 d_bblen((payload_bytesize + GRSF_PAYLOAD_OVERHEAD) * GRSF_BITS_PER_BYTE),
56 d_bitbuf(new unsigned char[d_bblen]),
57 d_pktbuf(new unsigned char[d_bblen / GRSF_BITS_PER_BYTE]),
58 d_bbi(0)
59 {
60 d_avbi = 0;
61 d_accum = 0.0;
62 d_avg = 0.0;
63 for (int i = 0; i < AVG_PERIOD; i++)
64 d_avgbuf[i] = 0.0;
65
66 #ifdef DEBUG_SIMPLE_CORRELATOR
67 d_debug_fp = fopen("corr.log", "w");
68 #endif
69 enter_looking();
70 }
71
~simple_correlator_impl()72 simple_correlator_impl::~simple_correlator_impl()
73 {
74 #ifdef DEBUG_SIMPLE_CORRELATOR
75 fclose(d_debug_fp);
76 #endif
77 delete[] d_bitbuf;
78 delete[] d_pktbuf;
79 }
80
enter_looking()81 void simple_correlator_impl::enter_looking()
82 {
83 fflush(stdout);
84 // fprintf(stderr, ">>> enter_looking\n");
85 d_state = ST_LOOKING;
86 for (int i = 0; i < OVERSAMPLE; i++)
87 d_shift_reg[i] = 0;
88 d_osi = 0;
89
90 d_avbi = 0;
91 d_avg = d_avg * 0.5;
92 d_accum = 0;
93 for (int i = 0; i < AVG_PERIOD; i++)
94 d_avgbuf[i] = 0.0;
95 }
96
enter_under_threshold()97 void simple_correlator_impl::enter_under_threshold()
98 {
99 fflush(stdout);
100 // fprintf(stderr, ">>> enter_under_threshold\n");
101 d_state = ST_UNDER_THRESHOLD;
102 d_transition_osi = d_osi;
103 }
104
enter_locked()105 void simple_correlator_impl::enter_locked()
106 {
107 d_state = ST_LOCKED;
108 int delta = sub_index(d_osi, d_transition_osi);
109 d_center_osi = add_index(d_transition_osi, delta / 2);
110 // d_center_osi = add_index(d_center_osi, 3); // FIXME
111 d_bbi = 0;
112 fflush(stdout);
113 // fprintf(stderr, ">>> enter_locked d_center_osi = %d\n", d_center_osi);
114
115 d_avg = std::max(-1.0, std::min(1.0, d_accum * (1.0 / AVG_PERIOD)));
116 // fprintf(stderr, ">>> enter_locked d_avg = %g\n", d_avg);
117 }
118
packit(unsigned char * pktbuf,const unsigned char * bitbuf,int bitcount)119 static void packit(unsigned char* pktbuf, const unsigned char* bitbuf, int bitcount)
120 {
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);
130 *pktbuf++ = t;
131 }
132 }
133
update_avg(float x)134 void simple_correlator_impl::update_avg(float x)
135 {
136 d_accum -= d_avgbuf[d_avbi];
137 d_avgbuf[d_avbi] = x;
138 d_accum += x;
139 d_avbi = (d_avbi + 1) & (AVG_PERIOD - 1);
140 }
141
general_work(int noutput_items,gr_vector_int & ninput_items,gr_vector_const_void_star & input_items,gr_vector_void_star & output_items)142 int simple_correlator_impl::general_work(int noutput_items,
143 gr_vector_int& ninput_items,
144 gr_vector_const_void_star& input_items,
145 gr_vector_void_star& output_items)
146 {
147 const float* in = (const float*)input_items[0];
148 unsigned char* out = (unsigned char*)output_items[0];
149
150 int n = 0;
151 int nin = ninput_items[0];
152 int decision;
153 int hamming_dist;
154
155 #ifdef DEBUG_SIMPLE_CORRELATOR
156 struct debug_data {
157 float raw_data;
158 float sampled;
159 float enter_locked;
160 } debug_data;
161 #endif
162
163 while (n < nin) {
164
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;
169 #endif
170
171 switch (d_state) {
172 case ST_LOCKED:
173 if (d_osi == d_center_osi) {
174
175 #ifdef DEBUG_SIMPLE_CORRELATOR
176 debug_data.sampled = 1.0;
177 #endif
178 decision = slice(in[n]);
179
180 d_bitbuf[d_bbi] = decision;
181 d_bbi++;
182 if (d_bbi >= d_bblen) {
183 // printf("got whole packet\n");
184 packit(d_pktbuf, d_bitbuf, d_bbi);
185 // printf("seqno %3d\n", d_pktbuf[0]);
186 memcpy(out, &d_pktbuf[GRSF_PAYLOAD_OVERHEAD], d_payload_bytesize);
187 enter_looking();
188 consume_each(n + 1);
189 return d_payload_bytesize;
190 }
191 }
192 break;
193
194 case ST_LOOKING:
195 case ST_UNDER_THRESHOLD:
196 update_avg(in[n]);
197 decision = slice(in[n]);
198 d_shift_reg[d_osi] = (d_shift_reg[d_osi] << 1) | decision;
199
200 hamming_dist = gr::blocks::count_bits64(d_shift_reg[d_osi] ^ GRSF_SYNC);
201 // printf("%2d %d\n", hamming_dist, d_osi);
202
203 if (d_state == ST_LOOKING && hamming_dist <= THRESHOLD) {
204 // We're seeing a good PN code, remember location
205 enter_under_threshold();
206 } else if (d_state == ST_UNDER_THRESHOLD && hamming_dist > THRESHOLD) {
207 // no longer seeing good PN code, compute center of goodness
208 enter_locked();
209 #ifdef DEBUG_SIMPLE_CORRELATOR
210 debug_data.enter_locked = 1.0;
211 #endif
212 }
213 break;
214 default:
215 assert(0);
216 }
217
218 #ifdef DEBUG_SIMPLE_CORRELATOR
219 fwrite(&debug_data, sizeof(debug_data), 1, d_debug_fp);
220 #endif
221
222 d_osi = add_index(d_osi, 1);
223 n++;
224 }
225
226 consume_each(n);
227 return 0;
228 }
229
230 } /* namespace digital */
231 } /* namespace gr */
232