IvPFunction* BHV_Pulse::onRunState()
{
// Part 1: Build the IvP function
IvPFunction *ipf = 0;
UpdateStateVariables();
double curr_time = getBufferCurrTime();
if (m_pulse) {
// this part might not be necessary?
// do
// ;
// while ((curr_time - m_wpt_time) < 5.0);
if ((curr_time - m_wpt_time) >= 5.0) { //just to be super super careful
XYRangePulse pulse;
pulse.set_x(m_x);
pulse.set_y(m_y);
pulse.set_label("bhv_pulse");
pulse.set_color("edge","yellow");
pulse.set_color("fill","yellow");
pulse.set_time(curr_time);
pulse.set_rad(m_range);
pulse.set_duration(m_pulse_duration);
string spec = pulse.get_spec();
postMessage("VIEW_RANGE_PULSE", spec);
m_pulse = false;
}
}
// Part N: Prior to returning the IvP function, apply the priority wt
// Actual weight applied may be some value different than the configured
// m_priority_wt, depending on the behavior author's insite.
if(ipf)
ipf->setPWT(m_priority_wt);
return(ipf);
}
IvPFunction *BHV_ZigLeg::onRunState()
{
// Grab relevant variables from Helm
bool ok1, ok2, ok3, ok4;
double x = getBufferDoubleVal("NAV_X", ok1);
double y = getBufferDoubleVal("NAV_Y", ok2);
double idx = getBufferDoubleVal("WPT_INDEX", ok3);
double heading = getBufferDoubleVal("NAV_HEADING", ok4);
if (!ok1 || !ok2 || !ok3 || !ok4) {
postWMessage("Error, missing info");
return(0);
}
// check if index has changed
if (idx != _last_idx) {
// start the clock
_hit_time = getBufferCurrTime();
_armed = true;
_zig_armed = true;
_last_idx = idx;
}
// check clock
if (_armed && getBufferCurrTime() - _hit_time > 5.0) {
// BOOM!
XYRangePulse pulse;
pulse.set_x(x);
pulse.set_y(y);
pulse.set_label("bhv_pulse");
pulse.set_rad(_pulse_range);
pulse.set_duration(_pulse_duration);
pulse.set_time(getBufferCurrTime());
pulse.set_color("edge", "yellow");
pulse.set_color("fill", "yellow");
string spec = pulse.get_spec();
postMessage("VIEW_RANGE_PULSE",spec);
_armed = false;
}
IvPFunction *ipf = 0;
if (getBufferCurrTime() - _hit_time > 5.0 &&
getBufferCurrTime() - _hit_time < _zig_duration + 5.0) {
// cout << "Changing Heading" << endl;
if (_zig_armed) {
_last_heading = heading;
_zig_armed = false;
}
double new_heading = _last_heading + _zig_angle;
if (new_heading > 360) {
new_heading = new_heading - 360;
}
ZAIC_PEAK head_zaic(m_domain, "course");
head_zaic.setSummit(new_heading);
head_zaic.setPeakWidth(0);
head_zaic.setBaseWidth(20.0);
head_zaic.setSummitDelta(0);
head_zaic.setValueWrap(true);
if ( head_zaic.stateOK() == false ) {
string warnings = "Course Zaic problems " + head_zaic.getWarnings();
postWMessage(warnings);
return(0);
}
ipf = head_zaic.extractIvPFunction();
if (ipf) {
ipf -> setPWT(m_priority_wt);
}
}
return(ipf);
}
