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; }