root/ossiedev/branches/jgaeddert/0.6.x/components/DataStreamer/src/DataStreamerSink.cpp @ 7782

Revision 7782, 15.2 KB (checked in by jgaeddert, 5 years ago)

fixing timing recovery in data streamer sink component; backwards SKIP/STUFF loop control than prototype due to data being pushed into front of buffer rather than back
  • Property svn:eol-style set to native
Line 
1/****************************************************************************
2
3Copyright 2008 Virginia Polytechnic Institute and State University
4
5This file is part of the OSSIE DataStreamerSink.
6
7OSSIE DataStreamerSink is free software; you can redistribute it and/or modify
8it under the terms of the GNU General Public License as published by
9the Free Software Foundation; either version 2 of the License, or
10(at your option) any later version.
11
12OSSIE DataStreamerSink is distributed in the hope that it will be useful,
13but WITHOUT ANY WARRANTY; without even the implied warranty of
14MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
15GNU General Public License for more details.
16
17You should have received a copy of the GNU General Public License
18along with OSSIE DataStreamerSink; if not, write to the Free Software
19Foundation, Inc., 59 Temple Place, Suite 330, Boston, MA  02111-1307  USA
20
21****************************************************************************/
22
23
24#include "DataStreamerSink.h"
25
26DataStreamerSink_i::DataStreamerSink_i(const char *uuid, omni_condition *condition) :
27    Resource_impl(uuid), component_running(condition)
28{
29    samplesOut = new standardInterfaces_i::complexShort_u("samples_out");
30    dataIn = new standardInterfaces_i::complexShort_p("data_in");
31
32    //Create the thread for the writer's processing function
33    processing_thread = new omni_thread(Run, (void *) this);
34
35    //Start the thread containing the writer's processing function
36    processing_thread->start();
37
38}
39
40DataStreamerSink_i::~DataStreamerSink_i(void)
41{   
42    delete samplesOut;
43    delete dataIn;
44}
45
46// Static function for omni thread
47void DataStreamerSink_i::Run( void * data )
48{
49    ((DataStreamerSink_i*)data)->ProcessData();
50}
51
52CORBA::Object_ptr DataStreamerSink_i::getPort( const char* portName ) throw (
53    CORBA::SystemException, CF::PortSupplier::UnknownPort)
54{
55    DEBUG(3, DataStreamerSink, "getPort() invoked with " << portName)
56   
57    CORBA::Object_var p;
58
59    p = samplesOut->getPort(portName);
60
61    if (!CORBA::is_nil(p))
62        return p._retn();
63
64    p = dataIn->getPort(portName);
65
66    if (!CORBA::is_nil(p))
67        return p._retn();
68
69    /*exception*/
70    throw CF::PortSupplier::UnknownPort();
71}
72
73void DataStreamerSink_i::start() throw (CORBA::SystemException,
74    CF::Resource::StartError)
75{
76    DEBUG(3, DataStreamerSink, "start() invoked")
77}
78
79void DataStreamerSink_i::stop() throw (CORBA::SystemException, CF::Resource::StopError)
80{ 
81    DEBUG(3, DataStreamerSink, "stop() invoked")
82}
83
84void DataStreamerSink_i::releaseObject() throw (CORBA::SystemException,
85    CF::LifeCycle::ReleaseError)
86{
87    DEBUG(3, DataStreamerSink, "releaseObject() invoked")
88   
89    component_running->signal();
90}
91
92void DataStreamerSink_i::initialize() throw (CF::LifeCycle::InitializeError,
93    CORBA::SystemException)
94{
95    DEBUG(3, DataStreamerSink, "initialize() invoked")
96}
97
98void DataStreamerSink_i::configure(const CF::Properties& props)
99throw (CORBA::SystemException,
100    CF::PropertySet::InvalidConfiguration,
101    CF::PropertySet::PartialConfiguration)
102{
103    DEBUG(3, DataStreamerSink, "configure() invoked")
104   
105    std::cout << "props length : " << props.length() << std::endl;
106
107    for (unsigned int i = 0; i <props.length(); i++) {
108        std::cout << "Property id : " << props[i].id << std::endl;
109
110        if (strcmp(props[i].id, "DCE:d2f9ae84-17b4-11dd-a212-00123f63025f") == 0) {
111            // modulation scheme
112        } else if (strcmp(props[i].id, "DCE:d704c6ee-17b4-11dd-a1ba-00123f63025f") == 0) {
113            // modulation depth
114        } else {
115            // unknown property
116            std::cerr << "ERROR: DataStreamerSink::configure() unknown property \""
117               << props[i].id << "\"" << std::endl;
118            throw CF::PropertySet::InvalidConfiguration();
119        }
120    }
121}
122
123void DataStreamerSink_i::ProcessData()
124{
125    DEBUG(3, DataStreamerSink, "ProcessData() invoked")
126
127    PortTypes::ShortSequence I_out, Q_out;
128
129    PortTypes::ShortSequence *I_in(NULL), *Q_in(NULL);
130    CORBA::UShort I_in_length, Q_in_length;
131
132    // bookkeeping variables
133    unsigned int i;
134    float I1, Q1, I2, Q2;
135    float q, q_hat;
136    float * ptr_i, * ptr_q;
137
138    // objects
139
140    // operational mode
141    enum {SEEK_HEADER, EXTRACT_HEADER, EXTRACT_FRAME} op_mode = SEEK_HEADER;
142
143    // AGC
144    float Ac = 4e3;
145    sigprocc_agc agc;
146    sigprocc_agc_init(&agc);
147    sigprocc_agc_set_target(&agc, 1.0f);
148    sigprocc_agc_set_bandwidth(&agc, 0.0f);
149
150    // Costas loop filter, NCO
151    sigprocc_lf2 costas_loop_filter;
152    sigprocc_lf2_init(&costas_loop_filter);
153    sigprocc_lf2_set_bandwidth(&costas_loop_filter, 1e-3f);
154
155    sigprocc_nco costas_loop_nco;
156    sigprocc_nco_set_frequency(&costas_loop_nco, 0.0f);
157    sigprocc_nco_set_phase(&costas_loop_nco, 0.0f);
158
159    // Symbol timing recovery, MF, dMF, buffers
160    bool enable_symbol_output = false;
161    enum {SHIFT, SKIP, STUFF} loop_control = SHIFT;
162    unsigned int k=4;
163    unsigned int m=5;
164    float beta = 0.35f;
165    unsigned int Npfb=16;
166    polyphase_filterbank* mf = polyphase_filterbank_create_prototype(
167        PULSE_SHAPE_RRCOS, false, k, m, beta, Npfb);
168    polyphase_filterbank* dmf = polyphase_filterbank_create_prototype(
169        PULSE_SHAPE_RRCOS, true, k, m, beta, Npfb);
170    unsigned int h_len = 2*k*m;
171    circular_buffer * buf_i = circular_buffer_create(h_len);
172    circular_buffer * buf_q = circular_buffer_create(h_len);
173    float mf_i;     // matched filter input (in-phase)
174    float mf_q;     // matched filter output (quadrature)
175    float dmf_i;    // derivative MF output (in-phase)
176    float dmf_q;    // derivative MF output (quadrature)
177    int b=0;   // filter bank select
178    unsigned int v=0;
179    float b_soft = (float) b;
180    float q_s(0.0f), q_hat_s(0.0f), q_prime_s(0.0f), buff2_s(0.0f);
181
182    float BT_s = 1e-2f;
183    float xi = 1.0f/sqrtf(2.0f);
184    float beta_s = 2*BT_s/(xi+1.0f/(4.0f*xi));
185    float alpha_s = 2*xi*beta_s;
186
187    // Phase recovery modem, filter, NCO
188    modem * frame_sync_demod = modem_create(MOD_BPSK, 1);
189
190    sigprocc_lf2 pll_filter;
191    sigprocc_lf2_init(&pll_filter);
192    sigprocc_lf2_set_bandwidth(&pll_filter, 1e-4f);
193
194    sigprocc_nco pll_nco;
195    sigprocc_nco_set_frequency(&pll_nco, 0.0f);
196    sigprocc_nco_set_phase(&pll_nco, 0.0f);
197
198    // Frame header PN sequence
199    unsigned int s;
200    msequence frame_sync_code = msequence_default[8];
201    binary_sequence * frame_sync_code_sequence;
202    frame_sync_code_sequence = binary_sequence_create(frame_sync_code.n);
203    binary_sequence_init_msequence(frame_sync_code_sequence, &frame_sync_code);
204
205    int frame_sync_code_threshold = frame_sync_code.n / 2;
206
207    binary_sequence * frame_header_buffer;
208    frame_header_buffer = binary_sequence_create(frame_sync_code.n);
209    int rxy;
210
211    // Frame data
212    unsigned int num_symbols_extracted = 0;
213    unsigned int frame_length = 256;
214    modem * demodulator = modem_create(MOD_PSK, 2);
215    //modem * demodulator = modem_create(MOD_ARB, 6);
216    //modem_arb_init_file(demodulator, "/home/jgaeddert/src/sigprocc/img/modem_arb_vt.dat");
217
218    unsigned int num_bit_errors = 0;
219    unsigned int s_gen;
220    msequence data_source = msequence_default[12];
221
222    while (true) {
223        dataIn->getData(I_in, Q_in);
224
225        I_in_length = I_in->length();
226        Q_in_length = Q_in->length();
227
228        // define length of output
229        I_out.length(frame_length);
230        Q_out.length(frame_length);
231
232        //printf("DataStreamerSink got %u samples\n", I_in_length);
233
234        for (i=0; i<I_in_length; i++) {
235            I1 = (float) (*I_in)[i];
236            Q1 = (float) (*Q_in)[i];
237
238            I1 /= Ac;
239            Q1 /= Ac;
240
241            //------------------------------------------------
242            // AGC
243            //------------------------------------------------
244#if 1
245            sigprocc_agc_apply_gain(&agc, I1, Q1, &I2, &Q2);
246#else
247            I2 = I1;
248            Q2 = Q1;
249#endif
250
251#if 1
252            // Costas Loop (BPSK header)
253            sigprocc_nco_mix_down(&costas_loop_nco, I2, Q2, &I1, &Q1);
254            if (op_mode != EXTRACT_FRAME) {
255                // demodulate
256                demodulate(frame_sync_demod, I1, Q1, &s);
257
258                // compute phase error
259                get_demodulator_phase_error(frame_sync_demod, &q);
260
261                // filter phase error
262                sigprocc_lf2_advance(&costas_loop_filter, q, &q_hat);
263
264                // adjust NCO frequency
265                sigprocc_nco_set_frequency(&costas_loop_nco, q_hat);
266
267                // update NCO
268                sigprocc_nco_step(&costas_loop_nco);
269            }
270#else
271            I1 = I2;
272            Q1 = Q2;
273#endif
274            v++;
275            switch (loop_control) {
276            case SHIFT:
277                // push value onto buffer
278                circular_buffer_push_front(buf_i, I1);
279                circular_buffer_push_front(buf_q, Q1);
280                break;
281            case SKIP:
282                // 'skip' input sample
283                //printf("warning! loop control skip not yet tested!\n");
284                printf("skip\n");
285                circular_buffer_push_front(buf_i, I1);
286                circular_buffer_push_front(buf_q, Q1);
287                //i++;
288                v--;
289                loop_control = SHIFT;
290                continue;
291            case STUFF:
292                // 'stuff' sample; repeat
293                //printf("warning! loop control stuff not yet tested!\n");
294                printf("stuff\n");
295                i--;
296                //v++;
297                loop_control = SHIFT;
298                continue;
299            default:
300                printf("error: unknown loop control\n");
301                throw (0);
302            }
303
304            //------------------------------------------------
305            // Symbol timing recovery
306            //------------------------------------------------
307            //v++;
308            if (v>=k) {
309                enable_symbol_output = true;
310                v -= k;
311            } else {
312                enable_symbol_output = false;
313            }
314
315            if (enable_symbol_output) {
316                // compute (derivative) matched filter outputs
317                ptr_i = circular_buffer_getpointer(buf_i);
318                ptr_q = circular_buffer_getpointer(buf_q);
319                mf_i  = polyphase_filterbank_compute_output(mf, b, ptr_i) / k;
320                mf_q  = polyphase_filterbank_compute_output(mf, b, ptr_q) / k;
321                dmf_i = polyphase_filterbank_compute_output(dmf, b, ptr_i) / k;
322                dmf_q = polyphase_filterbank_compute_output(dmf, b, ptr_q) / k;
323
324                // symbol timing loop control
325                q_s = -( mf_i*dmf_i + mf_q*dmf_q ) / 2.0f;
326
327                q_prime_s = q_s*beta_s + buff2_s*0.95f;
328                q_hat_s = q_s*alpha_s + q_prime_s; // filtered error signal
329                buff2_s = q_prime_s;
330
331                //q_hat_s = beta_s*q_hat_s - alpha_s*(q_hat_s-q_s);
332                b_soft = b_soft - q_hat_s*(float)(Npfb);
333                b = (int) roundf(b_soft);
334                //printf("  q_s: %f, q_hat_s: %f\n", q_s, q_hat_s);
335
336                // shift/skip/stuff
337                if (b < 0) {
338                    while (b<0) {
339                        b += Npfb;
340                        b_soft += (float) Npfb;
341                    }
342                    loop_control = STUFF;
343                } else if (b >= Npfb) {
344                    while (b >= Npfb) {
345                        b -= Npfb;
346                        b_soft -= (float) Npfb;
347                    }
348                    loop_control = SKIP;
349                } else {
350                    loop_control = SHIFT;
351                }
352
353                switch (op_mode) {
354                    case SEEK_HEADER:
355
356                        if (mf_i > 0) binary_sequence_push(frame_header_buffer, 1);
357                        else          binary_sequence_push(frame_header_buffer, 0);
358
359                        // correlate with header pn sequence
360                        ///\todo figure out why this negative sign is necessary
361                        rxy = -binary_sequence_correlate(
362                                frame_header_buffer, frame_sync_code_sequence);
363
364                        if (abs(rxy) > frame_sync_code_threshold) {
365                            printf("HEADER FOUND! rxy=%f\n",
366                                    (float)rxy / (float)(frame_sync_code.n));
367                            printf("  mf_i: %f, mf_q: %f\n", mf_i, mf_q);
368                            printf("  filterbank index: %f: %u / %u\n", b_soft, b, Npfb);
369                            printf("  q_hat_s: %E\n", q_hat_s);
370
371                            op_mode = EXTRACT_HEADER;
372
373                            num_symbols_extracted = 0;
374                            num_bit_errors = 0;
375
376                            // reset data source generator
377                            msequence_reset(&data_source);
378
379                            // reset loop filter
380                            sigprocc_lf2_init(&pll_filter);
381                            sigprocc_lf2_set_bandwidth(&pll_filter, 1e-6f);
382
383                            if (rxy < 0) {
384                                printf("  >> INVERTED HEADER\n");
385                                // adjust costas_loop_nco by pi
386                                sigprocc_nco_adjust_phase(&costas_loop_nco, M_PI);
387                            }
388                        }
389
390                        break;
391                    case EXTRACT_HEADER:
392                        op_mode = EXTRACT_FRAME;
393                       
394                    case EXTRACT_FRAME:
395                        sigprocc_nco_mix_down(&pll_nco, mf_i, mf_q, &I1, &Q1);
396
397                        I_out[num_symbols_extracted] = (short) (I1 * 10000);
398                        Q_out[num_symbols_extracted] = (short) (Q1 * 10000);
399
400                        // demodulate and count bit errors
401                        demodulate(demodulator, I1, Q1, &s);
402                        s_gen = msequence_generate_symbol(&data_source, demodulator->m);
403                        num_bit_errors += count_bit_errors(s, s_gen);
404
405                        num_symbols_extracted++;
406
407                        // compute phase error
408                        get_demodulator_phase_error(demodulator, &q);
409
410                        // filter phase error
411                        sigprocc_lf2_advance(&pll_filter, q, &q_hat);
412
413                        // adjust NCO frequency
414                        sigprocc_nco_set_frequency(&pll_nco, q_hat);
415
416                        // update NCO
417                        sigprocc_nco_step(&pll_nco);
418
419                        if (num_symbols_extracted==frame_length) {
420                            printf("  FRAME EXTRACTED\n");
421                            op_mode = SEEK_HEADER;
422                            samplesOut->pushPacket(I_out, Q_out);
423                            // print bit errors:
424                            printf("  bit errors: %u / %u\n\n",
425                                num_bit_errors, frame_length*(demodulator->m));
426                        }
427                        break;
428                    default:
429                        printf("unexpected operational mode\n");
430                        throw (0);
431
432                } // op_mode switch
433
434            } // enable_output
435
436        } // loop through input samples
437
438        dataIn->bufferEmptied();
439    }
440
441    // free up the memory
442    free_binary_sequence(frame_sync_code_sequence);
443    free_binary_sequence(frame_header_buffer);
444    free_polyphase_filterbank(mf);
445    free_polyphase_filterbank(dmf);
446}
447
448
Note: See TracBrowser for help on using the browser.