C++ Hz函数代码示例

void FDTD2D::updateE(){
  int ii,jj;
  double CFL2 = CFL;///2.;
  // Bulk update, Ex
  for( jj=1; jj<Ny-1; jj++){
    for( ii=0; ii<Nx-1; ii++){
      Ex(ii,jj) += ExC(ii,jj)*(Hz(ii,jj) - Hz(ii,jj-1));
    }
  }
  // Bulk update, Ey
  for( jj=0; jj<Ny-1; jj++){
    for( ii=1; ii<Nx-1; ii++){
      Ey(ii,jj) -= EyC(ii,jj)*(Hz(ii,jj) - Hz(ii-1,jj));
    }
  }
  // Boundary update, Ex
  for( ii=0; ii<Nx-1; ii++){
    Ex(ii,0)    = (1.-CFL2)*Ex(ii,0)    + CFL2*Ex(ii,1);
    Ex(ii,Ny-1) = (1.-CFL2)*Ex(ii,Ny-1) + CFL2*Ex(ii,Ny-2);
  }
  // Boundary update, Ey
  for( jj=0; jj<Ny-1; jj++){
    Ey(0,jj)    = (1.-CFL2)*Ey(0,jj)    + CFL2*Ey(1,jj);
    Ey(Nx-1,jj) = (1.-CFL2)*Ey(Nx-1,jj) + CFL2*Ey(Nx-2,jj);
  }
  return;
}

int ft100_get_mode(RIG *rig, vfo_t vfo, rmode_t *mode, pbwidth_t *width) {

    struct ft100_priv_data *priv = (struct ft100_priv_data*)rig->state.priv;
    int ret;

    if( !mode || !width )  return -RIG_EINVAL;

    ret = ft100_read_status(rig);
    if (ret < 0)
	return ret;

    switch( priv->status.mode & 0x0f ) {
    case 0x00:
      *mode = RIG_MODE_LSB;
      break;
    case 0x01:
      *mode = RIG_MODE_USB;
      break;
    case 0x02:
      *mode = RIG_MODE_CW;
      break;
    case 0x03:
      *mode = RIG_MODE_CWR;
      break;
    case 0x04:
      *mode = RIG_MODE_AM;
      break;
    case 0x05:
      *mode = RIG_MODE_RTTY;
      break;
    case 0x06:
      *mode = RIG_MODE_FM;
      break;
    case 0x07:
      *mode = RIG_MODE_WFM;
      break;
    default:
      *mode = RIG_MODE_NONE;
    };

    switch( priv->status.mode >> 4 ) {
    case 0x00:
      *width = Hz(6000);
      break;
    case 0x01:
      *width = Hz(2400);
      break;
    case 0x02:
      *width = Hz(500);
      break;
    case 0x03:
      *width = Hz(300);
      break;
    default:
      *width = RIG_PASSBAND_NORMAL;
    };

    return RIG_OK;
}

void KneeJointR::checkInitPos( SP::SiconosVector x1 ,  SP::SiconosVector x2 )
{

  //x1->display();
  double X1 = x1->getValue(0);
  double Y1 = x1->getValue(1);
  double Z1 = x1->getValue(2);
  double q10 = x1->getValue(3);
  double q11 = x1->getValue(4);
  double q12 = x1->getValue(5);
  double q13 = x1->getValue(6);
  double X2 = 0;
  double Y2 = 0;
  double Z2 = 0;
  double q20 = 1;
  double q21 = 0;
  double q22 = 0;
  double q23 = 0;
  if(x2)
  {
    //printf("checkInitPos x2:\n");
    //x2->display();
    X2 = x2->getValue(0);
    Y2 = x2->getValue(1);
    Z2 = x2->getValue(2);
    q20 = x2->getValue(3);
    q21 = x2->getValue(4);
    q22 = x2->getValue(5);
    q23 = x2->getValue(6);
  }

  if (Hx(X1, Y1, Z1, q10, q11, q12, q13, X2, Y2, Z2, q20, q21, q22, q23) > DBL_EPSILON )
  {
    std::cout << "KneeJointR::checkInitPos( SP::SiconosVector x1 ,  SP::SiconosVector x2 )" << std::endl;
    std::cout << " Hx is large :" << Hx(X1, Y1, Z1, q10, q11, q12, q13, X2, Y2, Z2, q20, q21, q22, q23) << std::endl;
  }
  if (Hy(X1, Y1, Z1, q10, q11, q12, q13, X2, Y2, Z2, q20, q21, q22, q23) > DBL_EPSILON )
  {
    std::cout << "KneeJointR::checkInitPos( SP::SiconosVector x1 ,  SP::SiconosVector x2 )" << std::endl;
    std::cout << " Hy is large :" << Hy(X1, Y1, Z1, q10, q11, q12, q13, X2, Y2, Z2, q20, q21, q22, q23) << std::endl;
  }
  if (Hz(X1, Y1, Z1, q10, q11, q12, q13, X2, Y2, Z2, q20, q21, q22, q23) > DBL_EPSILON )
  {
    std::cout << "KneeJointR::checkInitPos( SP::SiconosVector x1 ,  SP::SiconosVector x2 )" << std::endl;
    std::cout << " Hz is large :" << Hz(X1, Y1, Z1, q10, q11, q12, q13, X2, Y2, Z2, q20, q21, q22, q23) << std::endl;
  }
     
  
  // printf("checkInitPos Hx : %e\n", Hx(X1, Y1, Z1, q10, q11, q12, q13, X2, Y2, Z2, q20, q21, q22, q23));
  // printf("checkInitPos Hy : %e\n", Hy(X1, Y1, Z1, q10, q11, q12, q13, X2, Y2, Z2, q20, q21, q22, q23));
  // printf("checkInitPos Hz : %e\n", Hz(X1, Y1, Z1, q10, q11, q12, q13, X2, Y2, Z2, q20, q21, q22, q23));


}

