Ejemplo n.º 1
0
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();
}
Ejemplo n.º 2
0
REAL
value(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];
}
Ejemplo n.º 3
0
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;
        }
    }
}
Ejemplo n.º 4
0
void
Navigator::params_update()
{
	parameter_update_s param_update;
	orb_copy(ORB_ID(parameter_update), _param_update_sub, &param_update);
	updateParams();
}
Ejemplo n.º 5
0
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();
}
Ejemplo n.º 6
0
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();
};
Ejemplo n.º 7
0
void
GpsFailure::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;
	}
}
Ejemplo n.º 8
0
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();
}
Ejemplo n.º 9
0
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);
		}
	}
}
Ejemplo n.º 10
0
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();
}
Ejemplo n.º 11
0
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("");
  }

}
Ejemplo n.º 12
0
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];
}
Ejemplo n.º 13
0
Takeoff::Takeoff(Navigator *navigator, const char *name) :
    MissionBlock(navigator, name),
    _param_min_alt(this, "MIS_TAKEOFF_ALT", false)
{
    /* load initial params */
    updateParams();
}
Ejemplo n.º 14
0
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();
}
Ejemplo n.º 15
0
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();
}
Ejemplo n.º 16
0
void
RCLoss::on_activation()
{
	_rcl_state = RCL_STATE_NONE;
	updateParams();
	advance_rcl();
	set_rcl_item();
}
Ejemplo n.º 17
0
void
DataLinkLoss::on_activation()
{
	_dll_state = DLL_STATE_NONE;
	updateParams();
	advance_dll();
	set_dll_item();
}
Ejemplo n.º 18
0
void
DataLinkLoss::on_active()
{
	if (is_mission_item_reached()) {
		updateParams();
		advance_dll();
		set_dll_item();
	}
}
Ejemplo n.º 19
0
	void changeExpoure(float delta)
	{
		uboParams.exposure += delta;
		if (uboParams.exposure < 0.0f) {
			uboParams.exposure = 0.0f;
		}
		updateParams();
		updateTextOverlay();
	}
Ejemplo n.º 20
0
void
RCLoss::on_active()
{
	if (is_mission_item_reached()) {
		updateParams();
		advance_rcl();
		set_rcl_item();
	}
}
Ejemplo n.º 21
0
EngineFailure::EngineFailure(Navigator *navigator, const char *name) :
	MissionBlock(navigator, name),
	_ef_state(EF_STATE_NONE)
{
	/* load initial params */
	updateParams();
	/* initial reset */
	on_inactive();
}
Ejemplo n.º 22
0
void
GpsFailure::on_activation()
{
	_gpsf_state = GPSF_STATE_NONE;
	_timestamp_activation = hrt_absolute_time();
	updateParams();
	advance_gpsf();
	set_gpsf_item();
}
Ejemplo n.º 23
0
Geofence::Geofence() : _fence_pub(-1),
    _altitude_min(0),
    _altitude_max(0),
    _verticesCount(0),
    param_geofence_on(NULL, "GF_ON", false)
{
    /* Load initial params */
    updateParams();
}
Ejemplo n.º 24
0
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();
}
Ejemplo n.º 26
0
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();
}
Ejemplo n.º 27
0
Real
ParsedODEKernel::computeQpOffDiagJacobian(unsigned int jvar)
{
  int i = _arg_index[jvar];
  if (i < 0)
    return 0.0;

  updateParams();
  return evaluate(_func_dFdarg[i]);
}
Ejemplo n.º 28
0
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();

}
Ejemplo n.º 29
0
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);
}
Ejemplo n.º 30
0
void Mixer::setStereo(bool stereo)
{
    if (m_stereo != stereo)
    {
        m_stereo = stereo;

        m_mix.resize(m_stereo ? 2 : 1);

        updateParams();
    }
}