void
BasicUAVAutopilot::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();
}
}
}
//! Signal a bad vertical mode or incompatible
//! @param[in] desc description of bad vertical mode
void
signalBadVertical(const char* desc = DTR("vertical control mode %d not supported"))
{
setEntityState(IMC::EntityState::ESTA_ERROR, Utils::String::str(DTR(desc), m_vertical_mode));
err("%s", Utils::String::str(DTR(desc), m_vertical_mode).c_str());
requestDeactivation();
}
void
consume(const IMC::ControlLoops* msg)
{
if (!(msg->mask & IMC::CL_SPEED))
return;
if (msg->scope_ref < m_scope_ref)
return;
m_scope_ref = msg->scope_ref;
if (isActive() == msg->enable)
return;
if (!msg->enable)
{
requestDeactivation();
debug("disabling");
}
else
{
requestActivation();
debug("enabling");
}
reset();
}
void
BasicRemoteOperation::consume(const IMC::ControlLoops* msg)
{
if (!(msg->mask & IMC::CL_TELEOPERATION))
return;
// If this scope is obsolete, ignore message
if (msg->scope_ref < m_scope_ref)
return;
m_scope_ref = msg->scope_ref;
if (msg->enable == isActive())
return;
if (msg->enable == IMC::ControlLoops::CL_ENABLE)
{
requestActivation();
setEntityState(IMC::EntityState::ESTA_NORMAL, Status::CODE_ACTIVE);
}
else
{
requestDeactivation();
setEntityState(IMC::EntityState::ESTA_NORMAL, Status::CODE_IDLE);
}
}
void
BasicRemoteOperation::consume(const IMC::ControlLoops* msg)
{
if (!(msg->mask & IMC::CL_TELEOPERATION))
return;
// If this scope is obsolete, ignore message
if (msg->scope_ref < m_scope_ref)
return;
m_scope_ref = msg->scope_ref;
if (msg->enable == isActive())
return;
if (msg->enable == IMC::ControlLoops::CL_ENABLE)
{
requestActivation();
if (m_teleop_src != 0)
{
std::string state = Utils::String::str(DTR("teleoperation by %s"), m_ctx.resolver.resolve(m_teleop_src));
setEntityState(IMC::EntityState::ESTA_NORMAL, state);
}
else
setEntityState(IMC::EntityState::ESTA_NORMAL, Status::CODE_ACTIVE);
}
else
{
requestDeactivation();
setEntityState(IMC::EntityState::ESTA_NORMAL, Status::CODE_IDLE);
}
}
void
Maneuver::consume(const IMC::StopManeuver* sm)
{
(void)sm;
if (isActive())
requestDeactivation();
}
void
Maneuver::signalCompletion(const std::string& msg)
{
debug("%s", msg.c_str());
requestDeactivation();
m_mcs.state = IMC::ManeuverControlState::MCS_DONE;
m_mcs.info = msg;
m_mcs.eta = 0;
dispatch(m_mcs);
}
void
Maneuver::signalError(const std::string& msg)
{
err("%s", msg.c_str());
requestDeactivation();
m_mcs.state = IMC::ManeuverControlState::MCS_ERROR;
m_mcs.info = msg;
m_mcs.eta = 0;
dispatch(m_mcs);
}
Task(const std::string& name, Tasks::Context& ctx):
Tasks::Task(name, ctx),
m_origin(NULL),
m_lbl_cfg(NULL),
m_prng(NULL)
{
// Define configuration parameters.
paramActive(Tasks::Parameter::SCOPE_MANEUVER,
Tasks::Parameter::VISIBILITY_USER,
true);
param("Ping Delay", m_args.ping_delay)
.units(Units::Second)
.defaultValue("1.0")
.description("");
param("Bad Range Probability", m_args.bad_range_prob)
.units(Units::Percentage)
.minimumValue("0")
.maximumValue("100")
.defaultValue("5.0")
.description("Probability of a bad range");
param("Standard Deviation", m_args.sigma)
.units(Units::Meter)
.defaultValue("1.25")
.description("Standard deviation of \"good\" range. "
"It is assumed that range errors are Gaussian distributed "
"with mean 0 and std. dev. sigma. "
"Rule of thumb (the usual one): to achieve 95% of range errors "
"with absolute value less than E, set sigma = E / 2. "
"For 99% of errors lower than E set sigma = E / 3");
param("PRNG Type", m_args.prng_type)
.defaultValue(Random::Factory::c_default);
param("PRNG Seed", m_args.prng_seed)
.defaultValue("-1");
setEntityState(IMC::EntityState::ESTA_BOOT, Status::CODE_WAIT_GPS_FIX);
requestDeactivation();
// Register consumers.
bind<IMC::LblConfig>(this);
bind<IMC::GpsFix>(this);
bind<IMC::SimulatedState>(this);
}
//! Constructor.
BasicRemoteOperation::BasicRemoteOperation(const std::string& name, Tasks::Context& ctx):
Tasks::Periodic(name, ctx),
m_connection(false),
m_connection_timeout(1.0),
m_last_action(-1.0),
m_scope_ref(0.0)
{
param("Connection Timeout", m_connection_timeout)
.defaultValue("1.0")
.units(Units::Second);
addActionButton("Exit");
m_actions.op = IMC::RemoteActionsRequest::OP_REPORT;
requestDeactivation();
// Register handler routines.
bind<IMC::RemoteActions>(this);
bind<IMC::RemoteActionsRequest>(this);
bind<IMC::ControlLoops>(this);
}
void
onResourceInitialization(void)
{
requestDeactivation();
reset();
m_act.id = 0;
m_rpm_pid.setGains(m_args.rpm_gains);
m_rpm_pid.setOutputLimits(m_args.min_thrust, m_args.max_thrust);
m_mps_pid.setGains(m_args.mps_gains);
m_mps_pid.setIntegralLimits(m_args.max_int_mps);
// Do not set MPS pid output limits since we use a feedforward gain
// Log parcels
if (m_args.log_parcels)
{
m_rpm_pid.enableParcels(this, &m_parcel_rpm);
m_mps_pid.enableParcels(this, &m_parcel_mps);
}
m_last_act.value = 0.0;
}
//! Initialize resources.
void
onResourceInitialization(void)
{
requestDeactivation();
}
void
BasicUAVAutopilot::onResourceInitialization(void)
{
requestDeactivation();
}
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);
}
