//! 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); } } }