//! Main loop.
void
onMain(void)
{
Counter<double> m_mon_timer(1.0);
while (!stopping())
{
waitForMessages(1.0);
if (m_wdog.overflow())
{
setEntityState(IMC::EntityState::ESTA_ERROR, Status::CODE_COM_ERROR);
throw RestartNeeded(Status::getString(Status::CODE_COM_ERROR), c_restart_delay);
}
else
{
setEntityState(IMC::EntityState::ESTA_NORMAL, Status::CODE_ACTIVE);
}
if (m_mon_timer.overflow())
{
m_mon_timer.reset();
getMonitors();
}
}
}
void
onMain(void)
{
while (!stopping())
{
if (!isInitialized() || !isActive())
{
waitForMessages(1.0);
continue;
}
if (!m_ping_time.overflow())
{
waitForMessages(m_args.ping_delay);
continue;
}
double tstamp = Time::Clock::getSinceEpoch();
for (m_beacon = m_beacons.begin(); m_beacon < m_beacons.end(); ++m_beacon)
{
// Compute error.
double error = 0;
if (m_args.bad_range_prob > 0 && m_prng->random() % 100 <= m_args.bad_range_prob)
error = c_huge_error;
else if (m_args.sigma > 0)
error = m_prng->gaussian() * m_args.sigma;
//! Compute range.
double xd = m_sstate.x - m_beacon->x;
double yd = m_sstate.y - m_beacon->y;
double zd = m_sstate.z - m_beacon->depth;
IMC::LblRange range;
range.setTimeStamp(tstamp);
range.id = m_beacon->id;
range.range = std::sqrt(xd * xd + yd * yd + zd * zd);
range.range += error;
dispatch(range, DF_KEEP_TIME);
trace("beacon %u, range of %0.2f m, simulated error of %0.2f m",
range.id, range.range, error);
}
// Setup next ping.
m_ping_time.reset();
}
}
void
onMain(void)
{
while (!stopping())
{
waitForMessages(1.0);
}
}
void
BasicUAVAutopilot::onMain(void)
{
while (!stopping())
{
waitForMessages(1.0);
}
}
void
onMain(void)
{
setInitialState();
while (!stopping())
{
if (m_report_timer.overflow())
{
if (m_args.progress)
reportProgress();
dispatch(m_pcs);
m_report_timer.reset();
}
double now = Clock::get();
if ((getEntityState() == IMC::EntityState::ESTA_NORMAL) &&
(now - m_last_vstate >= c_vs_timeout))
{
changeMode(IMC::PlanControlState::PCS_BLOCKED, DTR("vehicle state timeout"));
m_last_vstate = now;
}
// got requests to process
if (!pendingReply() && m_requests.size())
{
processRequest(&m_requests.front());
m_requests.pop();
}
double delta = m_vc_reply_deadline < 0 ? 1 : m_vc_reply_deadline - now;
if (delta > 0)
{
waitForMessages(std::min(1.0, delta));
continue;
}
// handle reply timeout
m_vc_reply_deadline = -1;
changeMode(IMC::PlanControlState::PCS_READY, DTR("vehicle reply timeout"));
// Popping all requests
while (m_requests.size())
m_requests.pop();
// Increment local request id to prevent old replies from being processed
++m_vreq_ctr;
err(DTR("cleared all requests"));
}
}
void
onMain(void)
{
while (!stopping())
{
waitForMessages(1.0);
// Check if we receive valid GPS data.
if (m_time_without_gps.overflow())
setEntityState(IMC::EntityState::ESTA_ERROR, Status::CODE_WAIT_GPS_FIX);
}
}
void
Maneuver::onMain(void)
{
dispatch(m_rm);
while (!stopping())
{
if (isActive())
onStateReport();
waitForMessages(1.0);
}
}
void
Daemon::onMain(void)
{
while (!stopping())
{
waitForMessages(1.0);
if (m_periodic_counter.overflow())
{
m_periodic_counter.reset();
dispatchPeriodic();
}
}
}
void
onMain(void)
{
while (!stopping())
{
if (isActive())
{
m_delay.wait();
scanAnalogueInputs();
readTemperature();
consumeMessages();
}
else
{
waitForMessages(1.0);
}
}
}
void
task(void)
{
while (!stopping())
{
consumeMessages();
if (isActive() && (m_sock != NULL))
{
try
{
for (unsigned i = 0; i < m_args.data_points; ++i)
ping(i);
if (m_args.save_to_file)
handleSonarData();
else
dispatch(m_ping);
if (m_args.range_modifier)
{
if (m_range_counter.overflow())
{
checkAltitude();
m_range_counter.reset();
}
}
}
catch (std::exception& e)
{
err("%s", e.what());
setEntityState(IMC::EntityState::ESTA_ERROR, Status::CODE_COM_ERROR);
throw RestartNeeded(DTR(Status::getString(CODE_COM_ERROR)), 5);
}
}
else
{
waitForMessages(1.0);
if (m_activating)
checkActivationProgress();
}
}
}
void
onMain(void)
{
setInitialState();
while (!stopping())
{
if (m_report_timer.overflow())
{
if (m_args.progress)
reportProgress();
dispatch(m_pcs);
m_report_timer.reset();
}
double now = Clock::get();
if ((getEntityState() == IMC::EntityState::ESTA_NORMAL) &&
(now - m_last_vstate >= c_vs_timeout))
{
changeMode(IMC::PlanControlState::PCS_BLOCKED, DTR("vehicle state timeout"));
m_last_vstate = now;
}
double delta = m_vc_reply_deadline < 0 ? 1 : m_vc_reply_deadline - now;
if (delta > 0)
{
waitForMessages(std::min(1.0, delta));
continue;
}
// handle reply timeout
m_vc_reply_deadline = -1;
changeMode(IMC::PlanControlState::PCS_READY, DTR("vehicle reply timeout"));
}
}
void
BasicDeviceDriver::onMain(void)
{
while (!stopping())
{
if (isActive())
consumeMessages();
else if (hasQueuedStates())
updateStateMachine();
else
waitForMessages(1.0);
try
{
updateStateMachine();
}
catch (std::runtime_error& e)
{
throw RestartNeeded(e.what(), 5);
}
}
}
