//! Update yoyo motion
//! @param[in] startup whether or not the yoyo motion is starting now
//! @param[in] state_z current z position (negative if altitude)
//! @param[in] pitch current pitch angle
//! @return new pitch command
float
update(bool startup, float state_z, float pitch)
{
if (!startup)
{
double t_dist = m_dir * (state_z - m_target_z);
if (t_dist >= 0)
{
m_old_pitch = trimPitch(m_dir * m_pitch_ref, pitch);
return m_old_pitch;
}
m_dir = -m_dir;
m_source_z = m_target_z;
}
else if (!m_dir)
{
// Start *or* resume (after being off-track)
// yoyo mode by figuring out whether to go up or down
double min_z = m_z_ref - m_amplitude;
double max_z = m_z_ref + m_amplitude;
double dmin = std::fabs(state_z - min_z);
double dmax = std::fabs(state_z - max_z);
m_dir = dmin < dmax ? -1 : 1;
m_source_z = state_z;
}
m_target_z = m_z_ref - m_dir * m_amplitude;
std::stringstream ss;
ss << (m_dir < 0 ? "descending " : "ascending ")
<< m_source_z << " --> " << m_target_z;
m_task->debug("%s", ss.str().c_str());
// Trim pitch value for more smooth control
// (this is an optional measure, in principle pitch controller
// should "know" how to proceed smoothly)
m_old_pitch = trimPitch(m_dir * m_pitch_ref, pitch);
return m_old_pitch;
}
//! Stabilize pitch in a certain value
//! @param[in] desired_pitch pitch angle at which to stabilize
//! @param[in] current_pitch current pitch angle
//! @return pitch angle to command
inline float
stabilize(float desired_pitch, float current_pitch)
{
return trimPitch(desired_pitch, current_pitch);
}
//! Update yoyo motion
//! @param[in] startup whether or not the yoyo motion is starting now
//! @param[in] depth current depth position
//! @param[in] altitude current altitude position
//! @param[in] pitch current pitch angle
//! @return new pitch command
float
update(bool startup, float depth, float alt, float pitch)
{
float state_z;
if (m_zunits == IMC::Z_DEPTH)
state_z = depth;
else
state_z = alt;
if (m_invert)
{
m_dir = -m_dir;
m_source_z = m_target_z;
m_invert = false;
m_task->debug("ascending due to brake");
}
else if (!startup)
{
double t_dist;
if (m_zunits == IMC::Z_DEPTH)
{
// special case for yoyo at surface.
if (m_z_ref + m_amplitude < c_dist_to_ref)
{
m_old_pitch = trimPitch(m_dir * m_pitch_ref, pitch);
return m_old_pitch;
}
t_dist = m_dir * (state_z - m_target_z);
}
else
{
t_dist = m_dir * (m_target_z - state_z);
}
if (t_dist >= std::min(m_amplitude, c_dist_to_ref)
&& withinLimits(depth, alt))
{
m_old_pitch = trimPitch(m_dir * m_pitch_ref, pitch);
return m_old_pitch;
}
m_dir = -m_dir;
m_source_z = m_target_z;
}
else if (m_dir == DIR_NONE)
{
// Start *or* resume (after being off-track)
// yoyo mode by figuring out whether to go up or down
double min_z = m_z_ref - m_amplitude;
double max_z = m_z_ref + m_amplitude;
double dmin = std::fabs(state_z - min_z);
double dmax = std::fabs(state_z - max_z);
if (m_zunits == IMC::Z_DEPTH)
{
if (max_z > c_dist_to_ref)
m_dir = dmin < dmax ? DIR_DOWN : DIR_UP;
else
m_dir = DIR_UP;
}
else
{
m_dir = dmin < dmax ? DIR_UP : DIR_DOWN;
}
m_source_z = state_z;
}
// update target
if (m_zunits == IMC::Z_DEPTH)
{
// keep vehicle within surface and maximum absolute depth
if (m_dir == DIR_UP)
m_target_z = std::max(0.0f, m_z_ref - m_amplitude);
else
m_target_z = std::min(m_max_depth - c_depth_margin, m_z_ref + m_amplitude);
}
else
{
// keep the vehicle above minimum altitude
if (m_dir == DIR_UP)
m_target_z = m_z_ref + m_amplitude;
else
m_target_z = std::max(m_min_alt, m_z_ref - m_amplitude);
}
std::stringstream ss;
ss << (m_dir < 0 ? "descending " : "ascending ")
<< m_source_z << " --> " << m_target_z;
m_task->debug("%s", ss.str().c_str());
// Trim pitch value for more smooth control
// (this is an optional measure, in principle pitch controller
// should "know" how to proceed smoothly)
m_old_pitch = trimPitch(m_dir * m_pitch_ref, pitch);
return m_old_pitch;
}
