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
consume(const IMC::VehicleState* vs)
{
if (getEntityState() == IMC::EntityState::ESTA_BOOT)
return;
m_last_vstate = Clock::get();
switch (vs->op_mode)
{
case IMC::VehicleState::VS_SERVICE:
onVehicleService(vs);
break;
case IMC::VehicleState::VS_CALIBRATION:
onVehicleCalibration(vs);
break;
case IMC::VehicleState::VS_ERROR:
case IMC::VehicleState::VS_BOOT:
onVehicleError(vs);
break;
case IMC::VehicleState::VS_MANEUVER:
onVehicleManeuver(vs);
break;
case IMC::VehicleState::VS_EXTERNAL:
onVehicleExternalControl(vs);
break;
}
// update calibration status
if (m_plan != NULL && initMode())
{
m_plan->updateCalibration(vs);
if (m_plan->isCalibrationDone())
{
if ((vs->op_mode == IMC::VehicleState::VS_CALIBRATION) &&
!pendingReply())
{
IMC::PlanManeuver* pman = m_plan->loadStartManeuver();
startManeuver(pman);
}
}
else if (m_plan->hasCalibrationFailed())
{
onFailure(m_plan->getCalibrationInfo());
m_reply.plan_id = m_spec.plan_id;
changeMode(IMC::PlanControlState::PCS_READY, m_plan->getCalibrationInfo());
}
}
}
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
processRequest(const IMC::PlanControl* pc)
{
m_reply.setDestination(pc->getSource());
m_reply.setDestinationEntity(pc->getSourceEntity());
m_reply.request_id = pc->request_id;
m_reply.op = pc->op;
m_reply.plan_id = pc->plan_id;
inf(DTR("request -- %s (%s)"),
DTR(c_op_desc[m_reply.op]),
m_reply.plan_id.c_str());
if (getEntityState() != IMC::EntityState::ESTA_NORMAL)
{
onFailure(DTR("engine not ready: entity state not normal"));
return;
}
switch (pc->op)
{
case IMC::PlanControl::PC_START:
if (!startPlan(pc->plan_id, pc->arg.isNull() ? 0 : pc->arg.get(), pc->flags))
vehicleRequest(IMC::VehicleCommand::VC_STOP_MANEUVER);
break;
case IMC::PlanControl::PC_STOP:
stopPlan();
break;
case IMC::PlanControl::PC_LOAD:
loadPlan(pc->plan_id, pc->arg.isNull() ? 0 : pc->arg.get(), false);
break;
case IMC::PlanControl::PC_GET:
getPlan();
break;
default:
onFailure(DTR("plan control operation not supported"));
break;
}
}