git.gag.com Git - debian/gnuradio/blob - gnuradio-core/src/lib/general/gr_ofdm_frame_sink.cc

   1 /* -*- c++ -*- */
   2 /*
   3  * Copyright 2007 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 <gr_ofdm_frame_sink.h>
  28 #include <gr_io_signature.h>
  29 #include <gr_expj.h>
  30 #include <gr_math.h>
  31 #include <math.h>
  32 #include <cstdio>
  33 #include <stdexcept>
  34 #include <iostream>
  35
  36 #define VERBOSE 0
  37
  38 inline void
  39 gr_ofdm_frame_sink::enter_search()
  40 {
  41   if (VERBOSE)
  42     fprintf(stderr, "@ enter_search\n");
  43
  44   d_state = STATE_SYNC_SEARCH;
  45
  46 }
  47
  48 inline void
  49 gr_ofdm_frame_sink::enter_have_sync()
  50 {
  51   if (VERBOSE)
  52     fprintf(stderr, "@ enter_have_sync\n");
  53
  54   d_state = STATE_HAVE_SYNC;
  55
  56   // clear state of demapper
  57   d_byte_offset = 0;
  58   d_partial_byte = 0;
  59
  60   d_header = 0;
  61   d_headerbytelen_cnt = 0;
  62
  63   // Resetting PLL
  64   d_freq = 0.0;
  65   d_phase = 0.0;
  66   fill(d_dfe.begin(), d_dfe.end(), gr_complex(1.0,0.0));
  67 }
  68
  69 inline void
  70 gr_ofdm_frame_sink::enter_have_header()
  71 {
  72   d_state = STATE_HAVE_HEADER;
  73
  74   // header consists of two 16-bit shorts in network byte order
  75   // payload length is lower 12 bits
  76   // whitener offset is upper 4 bits
  77   d_packetlen = (d_header >> 16) & 0x0fff;
  78   d_packet_whitener_offset = (d_header >> 28) & 0x000f;
  79   d_packetlen_cnt = 0;
  80
  81   if (VERBOSE)
  82     fprintf(stderr, "@ enter_have_header (payload_len = %d) (offset = %d)\n",
  83             d_packetlen, d_packet_whitener_offset);
  84 }
  85
  86
  87 unsigned char gr_ofdm_frame_sink::slicer(const gr_complex x)
  88 {
  89   unsigned int table_size = d_sym_value_out.size();
  90   unsigned int min_index = 0;
  91   float min_euclid_dist = norm(x - d_sym_position[0]);
  92   float euclid_dist = 0;
  93
  94   for (unsigned int j = 1; j < table_size; j++){
  95     euclid_dist = norm(x - d_sym_position[j]);
  96     if (euclid_dist < min_euclid_dist){
  97       min_euclid_dist = euclid_dist;
  98       min_index = j;
  99     }
 100   }
 101   return d_sym_value_out[min_index];
 102 }
 103
 104 unsigned int gr_ofdm_frame_sink::demapper(const gr_complex *in,
 105                                           unsigned char *out)
 106 {
 107   unsigned int i=0, bytes_produced=0;
 108   gr_complex carrier;
 109
 110   carrier=gr_expj(d_phase);
 111
 112   gr_complex accum_error = 0.0;
 113   while(i < d_occupied_carriers) {
 114     if(d_nresid > 0) {
 115       d_partial_byte |= d_resid;
 116       d_byte_offset += d_nresid;
 117       d_nresid = 0;
 118       d_resid = 0;
 119     }
 120
 121     while((d_byte_offset < 8) && (i < d_occupied_carriers)) {
 122       gr_complex sigrot = in[i]*carrier*d_dfe[i];
 123
 124       if(d_derotated_output != NULL){
 125         d_derotated_output[i] = sigrot;
 126       }
 127
 128       unsigned char bits = slicer(sigrot);
 129
 130       gr_complex closest_sym = d_sym_position[bits];
 131
 132       accum_error += sigrot * conj(closest_sym);
 133
 134       // FIX THE FOLLOWING STATEMENT
 135       if (norm(sigrot)> 0.001) d_dfe[i] +=  d_eq_gain*(closest_sym/sigrot-d_dfe[i]);
 136
 137       i++;
 138
 139       if((8 - d_byte_offset) >= d_nbits) {
 140         d_partial_byte |= bits << (d_byte_offset);
 141         d_byte_offset += d_nbits;
 142       }
 143       else {
 144         d_nresid = d_nbits-(8-d_byte_offset);
 145         int mask = ((1<<(8-d_byte_offset))-1);
 146         d_partial_byte |= (bits & mask) << d_byte_offset;
 147         d_resid = bits >> (8-d_byte_offset);
 148         d_byte_offset += (d_nbits - d_nresid);
 149       }
 150       //printf("demod symbol: %.4f + j%.4f   bits: %x   partial_byte: %x   byte_offset: %d   resid: %x   nresid: %d\n",
 151       //     in[i-1].real(), in[i-1].imag(), bits, d_partial_byte, d_byte_offset, d_resid, d_nresid);
 152     }
 153
 154     if(d_byte_offset == 8) {
 155       //printf("demod byte: %x \n\n", d_partial_byte);
 156       out[bytes_produced++] = d_partial_byte;
 157       d_byte_offset = 0;
 158       d_partial_byte = 0;
 159     }
 160   }
 161   //std::cerr << "accum_error " << accum_error << std::endl;
 162
 163   float angle = arg(accum_error);
 164
 165   d_freq = d_freq - d_freq_gain*angle;
 166   d_phase = d_phase + d_freq - d_phase_gain*angle;
 167   if (d_phase >= 2*M_PI) d_phase -= 2*M_PI;
 168   if (d_phase <0) d_phase += 2*M_PI;
 169
 170   //if(VERBOSE)
 171   //  std::cerr << angle << "\t" << d_freq << "\t" << d_phase << "\t" << std::endl;
 172
 173   return bytes_produced;
 174 }
 175
 176
 177 gr_ofdm_frame_sink_sptr
 178 gr_make_ofdm_frame_sink(const std::vector<gr_complex> &sym_position,
 179                         const std::vector<unsigned char> &sym_value_out,
 180                         gr_msg_queue_sptr target_queue, unsigned int occupied_carriers,
 181                         float phase_gain, float freq_gain)
 182 {
 183   return gr_ofdm_frame_sink_sptr(new gr_ofdm_frame_sink(sym_position, sym_value_out,
 184                                                         target_queue, occupied_carriers,
 185                                                         phase_gain, freq_gain));
 186 }
 187
 188
 189 gr_ofdm_frame_sink::gr_ofdm_frame_sink(const std::vector<gr_complex> &sym_position,
 190                                        const std::vector<unsigned char> &sym_value_out,
 191                                        gr_msg_queue_sptr target_queue, unsigned int occupied_carriers,
 192                                        float phase_gain, float freq_gain)
 193   : gr_sync_block ("ofdm_frame_sink",
 194                    gr_make_io_signature2 (2, 2, sizeof(gr_complex)*occupied_carriers, sizeof(char)),
 195                    gr_make_io_signature (1, 1, sizeof(gr_complex)*occupied_carriers)),
 196     d_target_queue(target_queue), d_occupied_carriers(occupied_carriers),
 197     d_byte_offset(0), d_partial_byte(0),
 198     d_resid(0), d_nresid(0),d_phase(0),d_freq(0),d_phase_gain(phase_gain),d_freq_gain(freq_gain),
 199     d_eq_gain(0.05)
 200 {
 201   d_bytes_out = new unsigned char[d_occupied_carriers];
 202   d_dfe.resize(occupied_carriers);
 203   fill(d_dfe.begin(), d_dfe.end(), gr_complex(1.0,0.0));
 204
 205   set_sym_value_out(sym_position, sym_value_out);
 206
 207   enter_search();
 208 }
 209
 210 gr_ofdm_frame_sink::~gr_ofdm_frame_sink ()
 211 {
 212   delete [] d_bytes_out;
 213 }
 214
 215 bool
 216 gr_ofdm_frame_sink::set_sym_value_out(const std::vector<gr_complex> &sym_position,
 217                                       const std::vector<unsigned char> &sym_value_out)
 218 {
 219   if (sym_position.size() != sym_value_out.size())
 220     return false;
 221
 222   if (sym_position.size()<1)
 223     return false;
 224
 225   d_sym_position  = sym_position;
 226   d_sym_value_out = sym_value_out;
 227   d_nbits = (unsigned long)ceil(log10(d_sym_value_out.size()) / log10(2.0));
 228
 229   return true;
 230 }
 231
 232
 233 int
 234 gr_ofdm_frame_sink::work (int noutput_items,
 235                           gr_vector_const_void_star &input_items,
 236                           gr_vector_void_star &output_items)
 237 {
 238   const gr_complex *in = (const gr_complex *) input_items[0];
 239   const char *sig = (const char *) input_items[1];
 240   unsigned int j = 0;
 241   unsigned int bytes=0;
 242
 243   // If the output is connected, send it the derotated symbols
 244   if(output_items.size() >= 1)
 245     d_derotated_output = (gr_complex *)output_items[0];
 246   else
 247     d_derotated_output = NULL;
 248
 249   if (VERBOSE)
 250     fprintf(stderr,">>> Entering state machine\n");
 251
 252   switch(d_state) {
 253
 254   case STATE_SYNC_SEARCH:    // Look for flag indicating beginning of pkt
 255     if (VERBOSE)
 256       fprintf(stderr,"SYNC Search, noutput=%d\n", noutput_items);
 257
 258     if (sig[0]) {  // Found it, set up for header decode
 259       enter_have_sync();
 260     }
 261     break;
 262
 263   case STATE_HAVE_SYNC:
 264     // only demod after getting the preamble signal; otherwise, the
 265     // equalizer taps will screw with the PLL performance
 266     bytes = demapper(&in[0], d_bytes_out);
 267
 268     if (VERBOSE) {
 269       if(sig[0])
 270         printf("ERROR -- Found SYNC in HAVE_SYNC\n");
 271       fprintf(stderr,"Header Search bitcnt=%d, header=0x%08x\n",
 272               d_headerbytelen_cnt, d_header);
 273     }
 274
 275     j = 0;
 276     while(j < bytes) {
 277       d_header = (d_header << 8) | (d_bytes_out[j] & 0xFF);
 278       j++;
 279
 280       if (++d_headerbytelen_cnt == HEADERBYTELEN) {
 281
 282         if (VERBOSE)
 283           fprintf(stderr, "got header: 0x%08x\n", d_header);
 284
 285         // we have a full header, check to see if it has been received properly
 286         if (header_ok()){
 287           enter_have_header();
 288
 289           if (VERBOSE)
 290             printf("\nPacket Length: %d\n", d_packetlen);
 291
 292           while((j < bytes) && (d_packetlen_cnt < d_packetlen)) {
 293             d_packet[d_packetlen_cnt++] = d_bytes_out[j++];
 294           }
 295
 296           if(d_packetlen_cnt == d_packetlen) {
 297             gr_message_sptr msg =
 298               gr_make_message(0, d_packet_whitener_offset, 0, d_packetlen);
 299             memcpy(msg->msg(), d_packet, d_packetlen_cnt);
 300             d_target_queue->insert_tail(msg);           // send it
 301             msg.reset();                                // free it up
 302
 303             enter_search();
 304           }
 305         }
 306         else {
 307           enter_search();                               // bad header
 308         }
 309       }
 310     }
 311     break;
 312
 313   case STATE_HAVE_HEADER:
 314     bytes = demapper(&in[0], d_bytes_out);
 315
 316     if (VERBOSE) {
 317       if(sig[0])
 318         printf("ERROR -- Found SYNC in HAVE_HEADER at %d, length of %d\n", d_packetlen_cnt, d_packetlen);
 319       fprintf(stderr,"Packet Build\n");
 320     }
 321
 322     j = 0;
 323     while(j < bytes) {
 324       d_packet[d_packetlen_cnt++] = d_bytes_out[j++];
 325
 326       if (d_packetlen_cnt == d_packetlen){              // packet is filled
 327         // build a message
 328         // NOTE: passing header field as arg1 is not scalable
 329         gr_message_sptr msg =
 330           gr_make_message(0, d_packet_whitener_offset, 0, d_packetlen_cnt);
 331         memcpy(msg->msg(), d_packet, d_packetlen_cnt);
 332
 333         d_target_queue->insert_tail(msg);               // send it
 334         msg.reset();                            // free it up
 335
 336         enter_search();
 337         break;
 338       }
 339     }
 340     break;
 341
 342   default:
 343     assert(0);
 344
 345   } // switch
 346
 347   return 1;
 348 }