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();
}
}
}
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
consume(const IMC::SimulatedState* msg)
{
if (!isActive())
{
m_vel[0] = msg->u;
m_vel[1] = msg->v;
m_vel[2] = msg->w;
requestActivation();
}
// Compute time delta.
double tstep = m_delta.getDelta();
// Check if we have a valid time delta.
if (tstep <= 0)
return;
// Define Euler Angles variables and add gaussian noise component.
if (m_args.euler)
{
m_euler.phi = Angles::normalizeRadian(msg->phi + m_prng->gaussian() * Angles::radians(m_args.stdev_euler));
m_euler.theta = Angles::normalizeRadian(msg->theta + m_prng->gaussian() * Angles::radians(m_args.stdev_euler));
m_euler.psi_magnetic = Angles::normalizeRadian(msg->psi + m_prng->gaussian() * Angles::radians(m_args.stdev_euler));
m_euler.psi = Angles::normalizeRadian(m_euler.psi_magnetic + m_heading_offset);
// Heading offset will increment through time according with gyro rate bias.
m_heading_offset += Angles::radians(m_args.gyro_bias / 3600) * tstep;
m_euler.setTimeStamp(msg->getTimeStamp());
dispatch(m_euler, DF_KEEP_TIME);
}
// Define Angular Velocity variables and add gaussian noise component.
m_agvel.x = Angles::normalizeRadian(msg->p + m_prng->gaussian() * Angles::radians(m_args.stdev_agvel));
m_agvel.y = Angles::normalizeRadian(msg->q + m_prng->gaussian() * Angles::radians(m_args.stdev_agvel));
m_agvel.z = Angles::normalizeRadian(msg->r + m_prng->gaussian() * Angles::radians(m_args.stdev_agvel));
// Compute acceleration values using simulated state velocity fields.
m_accel.x = (msg->u - m_vel[0]) / tstep;
m_accel.y = (msg->v - m_vel[1]) / tstep;
m_accel.z = (msg->w - m_vel[2]) / tstep;
// Store velocity for next iteration.
m_vel[0] = msg->u;
m_vel[1] = msg->v;
m_vel[2] = msg->w;
// Define messages timestamp and dispatch them to the bus.
m_agvel.setTimeStamp(msg->getTimeStamp());
m_accel.setTimeStamp(msg->getTimeStamp());
dispatch(m_agvel, DF_KEEP_TIME);
dispatch(m_accel, DF_KEEP_TIME);
setEntityState(IMC::EntityState::ESTA_NORMAL, Status::CODE_ACTIVE);
}
void
startManeuver(const M* maneuver)
{
debug("enabling");
signalProgress(65535, "in progress");
requestActivation();
static_cast<T*>(this)->consume(maneuver);
}
void
consume(const IMC::SimulatedState* msg)
{
if (!isActive())
{
setEntityState(IMC::EntityState::ESTA_NORMAL, Status::CODE_ACTIVE);
requestActivation();
}
m_sstate = *msg;
}
KUrlNavigatorButtonBase::KUrlNavigatorButtonBase(QWidget *parent) :
QPushButton(parent),
m_active(true),
m_displayHint(0)
{
setFocusPolicy(Qt::TabFocus);
setSizePolicy(QSizePolicy::Maximum, QSizePolicy::Fixed);
setMinimumHeight(parent->minimumHeight());
connect(this, SIGNAL(pressed()), parent, SLOT(requestActivation()));
}
void
startManeuver(const M* maneuver)
{
if (!isActive())
{
while (!tryLock())
{
Time::Delay::wait(0.5);
}
}
debug("enabling");
signalProgress(65535, "in progress");
static_cast<T*>(this)->consume(maneuver);
if (m_mcs.state == IMC::ManeuverControlState::MCS_EXECUTING)
requestActivation();
}