void FDTD2D::correctFieldsH(){
  // Left partition at Lx+0.5. Right at Nx-Lx+0.5.
  // As incident wave is Ey/Bz polarised, no need to mess with top or bottom partitions.
  double l_source = gauss_der_source(t-(Lx)*ds/c, sigma, mean);
  double r_source = gauss_der_source(t-(Nx-Lx+1)*ds/c, sigma, mean);
  for( int jj = Ly; jj <= Ny-Ly; jj++){
    // Ey to the left is scattered
    Hz(Lx,jj)    += HzC(Lx,jj)    * l_source;
    Hz(Nx-Lx,jj) -= HzC(Nx-Lx,jj) * r_source;
  }
  // Corner points need additional attention
  return;
}

/*
 * Tuning Factor Calculations
 * assumes rig!=NULL, rig->state.priv!=NULL
 * assumes priv->mode in supported modes.
 */
static void tentec_tuning_factor_calc(RIG *rig)
{
    struct tentec_priv_data *priv;
    freq_t tfreq;
    int adjtfreq, mcor, fcor, cwbfo;

    priv = (struct tentec_priv_data *)rig->state.priv;
    cwbfo = 0;

    /* computed fcor only used if mode is not CW */
    fcor = (int)floor((double)priv->width / 2.0) + 200;

    switch (priv->mode) {
    case RIG_MODE_AM:
    case RIG_MODE_FM:
        mcor=0; break;
	case RIG_MODE_CW:
	    mcor=-1; cwbfo = priv->cwbfo; fcor = 0; break;
    case RIG_MODE_LSB:
		mcor=-1; break;
    case RIG_MODE_USB:
        mcor=1; break;
    default:
        rig_debug(RIG_DEBUG_BUG, "tentec_tuning_factor_calc: invalid mode!\n");
        mcor=1; break;
    }
    tfreq = priv->freq / (freq_t)Hz(1);

    adjtfreq = (int)tfreq - 1250 + (int)(mcor * (fcor + priv->pbt));

    priv->ctf = (adjtfreq / 2500) + 18000;
    priv->ftf = (int)floor((double)(adjtfreq % 2500) * 5.46);
    priv->btf = (int)floor((double)(fcor + priv->pbt + cwbfo + 8000) * 2.73);
}

void FDTD2D::print_heatmap( std::ofstream& outFile){
  // Prints data in a format compatible for heatmap plots 
  for( int jj=0; jj<Ny-1; jj++){
    for( int ii=0; ii<Nx-1; ii++){
      outFile << Hz(ii,jj) << ' ';
    }
    outFile << std::endl;
  }
  return;
}

