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::onDepth(void)
{
// Render slope top as invalid here
m_sdata->renderSlopeInvalid();
// if reference is for altitude now
if (m_z_ref.z_units == IMC::Z_ALTITUDE)
{
debug("units are altitude now. moving to tracking");
m_mstate = SM_TRACKING;
return;
}
// Do not attempt to interfere if we cant use altitude
if (!isAltitudeValid())
return;
// if reaching a limit in altitude
float depth_ref = m_estate.depth + m_estate.alt - m_z_ref.value;
// if altitude is not admissible for depth control
if (depth_ref < m_args->adm_alt && m_estate.alt < m_args->adm_alt)
{
debug("below admissible depth. moving to altitude control.");
m_forced = FC_ALTITUDE;
m_mstate = SM_TRACKING;
dispatchAdmAltitude();
return;
}
// check safety
if (!checkSafety())
return;
}
