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

Revision 7741, 12.3 KB (checked in by jgaeddert, 5 years ago)

adding PN sequence header, fleshing out data streamer sink component
  • 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    sigprocc_agc agc;
145    sigprocc_agc_init(&agc);
146    sigprocc_agc_set_target(&agc, 1.0f);
147    sigprocc_agc_set_bandwidth(&agc, 1e-3f);
148
149    // Costas loop filter, NCO
150    sigprocc_lf2 costas_loop_filter;
151    sigprocc_lf2_init(&costas_loop_filter);
152    sigprocc_lf2_set_bandwidth(&costas_loop_filter, 0.025f);
153
154    sigprocc_nco costas_loop_nco;
155    sigprocc_nco_set_frequency(&costas_loop_nco, 0.0f);
156    sigprocc_nco_set_phase(&costas_loop_nco, 0.0f);
157
158    // Symbol timing recovery, MF, dMF, buffers
159    bool enable_symbol_output = false;
160    unsigned int k=4;
161    unsigned int m=5;
162    float beta = 0.35f;
163    unsigned int Npfb=16;
164    polyphase_filterbank* mf = polyphase_filterbank_create_prototype(
165        PULSE_SHAPE_RRCOS, false, k, m, beta, Npfb);
166    polyphase_filterbank* dmf = polyphase_filterbank_create_prototype(
167        PULSE_SHAPE_RRCOS, true, k, m, beta, Npfb);
168    unsigned int h_len = 2*k*m;
169    circular_buffer * buf_i = circular_buffer_create(h_len);
170    circular_buffer * buf_q = circular_buffer_create(h_len);
171    float mf_i;     // matched filter input (in-phase)
172    float mf_q;     // matched filter output (quadrature)
173    float dmf_i;    // derivative MF output (in-phase)
174    float dmf_q;    // derivative MF output (quadrature)
175    unsigned int b=0;   // filter bank select
176    unsigned int v=0;
177
178    // Phase recovery modem, filter, NCO
179    modem * frame_sync_demod = modem_create(MOD_BPSK, 1);
180
181    sigprocc_lf2 pll_filter;
182    sigprocc_lf2_init(&pll_filter);
183    sigprocc_lf2_set_bandwidth(&pll_filter, 0.025f);
184
185    sigprocc_nco pll_nco;
186    sigprocc_nco_set_frequency(&pll_nco, 0.0f);
187    sigprocc_nco_set_phase(&pll_nco, 0.0f);
188
189    // Frame header PN sequence
190    unsigned int s;
191    msequence frame_sync_code = msequence_default[8];
192    binary_sequence * frame_sync_code_sequence;
193    frame_sync_code_sequence = binary_sequence_create(frame_sync_code.n);
194    binary_sequence_init_msequence(frame_sync_code_sequence, &frame_sync_code);
195
196    int frame_sync_code_threshold = frame_sync_code.n / 2;
197
198    binary_sequence * frame_header_buffer;
199    frame_header_buffer = binary_sequence_create(frame_sync_code.n);
200    int rxy;
201
202    // Frame data
203    unsigned int num_symbols_extracted = 0;
204    unsigned int frame_length = 256;
205    modem * demodulator = modem_create(MOD_QAM, 4);
206    unsigned int num_bit_errors = 0;
207    unsigned int s_gen;
208    msequence data_source = msequence_default[12];
209
210    while (true) {
211        dataIn->getData(I_in, Q_in);
212
213        I_in_length = I_in->length();
214        Q_in_length = Q_in->length();
215
216        // define length of output
217        I_out.length(frame_length);
218        Q_out.length(frame_length);
219
220        printf("DataStreamerSink got %u samples\n", I_in_length);
221
222        for (i=0; i<I_in_length; i++) {
223            I1 = (float) (*I_in)[i];
224            Q1 = (float) (*Q_in)[i];
225
226            I1 /= 4e3f;
227            Q1 /= 4e3f;
228
229            //------------------------------------------------
230            // AGC
231            //------------------------------------------------
232#if 1
233            sigprocc_agc_apply_gain(&agc, I1, Q1, &I2, &Q2);
234
235            // Costas Loop (BPSK header)
236            sigprocc_nco_mix_down(&costas_loop_nco, I2, Q2, &I1, &Q1);
237            if (op_mode != EXTRACT_FRAME) {
238                // demodulate
239                demodulate(frame_sync_demod, I1, Q1, &s);
240
241                // compute phase error
242                get_demodulator_phase_error(frame_sync_demod, &q);
243
244                // filter phase error
245                sigprocc_lf2_advance(&costas_loop_filter, q, &q_hat);
246
247                // adjust NCO frequency
248                sigprocc_nco_set_frequency(&costas_loop_nco, q_hat);
249
250                // update NCO
251                sigprocc_nco_step(&costas_loop_nco);
252            }
253#else
254            I2 = I1;
255            Q2 = Q1;
256#endif
257            // push values onto buffer
258            circular_buffer_push_front(buf_i, I2);
259            circular_buffer_push_front(buf_q, Q2);
260
261            //------------------------------------------------
262            // Symbol timing recovery
263            //------------------------------------------------
264            v++;
265            if (v==k) {
266                enable_symbol_output = true;
267                v=0;
268            } else {
269                enable_symbol_output = false;
270            }
271
272            if (enable_symbol_output) {
273                // compute (derivative) matched filter outputs
274                ptr_i = circular_buffer_getpointer(buf_i);
275                ptr_q = circular_buffer_getpointer(buf_q);
276                mf_i  = polyphase_filterbank_compute_output(mf, b, ptr_i) / k;
277                mf_q  = polyphase_filterbank_compute_output(mf, b, ptr_q) / k;
278                dmf_i = polyphase_filterbank_compute_output(dmf, b, ptr_i) / k;
279                dmf_q = polyphase_filterbank_compute_output(dmf, b, ptr_q) / k;
280
281                switch (op_mode) {
282                    case SEEK_HEADER:
283
284                        if (mf_i > 0) binary_sequence_push(frame_header_buffer, 1);
285                        else          binary_sequence_push(frame_header_buffer, 0);
286
287                        // correlate with header pn sequence
288                        rxy = binary_sequence_correlate(
289                                frame_header_buffer, frame_sync_code_sequence);
290
291                        if (abs(rxy) > frame_sync_code_threshold) {
292                            printf("HEADER FOUND! rxy=%f\n",
293                                    (float)rxy / (float)(frame_sync_code.n));
294                            printf("mf_i: %f, mf_q: %f\n", mf_i, mf_q);
295
296                            op_mode = EXTRACT_HEADER;
297
298                            num_symbols_extracted = 0;
299                            num_bit_errors = 0;
300
301                            // reset data source generator
302                            msequence_reset(&data_source);
303
304                            if (rxy < 0) {
305                                printf("  >> INVERTED HEADER\n");
306                                // todo: adjust costas_loop_nco by pi
307                            }
308                        }
309
310                        break;
311                    case EXTRACT_HEADER:
312                        op_mode = EXTRACT_FRAME;
313                       
314                    case EXTRACT_FRAME:
315                        I_out[num_symbols_extracted] = (short) (mf_i * 10000);
316                        Q_out[num_symbols_extracted] = (short) (mf_q * 10000);
317
318                        // demodulate and count bit errors
319                        demodulate(demodulator, mf_i, mf_q, &s);
320                        s_gen = msequence_generate_symbol(&data_source, demodulator->m);
321                        num_bit_errors += count_bit_errors(s, s_gen);
322
323                        num_symbols_extracted++;
324
325                        if (num_symbols_extracted==frame_length) {
326                            printf("  FRAME EXTRACTED\n");
327                            op_mode = SEEK_HEADER;
328                            samplesOut->pushPacket(I_out, Q_out);
329                            // print bit errors:
330                            printf("  bit errors: %u / %u\n\n",
331                                num_bit_errors, frame_length*(demodulator->m));
332                        }
333                        break;
334                    default:
335                        printf("unexpected operational mode\n");
336                        throw (0);
337                }
338
339                //------------------------------------------------
340                // Phase recovery (demodulation)
341                //------------------------------------------------
342
343                //------------------------------------------------
344                // Frame header detect, extract
345                //------------------------------------------------
346
347                //------------------------------------------------
348                // Count bit errors
349                //------------------------------------------------
350            }
351        }
352
353        dataIn->bufferEmptied();
354    }
355
356    // free up the memory
357    free_binary_sequence(frame_sync_code_sequence);
358    free_binary_sequence(frame_header_buffer);
359    free_polyphase_filterbank(mf);
360    free_polyphase_filterbank(dmf);
361}
362
363
Note: See TracBrowser for help on using the browser.