/* update magnetic field */
void updateH2d(Grid *g) {
  int mm, nn;

  if (Type == oneDGrid) {

    for (mm = 0; mm < SizeX - 1; mm++)
      Hz1(mm) = Chzh1(mm) * Hz1(mm) 
	- Chze1(mm) * (Ey1(mm + 1) - Ey1(mm));

  } else { 

    for (mm = 0; mm < SizeX - 1; mm++)    /*@ \label{updatetezA} @*/
      for (nn = 0; nn < SizeY - 1; nn++)
	Hz(mm, nn) = Chzh(mm, nn) * Hz(mm, nn) +
	  Chze(mm, nn) * ((Ex(mm, nn + 1) - Ex(mm, nn))
			  - (Ey(mm + 1, nn) - Ey(mm, nn)));
  }

  return;
}

double DiskSCFPotentialPlanarRforce(double R,double phi,
				    double t,
				    struct potentialArg * potentialArgs){
  //Supposed to be zero (bc H(0) supposed to be zero), but just to make sure
  double * args= potentialArgs->args;
  //Get args
  int nsigma_args= (int) *args;
  double * Sigma_args= args+1;
  double * hz_args= args+1+nsigma_args;
  //Calculate Rforce
  return -dSigmadR(R,Sigma_args) * Hz(0.,hz_args);
}

double DiskSCFPotentialRforce(double R,double Z, double phi,
			      double t,
			      struct potentialArg * potentialArgs){
  double * args= potentialArgs->args;
  //Get args
  int nsigma_args= (int) *args;
  double * Sigma_args= args+1;
  double * hz_args= args+1+nsigma_args;
  //Calculate Rforce
  double r= sqrt( R * R + Z * Z );
  return -dSigmadR(r,Sigma_args) * Hz(Z,hz_args) * R / r;
}

/* update electric field */
void updateE2d(Grid *g) {
  int mm, nn;

  if (Type == oneDGrid) {

    for (mm = 1; mm < SizeX - 1; mm++)
      Ey1(mm) = Ceye1(mm) * Ey1(mm) 
	- Ceyh1(mm) * (Hz1(mm) - Hz1(mm - 1));

  } else { 

    for (mm = 0; mm < SizeX - 1; mm++)    /*@ \label{updatetezB} @*/
      for (nn = 1; nn < SizeY - 1; nn++)
	Ex(mm, nn) = Cexe(mm, nn) * Ex(mm, nn) +
	  Cexh(mm, nn) * (Hz(mm, nn) - Hz(mm, nn - 1));

    for (mm = 1; mm < SizeX - 1; mm++)
      for (nn = 0; nn < SizeY - 1; nn++)
	Ey(mm, nn) = Ceye(mm, nn) * Ey(mm, nn) -
	  Ceyh(mm, nn) * (Hz(mm, nn) - Hz(mm - 1, nn));
  }

  return;
}

/**
 * Print an EDID monitor descriptor block
 *
 * @param monitor	The EDID monitor descriptor block
 * @have_timing		Modifies to 1 if the desciptor contains timing info
 */
static void edid_print_dtd(struct edid_monitor_descriptor *monitor,
			   unsigned int *have_timing)
{
	unsigned char *bytes = (unsigned char *)monitor;
	struct edid_detailed_timing *timing =
			(struct edid_detailed_timing *)monitor;

	if (bytes[0] == 0 && bytes[1] == 0) {
		if (monitor->type == EDID_MONITOR_DESCRIPTOR_SERIAL)
			printf("Monitor serial number: %s\n",
			       snip(monitor->data.string));
		else if (monitor->type == EDID_MONITOR_DESCRIPTOR_ASCII)
			printf("Monitor ID: %s\n",
			       snip(monitor->data.string));
		else if (monitor->type == EDID_MONITOR_DESCRIPTOR_NAME)
			printf("Monitor name: %s\n",
			       snip(monitor->data.string));
		else if (monitor->type == EDID_MONITOR_DESCRIPTOR_RANGE)
			printf("Monitor range limits, horizontal sync: "
			       "%d-%d kHz, vertical refresh: "
			       "%d-%d Hz, max pixel clock: "
			       "%d MHz\n",
			       monitor->data.range_data.horizontal_min,
			       monitor->data.range_data.horizontal_max,
			       monitor->data.range_data.vertical_min,
			       monitor->data.range_data.vertical_max,
			       monitor->data.range_data.pixel_clock_max * 10);
	} else {
		uint32_t pixclock, h_active, h_blanking, v_active, v_blanking;
		uint32_t h_total, v_total, vfreq;

		pixclock = EDID_DETAILED_TIMING_PIXEL_CLOCK(*timing);
		h_active = EDID_DETAILED_TIMING_HORIZONTAL_ACTIVE(*timing);
		h_blanking = EDID_DETAILED_TIMING_HORIZONTAL_BLANKING(*timing);
		v_active = EDID_DETAILED_TIMING_VERTICAL_ACTIVE(*timing);
		v_blanking = EDID_DETAILED_TIMING_VERTICAL_BLANKING(*timing);

		h_total = h_active + h_blanking;
		v_total = v_active + v_blanking;
		if (v_total * h_total)
			vfreq = pixclock / (v_total * h_total);
		else
			vfreq = 1; /* Error case */
		printf("\t%dx%d\%c\t%d Hz (detailed)\n", h_active,
		       v_active, h_active > 1000 ? ' ' : '\t', vfreq);
		*have_timing = 1;
	}
}

