void
BasicDeviceDriver::consume(const IMC::EstimatedState* msg)
{
if (msg->getSource() != getSystemId())
return;
if (!isActive())
return;
onEstimatedState(*msg);
}
void
BasicUAVAutopilot::consume(const IMC::EstimatedState* msg)
{
if (!isActive())
return;
m_estate = *msg;
// Compute time delta.
float timestep = m_last_estate.getDelta();
// Check if we have a valid time delta.
if (timestep < 0.0)
return;
onEstimatedState(timestep, msg);
}
void
BasicAutopilot::consume(const IMC::EstimatedState* msg)
{
if (!isActive())
return;
if (msg->getSource() != getSystemId())
return;
m_estate = *msg;
// check if vertical control mode is valid
if (m_vertical_mode >= VERTICAL_MODE_SIZE)
{
signalBadVertical(DTR("invalid vertical control mode %d"));
return;
}
else if (m_vertical_mode == VERTICAL_MODE_NONE)
{
float delta = m_vmode_delta.getDelta();
if (delta > 0.0)
m_vmode_wait += delta;
if (m_vmode_wait > c_mode_timeout)
{
m_vmode_wait = 0.0;
signalBadVertical(DTR("timed out while waiting for vertical control mode"));
}
return;
}
else if (m_vertical_mode == VERTICAL_MODE_ALTITUDE)
{
// Required depth for bottom following = current depth + required altitude correction
// in case the follow depth is lower than 0.0 it will be capped since the btrack
// is not possible
// check if we have altitude measurements
if (msg->alt < 0.0)
{
setEntityState(IMC::EntityState::ESTA_ERROR, DTR(c_no_alt));
err("%s", DTR(c_no_alt));
return;
}
m_bottom_follow_depth = m_estate.depth + (msg->alt - m_vertical_ref);
if (m_bottom_follow_depth < 0.0)
{
if (m_btrack_wrn.overflow())
{
std::string desc = String::str(DTR("water column not deep enough for altitude control ( %0.2f < %0.2f )"), msg->alt + m_estate.depth, m_vertical_ref);
setEntityState(IMC::EntityState::ESTA_NORMAL, desc);
war("%s", desc.c_str());
m_btrack_wrn.reset();
}
}
else
{
m_btrack_wrn.reset();
}
// Will not let the bottom follow depth be lower than zero
// to avoid causing excessive controller integration
m_bottom_follow_depth = std::max(m_bottom_follow_depth, (float)0.0);
}
else if (m_vertical_mode == VERTICAL_MODE_PITCH)
{
if (m_estate.depth >= m_max_depth - c_depth_margin)
{
const std::string desc = DTR("getting too close to maximum admissible depth");
setEntityState(IMC::EntityState::ESTA_ERROR, desc);
err("%s", desc.c_str());
requestDeactivation();
return;
}
}
// check if yaw control mode is valid
if (m_yaw_mode >= YAW_MODE_SIZE)
{
signalBadYaw(DTR("invalid yaw control mode %d"));
return;
}
else if (m_yaw_mode == YAW_MODE_NONE)
{
float delta = m_ymode_delta.getDelta();
if (delta > 0.0)
m_ymode_wait += delta;
if (m_ymode_wait > c_mode_timeout)
{
m_ymode_wait = 0.0;
signalBadYaw(DTR("timed out waiting for yaw control mode"));
}
return;
}
// Compute time delta.
float timestep = m_last_estate.getDelta();
// Check if we have a valid time delta.
if (timestep < 0.0)
return;
onEstimatedState(timestep, msg);
}
