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

   1 /* -*- c++ -*- */
   2 /*
   3  * Copyright 2007,2008 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 #include <string.h>
  36
  37 #define VERBOSE 0
  38
  39 inline void
  40 gr_ofdm_frame_sink::enter_search()
  41 {
  42   if (VERBOSE)
  43     fprintf(stderr, "@ enter_search\n");
  44
  45   d_state = STATE_SYNC_SEARCH;
  46
  47 }
  48
  49 inline void
  50 gr_ofdm_frame_sink::enter_have_sync()
  51 {
  52   if (VERBOSE)
  53     fprintf(stderr, "@ enter_have_sync\n");
  54
  55   d_state = STATE_HAVE_SYNC;
  56
  57   // clear state of demapper
  58   d_byte_offset = 0;
  59   d_partial_byte = 0;
  60
  61   d_header = 0;
  62   d_headerbytelen_cnt = 0;
  63
  64   // Resetting PLL
  65   d_freq = 0.0;
  66   d_phase = 0.0;
  67   fill(d_dfe.begin(), d_dfe.end(), gr_complex(1.0,0.0));
  68 }
  69
  70 inline void
  71 gr_ofdm_frame_sink::enter_have_header()
  72 {
  73   d_state = STATE_HAVE_HEADER;
  74
  75   // header consists of two 16-bit shorts in network byte order
  76   // payload length is lower 12 bits
  77   // whitener offset is upper 4 bits
  78   d_packetlen = (d_header >> 16) & 0x0fff;
  79   d_packet_whitener_offset = (d_header >> 28) & 0x000f;
  80   d_packetlen_cnt = 0;
  81
  82   if (VERBOSE)
  83     fprintf(stderr, "@ enter_have_header (payload_len = %d) (offset = %d)\n",
  84             d_packetlen, d_packet_whitener_offset);
  85 }
  86
  87
  88 unsigned char gr_ofdm_frame_sink::slicer(const gr_complex x)
  89 {
  90   unsigned int table_size = d_sym_value_out.size();
  91   unsigned int min_index = 0;
  92   float min_euclid_dist = norm(x - d_sym_position[0]);
  93   float euclid_dist = 0;
  94
  95   for (unsigned int j = 1; j < table_size; j++){
  96     euclid_dist = norm(x - d_sym_position[j]);
  97     if (euclid_dist < min_euclid_dist){
  98       min_euclid_dist = euclid_dist;
  99       min_index = j;
 100     }
 101   }
 102   return d_sym_value_out[min_index];
 103 }
 104
 105 unsigned int gr_ofdm_frame_sink::demapper(const gr_complex *in,
 106                                           unsigned char *out)
 107 {
 108   unsigned int i=0, bytes_produced=0;
 109   gr_complex carrier;
 110
 111   carrier=gr_expj(d_phase);
 112
 113   gr_complex accum_error = 0.0;
 114   //while(i < d_occupied_carriers) {
 115   while(i < d_subcarrier_map.size()) {
 116     if(d_nresid > 0) {
 117       d_partial_byte |= d_resid;
 118       d_byte_offset += d_nresid;
 119       d_nresid = 0;
 120       d_resid = 0;
 121     }
 122
 123     //while((d_byte_offset < 8) && (i < d_occupied_carriers)) {
 124     while((d_byte_offset < 8) && (i < d_subcarrier_map.size())) {
 125       //gr_complex sigrot = in[i]*carrier*d_dfe[i];
 126       gr_complex sigrot = in[d_subcarrier_map[i]]*carrier*d_dfe[i];
 127
 128       if(d_derotated_output != NULL){
 129         d_derotated_output[i] = sigrot;
 130       }
 131
 132       unsigned char bits = slicer(sigrot);
 133
 134       gr_complex closest_sym = d_sym_position[bits];
 135
 136       accum_error += sigrot * conj(closest_sym);
 137
 138       // FIX THE FOLLOWING STATEMENT
 139       if (norm(sigrot)> 0.001) d_dfe[i] +=  d_eq_gain*(closest_sym/sigrot-d_dfe[i]);
 140
 141       i++;
 142
 143       if((8 - d_byte_offset) >= d_nbits) {
 144         d_partial_byte |= bits << (d_byte_offset);
 145         d_byte_offset += d_nbits;
 146       }
 147       else {
 148         d_nresid = d_nbits-(8-d_byte_offset);
 149         int mask = ((1<<(8-d_byte_offset))-1);
 150         d_partial_byte |= (bits & mask) << d_byte_offset;
 151         d_resid = bits >> (8-d_byte_offset);
 152         d_byte_offset += (d_nbits - d_nresid);
 153       }
 154       //printf("demod symbol: %.4f + j%.4f   bits: %x   partial_byte: %x   byte_offset: %d   resid: %x   nresid: %d\n",
 155       //     in[i-1].real(), in[i-1].imag(), bits, d_partial_byte, d_byte_offset, d_resid, d_nresid);
 156     }
 157
 158     if(d_byte_offset == 8) {
 159       //printf("demod byte: %x \n\n", d_partial_byte);
 160       out[bytes_produced++] = d_partial_byte;
 161       d_byte_offset = 0;
 162       d_partial_byte = 0;
 163     }
 164   }
 165   //std::cerr << "accum_error " << accum_error << std::endl;
 166
 167   float angle = arg(accum_error);
 168
 169   d_freq = d_freq - d_freq_gain*angle;
 170   d_phase = d_phase + d_freq - d_phase_gain*angle;
 171   if (d_phase >= 2*M_PI) d_phase -= 2*M_PI;
 172   if (d_phase <0) d_phase += 2*M_PI;
 173
 174   //if(VERBOSE)
 175   //  std::cerr << angle << "\t" << d_freq << "\t" << d_phase << "\t" << std::endl;
 176
 177   return bytes_produced;
 178 }
 179
 180
 181 gr_ofdm_frame_sink_sptr
 182 gr_make_ofdm_frame_sink(const std::vector<gr_complex> &sym_position,
 183                         const std::vector<unsigned char> &sym_value_out,
 184                         gr_msg_queue_sptr target_queue, unsigned int occupied_carriers,
 185                         float phase_gain, float freq_gain)
 186 {
 187   return gr_ofdm_frame_sink_sptr(new gr_ofdm_frame_sink(sym_position, sym_value_out,
 188                                                         target_queue, occupied_carriers,
 189                                                         phase_gain, freq_gain));
 190 }
 191
 192
 193 gr_ofdm_frame_sink::gr_ofdm_frame_sink(const std::vector<gr_complex> &sym_position,
 194                                        const std::vector<unsigned char> &sym_value_out,
 195                                        gr_msg_queue_sptr target_queue, unsigned int occupied_carriers,
 196                                        float phase_gain, float freq_gain)
 197   : gr_sync_block ("ofdm_frame_sink",
 198                    gr_make_io_signature2 (2, 2, sizeof(gr_complex)*occupied_carriers, sizeof(char)),
 199                    gr_make_io_signature (1, 1, sizeof(gr_complex)*occupied_carriers)),
 200     d_target_queue(target_queue), d_occupied_carriers(occupied_carriers),
 201     d_byte_offset(0), d_partial_byte(0),
 202     d_resid(0), d_nresid(0),d_phase(0),d_freq(0),d_phase_gain(phase_gain),d_freq_gain(freq_gain),
 203     d_eq_gain(0.05)
 204 {
 205   std::string carriers = "FE7F";
 206
 207   // A bit hacky to fill out carriers to occupied_carriers length
 208   int diff = (d_occupied_carriers - 4*carriers.length());
 209   while(diff > 7) {
 210     carriers.insert(0, "f");
 211     carriers.insert(carriers.length(), "f");
 212     diff -= 8;
 213   }
 214
 215   // if there's extras left to be processed
 216   // divide remaining to put on either side of current map
 217   // all of this is done to stick with the concept of a carrier map string that
 218   // can be later passed by the user, even though it'd be cleaner to just do this
 219   // on the carrier map itself
 220   int diff_left=0;
 221   int diff_right=0;
 222
 223   // dictionary to convert from integers to ascii hex representation
 224   char abc[16] = {'0', '1', '2', '3', '4', '5', '6', '7',
 225                   '8', '9', 'a', 'b', 'c', 'd', 'e', 'f'};
 226   if(diff > 0) {
 227     char c[2] = {0,0};
 228
 229     diff_left = (int)ceil((float)diff/2.0f);  // number of carriers to put on the left side
 230     c[0] = abc[(1 << diff_left) - 1];         // convert to bits and move to ASCI integer
 231     carriers.insert(0, c);
 232
 233     diff_right = diff - diff_left;            // number of carriers to put on the right side
 234     c[0] = abc[0xF^((1 << diff_right) - 1)];  // convert to bits and move to ASCI integer
 235     carriers.insert(carriers.length(), c);
 236   }
 237
 238   // It seemed like such a good idea at the time...
 239   // because we are only dealing with the occupied_carriers
 240   // at this point, the diff_left in the following compensates
 241   // for any offset from the 0th carrier introduced
 242   unsigned int i,j,k;
 243   for(i = 0; i < (d_occupied_carriers/4)+diff_left; i++) {
 244     char c = carriers[i];
 245     for(j = 0; j < 4; j++) {
 246       k = (strtol(&c, NULL, 16) >> (3-j)) & 0x1;
 247       if(k) {
 248         d_subcarrier_map.push_back(4*i + j - diff_left);
 249       }
 250     }
 251   }
 252
 253   // make sure we stay in the limit currently imposed by the occupied_carriers
 254   if(d_subcarrier_map.size() > d_occupied_carriers) {
 255     throw std::invalid_argument("gr_ofdm_mapper_bcv: subcarriers allocated exceeds size of occupied carriers");
 256   }
 257
 258   d_bytes_out = new unsigned char[d_occupied_carriers];
 259   d_dfe.resize(occupied_carriers);
 260   fill(d_dfe.begin(), d_dfe.end(), gr_complex(1.0,0.0));
 261
 262   set_sym_value_out(sym_position, sym_value_out);
 263
 264   enter_search();
 265 }
 266
 267 gr_ofdm_frame_sink::~gr_ofdm_frame_sink ()
 268 {
 269   delete [] d_bytes_out;
 270 }
 271
 272 bool
 273 gr_ofdm_frame_sink::set_sym_value_out(const std::vector<gr_complex> &sym_position,
 274                                       const std::vector<unsigned char> &sym_value_out)
 275 {
 276   if (sym_position.size() != sym_value_out.size())
 277     return false;
 278
 279   if (sym_position.size()<1)
 280     return false;
 281
 282   d_sym_position  = sym_position;
 283   d_sym_value_out = sym_value_out;
 284   d_nbits = (unsigned long)ceil(log10(d_sym_value_out.size()) / log10(2.0));
 285
 286   return true;
 287 }
 288
 289
 290 int
 291 gr_ofdm_frame_sink::work (int noutput_items,
 292                           gr_vector_const_void_star &input_items,
 293                           gr_vector_void_star &output_items)
 294 {
 295   const gr_complex *in = (const gr_complex *) input_items[0];
 296   const char *sig = (const char *) input_items[1];
 297   unsigned int j = 0;
 298   unsigned int bytes=0;
 299
 300   // If the output is connected, send it the derotated symbols
 301   if(output_items.size() >= 1)
 302     d_derotated_output = (gr_complex *)output_items[0];
 303   else
 304     d_derotated_output = NULL;
 305
 306   if (VERBOSE)
 307     fprintf(stderr,">>> Entering state machine\n");
 308
 309   switch(d_state) {
 310
 311   case STATE_SYNC_SEARCH:    // Look for flag indicating beginning of pkt
 312     if (VERBOSE)
 313       fprintf(stderr,"SYNC Search, noutput=%d\n", noutput_items);
 314
 315     if (sig[0]) {  // Found it, set up for header decode
 316       enter_have_sync();
 317     }
 318     break;
 319
 320   case STATE_HAVE_SYNC:
 321     // only demod after getting the preamble signal; otherwise, the
 322     // equalizer taps will screw with the PLL performance
 323     bytes = demapper(&in[0], d_bytes_out);
 324
 325     if (VERBOSE) {
 326       if(sig[0])
 327         printf("ERROR -- Found SYNC in HAVE_SYNC\n");
 328       fprintf(stderr,"Header Search bitcnt=%d, header=0x%08x\n",
 329               d_headerbytelen_cnt, d_header);
 330     }
 331
 332     j = 0;
 333     while(j < bytes) {
 334       d_header = (d_header << 8) | (d_bytes_out[j] & 0xFF);
 335       j++;
 336
 337       if (++d_headerbytelen_cnt == HEADERBYTELEN) {
 338
 339         if (VERBOSE)
 340           fprintf(stderr, "got header: 0x%08x\n", d_header);
 341
 342         // we have a full header, check to see if it has been received properly
 343         if (header_ok()){
 344           enter_have_header();
 345
 346           if (VERBOSE)
 347             printf("\nPacket Length: %d\n", d_packetlen);
 348
 349           while((j < bytes) && (d_packetlen_cnt < d_packetlen)) {
 350             d_packet[d_packetlen_cnt++] = d_bytes_out[j++];
 351           }
 352
 353           if(d_packetlen_cnt == d_packetlen) {
 354             gr_message_sptr msg =
 355               gr_make_message(0, d_packet_whitener_offset, 0, d_packetlen);
 356             memcpy(msg->msg(), d_packet, d_packetlen_cnt);
 357             d_target_queue->insert_tail(msg);           // send it
 358             msg.reset();                                // free it up
 359
 360             enter_search();
 361           }
 362         }
 363         else {
 364           enter_search();                               // bad header
 365         }
 366       }
 367     }
 368     break;
 369
 370   case STATE_HAVE_HEADER:
 371     bytes = demapper(&in[0], d_bytes_out);
 372
 373     if (VERBOSE) {
 374       if(sig[0])
 375         printf("ERROR -- Found SYNC in HAVE_HEADER at %d, length of %d\n", d_packetlen_cnt, d_packetlen);
 376       fprintf(stderr,"Packet Build\n");
 377     }
 378
 379     j = 0;
 380     while(j < bytes) {
 381       d_packet[d_packetlen_cnt++] = d_bytes_out[j++];
 382
 383       if (d_packetlen_cnt == d_packetlen){              // packet is filled
 384         // build a message
 385         // NOTE: passing header field as arg1 is not scalable
 386         gr_message_sptr msg =
 387           gr_make_message(0, d_packet_whitener_offset, 0, d_packetlen_cnt);
 388         memcpy(msg->msg(), d_packet, d_packetlen_cnt);
 389
 390         d_target_queue->insert_tail(msg);               // send it
 391         msg.reset();                            // free it up
 392
 393         enter_search();
 394         break;
 395       }
 396     }
 397     break;
 398
 399   default:
 400     assert(0);
 401
 402   } // switch
 403
 404   return 1;
 405 }