MHz calculate_cpu_frequency(){
  
  // We expect the cpu_sampling_irq_handler to push in samples;
  while (_cpu_timestamps.size() < do_samples_)
    OS::halt();
      
  debug("_cpu_sampling_freq_divider_ : %i \n",_cpu_sampling_freq_divider_);
  
  #ifdef DEBUG
  for (auto t : _cpu_timestamps) debug("%lu \n",(uint32_t)t);
  #endif
  
  // Subtract the time it takes to measure time :-)
  auto t1 = OS::cycles_since_boot();
  OS::cycles_since_boot();
  auto t3 = OS::cycles_since_boot();
  auto overhead = (t3 - t1) * 2;
  
  debug ("Overhead: %lu \n", (uint32_t)overhead);
  
  for (size_t i = 1; i < _cpu_timestamps.size(); i++){
    // Compute delta in cycles
    auto cycles = _cpu_timestamps[i] - _cpu_timestamps[i-1] + overhead;
    // Cycles pr. second == Hertz
    auto freq = cycles / (1 / test_frequency().count());
    _cpu_freq_samples.push_back(freq);    
    debug("%lu - %lu = Delta: %lu Current PIT-Freq: %f Hz CPU Freq: %f MHz \n",
	  (uint32_t)_cpu_timestamps[i], (uint32_t)_cpu_timestamps[i-1],
	  (uint32_t)cycles, Hz(test_frequency()), freq);        
  }
  
  
#ifdef DEBUG
  double sum = 0;  
  for (auto freq : _cpu_freq_samples)
    sum += freq;  
  double mean = sum / _cpu_freq_samples.size();
#endif
  
  std::sort(_cpu_freq_samples.begin(), _cpu_freq_samples.end());
  double median = _cpu_freq_samples[_cpu_freq_samples.size() / 2];
  
  debug("<cpu_freq> MEAN: %f MEDIAN: %f \n",mean, median);
  _CPUFreq_ = median;
  
  return MHz(median);
  
}

