root/experimental/components/SymbolSyncPoly/branches/SymbolSyncPoly-metadata/src/FrameSynchronizerDSP.cpp @ 5573

Revision 5573, 18.6 KB (checked in by jgaeddert, 6 years ago)

removing costas loop processing block from frame synchronizer; consumes too much processing; carrier offset estimation works MUCH better
  • Property svn:eol-style set to native
Line 
1/****************************************************************************
2
3Copyright 2007 Virginia Polytechnic Institute and State University
4
5This file is part of the OSSIE FrameSynchronizer.
6
7OSSIE FrameSynchronizer is free software; you can redistribute it and/or
8modify it 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 FrameSynchronizer is distributed in the hope that it will be
13useful, but 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 FrameSynchronizer; 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 <iostream>
25#include "FrameSynchronizerDSP.h"
26#include <math.h>
27
28// Default constructor
29FrameSynchronizerDSP::FrameSynchronizerDSP()
30{
31    inputBuffer.SetBufferSize( FRAME_SYNC_BUFFER_SIZE );
32    inputBuffer.Release();
33
34    // Load buffer with zeros
35    for (unsigned int i=0; i<FRAME_SYNC_BUFFER_SIZE; i++)
36        inputBuffer.Push( 0 );
37
38    operationalMode = EXTRACT_PN_FRAME_SYNC_CODE;
39    numControlCodesReceived = 0;
40
41    controlCode = new int[NUM_CONTROL_CODES];
42
43    r_frame_sync = 0;
44
45    UpdatePhaseLoopFilterCoefficients(0.0025f);
46    UpdateTimingLoopFilterCoefficients(ALPHA_T_HI, BETA_T_HI);
47
48    e_i = 0;
49    e_q = Ac;
50    lock_zeta = 0.02;
51    header_phase_locked = false;
52    header_phase_lock_delay = 64;
53    header_phase_lock_delay_counter = 0;
54
55#ifdef FS_LOGGING
56    input_log.open("framesync_input_log.dat");
57#endif
58}
59
60// Destructor
61FrameSynchronizerDSP::~FrameSynchronizerDSP()
62{
63    delete [] controlCode;
64
65#ifdef FS_LOGGING
66    input_log.close();
67#endif
68}
69
70// Find frame header using conventional method
71bool FrameSynchronizerDSP::FindFrameHeader(
72    short * I_in,
73    short * Q_in,
74    unsigned int input_length,
75    unsigned int &num_read,
76    short * I_out,
77    short * Q_out,
78    unsigned int output_length,
79    unsigned int &num_written)
80{
81    unsigned int i(0);
82    num_read = 0;
83    num_written = 0;
84    alpha_c = 0.04f;
85    beta_c = 0.0028284f;
86    alpha_s = 0.027f;
87    beta_s = 1.024f;
88
89    while ( i<input_length ) {
90        // Increment sample counter for output enable
91        v++;
92
93        PushInputs( I_in, Q_in, i, input_length );
94
95        if ( v >= (signed int) k ) {
96            // Enable output
97            v -= k;
98
99            // Decimate values; compute filter outputs mf_i, mf_q, dmf_i, dmf_q
100            ComputeFilterBankOutputs();
101
102            I_out[num_written] = mf_i;
103            Q_out[num_written] = mf_q;
104            num_written++;
105
106            // push sample into buffer and correlate with frame sync code
107            inputBuffer.Push(mf_i);
108            r = CorrelateSequence( pnFrameSyncCode, 255 );
109
110            if ( abs(r) > int( Ac/2 ) ) {
111                r_frame_sync = abs(r);
112                std::cout << "PN FRAME SYNC CODE FOUND!!!" << std::endl;
113                std::cout << "  r = " << r_frame_sync << std::endl;
114
115                header_phase_lock_delay_counter = 0;
116                // check for phase inversion; if so inject pi radians into NCO
117                // [not sure how well this will work with band-limited pulse shapes]
118                if ( r<0 ) {
119                    std::cout << "  [Frame sync found negative PN code]" << std::endl;
120                    nco_control += PI;
121                }
122
123                operationalMode = EXTRACT_CONTROL_CODES;
124                inputBuffer.Release();
125                numControlCodesReceived = 0;
126
127                num_read = i;
128                return true;
129            }
130
131            // generate timing error
132            GenerateTimingError();
133
134            // advance timing loop filter
135            AdvanceTimingLoopFilter();
136
137            // compute filter bank index
138            ComputeFilterBankIndex();
139
140
141            // Update lock detection
142            e_i = lock_zeta*(float)(abs(mf_i)) + (1-lock_zeta)*e_i;
143            e_q = lock_zeta*(float)(abs(mf_q)) + (1-lock_zeta)*e_q;
144            header_phase_lock_delay_counter++;
145            if ( header_phase_lock_delay_counter >= header_phase_lock_delay ) {
146                // Calculate lock detection trigger
147                lock_detect = e_i/e_q;
148                header_phase_lock_delay_counter = 0;
149
150                if ( lock_detect < 1.25f ) {
151                    // Not locked to BPSK header (quadrature signal energy is too large)
152                    //   => estimate carrier offset and apply to NCO
153                    //printf("estimating offset...\n");
154                    //printf("  lock_detect = %f\n", lock_detect);
155                   
156                    unsigned int N_est = (input_length < 64) ? input_length : 64;
157                    wcTs = 2*PI*EstimateCarrierOffset(I_in, Q_in, N_est, false);
158                    //printf("  estimate: %f\n", wcTs/(2*PI));
159                    cv = 0;
160                    nco_control = 0;
161                    q_c = 0;
162                    q_hat_c = 0;
163                    buff2_c = 0;
164                    q_prime_c = 0;
165                    e_i = Ac/2;
166                    e_q = Ac/2;
167                }
168            } else {
169                // Calculate phase error
170
171                q_c = GeneratePhaseError( mf_i, mf_q );
172                q_c /= Ac;
173            }
174
175            AdvancePhaseLoopFilter();
176
177        }
178        UpdateNCO();
179
180    }
181
182    num_read = input_length;
183    return false;
184}
185
186
187// Extract frame header
188bool FrameSynchronizerDSP::ExtractFrameHeader(
189    short * I_in,
190    short * Q_in,
191    unsigned int input_length,
192    unsigned int &num_read,
193    short * I_out,
194    short * Q_out,
195    unsigned int output_length,
196    unsigned int &num_written)
197{
198    unsigned int i(0);
199    num_read = 0;
200    num_written = 0;
201    //alpha_c = 0.04f;
202    //beta_c = 0.0028284f;
203
204    while ( i<input_length ) {
205        // Increment sample counter for output enable
206        v++;
207
208        PushInputs( I_in, Q_in, i, input_length );
209
210        if ( v >= (signed int) k ) {
211            // Enable output
212            v -= k;
213
214            // Decimate values; compute filter outputs mf_i, mf_q, dmf_i, dmf_q
215            ComputeFilterBankOutputs();
216
217            //I_out[num_written] = mf_i;
218            //Q_out[num_written] = mf_q;
219            //num_written++;
220
221            // Push input into buffer
222            inputBuffer.Push( mf_i );
223
224            if ( inputBuffer.GetNumElements() == 31 ) {
225                // Correlate input sequence with pnControlCode
226                r = CorrelateSequence( pnControlCode, 31 );
227                //printf("  code %d, rxy = %d\n", numControlCodesReceived, r);
228
229                // Store result into array
230                controlCode[numControlCodesReceived++] = r;
231
232                // Release buffer for next sequence
233                inputBuffer.Release();
234
235                if ( numControlCodesReceived == NUM_CONTROL_CODES ) {
236                    printf("control codes extracted!\n");
237                    numFrameSymbolsReceived = 0;
238
239                    operationalMode = DECODE_FRAME_HEADER;
240                    num_read = i;
241                    return true;
242                }
243            }
244
245            // generate timing error
246            GenerateTimingError();
247
248            // advance timing loop filter
249            AdvanceTimingLoopFilter();
250
251            // compute filter bank index
252            ComputeFilterBankIndex();
253
254            q_c = GeneratePhaseError( mf_i, mf_q );
255            q_c /= Ac;
256
257            AdvancePhaseLoopFilter();
258        }
259
260        UpdateNCO();
261
262    }
263
264    num_read = input_length;
265    return false;
266}
267
268
269bool FrameSynchronizerDSP::DecodeFrameHeader()
270{
271    // switch modulation type
272    unsigned int ms = DecodeControlSequence(0,2);
273    switch (ms) {
274    case (0):
275        SetModulationScheme( SigProc::BPSK );
276        break;
277    case (1):
278        SetModulationScheme( SigProc::QPSK );
279        break;
280    case (2):
281        SetModulationScheme( SigProc::PSK8 );
282        break;
283    case (3):
284        SetModulationScheme( SigProc::QAM16 );
285        break;
286    case (4):
287        SetModulationScheme( SigProc::PAM4 );
288        break;
289    default:;
290    }
291
292    // specify frame size
293    unsigned int fs = DecodeControlSequence(3,4);
294    switch ( fs ) {
295    case 0:
296        frameSize = FRAME_SIZE_1;
297        break;
298    case 1:
299        frameSize = FRAME_SIZE_2;
300        break;
301    case 2:
302        frameSize = FRAME_SIZE_3;
303        break;
304    case 3:
305        frameSize = FRAME_SIZE_4;
306        break;
307    }
308
309
310    std::cout << "FRAME HEADER EXTRACTED (mod: " << ms
311              << ", frame size: " << frameSize << ") : r="
312              << r_frame_sync << std::endl;
313    std::cout << "  control : ";
314    for (unsigned int ii=0; ii<NUM_CONTROL_CODES; ii++)
315        std::cout << controlCode[ii] << " ";
316    std::cout << std::endl;
317   
318    operationalMode = EXTRACT_FRAME;
319
320    return true;
321}
322
323
324// Extract frame symbols
325bool FrameSynchronizerDSP::ExtractFrameSymbols(
326    short * I_in,
327    short * Q_in,
328    unsigned int input_length,
329    unsigned int &num_read,
330    short * I_out,
331    short * Q_out,
332    unsigned int output_length,
333    unsigned int &num_written)
334{
335    return ExtractFrameSymbolsFixedLength(
336        I_in, Q_in, input_length, num_read,
337        I_out, Q_out, output_length, num_written);
338}
339
340// Extract frame symbols
341bool FrameSynchronizerDSP::ExtractFrameSymbolsFixedLength(
342    short * I_in,
343    short * Q_in,
344    unsigned int input_length,
345    unsigned int &num_read,
346    short * I_out,
347    short * Q_out,
348    unsigned int output_length,
349    unsigned int &num_written)
350{
351    unsigned int i(0);
352    num_read = 0;
353    num_written = 0;
354    // reduce bandwidth of carrier loop filter
355    //alpha_c = 0.002f;
356    //beta_c =  0.000005f;
357
358    while ( i<input_length ) {
359        // Increment sample counter for output enable
360        v++;
361
362        PushInputs( I_in, Q_in, i, input_length );
363
364        if ( v >= (signed int) k ) {
365            // Enable output
366            v -= k;
367
368            // Decimate values; compute filter outputs mf_i, mf_q, dmf_i, dmf_q
369            ComputeFilterBankOutputs();
370
371            I_out[num_written] = mf_i;
372            Q_out[num_written] = -mf_q;
373
374            num_written++;
375            numFrameSymbolsReceived++;
376
377            if ( numFrameSymbolsReceived == frameSize+512 ) {
378                // Frame extracted
379                std::cout << "FRAME EXTRACTED" << std::endl;
380
381                // switch mode
382                SetModulationScheme( SigProc::BPSK );
383                //operationalMode = EXTRACT_EOM_CODE;
384                operationalMode = EXTRACT_PN_FRAME_SYNC_CODE;
385                num_read = i;
386                return true;
387            }
388
389            // generate timing error
390            GenerateTimingError();
391
392            // advance timing loop filter
393            AdvanceTimingLoopFilter();
394
395            // compute filter bank index
396            ComputeFilterBankIndex();
397
398            q_c = GeneratePhaseError( mf_i, mf_q );
399            q_c /= Ac;
400
401            AdvancePhaseLoopFilter();
402
403        }
404        UpdateNCO();
405
406    }
407
408    num_read = input_length;
409    return false;
410}
411
412
413
414
415// Main signal processing loop
416void FrameSynchronizerDSP::SynchronizeAndDecimate(short * I_in,
417        short * Q_in,
418        unsigned int N_in,
419        short * I_out,
420        short * Q_out,
421        unsigned int & N_out)
422{
423    if ( !isConfigured ) {
424        std::cerr << "ERROR: SymbolSyncPolyDSP::SynchronizeAndDecimate() :" << std::endl
425                  << "  => must call ConfigureFilterBank() before invoking this method" << std::endl;
426        throw 0;
427    }
428
429    unsigned int i(0);
430    unsigned int N_out_max(N_out);
431    N_out = 0;
432
433    while ( i<N_in ) {
434        // Increment sample counter for output enable
435        v++;
436
437        PushInputs( I_in, Q_in, i, N_in );
438
439        if ( v >= (signed int) k ) {
440            // Enable output
441            v -= k;
442
443            // Decimate values; compute filter outputs mf_i, mf_q, dmf_i, dmf_q
444            ComputeFilterBankOutputs();
445#ifdef FS_LOGGING
446            input_log << mf_i << "  " << mf_q << std::endl;
447#endif
448
449            //
450            switch ( operationalMode ) {
451            case EXTRACT_PN_FRAME_SYNC_CODE:
452                // correlate to find pnFrameSyncCode
453                inputBuffer.Push( mf_i );
454                r = CorrelateSequence( pnFrameSyncCode, 255 );
455
456                if ( abs(r) > int( Ac/2 ) ) {
457                    r_frame_sync = abs(r);
458                    //std::cout << "PN FRAME SYNC CODE FOUND!!!" << std::endl;
459
460                    // check for phase inversion; if so inject pi radians into NCO
461                    // [not sure how well this will work with band-limited pulse shapes]
462                    if ( r<0 ) {
463                        std::cout << "  [Frame sync found negative PN code]" << std::endl;
464                        nco_control += PI;
465                    }
466
467                    operationalMode = EXTRACT_CONTROL_CODES;
468                    inputBuffer.Release();
469                    numControlCodesReceived = 0;
470
471                    // reduce bandwidth of loop filters for steady tracking mode
472                    UpdatePhaseLoopFilterCoefficients(0.0025f);
473                    //UpdateTimingLoopFilterCoefficients(ALPHA_T_LO, BETA_T_LO);
474
475                } else {
476                    ///\todo Estimate timing/carrier information if signal is not found
477                    /// within a certain amount of time
478                }
479
480                break;
481
482            case EXTRACT_CONTROL_CODES:
483
484                // Push input into buffer
485                inputBuffer.Push( mf_i );
486
487                if ( inputBuffer.GetNumElements() == 31 ) {
488                    // Correlate input sequence with pnControlCode
489                    r = CorrelateSequence( pnControlCode, 31 );
490
491                    // Store result into array
492                    controlCode[numControlCodesReceived++] = r;
493
494                    // Release buffer for next sequence
495                    inputBuffer.Release();
496
497                    if ( numControlCodesReceived == NUM_CONTROL_CODES ) {
498                        // switch modulation type
499                        unsigned int ms = DecodeControlSequence(0,2);
500                        switch (ms) {
501                        case (0):
502                            SetModulationScheme( SigProc::BPSK );
503                            break;
504                        case (1):
505                            SetModulationScheme( SigProc::QPSK );
506                            break;
507                        case (2):
508                            SetModulationScheme( SigProc::PSK8 );
509                            break;
510                        case (3):
511                            SetModulationScheme( SigProc::QAM16 );
512                            break;
513                        case (4):
514                            SetModulationScheme( SigProc::PAM4 );
515                            break;
516                        default:;
517                        }
518   
519                        // specify frame size
520                        unsigned int fs = DecodeControlSequence(3,4);
521                        switch ( fs ) {
522                        case 0:
523                            frameSize = FRAME_SIZE_1;
524                            break;
525                        case 1:
526                            frameSize = FRAME_SIZE_2;
527                            break;
528                        case 2:
529                            frameSize = FRAME_SIZE_3;
530                            break;
531                        case 3:
532                            frameSize = FRAME_SIZE_4;
533                            break;
534                        }
535                        numFrameSymbolsReceived = 0;
536   
537                        std::cout << "FRAME HEADER EXTRACTED (mod: " << ms
538                                  << ", frame size: " << frameSize << ") : r="
539                                  << r_frame_sync << std::endl;
540                        std::cout << "  control : ";
541                        for (unsigned int ii=0; ii<NUM_CONTROL_CODES; ii++)
542                            std::cout << controlCode[ii] << " ";
543                        std::cout << std::endl;
544   
545                        operationalMode = EXTRACT_FRAME;
546                    }
547                }
548                break;
549
550            case EXTRACT_FRAME:
551                // Store result in output buffer
552                I_out[N_out] = mf_i;
553                Q_out[N_out] = -mf_q;
554
555                // Throw exception if buffer is too small
556                if ( N_out == N_out_max ) {
557                    std::cerr << "ERROR: FrameSynchronizerDSP::SynchronizeAndDecimate() :"
558                              << std::endl << "  => output array is full " << std::endl;
559                    throw 0;
560                } else {
561                    N_out++;
562                    numFrameSymbolsReceived++;
563
564                    if ( numFrameSymbolsReceived == frameSize ) {
565                        // Frame extracted
566                        std::cout << "FRAME EXTRACTED" << std::endl;
567
568                        // switch mode
569                        SetModulationScheme( SigProc::BPSK );
570                        operationalMode = EXTRACT_EOM_CODE;
571                    }
572                }
573                break;
574            case EXTRACT_EOM_CODE:
575                ///\todo extract EOM code
576
577                operationalMode = EXTRACT_PN_FRAME_SYNC_CODE;
578
579                // increase bandwidth for acquisition
580                //UpdatePhaseLoopFilterCoefficients(0.05);
581                //UpdateTimingLoopFilterCoefficients(ALPHA_T_HI, BETA_T_HI);
582                break;
583            default:;
584            }
585
586            GenerateTimingError();
587
588            AdvanceTimingLoopFilter();
589
590            ComputeFilterBankIndex();
591
592            // Calculate phase error
593
594            q_c = GeneratePhaseError( mf_i, mf_q );
595            q_c /= Ac;
596
597            AdvancePhaseLoopFilter();
598
599        }
600
601        UpdateNCO();
602
603    } // while
604
605}
606
607
608// Correlate input sequence with PN code
609int FrameSynchronizerDSP::CorrelateSequence( char * _pncode, unsigned int _N )
610{
611    int rxy(0);
612
613    for (unsigned int i=0; i<_N; i++)
614        rxy += ( _pncode[i] ) ? inputBuffer[i] : -inputBuffer[i];
615
616    // Scale by length
617    rxy /= int(_N);
618
619    return rxy;
620}
621
622unsigned int FrameSynchronizerDSP::DecodeControlSequence(
623        unsigned int _i1,
624        unsigned int _i2 )
625{
626    if ( _i1 > _i2 ) {
627        std::cerr << "ERROR: FrameSyncrhonizerDSP:::DecodeControlSequence "
628                  << "_i1 > _i2 (" << _i1 << ">" << _i2 << ")" << std::endl;
629        throw 0;
630    }
631
632    unsigned int val(0), tmp, len(_i2 - _i1);
633    for (unsigned int i=0; i<=len; i++) {
634        tmp = ( controlCode[i+_i1] > 0 ) ? 1 : 0;
635        tmp <<= len-i;
636        val += tmp;
637    }
638    return val;
639}
640
641
Note: See TracBrowser for help on using the browser.