自学教程：C++ updateParams函数代码示例

Navigator::Navigator() :	SuperBlock(nullptr, "NAV"),	_loop_perf(perf_alloc(PC_ELAPSED, "navigator")),	_geofence(this),	_mission(this, "MIS"),	_loiter(this, "LOI"),	_takeoff(this, "TKF"),	_land(this, "LND"),	_rtl(this, "RTL"),	_rcLoss(this, "RCL"),	_dataLinkLoss(this, "DLL"),	_engineFailure(this, "EF"),	_gpsFailure(this, "GPSF"),	_follow_target(this, "TAR"),	_param_loiter_radius(this, "LOITER_RAD"),	_param_acceptance_radius(this, "ACC_RAD"),	_param_fw_alt_acceptance_radius(this, "FW_ALT_RAD"),	_param_mc_alt_acceptance_radius(this, "MC_ALT_RAD"){	/* Create a list of our possible navigation types */	_navigation_mode_array[0] = &_mission;	_navigation_mode_array[1] = &_loiter;	_navigation_mode_array[2] = &_rtl;	_navigation_mode_array[3] = &_dataLinkLoss;	_navigation_mode_array[4] = &_engineFailure;	_navigation_mode_array[5] = &_gpsFailure;	_navigation_mode_array[6] = &_rcLoss;	_navigation_mode_array[7] = &_takeoff;	_navigation_mode_array[8] = &_land;	_navigation_mode_array[9] = &_follow_target;	updateParams();}

REALvalue(const unsigned i,      const REAL s0,      const REAL t,      const REAL alpha,      const REAL nu,      const REAL beta,      const unsigned int numX,      const unsigned int numY,      const unsigned int numT){  REAL strike;  PrivGlobs    globs(numX, numY, numT);  strike = 0.001*i;  initGrid(s0, alpha, nu, t, numX, numY, numT, globs);  initOperator(globs.myX, globs.myDxx);  initOperator(globs.myY, globs.myDyy);  setPayoff(strike, globs);  for(int i = globs.myTimeline.size()-2; i >= 0; i--) {    updateParams(i,alpha,beta,nu,globs);    rollback(i, globs);  }  return globs.myResult[globs.myXindex][globs.myYindex];}

void LDA::gibbs(int K,double alpha,double beta) {    this->K=K;    this->alpha=alpha;    this->beta=beta;    if(SAMPLE_LAG >0) {        thetasum = MatrixXd(documentsSize(),K);        phisum= MatrixXd(K,V);        numstats=0;    }    initialState(K);    for(int i=0; i<ITERATIONS; i++) {        for(int m=0; m<z.rows(); m++) {            for(int n=0; n<z.cols(); n++) {                z(m,n)=sampleFullConditional(m,n);            }        }        if(i > BURN_IN && (i%THIN_INTERVAL==0)) {            dispcol++;        }        if ((i > BURN_IN) && (SAMPLE_LAG > 0) && (i % SAMPLE_LAG == 0)) {            updateParams();            if (i % THIN_INTERVAL != 0)                dispcol++;        }        if (dispcol >= 100) {            dispcol = 0;        }    }}

voidNavigator::params_update(){	parameter_update_s param_update;	orb_copy(ORB_ID(parameter_update), _param_update_sub, &param_update);	updateParams();}

Mission::Mission(Navigator *navigator, const char *name) :	MissionBlock(navigator, name),	_param_onboard_enabled(this, "MIS_ONBOARD_EN", false),	_param_takeoff_alt(this, "MIS_TAKEOFF_ALT", false),	_param_dist_1wp(this, "MIS_DIST_1WP", false),	_param_altmode(this, "MIS_ALTMODE", false),	_param_yawmode(this, "MIS_YAWMODE", false),	_param_force_vtol(this, "VT_NAV_FORCE_VT", false),	_param_fw_climbout_diff(this, "FW_CLMBOUT_DIFF", false),	_onboard_mission{},	_offboard_mission{},	_current_onboard_mission_index(-1),	_current_offboard_mission_index(-1),	_need_takeoff(true),	_mission_type(MISSION_TYPE_NONE),	_inited(false),	_home_inited(false),	_need_mission_reset(false),	_missionFeasibilityChecker(),	_min_current_sp_distance_xy(FLT_MAX),	_mission_item_previous_alt(NAN),	_distance_current_previous(0.0f),	_work_item_type(WORK_ITEM_TYPE_DEFAULT){	/* load initial params */	updateParams();}

