void
BottomTracker::onAvoiding(void)
{
// If ranges or altitude cannot be used, then we're clueless
if (m_sdata->isSurface(m_estate) || !isAltitudeValid())
{
err(DTR("unable to avoid obstacle"));
return;
}
// check if slope is safe right now and
// check if buoyancy has pulled the vehicle up to a safe depth/altitude
if (!m_sdata->isTooSteep() && !m_sdata->isRangeLow(m_estate.theta))
{
if ((m_z_ref.z_units == IMC::Z_ALTITUDE) && (m_estate.alt >= m_z_ref.value) &&
m_estate.alt >= m_args->min_alt + c_alt_hyst)
{
debug("avoiding: above altitude reference and slope is safe -> tracking");
// Stop braking
brake(false);
dispatchSameZ();
m_mstate = SM_TRACKING;
return;
}
else if ((m_forced == FC_ALTITUDE) && (m_estate.alt >= m_args->adm_alt))
{
debug("avoiding: slope is safe, keep forcing altitude -> tracking");
// Stop braking
brake(false);
dispatchAdmAltitude();
m_mstate = SM_TRACKING;
return;
}
else if (m_z_ref.z_units == IMC::Z_DEPTH)
{
debug("avoiding: units are depth, carry on -> depth");
// Stop braking
brake(false);
dispatchSameZ();
m_mstate = SM_DEPTH;
return;
}
}
}
void
BottomTracker::onLimitDepth(void)
{
// if reference is for altitude now
if ((m_z_ref.z_units == IMC::Z_ALTITUDE) && (m_forced != FC_DEPTH))
{
debug("limit depth: units are altitude now -> tracking");
m_forced = FC_NONE;
dispatchSameZ();
m_mstate = SM_TRACKING;
return;
}
if ((m_z_ref.z_units == IMC::Z_DEPTH) && (m_z_ref.value < m_args->depth_limit))
{
debug("limit depth: units are depth now -> idle");
m_forced = FC_NONE;
m_mstate = SM_IDLE;
dispatchSameZ();
return;
}
if (m_sdata->isRangeLow(m_estate.theta))
{
debug(String::str("limit depth: frange is too low: %.2f -> avoiding",
m_sdata->getFRange()));
m_forced = FC_NONE;
brake(true);
m_mstate = SM_AVOIDING;
return;
}
// check if depth control is being forced and if we can switch back
if ((m_forced == FC_DEPTH) &&
(m_estate.depth + m_estate.alt - m_z_ref.value < m_args->depth_limit))
{
debug("limit depth: depth is no longer near the limit -> tracking");
m_forced = FC_NONE;
dispatchSameZ();
m_mstate = SM_TRACKING;
return;
}
}
void
BottomTracker::onUnsafe(void)
{
m_cparcel.d = m_sdata->updateSlopeTop(m_estate);
// Test if slope top is no longer an issue
bool away_top = m_sdata->isTopCleared();
// Can we use altitude
if (!isAltitudeValid())
{
if (away_top)
{
debug("cannot use altitude");
debug("moving away from slope top or ");
debug(String::str("distance to slope top is short: %.2f",
m_sdata->getDistanceToSlope()));
debug("moving to tracking");
dispatchSameZ();
m_mstate = SM_TRACKING;
m_sdata->renderSlopeInvalid();
}
return;
}
// check if altitude or forward range value is becoming dangerous
if ((m_estate.alt < m_args->min_alt) || m_sdata->isRangeLow())
{
if (m_estate.alt < m_args->min_alt)
debug(String::str("altitude is too low: %.2f.", m_estate.alt));
else
debug(String::str("frange is too low: %.2f.", m_sdata->getFRange()));
brake(true);
m_mstate = SM_AVOIDING;
return;
}
if (m_sdata->isSurface(m_estate))
{
debug("cannot use range. tracking");
dispatchSameZ();
m_mstate = SM_TRACKING;
return;
}
// check if slope is safe
if (!m_sdata->isTooSteep())
{
if (away_top)
{
debug("moving away from slope top or ");
debug(String::str("distance to slope top is short: %.2f",
m_sdata->getDistanceToSlope()));
debug("moving to tracking");
// dispatch same z reference sent by upper layer
dispatchSameZ();
m_mstate = SM_TRACKING;
m_sdata->renderSlopeInvalid();
return;
}
} // check if slope is becoming steeper
else if (m_sdata->isSlopeIncreasing())
{
if (m_args->check_trend || (!m_args->check_trend && m_estate.theta < 0.0))
{
debug(String::str("slope is becoming steeper %.2f",
Angles::degrees(m_sdata->getSlope())));
dispatchSafeDepth();
}
}
}
void
BottomTracker::onTracking(void)
{
// Render slope top as invalid here
m_sdata->renderSlopeInvalid();
float depth_ref = m_estate.depth + m_estate.alt - m_z_ref.value;
// if units are now altitude
if (m_forced == FC_ALTITUDE)
{
if (m_z_ref.z_units == IMC::Z_ALTITUDE)
{
debug("units are altitude now. moving to tracking");
m_forced = FC_NONE;
m_mstate = SM_TRACKING;
return;
}
else if (depth_ref >= m_args->adm_alt + c_depth_hyst)
{
debug("depth reference is now safe");
m_forced = FC_NONE;
dispatchSameZ();
m_mstate = SM_DEPTH;
return;
}
} // if reference is for depth now
else if ((m_forced != FC_ALTITUDE) && (m_z_ref.z_units == IMC::Z_DEPTH))
{
debug("units are depth now");
m_mstate = SM_DEPTH;
return;
}
// Do not attempt to interfere if we cant use altitude
if (!isAltitudeValid())
return;
// check if altitude value is becoming dangerous
if (m_estate.alt < m_args->min_alt)
{
debug(String::str("altitude is too low: %.2f.", m_estate.alt));
brake(true);
m_mstate = SM_AVOIDING;
return;
}
// check safety
if (!checkSafety())
return;
// if reaching a limit in altitude
if (depth_ref > m_args->depth_limit + c_depth_hyst &&
m_estate.depth > m_args->depth_limit)
{
info(DTR("depth is reaching unacceptable values, forcing depth control"));
m_forced = FC_DEPTH;
dispatchLimitDepth();
m_mstate = SM_LIMITDEPTH;
return;
}
}