void KneeJointR::computeh(double time, BlockVector& q0, SiconosVector& y)
{
  DEBUG_BEGIN("KneeJointR::computeh(double time, BlockVector& q0, SiconosVector& y)\n");
  DEBUG_EXPR(q0.display());

  double X1 = q0.getValue(0);
  double Y1 = q0.getValue(1);
  double Z1 = q0.getValue(2);
  double q10 = q0.getValue(3);
  double q11 = q0.getValue(4);
  double q12 = q0.getValue(5);
  double q13 = q0.getValue(6);
  DEBUG_PRINTF("X1 = %12.8e,\t Y1 = %12.8e,\t Z1 = %12.8e,\n",X1,Y1,Z1);
  DEBUG_PRINTF("q10 = %12.8e,\t q11 = %12.8e,\t q12 = %12.8e,\t q13 = %12.8e,\n",q10,q11,q12,q13);
  double X2 = 0;
  double Y2 = 0;
  double Z2 = 0;
  double q20 = 1;
  double q21 = 0;
  double q22 = 0;
  double q23 = 0;
  if(q0.getNumberOfBlocks()>1)
  {
    // SP::SiconosVector x2 = _d2->q();
    // DEBUG_EXPR( _d2->q()->display(););
    X2 = q0.getValue(7);
    Y2 = q0.getValue(8);
    Z2 = q0.getValue(9);
    q20 = q0.getValue(10);
    q21 = q0.getValue(11);
    q22 = q0.getValue(12);
    q23 = q0.getValue(13);
  }
  y.setValue(0, Hx(X1, Y1, Z1, q10, q11, q12, q13, X2, Y2, Z2, q20, q21, q22, q23));
  y.setValue(1, Hy(X1, Y1, Z1, q10, q11, q12, q13, X2, Y2, Z2, q20, q21, q22, q23));
  y.setValue(2, Hz(X1, Y1, Z1, q10, q11, q12, q13, X2, Y2, Z2, q20, q21, q22, q23));
  DEBUG_EXPR(y.display());
  DEBUG_END("KneeJointR::computeh(double time, BlockVector& q0, SiconosVector& y)\n");
    
}

 .write_delay =        FT1000D_WRITE_DELAY,
 .post_write_delay =   FT1000D_POST_WRITE_DELAY,
 .timeout =            2000,
 .retry =              0,
 .has_get_func =       RIG_FUNC_LOCK | RIG_FUNC_TUNER | RIG_FUNC_MON,
 .has_set_func =       RIG_FUNC_LOCK | RIG_FUNC_TUNER,
 .has_get_level =      RIG_LEVEL_STRENGTH | RIG_LEVEL_SWR | RIG_LEVEL_ALC | \
                       RIG_LEVEL_ALC | RIG_LEVEL_RFPOWER,
 .has_set_level =      RIG_LEVEL_NONE,
 .has_get_parm =       RIG_PARM_NONE,
 .has_set_parm =       RIG_PARM_BACKLIGHT,
 .ctcss_list =         NULL,
 .dcs_list =           NULL,
 .preamp =             { RIG_DBLST_END, },
 .attenuator =         { RIG_DBLST_END, },
 .max_rit =            Hz(9999),
 .max_xit =            Hz(9999),
 .max_ifshift =        Hz(1200),
 .vfo_ops =            RIG_OP_CPY | RIG_OP_FROM_VFO | RIG_OP_TO_VFO |
                       RIG_OP_UP | RIG_OP_DOWN | RIG_OP_TUNE | RIG_OP_TOGGLE,
 .targetable_vfo =     RIG_TARGETABLE_ALL,
 .transceive =         RIG_TRN_OFF,        /* Yaesus have to be polled, sigh */
 .bank_qty =           0,
 .chan_desc_sz =       0,
 .chan_list =          {
                         {1, 99, RIG_MTYPE_MEM, FT1000D_MEM_CAP},
                          RIG_CHAN_END,
                       },
 .rx_range_list1 =     {
   {kHz(100), MHz(30), FT1000D_ALL_RX_MODES, -1, -1, FT1000D_VFO_ALL, FT1000D_ANTS},   /* General coverage + ham */
   RIG_FRNG_END,

    .retry =              3,
    .has_get_func =       FT950_FUNCS,
    .has_set_func =       FT950_FUNCS,
    .has_get_level =      FT950_LEVELS,
    .has_set_level =      RIG_LEVEL_SET(FT950_LEVELS),
    .has_get_parm =       RIG_PARM_NONE,
    .has_set_parm =       RIG_PARM_NONE,
    .level_gran = {
	[LVL_RAWSTR] = { .min = { .i = 0 }, .max = { .i = 255 } },
	[LVL_CWPITCH] = { .min = { .i = 300 }, .max = { .i = 1050 }, .step = { .i = 50 } },
    },
    .ctcss_list =         common_ctcss_list,
    .dcs_list =           NULL,
    .preamp =             { 10, 20, RIG_DBLST_END, }, /* TBC */
    .attenuator =         { 6, 12, 18, RIG_DBLST_END, },
    .max_rit =            Hz(9999),
    .max_xit =            Hz(9999),
    .max_ifshift =        Hz(1000),
    .vfo_ops =            FT950_VFO_OPS,
    .targetable_vfo =     RIG_TARGETABLE_FREQ,
    .transceive =         RIG_TRN_OFF,        /* May enable later as the 950 has an Auto Info command */
    .bank_qty =           0,
    .chan_desc_sz =       0,
    .str_cal =            FT950_STR_CAL,
    .chan_list =          {
               {   1,  99, RIG_MTYPE_MEM,  NEWCAT_MEM_CAP },
               { 100, 117, RIG_MTYPE_EDGE, NEWCAT_MEM_CAP },    /* two by two */
               { 125, 128, RIG_MTYPE_BAND, NEWCAT_MEM_CAP },    /* 60M Channels U51-U54 or US1-US4, if available */
               { 130, 130, RIG_MTYPE_BAND, NEWCAT_MEM_CAP },    /* 60M Channel U55 or US5, if available */
               { 131, 131, RIG_MTYPE_BAND, NEWCAT_MEM_CAP },    /* EU5, 5167.5 KHz Alaska Emergency Freq, if available */