chest_rcv_mimo.c

软件无线电的平台

	}
	swr_fft_complex( f_inv, logb( f_len ), SWR_FFT );
	for ( i=0; i<f_len; i++ ){
	  f_inv[i] = pow(2,24)/f_inv[i];
	}
	swr_fft_complex( f_inv, logb( f_len ), SWR_IFFT );
	f_max = 1;
	for ( i=0; i<f_len; i++ ){
	  f_abs += cabs( f_inv[i] );
	  f_max = max( cabs( f_inv[i] ), (double)f_max );
	}
	for ( i=0; i<f_len; i++ ){
	  f_inv[i] /= f_max;
	  // filter_inv is just for displaying the filter
	  channel_inv[i+ch_off].real = creal( f_inv[i] ) * 32767;
	  channel_inv[i+ch_off].imag = cimag( f_inv[i] ) * 32767;
	  if ( i < 5 ){
	    PR_DBG_CL( 5, "%i: %i/%i\n", i, 
		       channel_inv[i+ch_off].real, channel_inv[i+ch_off].imag );
	  }
	}
	f_abs /= f_max;
      }
    }
  }

  /******************Noise Variance Calculation Begins***************
    noise1 = receivedvalue1 - h11*s1 - h12*s2 - h13s3 - h14s4 etc..     
    var(noise) = sum( noise ^ 2) - mean( noise )^2  
    Flat channel is assumed! Hence every 'L'the element starting at 0 
    of filter is the channel coefficient.
    We are to calculate the variance of a complex noise variable.
    We assume independent and equal components, hence we calculate 
    only the variance of the real component.
  ******************************************************************/

  //Mid points to the received midamble
  mid            = in + data_len[0];
  channel_offset = seq_ch_taps; //Channel Length
  length         = 2 * seq_length;

  for ( i = 0; i < seq_length; i++) {
    //real_term = channel[0].real * seq[ 2*i ]  - channel[0].imag * seq[ 2*i + 1 ];
    real_term = 0;
    imag_term = 0;
    for ( j = 0; j < num_of_antennas; j++) {
      //Careful with cyclic shifts
      seq_offset = seq_length - j * channel_offset;
      real_term += channel[ j * channel_offset ].real * 
	seq[ ( 2 * ( i + seq_offset) ) % length] -
	channel[ j * channel_offset ].imag *
	seq[ ( 2 * ( i + seq_offset) + 1 ) % length];
      
      imag_term += channel[ j * channel_offset ].imag * 
	seq[ ( 2 * ( i + seq_offset) ) % length] +
	channel[ j * channel_offset ].real *
	seq[ ( 2 * ( i + seq_offset) + 1 ) % length];
    }
    
    noise_term_real     += mid[ i ].real - real_term;
    noise_term_imag     += mid[ i ].imag - imag_term;
    var_noise_term_real += ( mid[ i ].real - real_term ) * 
      (mid[ i ].real - real_term);
    var_noise_term_imag += ( mid[ i ].imag - imag_term ) * 
      (mid[ i ].imag - imag_term);
    
    PR_DBG( 4, "in module var_noise:(%g,%g) mid.real:%i, real:%i\n", 
	    var_noise_term_real, var_noise_term_imag,
	    mid[i].real, real_term );
  }

  noise_term_real = noise_term_real / seq_length;
  noise_term_real = noise_term_real * noise_term_real;
  noise_term_imag = noise_term_imag / seq_length;
  noise_term_imag = noise_term_imag * noise_term_imag;
  
  noise_var_real = var_noise_term_real / seq_length - noise_term_real;
  noise_var_imag = var_noise_term_imag / seq_length - noise_term_imag;
  noise_var =  noise_var_real + noise_var_imag; 

  PR_DBG( 2, "in module noise_Var_real = %i, noise_var_imag = %i \n", 
	  noise_var_real, noise_var_imag );
  for ( i=0; i<num_of_antennas; i++ ){
    j = seq_ch_taps * i;
    PR_DBG( 2, "h[%i] = %i + %ii\n", i, channel[ j ].real, channel[ j ].imag );
  }

  /******************Noise Variance Calculation Ends******************/
  
  sig_power = 0.;
  for( i = 0; i < size_in(0);i++){
    sig_power += in[i].real * in[i].real + in[i].imag * in[i].imag;
  }
  sig_power /= size_in(0);
  //sig_power -= noise_var; 

  // Search for the peak in the midamble
  peak_pos = 0;
  peak_amp = 0;
  mid_amp = 0;
  for ( i=0; i<seq_ch_taps; i++ ){
    int pos = i + private->index * seq_ch_taps;
    mid_amp += hypot( channel[pos].real, channel[pos].imag );
    if ( hypot( channel[pos].real, channel[pos].imag ) > peak_amp ){
      peak_pos = pos;
      peak_amp = hypot( channel[pos].real, channel[pos].imag );
    }
  }
  mid_amp /= seq_ch_taps;
  mid_amp *= mid_amp;

  // Copy the data as-is, but perhaps shifted
  PR_DBG( 4, "f Peak_position : %i\n", peak_pos );
  if ( private->align ){
    PR_DBG( 2, "Found maximum filter at %i\n", peak_pos );
    if ( !private->calc_taps ){
      in += ( peak_pos % seq_ch_taps ) - circ_ext;
    }
    peak_pos -= circ_ext;
  }


  /**
   * If calc_taps is > 0, then we calculate a matched filter with length
   * calc_taps
   */
  if ( private->calc_taps > 0 ){
    for ( ch=0; ch<num_of_antennas; ch++ ){
      if ( out[ch] ){
	// f points to the end of the filter
	complex double *f_inv = f_invs[ch];
	complex double *f = f_inv + seq_ch_taps - 1;
	double m_amp = 0, nv = 0;
	int train_len = seq_length;
	SYMBOL_COMPLEX *matched_mid = private->mid,
	  *mid_seq = (SYMBOL_COMPLEX*)seq;

	// First we search for the peak, so that we do the convolution with the
	// right data
	peak_pos = 0;
	peak_amp = 0;
	for ( i=0; i<seq_ch_taps; i++ ){
	  if ( cabs( f_inv[ i ] ) > peak_amp ){
	    peak_pos = i;
	    peak_amp = cabs( f_inv[ i ] );
	  }
	}
	peak_pos = seq_ch_taps - peak_pos - 1;

	mid = in + data_len[0] + circ_ext + peak_pos;
	for ( i=0; i<seq_ch_taps; i++ ){
	  if ( cabs( SC_TO_CD( mid[i] ) ) > m_amp ){
	    m_amp = cabs( SC_TO_CD( mid[i] ) );
	  }
	}
	f_abs *= m_amp / 16000.;
    
	in -= circ_ext - 1;
	if ( private->align ){
	  // Make sure we won't get out of the filter
	  int diff = max( 0, peak_pos - private->calc_taps / 2 );
	  in += diff;
	  f -= diff;
	}
	PR_DBG( 4, "f_inv Peak_position : %i\n", peak_pos );

	// Do a matched filtering
	// For the first half of the data
	do_mafi( in, out[ch], f, f_abs, private->calc_taps, data_len[0] );
	// For the second half of the data
	do_mafi( in + data_len[0] + train_len + circ_ext,
		 out[ch] + data_len[0], 
		 f, f_abs, private->calc_taps, data_len[1] );
	// Calculate the matched filter of the training sequence
	do_mafi( in + data_len[0] + circ_ext,
		 matched_mid,
		 f, f_abs, private->calc_taps, train_len );

	// Do something with matched_mid
	//    matched_mid[0:train_len-1 ] <->
	//mid_seq[0:train_len-1];


	// to check if the received midamble corresponds to the
	// tranmsitted one
	for ( i = 0; i < train_len; i++) {
	  PR_DBG( 4, "Tx_mid, Rx_mid: %i %i\n", mid_seq[i].imag,
		  matched_mid[i].imag );
	}
    
	// compute the mid_amp
	for ( i = 0; i < train_len; i++){
	  tmp +=  (int)mid_seq[i].real * (int)matched_mid[i].real +
	    (int)mid_seq[i].imag * (int)matched_mid[i].imag;
	}
	if ( tmp > 0 ){
	  mid_amp = (int)(tmp/train_len);
	} else {
	  mid_amp = 1;
	}

	// calculate the noise variance
	for ( i = 0; i < train_len; i++){
	  int re, im;
	  re = matched_mid[i].real - mid_amp * mid_seq[i].real;
	  im = matched_mid[i].imag - mid_amp * mid_seq[i].imag;
	  nv += re * re + im * im;
	}
	noise_var = nv / train_len;
      }
    }    
  } else {
    for ( ch=0; ch<MAX_CHANNELS; ch++ ){
      if ( out[ch] ){
	//Copy the first data stream to out
	memcpy( out[ch], in, data_len[0] * sizeof( SYMBOL_COMPLEX ) );
	//Copy the second data stream to out
	PR_DBG( 4, "Offset is: %i\n", data_len[0] + seq_length + circ_ext );
	memcpy( out[ch] + data_len[0],
		in + data_len[0] + seq_length + circ_ext,
		data_len[1] * sizeof( SYMBOL_COMPLEX ) );
      }
    }
  }

  swr_sdb_get_stats_struct( context->id, (void**)&stats );

  stats->noise_var      = noise_var;
  stats->noise_var_real = noise_var_real;
  stats->noise_var_imag = noise_var_imag;
  stats->ch_length      = channel_offset;
  if ( ( mid_amp > 0 ) && ( noise_var > 0 ) ){
    stats->snr          = 10 * (log10(mid_amp)*2 - log10(noise_var) ); 
  } else {
    stats->snr          = -20;
  }
  stats->peak_pos       = peak_pos;
  stats->peak_amp       = peak_amp;
  stats->mid_amp        = mid_amp;
  stats->var11          = noise_var_real + noise_var_imag * I;
  swr_sdb_free_stats_struct( context->id, (void**)&stats );
  return(0);
}