double SMC::getPosteriorTheta(StateProgression * currState,/                              int currTime,/                              vector< vector<double> > * cloudData,/                              Params params){    int currentClusters = currState->stateProg[currTime>0?(currTime-timeOffset):0].size();    VectorXd postProbTheta(currentClusters);    for(int i=0;i<currentClusters;i++){        Params par(CRP);        if( currState->clusterSizes.back()[i] > 0 ){            if( currTime == 0){                Eigen::MatrixXd clusteredData(getDataOfCluster(i, & currState->assignments, cloudData));                par = updateParams(clusteredData, params, 3);            }else                par = calculatePosteriorParams(currTime, currState,  params, cloudData,i);                vector<double> pr = currState->stateProg[currTime][i].mean;                Vector3d tempMean(3);                tempMean(0) = pr[0];                tempMean(1) = pr[1];                tempMean(2) = pr[2];                postProbTheta(i) = log(ut.multivariateNormalPDF( tempMean,/                                                                par.mu0,/                                                                currState->stateProg[currTime][i].covar.array()/par.kappa0,/                                                                3));        }    }    return  postProbTheta.sum()==-INFINITY?0:postProbTheta.sum();};

voidGpsFailure::advance_gpsf(){	updateParams();	switch (_gpsf_state) {	case GPSF_STATE_NONE:		_gpsf_state = GPSF_STATE_LOITER;		warnx("gpsf loiter");		mavlink_log_critical(_navigator->get_mavlink_log_pub(), "GPS failed: open loop loiter");		break;	case GPSF_STATE_LOITER:		_gpsf_state = GPSF_STATE_TERMINATE;		warnx("gpsf terminate");		mavlink_log_emergency(_navigator->get_mavlink_log_pub(), "no gps recovery, termination");		warnx("mavlink sent");		break;	case GPSF_STATE_TERMINATE:		warnx("gpsf end");		_gpsf_state = GPSF_STATE_END;	default:		break;	}}

