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();
}
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];
}
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;
}
}
}
void
Navigator::params_update()
{
parameter_update_s param_update;
orb_copy(ORB_ID(parameter_update), _param_update_sub, ¶m_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();
};
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;
}
}
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();
}
void
RCLoss::on_activation()
{
_rcl_state = RCL_STATE_NONE;
updateParams();
advance_rcl();
set_rcl_item();
}
void
DataLinkLoss::on_activation()
{
_dll_state = DLL_STATE_NONE;
updateParams();
advance_dll();
set_dll_item();
}
void
DataLinkLoss::on_active()
{
if (is_mission_item_reached()) {
updateParams();
advance_dll();
set_dll_item();
}
}
void changeExpoure(float delta)
{
uboParams.exposure += delta;
if (uboParams.exposure < 0.0f) {
uboParams.exposure = 0.0f;
}
updateParams();
updateTextOverlay();
}
void
RCLoss::on_active()
{
if (is_mission_item_reached()) {
updateParams();
advance_rcl();
set_rcl_item();
}
}
EngineFailure::EngineFailure(Navigator *navigator, const char *name) :
MissionBlock(navigator, name),
_ef_state(EF_STATE_NONE)
{
/* load initial params */
updateParams();
/* initial reset */
on_inactive();
}
void
GpsFailure::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();
}
Real
ParsedODEKernel::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();
}
}