/* int rcv_mimo_custom( swr_sdb_t *context */


/*
 * This is the `destructor'.
 */
int rcv_mimo_finalize( swr_sdb_t *context ) {
  if ( sizeof( private_t ) > 0 )
    swr_free( private );
  MOD_DEC_USE_COUNT;
  return 0;
}


/*
 * This function is called upon "insmod" and is used to register the
 * different parts of the module to the SPM.
 */
swr_spc_id_t rcv_mimo_id;
int rcv_mimo_module_init(void) {
  swr_spc_desc_t *desc;
  int out;

  /**
   * Get a description-part from SPM
   * Give the following parameters:
   * Input-ports, output-ports, config-params, stat-params
   */
  desc = swr_spc_get_new_desc( 1, MAX_CHANNELS, 5, 12 );
  if ( !desc ) {
    PR_DBG( 0, "Can't initialise the module. This is BAD!\n" );
    return -1;
  }

  /**
   * Define the different parts of config and stats. You have to define
   * them in the same order as they appear in the structures. The names
   * can be freely chosen.
   *
   * UM_CONFIG_{INT,DOUBLE,STRING128,POINTER}( "name" );
   * UM_STATS_{INT,DOUBLE,STRING128,POINTER,BLOCK}( "name" );
   */
  UM_CONFIG_INT( "type" );
  UM_CONFIG_INT( "num_of_antennas" );
  UM_CONFIG_INT( "calc_taps" );
  UM_CONFIG_INT( "align" );
  UM_CONFIG_INT( "circ_ext" );
  UM_STATS_BLOCK( "channel" );
  UM_STATS_BLOCK( "channel_inv" );
  UM_STATS_BLOCK( "midamble" );
  UM_STATS_DOUBLE( "snr" );
  UM_STATS_INT( "noise_var" );
  UM_STATS_INT( "noise_var_real" );
  UM_STATS_INT( "noise_var_imag" );
  UM_STATS_INT( "ch_length" );
  UM_STATS_INT( "peak_pos" );
  UM_STATS_INT( "peak_amp" );
  UM_STATS_INT( "mid_amp" );
  UM_STATS_DOUBLE_COMPLEX( "var11" );
  
  /**
   * The in- and outputs have also to be defined in the right order. First
   * port first. The additional flag is not used yet, but it will...
   *
   * UM_INPUT( SIG_{U8,SYMBOL_{S16,COMPLEX,MMX},SAMPLE_S12,S32}, 0 );
   * UM_OUTPUT( SIG_{U8,SYMBOL_{S16,COMPLEX,MMX},SAMPLE_S12,S32}, 0 );
   */

  UM_INPUT( SIG_SYMBOL_COMPLEX, 0 );
  for ( out = 0; out < MAX_CHANNELS; out++ ){
    UM_OUTPUT( SIG_SYMBOL_COMPLEX, 0 );
  }

  // Initialise the callback-functions. Delete the ones you don't use
  desc->fn_init              = rcv_mimo_init;
  desc->fn_reconfigure       = rcv_mimo_reconfig;
  desc->fn_process_data      = rcv_mimo_pdata;
  desc->fn_configure_inputs  = rcv_mimo_configure_inputs;
  desc->fn_configure_outputs = rcv_mimo_configure_outputs;
  desc->fn_finalize          = rcv_mimo_finalize;

  // And register the module in the SPM. Change the name!
  rcv_mimo_id = swr_cdb_register_spc( &desc, "chest_rcv_mimo" );
  if ( rcv_mimo_id == SWR_SPM_INVALID_ID ) {
    swr_spc_free_desc( desc );
    PR_DBG( 0, "Couldn't register the module!\n" );
    return 1;
  }
  PR_DBG( 4, "Ready\n" );
  return 0;
}


/*
 * This is called upon rmmod
 */
void rcv_mimo_module_exit( void ) {
  PR_DBG( 4, "Freeing id: %i\n", rcv_mimo_id );
  if ( swr_cdb_unregister_spc( rcv_mimo_id ) < 0 ) {
    PR_DBG( 0, "Still in use somewhere\n" );
  }
}