void mouse(int button, int state, int x, int y){    if (bShowSliders)     {	    // call list mouse function        if (paramlist->Mouse(x, y, button, state))        {           updateParams();        }    }        int mods;    if (state == GLUT_DOWN)        buttonState |= 1<<button;    else if (state == GLUT_UP)        buttonState = 0;    mods = glutGetModifiers();    if (mods & GLUT_ACTIVE_SHIFT)     {        buttonState = 2;    }     else if (mods & GLUT_ACTIVE_CTRL)     {        buttonState = 3;    }    ox = x; oy = y;    glutPostRedisplay();}

bool Geofence::inside(const struct vehicle_global_position_s &global_position,		      const struct vehicle_gps_position_s &gps_position, float baro_altitude_amsl,		      const struct home_position_s home_pos, bool home_position_set){	updateParams();	_home_pos = home_pos;	_home_pos_set = home_position_set;	if (getAltitudeMode() == Geofence::GF_ALT_MODE_WGS84) {		if (getSource() == Geofence::GF_SOURCE_GLOBALPOS) {			return inside(global_position);		} else {			return inside((double)gps_position.lat * 1.0e-7, (double)gps_position.lon * 1.0e-7,				      (double)gps_position.alt * 1.0e-3);		}	} else {		if (getSource() == Geofence::GF_SOURCE_GLOBALPOS) {			return inside(global_position, baro_altitude_amsl);		} else {			return inside((double)gps_position.lat * 1.0e-7, (double)gps_position.lon * 1.0e-7,				      baro_altitude_amsl);		}	}}

Mission::Mission(Navigator *navigator, const char *name) :	MissionBlock(navigator, name),	_param_onboard_enabled(this, "MIS_ONBOARD_EN", false),	_param_takeoff_alt(this, "MIS_TAKEOFF_ALT", false),	_param_dist_1wp(this, "MIS_DIST_1WP", false),	_param_altmode(this, "MIS_ALTMODE", false),	_param_yawmode(this, "MIS_YAWMODE", false),	_onboard_mission{},	_offboard_mission{},	_current_onboard_mission_index(-1),	_current_offboard_mission_index(-1),	_need_takeoff(true),	_takeoff(false),	_mission_type(MISSION_TYPE_NONE),	_inited(false),	_home_inited(false),	_missionFeasiblityChecker(),	_min_current_sp_distance_xy(FLT_MAX),	_mission_item_previous_alt(NAN),  	_on_arrival_yaw(NAN),	_distance_current_previous(0.0f){	/* load initial params */	updateParams();}

void GeneratorWidget::refresh(){  if (mp_SignInstance != nullptr)  {    setAvailableWare(true);    QString DimStr = tr("scalar");    if (static_cast<const openfluid::fluidx::GeneratorDescriptor*>(mp_Desc)->getVariableSize() > 1)      DimStr = tr("vector");    ui->NameLabel->setText(        tr("Produces %1 variable %2 on %3 (%4)")        .arg(DimStr)        .arg(QString::fromStdString(static_cast<const openfluid::fluidx::GeneratorDescriptor*>(mp_Desc)                                    ->getVariableName()))        .arg(QString::fromStdString(static_cast<const openfluid::fluidx::GeneratorDescriptor*>(mp_Desc)                                    ->getUnitsClass()))        .arg(QString::fromStdString(mp_SignInstance->Signature->Name)));    // TODO add produced variable in signature    ui->InfosSideWidget->update(mp_SignInstance);    updateParams();  }  else  {    setAvailableWare(false);    ui->NameLabel->setText("");  }}

REAL   value(   PrivGlobs    globs,                const REAL s0,                const REAL strike,                const REAL t,                const REAL alpha,                const REAL nu,                const REAL beta,                const unsigned int numX,                const unsigned int numY,                const unsigned int numT) {    initGrid(s0,alpha,nu,t, numX, numY, numT, globs);    initOperator(globs.myX,globs.myDxx);    initOperator(globs.myY,globs.myDyy);    setPayoff(strike, globs);    // globs is global and cannot be privatized thus this loop cannot be    // parallelized yet.    // If updateParams and rollback is independent on i and globs, loop can be    // parallelized by privatization of initGrid, initOperator and setPayoff calls.    // If they write indepedently to globs, privatization is not needed.    for(int i = globs.myTimeline.size()-2;i>=0;--i) // seq    {        updateParams(i,alpha,beta,nu,globs);        rollback(i, globs);    }    return globs.myResult[globs.myXindex][globs.myYindex];}

Takeoff::Takeoff(Navigator *navigator, const char *name) :    MissionBlock(navigator, name),    _param_min_alt(this, "MIS_TAKEOFF_ALT", false){    /* load initial params */    updateParams();}

FollowTarget::FollowTarget(Navigator *navigator, const char *name) :	MissionBlock(navigator, name),	_navigator(navigator),	_param_min_alt(this, "NAV_MIN_FT_HT", false),	_param_tracking_dist(this, "NAV_FT_DST", false),	_param_tracking_side(this, "NAV_FT_FS", false),	_param_tracking_resp(this, "NAV_FT_RS", false),	_param_yaw_auto_max(this, "MC_YAWRAUTO_MAX", false),	_follow_target_state(SET_WAIT_FOR_TARGET_POSITION),	_follow_target_position(FOLLOW_FROM_BEHIND),	_follow_target_sub(-1),	_step_time_in_ms(0.0f),	_follow_offset(OFFSET_M),	_target_updates(0),	_last_update_time(0),	_current_target_motion(),	_previous_target_motion(),	_yaw_rate(0.0F),	_responsiveness(0.0F),	_yaw_auto_max(0.0F),	_yaw_angle(0.0F){	updateParams();	_current_target_motion = {};	_previous_target_motion =  {};	_current_vel.zero();	_step_vel.zero();	_est_target_vel.zero();	_target_distance.zero();	_target_position_offset.zero();	_target_position_delta.zero();}

Navigator::Navigator() :	SuperBlock(NULL, "NAV"),	_task_should_exit(false),	_navigator_task(-1),	_mavlink_fd(-1),	_global_pos_sub(-1),	_gps_pos_sub(-1),	_home_pos_sub(-1),	_vstatus_sub(-1),	_capabilities_sub(-1),	_control_mode_sub(-1),	_onboard_mission_sub(-1),	_offboard_mission_sub(-1),	_param_update_sub(-1),	_pos_sp_triplet_pub(-1),	_mission_result_pub(-1),	_att_sp_pub(-1),	_vstatus{},	_control_mode{},	_global_pos{},	_gps_pos{},	_sensor_combined{},	_home_pos{},	_mission_item{},	_nav_caps{},	_pos_sp_triplet{},	_mission_result{},	_att_sp{},	_mission_item_valid(false),	_loop_perf(perf_alloc(PC_ELAPSED, "navigator")),	_geofence{},	_geofence_violation_warning_sent(false),	_inside_fence(true),	_navigation_mode(nullptr),	_mission(this, "MIS"),	_loiter(this, "LOI"),	_rtl(this, "RTL"),	_rcLoss(this, "RCL"),	_dataLinkLoss(this, "DLL"),	_engineFailure(this, "EF"),	_gpsFailure(this, "GPSF"),	_can_loiter_at_sp(false),	_pos_sp_triplet_updated(false),	_param_loiter_radius(this, "LOITER_RAD"),	_param_acceptance_radius(this, "ACC_RAD"),	_param_datalinkloss_obc(this, "DLL_OBC"),	_param_rcloss_obc(this, "RCL_OBC"){	/* Create a list of our possible navigation types */	_navigation_mode_array[0] = &_mission;	_navigation_mode_array[1] = &_loiter;	_navigation_mode_array[2] = &_rtl;	_navigation_mode_array[3] = &_dataLinkLoss;	_navigation_mode_array[4] = &_engineFailure;	_navigation_mode_array[5] = &_gpsFailure;	_navigation_mode_array[6] = &_rcLoss;	updateParams();}

voidRCLoss::on_activation(){	_rcl_state = RCL_STATE_NONE;	updateParams();	advance_rcl();	set_rcl_item();}

voidDataLinkLoss::on_activation(){	_dll_state = DLL_STATE_NONE;	updateParams();	advance_dll();	set_dll_item();}

	void changeExpoure(float delta)	{		uboParams.exposure += delta;		if (uboParams.exposure < 0.0f) {			uboParams.exposure = 0.0f;		}		updateParams();		updateTextOverlay();	}

EngineFailure::EngineFailure(Navigator *navigator, const char *name) :	MissionBlock(navigator, name),	_ef_state(EF_STATE_NONE){	/* load initial params */	updateParams();	/* initial reset */	on_inactive();}

voidGpsFailure::on_activation(){	_gpsf_state = GPSF_STATE_NONE;	_timestamp_activation = hrt_absolute_time();	updateParams();	advance_gpsf();	set_gpsf_item();}

Geofence::Geofence() : _fence_pub(-1),    _altitude_min(0),    _altitude_max(0),    _verticesCount(0),    param_geofence_on(NULL, "GF_ON", false){    /* Load initial params */    updateParams();}

Loiter::Loiter(Navigator *navigator, const char *name) :	MissionBlock(navigator, name),	_param_min_alt(this, "MIS_LTRMIN_ALT", false),	_param_yawmode(this, "MIS_YAWMODE", false),	_loiter_pos_set(false){	// load initial params	updateParams();}

NavigatorMode::NavigatorMode(Navigator *navigator, const char *name) :    SuperBlock(navigator, name),    _navigator(navigator),    _first_run(true){    /* load initial params */    updateParams();    /* set initial mission items */    on_inactive();}

RCLoss::RCLoss(Navigator *navigator, const char *name) :	MissionBlock(navigator, name),	_param_loitertime(this, "LT"),	_rcl_state(RCL_STATE_NONE){	/* load initial params */	updateParams();	/* initial reset */	on_inactive();}

RealParsedODEKernel::computeQpOffDiagJacobian(unsigned int jvar){  int i = _arg_index[jvar];  if (i < 0)    return 0.0;  updateParams();  return evaluate(_func_dFdarg[i]);}

void BlockSegwayController::update() {	// wait for a sensor update, check for exit condition every 100 ms	if (poll(&_attPoll, 1, 100) < 0) return; // poll error	uint64_t newTimeStamp = hrt_absolute_time();	float dt = (newTimeStamp - _timeStamp) / 1.0e6f;	_timeStamp = newTimeStamp;	// check for sane values of dt	// to prevent large control responses	if (dt > 1.0f || dt < 0) return;	// set dt for all child blocks	setDt(dt);	// check for new updates	if (_param_update.updated()) updateParams();	// get new information from subscriptions	updateSubscriptions();	// default all output to zero unless handled by mode	for (unsigned i = 2; i < NUM_ACTUATOR_CONTROLS; i++)		_actuators.control[i] = 0.0f;	// only update guidance in auto mode	if (_status.main_state == MAIN_STATE_AUTO) {		// update guidance	}	// compute speed command	float spdCmd = -th2v.update(_att.pitch) - q2v.update(_att.pitchspeed);	// handle autopilot modes	if (_status.main_state == MAIN_STATE_AUTO ||	    _status.main_state == MAIN_STATE_ALTCTL ||	    _status.main_state == MAIN_STATE_POSCTL) {		_actuators.control[0] = spdCmd;		_actuators.control[1] = spdCmd;	} else if (_status.main_state == MAIN_STATE_MANUAL) {		if (_status.navigation_state == NAVIGATION_STATE_DIRECT) {			_actuators.control[CH_LEFT] = _manual.throttle;			_actuators.control[CH_RIGHT] = _manual.pitch;		} else if (_status.navigation_state == NAVIGATION_STATE_STABILIZE) {			_actuators.control[0] = spdCmd;			_actuators.control[1] = spdCmd;		}	}	// update all publications	updatePublications();}

Params SMC::calculatePosteriorParams( int currTime,/                                      StateProgression * currState,/                                      Params params,/                                      vector< vector<double> > * cloudData,                                      int curDataPoint){    MatrixXd data_t= getDataOfCluster(curDataPoint, & currState->assignments, cloudData);    Eigen::MatrixXd clusteredData(data_t.rows()+ params.auxiliaryNum, data_t.cols() );    if(clusteredData.rows()>0 &&  currState->stateProg[currTime-timeOffset].size()>0){        SufficientStatistics ss = currState->stateProg[currTime-timeOffset][curDataPoint];        VectorXd mean(ss.mean.size());        mean(0) = ss.mean[0];        mean(1) = ss.mean[1];        mean(2) = ss.mean[2];        //Format: params = {crp, del, #aux, tau0, v0, mu0, k0, q0, _,_,_<-#colorbins?}        Eigen::MatrixXd auxGausSamples = ut.sampleMultivariateNormal(mean,ss.covar, params.auxiliaryNum, 3);        Eigen::MatrixXd auxMultSamples = ut.sampleMultinomial( ss.categorical, params.auxiliaryNum);        Eigen::VectorXd auxExpSamples  = ut.exprnd(ss.exponential , params.auxiliaryNum);        MatrixXd C(auxGausSamples.transpose().rows(),/                      auxExpSamples.cols()*3/                   +  auxGausSamples.transpose().cols()/                   +  auxMultSamples.cols());        C << auxGausSamples.transpose() , auxExpSamples,auxExpSamples,auxExpSamples, auxMultSamples;        clusteredData << data_t,  C;        return updateParams(clusteredData, params, 3);    }else if(currState->stateProg[currTime-timeOffset].size()>0){        SufficientStatistics ss = currState->stateProg[currTime-timeOffset][curDataPoint];        VectorXd mean(ss.mean.size());        mean(0) = ss.mean[0];        mean(1) = ss.mean[1];        mean(2) = ss.mean[2];        Eigen::MatrixXd auxGausSamples =  ut.sampleMultivariateNormal(mean,ss.covar, params.auxiliaryNum, 3);        Eigen::MatrixXd auxMultSamples = ut.sampleMultinomial( ss.categorical, params.auxiliaryNum);        Eigen::VectorXd auxExpSamples  = ut.exprnd(ss.exponential , params.auxiliaryNum);        MatrixXd C( auxGausSamples.transpose().rows(), auxExpSamples.cols()*3 +/                    auxGausSamples.transpose().cols() + auxMultSamples.cols());        C << auxGausSamples.transpose() , auxExpSamples,auxExpSamples,auxExpSamples, auxMultSamples;        return updateParams(C, params, 3);    }else        return  updateParams(clusteredData, params, 3);}

void Mixer::setStereo(bool stereo){    if (m_stereo != stereo)    {        m_stereo = stereo;        m_mix.resize(m_stereo ? 2 : 1);        updateParams();    }}