IvPFunction *BHV_SimpleWaypoint::buildFunctionWithZAIC()
{
ZAIC_PEAK spd_zaic(m_domain, "speed");
spd_zaic.setSummit(m_desired_speed);
spd_zaic.setPeakWidth(0.5);
spd_zaic.setBaseWidth(1.0);
spd_zaic.setSummitDelta(0.8);
if(spd_zaic.stateOK() == false) {
string warnings = "Speed ZAIC problems " + spd_zaic.getWarnings();
postWMessage(warnings);
return(0);
}
double rel_ang_to_wpt = relAng(m_osx, m_osy, m_nextpt.x(), m_nextpt.y());
ZAIC_PEAK crs_zaic(m_domain, "course");
crs_zaic.setSummit(rel_ang_to_wpt);
crs_zaic.setPeakWidth(0);
crs_zaic.setBaseWidth(180.0);
crs_zaic.setSummitDelta(0);
crs_zaic.setValueWrap(true);
if(crs_zaic.stateOK() == false) {
string warnings = "Course ZAIC problems " + crs_zaic.getWarnings();
postWMessage(warnings);
return(0);
}
IvPFunction *spd_ipf = spd_zaic.extractIvPFunction();
IvPFunction *crs_ipf = crs_zaic.extractIvPFunction();
OF_Coupler coupler;
IvPFunction *ivp_function = coupler.couple(crs_ipf, spd_ipf, 50, 50);
return(ivp_function);
}
IvPFunction *BHV_StationKeep::onRunState()
{
// Set m_osx, m_osy, m_currtime, m_dist_to_station
if(!updateInfoIn())
return(0);
postMessage("DIST_TO_STATION", m_dist_to_station);
if(!m_station_set) {
postWMessage("STATION_POINT_NOT_SET");
postStationMessage(false);
return(0);
}
if(m_inner_radius > m_outer_radius)
m_outer_radius = m_inner_radius;
// If station-keeping at depth is enabled, determine current state.
updateHibernationState();
postMessage(m_pskeep_variable, toupper(m_pskeep_state));
postStationMessage(true);
double angle_to_station = relAng(m_osx, m_osy,
m_station_x, m_station_y);
double desired_speed = 0;
// If the pskeepp_state is hibernating it means that station-keeping
// at depth is enabled, but currently not warranting action. Action
// may also not be warranted due to being close enough to the
// station-keep point.
if((m_pskeep_state == "hibernating") ||
(m_dist_to_station <= m_inner_radius)) {
desired_speed = 0;
}
else if((m_dist_to_station > m_inner_radius) &&
(m_dist_to_station < m_outer_radius)) {
// Note: range cannot be zero due to above if-condition
double range = m_outer_radius - m_inner_radius;
double pct = (m_dist_to_station - m_inner_radius) / range;
desired_speed = pct * m_outer_speed;
}
else // m_dist_to_station >= m_outer_radius
desired_speed = m_extra_speed;
ZAIC_PEAK spd_zaic(m_domain, "speed");
spd_zaic.setSummit(desired_speed);
spd_zaic.setBaseWidth(0.4);
spd_zaic.setPeakWidth(0.0);
spd_zaic.setSummitDelta(0.0);
spd_zaic.setMinMaxUtil(0, 25);
IvPFunction *spd_ipf = spd_zaic.extractIvPFunction();
ZAIC_PEAK crs_zaic(m_domain, "course");
crs_zaic.setSummit(angle_to_station);
crs_zaic.setBaseWidth(180.0);
crs_zaic.setValueWrap(true);
IvPFunction *crs_ipf = crs_zaic.extractIvPFunction();
OF_Coupler coupler;
IvPFunction *ipf = coupler.couple(crs_ipf, spd_ipf);
if(ipf)
ipf->setPWT(m_priority_wt);
return(ipf);
}
