Periodic::Periodic(const std::string& name, Context& ctx):
Task(name, ctx),
m_run_count(0),
m_run_time(0)
{
param(DTR_RT("Execution Frequency"), m_frequency)
.units(Units::Hertz)
.defaultValue("1.0")
.description(DTR("Frequency at which task is executed"));
}
//! Constructor.
//! @param[in] name task name.
//! @param[in] ctx context.
Task(const std::string& name, Tasks::Context& ctx):
DUNE::Tasks::Task(name, ctx),
m_uart(NULL),
m_ctl(NULL),
m_mcu_ent(NULL)
{
// Define configuration parameters.
param("Serial Port - Device", m_args.uart_dev)
.defaultValue("")
.description("Serial port device used to communicate with the sensor");
param("Watchdog Timeout", m_args.wdog_tout)
.units(Units::Second)
.defaultValue("2.0")
.minimumValue("1.0")
.description("Watchdog timeout");
param("LED - Names", m_args.led_names)
.defaultValue("")
.size(c_led_count)
.description("List of LED names");
param(DTR_RT("LED - Patterns"), m_args.led_patterns)
.visibility(Tasks::Parameter::VISIBILITY_USER)
.scope(Tasks::Parameter::SCOPE_GLOBAL)
.defaultValue("")
.maximumSize(8)
.description(DTR_RT("List of LED patterns"));
param(DTR_RT("LED - Patterns Pulse Width"), m_args.led_patterns_pw)
.visibility(Tasks::Parameter::VISIBILITY_USER)
.scope(Tasks::Parameter::SCOPE_GLOBAL)
.minimumValue("0")
.maximumValue("20000")
.defaultValue("5000")
.description(DTR_RT("Pulse width for LED patterns"));
param(DTR_RT("LED - External Driver"), m_args.ext_drv)
.visibility(Tasks::Parameter::VISIBILITY_USER)
.scope(Tasks::Parameter::SCOPE_GLOBAL)
.defaultValue("false")
.description(DTR_RT("Enable external LED driver"));
param(DTR_RT("LED - External Trigger"), m_args.ext_trg)
.visibility(Tasks::Parameter::VISIBILITY_USER)
.scope(Tasks::Parameter::SCOPE_GLOBAL)
.defaultValue("false")
.description(DTR_RT("Enable external LED trigger"));
bind<IMC::SetLedBrightness>(this);
bind<IMC::QueryLedBrightness>(this);
}
namespace Status
{
static const char* c_status_messages[33] =
{
DTR_RT("initializing"),
DTR_RT("idle"),
DTR_RT("active"),
DTR_RT("activating"),
DTR_RT("deactivating"),
DTR_RT("input/output error"),
DTR_RT("internal error"),
DTR_RT("CPU usage is too high"),
DTR_RT("fuel is running low"),
DTR_RT("fuel level estimation is not reliable"),
DTR_RT("fuel reserve"),
DTR_RT("calibrating"),
DTR_RT("calibrated"),
DTR_RT("not aligned"),
DTR_RT("aligning"),
DTR_RT("aligned"),
DTR_RT("powering down"),
DTR_RT("communication error"),
DTR_RT("synchronized"),
DTR_RT("synchronizing"),
DTR_RT("not synchronized"),
DTR_RT("waiting for GPS fix"),
DTR_RT("waiting for configuration of LBL beacons"),
DTR_RT("waiting for solution to converge"),
DTR_RT("missing data"),
DTR_RT("invalid checksum"),
DTR_RT("invalid version"),
DTR_RT("active but without bottom lock"),
DTR_RT("no medium data: user must control device"),
DTR_RT("active (no medium data: need user input)"),
DTR_RT("idle (no medium data: need user input)"),
DTR_RT("connecting"),
DTR_RT("connected")
};
const char*
getString(Code code)
{
return c_status_messages[code];
}
}
namespace Control
{
//! Timeout when waiting for some control mode
static const float c_mode_timeout = 5.0f;
//! Cooldown for the warning thrown when bottom tracking cannot be done
static const float c_btrack_wrn_cooldown = 15.0f;
//! No altitude measurements message
static const char* c_no_alt = DTR_RT("no valid altitude measurements");
//! Depth margin when checking for maximum admissible depth
static const float c_depth_margin = 1.0;
BasicAutopilot::BasicAutopilot(const std::string& name, Tasks::Context& ctx,
const uint32_t controllable_loops, const uint32_t required_loops):
Tasks::Task(name, ctx),
m_vertical_mode(VERTICAL_MODE_NONE),
m_yaw_mode(YAW_MODE_NONE),
m_aloops(0),
m_controllable_loops(controllable_loops),
m_required_loops(required_loops),
m_scope_ref(0)
{
param("Heading Rate Bypass", m_hrate_bypass)
.defaultValue("false")
.description("Bypass heading rate controller and use reference directly on torques");
m_ctx.config.get("General", "Absolute Maximum Depth", "50.0", m_max_depth);
// Initialize entity state.
setEntityState(IMC::EntityState::ESTA_NORMAL, Status::CODE_IDLE);
// Register handler routines.
bind<IMC::EstimatedState>(this);
bind<IMC::DesiredHeadingRate>(this);
bind<IMC::DesiredHeading>(this);
bind<IMC::DesiredZ>(this);
bind<IMC::DesiredPitch>(this);
bind<IMC::DesiredVelocity>(this);
bind<IMC::ControlLoops>(this);
}
BasicAutopilot::~BasicAutopilot(void)
{ }
void
BasicAutopilot::onResourceInitialization(void)
{
requestDeactivation();
}
void
BasicAutopilot::reset(void)
{
m_vertical_ref = 0;
m_vertical_mode = VERTICAL_MODE_NONE;
m_vmode_wait = 0.0;
m_vmode_delta.clear();
m_yaw_ref = 0;
m_yaw_mode = YAW_MODE_NONE;
m_ymode_wait = 0.0;
m_ymode_delta.clear();
m_surge_ref = 0.0;
m_sway_ref = 0.0;
m_btrack_wrn.setTop(c_btrack_wrn_cooldown);
m_bottom_follow_depth = 0.0;
}
void
BasicAutopilot::consume(const IMC::DesiredHeading* msg)
{
if (!isActive())
return;
m_yaw_mode = YAW_MODE_HEADING;
m_yaw_ref = msg->value;
// always clear delta timer after changing mode
m_ymode_delta.clear();
}
void
BasicAutopilot::consume(const IMC::DesiredHeadingRate* msg)
{
if (!isActive())
return;
if (m_hrate_bypass)
m_yaw_mode = YAW_MODE_BYPASS;
else
m_yaw_mode = YAW_MODE_HRATE;
m_yaw_ref = msg->value;
// always clear delta timer after changing mode
m_ymode_delta.clear();
}
void
BasicAutopilot::consume(const IMC::DesiredPitch* msg)
{
if (!isActive())
return;
m_vertical_mode = VERTICAL_MODE_PITCH;
m_vertical_ref = msg->value;
// always clear delta timer after changing mode
m_vmode_delta.clear();
}
void
BasicAutopilot::consume(const IMC::DesiredZ* msg)
{
if (!isActive())
return;
m_vertical_ref = msg->value;
if (msg->z_units == IMC::Z_DEPTH)
{
m_vertical_mode = VERTICAL_MODE_DEPTH;
float limit = m_max_depth - c_depth_margin;
if (m_vertical_ref > limit)
{
m_vertical_ref = limit;
war(DTR("limiting depth to %.1f"), limit);
}
}
else if (msg->z_units == IMC::Z_ALTITUDE)
{
m_vertical_mode = VERTICAL_MODE_ALTITUDE;
// Avoid possible rough transition when changing from depth to altitude
m_bottom_follow_depth = m_estate.depth;
}
else
{
m_vertical_mode = VERTICAL_MODE_NONE;
}
// always clear delta timer after changing mode
m_vmode_delta.clear();
}
void
BasicAutopilot::consume(const IMC::DesiredVelocity* msg)
{
if (!isActive())
return;
uint8_t horizontal_flags = (IMC::DesiredVelocity::FL_SURGE | IMC::DesiredVelocity::FL_SWAY);
if ((msg->flags & horizontal_flags) == horizontal_flags)
{
m_surge_ref = msg->u;
m_sway_ref = msg->v;
}
if (msg->flags & IMC::DesiredVelocity::FL_YAW)
{
m_yaw_ref = msg->r;
m_yaw_mode = YAW_MODE_HRATE;
}
if (msg->flags & IMC::DesiredVelocity::FL_HEAVE)
{
m_vertical_mode = VERTICAL_MODE_HEAVE;
m_vertical_ref = msg->w;
}
}
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);
}
void
BasicAutopilot::consume(const IMC::ControlLoops* msg)
{
uint32_t loops = msg->mask & m_controllable_loops;
if (!loops)
return;
// If this scope is obsolete, ignore message
if (msg->scope_ref < m_scope_ref)
return;
m_scope_ref = msg->scope_ref;
bool was_active = isActive();
if (msg->enable)
{
if (!isActive())
{
requestActivation();
m_aloops = 0;
}
m_aloops |= loops;
}
else
{
m_aloops &= ~loops;
if (!m_aloops)
requestDeactivation();
}
if (was_active != isActive())
{
debug("%s", isActive() ? "enabling" : "disabling");
if (msg->enable)
{
requestActivation();
IMC::ControlLoops cloops;
cloops.enable = IMC::ControlLoops::CL_ENABLE;
cloops.mask = m_required_loops;
cloops.scope_ref = m_scope_ref;
dispatch(cloops);
}
else
{
requestDeactivation();
}
}
}
void
BasicAutopilot::onMain(void)
{
while (!stopping())
{
waitForMessages(1.0);
}
}
}
