void inProgress(const char* msg = DTR("in progress")) { answer(IMC::PlanDB::DBT_IN_PROGRESS, msg); }
CommandByte parse(uint8_t byte) { int8_t msg_size = 0; switch (m_state) { // Test for synchronization case FSM_STATE_NONE: m_togo = m_done = 0; msg_size = getCommandDataSize(byte); if (msg_size > 0) { m_togo = msg_size - 1; m_done = 1; m_data[0] = byte; // Store ID. m_state = FSM_STATE_MESSAGE; } break; // Parse message body. case FSM_STATE_MESSAGE: m_data[m_done] = byte; ++m_done; --m_togo; break; // Should never get here. default: debug("%s: 0x%02x", DTR("unexpected byte"), byte); break; } // Exit if we don't have a complete message. if (m_togo != 0 || m_state != FSM_STATE_MESSAGE) return MSG_NULL; m_state = FSM_STATE_NONE; if (!validateCheckSum(m_data, m_done)) { err("%s", DTR(Status::getString(Status::CODE_INVALID_CHECKSUM))); return MSG_NULL; } double tstamp = Clock::getSinceEpoch(); // Interpret parsed data. int16_t angle = 0; switch (m_data[0]) { case MSG_CONTMODE: return MSG_CONTMODE; case MSG_EEPROM: ByteCopy::fromBE(m_eeprom, m_data + 1); return MSG_EEPROM; case MSG_GS_EULER: m_euler.time = updateTimerCount(m_data + 7); ByteCopy::fromBE(angle, m_data + 1); m_euler.phi = Angles::radians((fp64_t)angle * c_euler_factor); ByteCopy::fromBE(angle, m_data + 3); m_euler.theta = Angles::radians((fp64_t)angle * c_euler_factor); ByteCopy::fromBE(angle, m_data + 5); m_euler.psi = Angles::radians((fp64_t)angle * c_euler_factor); m_euler.psi_magnetic = m_euler.psi; m_euler.setTimeStamp(tstamp); dispatch(m_euler, DF_KEEP_TIME); requestMessage(m_args.raw_ivecs ? MSG_RAW_IVECS : MSG_IVECS); return MSG_GS_EULER; case MSG_IVECS: // Acceleration. m_accel.time = updateTimerCount(m_data + 19); ByteCopy::fromBE(angle, m_data + 7); m_accel.x = (fp64_t)angle / m_accel_scale; ByteCopy::fromBE(angle, m_data + 9); m_accel.y = (fp64_t)angle / m_accel_scale; ByteCopy::fromBE(angle, m_data + 11); m_accel.z = (fp64_t)angle / m_accel_scale; m_accel.setTimeStamp(tstamp); // Angular velocity. ByteCopy::fromBE(angle, m_data + 13); m_agvel.x = (fp64_t)angle / m_gyro_scale; ByteCopy::fromBE(angle, m_data + 15); m_agvel.y = (fp64_t)angle / m_gyro_scale; ByteCopy::fromBE(angle, m_data + 17); m_agvel.z = (fp64_t)angle / m_gyro_scale; m_agvel.setTimeStamp(tstamp); dispatch(m_accel, DF_KEEP_TIME); dispatch(m_agvel, DF_KEEP_TIME); return MSG_IVECS; case MSG_RAW_IVECS: // Acceleration. m_accel.time = updateTimerCount(m_data + 19); ByteCopy::fromBE(angle, m_data + 7); m_accel.x = (fp64_t)angle / m_accel_scale; ByteCopy::fromBE(angle, m_data + 9); m_accel.y = (fp64_t)angle / m_accel_scale; ByteCopy::fromBE(angle, m_data + 11); m_accel.z = (fp64_t)angle / m_accel_scale; m_accel.setTimeStamp(tstamp); // Angular velocity. ByteCopy::fromBE(angle, m_data + 13); m_agvel.x = (fp64_t)angle / m_gyro_scale; ByteCopy::fromBE(angle, m_data + 15); m_agvel.y = (fp64_t)angle / m_gyro_scale; ByteCopy::fromBE(angle, m_data + 17); m_agvel.z = (fp64_t)angle / m_gyro_scale; m_agvel.setTimeStamp(tstamp); dispatch(m_accel, DF_KEEP_TIME); dispatch(m_agvel, DF_KEEP_TIME); return MSG_RAW_IVECS; } return MSG_NULL; }
Daemon::Daemon(DUNE::Tasks::Context& ctx, const std::string& profiles): DUNE::Tasks::Task("Daemon", ctx), m_tman(NULL), m_fs_capacity(0) { // Retrieve known IMC addresses. std::vector<std::string> addrs = m_ctx.config.options("IMC Addresses"); for (unsigned i = 0; i < addrs.size(); ++i) { unsigned id = IMC::AddressResolver::invalid(); m_ctx.config.get("IMC Addresses", addrs[i], "", id); m_ctx.resolver.insert(addrs[i], id); } // Add commonly used profiles. m_ctx.profiles.add("Hardware", DTR("Pure Hardware")); m_ctx.profiles.add("Simulation", DTR("Pure Simulation")); m_ctx.profiles.add("Hardware/Simulation", DTR("Simulation with Hardware-in-the-loop")); // Add user defined profiles. std::vector<std::string> pros = m_ctx.config.options("Profiles"); for (unsigned i = 0; i < pros.size(); ++i) { std::string desc; m_ctx.config.get("Profiles", pros[i], DTR("No description given"), desc); m_ctx.profiles.add(pros[i], desc); } m_ctx.mbus.pause(); // Register system name. std::string sys_name; m_ctx.config.get("General", "Vehicle", "unknown", sys_name); m_ctx.resolver.name(sys_name); unsigned id = resolveSystemName(sys_name); m_ctx.resolver.id(id); setEntityLabel("Daemon"); reserveEntities(); // Load saved configuration parameters. m_scfg_file = (m_ctx.dir_cfg / (sys_name + "-saved.ini")).str(); try { m_ctx.config.parseFile(m_scfg_file.c_str()); m_scfg.parseFile(m_scfg_file.c_str()); } catch (...) { } m_ctx.dir_log = m_ctx.dir_log / sys_name; m_ctx.dir_db = m_ctx.dir_db / sys_name; try { m_ctx.dir_db.create(); } catch (std::exception& e) { err("%s", e.what()); } try { m_ctx.dir_log.create(); m_fs_capacity = FileSystem::Path::storageCapacity(m_ctx.dir_log); } catch (std::exception& e) { err("%s", e.what()); } inf(DTR("system name: '%s' (%u)"), sys_name.c_str(), id); inf(DTR("registered tasks: %d"), Tasks::Factory::getRegisteredCount()); inf(DTR("base folder: '%s'"), ctx.dir_app.c_str()); inf(DTR("configuration folder: '%s'"), ctx.dir_cfg.c_str()); inf(DTR("web server folder: '%s'"), ctx.dir_www.c_str()); inf(DTR("log folder: '%s'"), ctx.dir_log.c_str()); inf(DTR("library folder: '%s'"), ctx.dir_lib.c_str()); if (!profiles.empty()) { m_ctx.profiles.select(profiles); inf(DTR("execution profiles: %s"), profiles.c_str()); } // CPU usage. m_ctx.config.get("General", "CPU Usage - Maximum", "65", m_cpu_max_usage); m_ctx.config.get("General", "CPU Usage - Moving Average Samples", "10", m_cpu_avg_samples); m_cpu_avg = new Math::MovingAverage<double>(m_cpu_avg_samples); m_tman = new DUNE::Tasks::Manager(m_ctx); bind<IMC::RestartSystem>(this); bind<IMC::EntityList>(this); bind<IMC::SaveEntityParameters>(this); bind<IMC::EntityParameters>(this); }
void onSuccess(const char* msg = DTR("OK")) { answer(IMC::PlanDB::DBT_SUCCESS, msg); }
void Maneuver::signalInvalidZ(void) { signalError(DTR("unsupported vertical reference")); }
void onResourceInitialization(void) { // Get modem address. std::string agent = getSystemName(); m_ctx.config.get(m_args.addr_section, agent, "1024", m_addr); if (m_addr == 1024) throw std::runtime_error(String::str(DTR("modem address for agent '%s' is invalid"), agent.c_str())); // Set modem address. { configureModem("CCCFG", "SRC", m_addr); if (!consumeResult(RS_SRC_ACKD)) { setAndSendState(STA_ERR_SRC); throw std::runtime_error(m_states[m_state].description); } } // Set NRV parameter. { configureModem("CCCFG", "NRV", 0); if (!consumeResult(RS_NRV_ACKD)) { setAndSendState(STA_ERR_STP); throw std::runtime_error(m_states[m_state].description); } } // Set CTO parameter. { configureModem("CCCFG", "CTO", c_cto); if (!consumeResult(RS_CTO_ACKD)) { setAndSendState(STA_ERR_STP); throw std::runtime_error(m_states[m_state].description); } } // Set TAT parameter. { configureModem("CCCFG", "TAT", m_args.turn_around_time); if (!consumeResult(RS_TAT_ACKD)) { setAndSendState(STA_ERR_STP); throw std::runtime_error(m_states[m_state].description); } } // Set XST parameter. { configureModem("CCCFG", "XST", 0); if (!consumeResult(RS_XST_ACKD)) { setAndSendState(STA_ERR_STP); throw std::runtime_error(m_states[m_state].description); } } if (m_beacons.empty()) setAndSendState(STA_NO_BEACONS); else setAndSendState(STA_IDLE); }
void Plan::buildGraph(const std::set<uint16_t>* supported_maneuvers) { bool start_maneuver_ok = false; if (!m_spec->maneuvers.size()) throw ParseError(m_spec->plan_id + DTR(": no maneuvers")); IMC::MessageList<IMC::PlanManeuver>::const_iterator mitr; mitr = m_spec->maneuvers.begin(); // parse maneuvers and transitions do { if (*mitr == NULL) { ++mitr; continue; } if ((*mitr)->data.isNull()) throw ParseError((*mitr)->maneuver_id + DTR(": actual maneuver not specified")); const IMC::Message* m = (*mitr)->data.get(); if (supported_maneuvers->find(m->getId()) == supported_maneuvers->end()) throw ParseError((*mitr)->maneuver_id + DTR(": maneuver is not supported")); if ((*mitr)->maneuver_id == m_spec->start_man_id) start_maneuver_ok = true; Node node; bool matched = false; node.pman = (*mitr); IMC::MessageList<IMC::PlanTransition>::const_iterator tritr; tritr = m_spec->transitions.begin(); for (; tritr != m_spec->transitions.end(); ++tritr) { if (*tritr == NULL) continue; if ((*tritr)->dest_man == (*mitr)->maneuver_id) matched = true; if ((*tritr)->source_man == (*mitr)->maneuver_id) node.trans.push_back((*tritr)); } // if a match was not found and this is not the start maneuver if (!matched && ((*mitr)->maneuver_id != m_spec->start_man_id)) { std::string str = DTR(": maneuver has no incoming transition" " and it's not the initial maneuver"); throw ParseError((*mitr)->maneuver_id + str); } m_graph[(*mitr)->maneuver_id] = node; ++mitr; } while (mitr != m_spec->maneuvers.end()); if (!start_maneuver_ok) throw ParseError(m_spec->start_man_id + DTR(": invalid start maneuver")); }
void BottomTracker::onTracking(void) { // Render slope top as invalid here m_sdata->renderSlopeInvalid(); float depth_ref = m_estate.depth + m_estate.alt - m_z_ref.value; // if units are now altitude if (m_forced == FC_ALTITUDE) { if (m_z_ref.z_units == IMC::Z_ALTITUDE) { debug("units are altitude now. moving to tracking"); m_forced = FC_NONE; m_mstate = SM_TRACKING; return; } else if (depth_ref >= m_args->adm_alt + c_depth_hyst) { debug("depth reference is now safe"); m_forced = FC_NONE; dispatchSameZ(); m_mstate = SM_DEPTH; return; } } // if reference is for depth now else if ((m_forced != FC_ALTITUDE) && (m_z_ref.z_units == IMC::Z_DEPTH)) { debug("units are depth now"); m_mstate = SM_DEPTH; return; } // Do not attempt to interfere if we cant use altitude if (!isAltitudeValid()) return; // check if altitude value is becoming dangerous if (m_estate.alt < m_args->min_alt) { debug(String::str("altitude is too low: %.2f.", m_estate.alt)); brake(true); m_mstate = SM_AVOIDING; return; } // check safety if (!checkSafety()) return; // if reaching a limit in altitude if (depth_ref > m_args->depth_limit + c_depth_hyst && m_estate.depth > m_args->depth_limit) { info(DTR("depth is reaching unacceptable values, forcing depth control")); m_forced = FC_DEPTH; dispatchLimitDepth(); m_mstate = SM_LIMITDEPTH; return; } }
void BottomTracker::debug(const std::string& msg) const { m_args->task->debug("[%s.%s] >> %s", DTR(c_bt_name.c_str()), DTR(c_str_states[m_mstate].c_str()), msg.c_str()); }
void Parameter::writeXML(std::ostream& os) const { os << "<param name=\"" << m_name << "\"" << " i18n-name=\"" << DTR(m_name.c_str()) << "\"" << " type=\"" << m_type_name << "\"" << " visibility=\"" << c_visibility_strs[m_visibility] << "\"" << " scope=\"" << c_scope_strs[m_scope] << "\""; if (!m_min_value.empty()) os << " min=\"" << m_min_value << "\""; if (!m_max_value.empty()) os << " max=\"" << m_max_value << "\""; if ((m_min_size == m_max_size) && m_min_size < UINT_MAX) { os << " size=\"" << m_min_size << "\""; } else { if (m_min_size < UINT_MAX) os << " min-size=\"" << m_min_size << "\""; if (m_max_size < UINT_MAX) os << " max-size=\"" << m_max_size << "\""; } if (!m_default.empty()) os << " default=\"" << m_default << "\""; if (m_units != Units::None) os << " units=\"" << Units::getAbbrev(m_units) << "\""; os << ">\n"; if (!m_desc.empty()) { os << "<desc>\n" << "<![CDATA[" << DTR(m_desc.c_str()) << "]]>\n" << "</desc>\n"; } if (!m_values.empty()) { os << "<values>\n" << "<![CDATA[" << m_values << "]]>\n" << "</values>\n"; os << "<values-i18n>\n" << "<![CDATA[" << DTR(m_values.c_str()) << "]]>\n" << "</values-i18n>\n"; } for (unsigned i = 0; i < m_values_if.size(); ++i) { os << "<values-if>\n" << "<param>" << m_values_if[i]->name << "</param>\n" << "<equals>" << m_values_if[i]->equals << "</equals>\n" << "<values><![CDATA[" << m_values_if[i]->values << "]]></values>\n" << "</values-if>\n"; } os << "</param>\n"; }
bool Plan::parse(std::string& desc, const std::set<uint16_t>* supported_maneuvers, bool plan_startup, const std::map<std::string, IMC::EntityInfo>& cinfo, Tasks::Task* task, const IMC::EstimatedState* state) { bool start_maneuver_ok = false; clear(); if (!m_spec->maneuvers.size()) { desc = m_spec->plan_id + DTR(": no maneuvers"); return false; } IMC::MessageList<IMC::PlanManeuver>::const_iterator mitr; mitr = m_spec->maneuvers.begin(); // parse maneuvers and transitions do { if (*mitr == NULL) { ++mitr; continue; } if ((*mitr)->data.isNull()) { desc = (*mitr)->maneuver_id + DTR(": actual maneuver not specified"); return false; } const IMC::Message* m = (*mitr)->data.get(); if (supported_maneuvers->find(m->getId()) == supported_maneuvers->end()) { desc = (*mitr)->maneuver_id + DTR(": maneuver is not supported"); return false; } if ((*mitr)->maneuver_id == m_spec->start_man_id) start_maneuver_ok = true; Node node; bool matched = false; node.pman = (*mitr); IMC::MessageList<IMC::PlanTransition>::const_iterator tritr; tritr = m_spec->transitions.begin(); for (; tritr != m_spec->transitions.end(); ++tritr) { if (*tritr == NULL) continue; if ((*tritr)->dest_man == (*mitr)->maneuver_id) matched = true; if ((*tritr)->source_man == (*mitr)->maneuver_id) node.trans.push_back((*tritr)); } // if a match was not found and this is not the start maneuver if (!matched && ((*mitr)->maneuver_id != m_spec->start_man_id)) { std::string str = DTR(": maneuver has no incoming transition" " and it's not the initial maneuver"); desc = (*mitr)->maneuver_id + str; return false; } m_graph[(*mitr)->maneuver_id] = node; ++mitr; } while (mitr != m_spec->maneuvers.end()); if (!start_maneuver_ok) { desc = m_spec->start_man_id + DTR(": invalid start maneuver"); return false; } if (m_compute_progress && plan_startup) { sequenceNodes(); if (m_sequential && state != NULL) { computeDurations(state); Memory::clear(m_sched); m_sched = new ActionSchedule(task, m_spec, m_seq_nodes, *m_durations, m_last_dur, cinfo); // Estimate necessary calibration time float diff = m_sched->getEarliestSchedule() - getExecutionDuration(); m_est_cal_time = (uint16_t)trimValue(diff, 0.0, diff); m_est_cal_time = (uint16_t)std::max(m_min_cal_time, m_est_cal_time); } else if (!m_sequential) { Memory::clear(m_sched); m_sched = new ActionSchedule(task, m_spec, m_seq_nodes, cinfo); m_est_cal_time = m_min_cal_time; } } m_last_id = m_spec->start_man_id; return true; }
Daemon::~Daemon(void) { m_ctx.mbus.pause(); delete m_tman; inf(DTR("clean shutdown")); }
int main(int argc, char** argv) { Tasks::Context context; I18N::setLanguage(context.dir_i18n); Scheduler::set(Scheduler::POLICY_RR); OptionParser options; options.executable("dune") .program(DUNE_SHORT_NAME) .copyright(DUNE_COPYRIGHT) .email(DUNE_CONTACT) .version(getFullVersion()) .date(getCompileDate()) .arch(DUNE_SYSTEM_NAME) .add("-d", "--config-dir", "Configuration directory", "DIR") .add("-w", "--www-dir", "HTTP server base directory", "DIR") .add("-c", "--config-file", "Load configuration file CONFIG", "CONFIG") .add("-m", "--lock-memory", "Lock memory") .add("-p", "--profiles", "Execution Profiles", "PROFILES") .add("-V", "--vehicle", "Vehicle name override", "VEHICLE") .add("-X", "--dump-params-xml", "Dump parameters XML to folder DIR", "DIR"); // Parse command line arguments. if (!options.parse(argc, argv)) { if (options.bad()) std::cerr << "ERROR: " << options.error() << std::endl; options.usage(); return 1; } // If requested, lock memory. if (!options.value("--lock-memory").empty()) { #if defined(DUNE_USING_TLSF) && defined(DUNE_CLIB_GNU) Resources::lockMemory(c_memory, c_memory_size); #else Resources::lockMemory(); #endif } // If requested, set alternate configuration directory. if (options.value("--config-dir") != "") { context.dir_cfg = options.value("--config-dir"); } // If requested, set alternate HTTP server directory. if (options.value("--www-dir") != "") { context.dir_www = options.value("--www-dir"); } DUNE::Tasks::Factory::registerDynamicTasks(context.dir_lib.c_str()); registerStaticTasks(); // Retrieve configuration file and try parsing it. if (options.value("--config-file") == "") { std::cerr << String::str(DTR("ERROR: no configuration file was given, " "see options --config-list and --config-file\n")) << std::endl; return 1; } Path cfg_file = context.dir_cfg / options.value("--config-file") + ".ini"; try { context.config.parseFile(cfg_file.c_str()); } catch (std::runtime_error& e) { try { cfg_file = context.dir_usr_cfg / options.value("--config-file") + ".ini"; context.config.parseFile(cfg_file.c_str()); context.dir_cfg = context.dir_usr_cfg; } catch (std::runtime_error& e2) { std::cerr << String::str("ERROR: %s\n", e.what()) << std::endl; std::cerr << String::str("ERROR: %s\n", e2.what()) << std::endl; return 1; } } if (!options.value("--vehicle").empty()) context.config.set("General", "Vehicle", options.value("--vehicle")); try { DUNE::Daemon daemon(context, options.value("--profiles")); // Parameters XML. if (options.value("--dump-params-xml") != "") { std::string lang = I18N::getLanguage(); std::string file = String::str("%s.%s.xml", daemon.getSystemName(), lang.c_str()); Path path = Path(options.value("--dump-params-xml")) / file; std::ofstream ofs(path.c_str()); if (!ofs.is_open()) { std::cerr << "ERROR: failed to create file '" << path << "'" << std::endl; return 1; } daemon.writeParamsXML(ofs); return 0; } return runDaemon(daemon); } catch (std::exception& e) { std::cerr << "ERROR: " << e.what() << std::endl; } }
void StationKeep::stopMoving(double range) { m_task->setControl(IMC::CL_NONE); m_task->inf(DTR("inside safe region (distance: %.1f m)"), range); }
ConnectionError(const std::string& error): Exception(std::string(DTR("connection error")) + ": " + error) { }
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); }
ConnectionClosed(void): ConnectionError(DTR("connection closed")) { }
void TCPSocket::listen(int backlog) { if (::listen(m_handle, backlog) < 0) throw NetworkError(DTR("unable to listen"), getLastErrorMessage()); }
void onResourceInitialization(void) { // Process micro-modem addresses. std::vector<std::string> addrs = m_ctx.config.options(m_args.addr_section); for (unsigned i = 0; i < addrs.size(); ++i) { unsigned mid = 0; m_ctx.config.get(m_args.addr_section, addrs[i], "0", mid); m_smap[addrs[i]] = mid; m_amap[mid] = addrs[i]; } // Get modem address. std::string agent = getSystemName(); m_ctx.config.get(m_args.addr_section, agent, "1024", m_addr); if (m_addr == 1024) throw std::runtime_error(String::str(DTR("modem address for agent '%s' is invalid"), agent.c_str())); // Set modem address. { configureModem("CCCFG", "SRC", m_addr); if (!consumeResult(RS_SRC_ACKD)) { setAndSendState(STA_ERR_SRC); throw std::runtime_error(m_states[m_state].description); } } // Set NRV parameter. { configureModem("CCCFG", "NRV", 0); if (!consumeResult(RS_NRV_ACKD)) { setAndSendState(STA_ERR_STP); throw std::runtime_error(m_states[m_state].description); } } // Set CTO parameter. { configureModem("CCCFG", "CTO", c_cto); if (!consumeResult(RS_CTO_ACKD)) { setAndSendState(STA_ERR_STP); throw std::runtime_error(m_states[m_state].description); } } // Set TAT parameter. { configureModem("CCCFG", "TAT", m_args.turn_around_time); if (!consumeResult(RS_TAT_ACKD)) { setAndSendState(STA_ERR_STP); throw std::runtime_error(m_states[m_state].description); } } // Set XST parameter. { configureModem("CCCFG", "XST", 0); if (!consumeResult(RS_XST_ACKD)) { setAndSendState(STA_ERR_STP); throw std::runtime_error(m_states[m_state].description); } } if (m_lbl.empty()) setAndSendState(STA_NO_BEACONS); else setAndSendState(STA_IDLE); }
ConnectionTimeout(void): ConnectionError(DTR("connection timeout")) { }
void task(void) { // Set servo positions. uint8_t data[c_servo_count + 1]; data[0] = 0; for (unsigned i = 0; i < c_servo_count; ++i) { unsigned nr = m_args.servo_renumbering[i]; float value = m_set_position[nr]; // compute elapsed time to trim according to max rotation rate double elapsedtime = Clock::get() - m_last_timestamp[nr]; if (elapsedtime > 0 && m_args.limit_servo_rate) { elapsedtime = trimValue(elapsedtime, 0, (m_args.servo_max - m_args.servo_min) / m_args.servo_rate_max); if (value - m_last_ref[nr] >= 0) value = std::min((double)value, m_last_ref[nr] + m_args.servo_rate_max * elapsedtime); else value = std::max((double)value, m_last_ref[nr] - m_args.servo_rate_max * elapsedtime); } // trim according to max and min rotation value = trimValue(value, m_args.servo_min, m_args.servo_max); // update variables used as previous m_last_ref[nr] = value; m_last_timestamp[nr] = Clock::get(); int ticks = (int)(256 + m_args.servo_orient[nr] * (value + m_args.servo_middle[nr]) * (400.0 / DUNE::Math::c_pi)); m_servo_ref[nr] = trimValue(ticks, 0, 511); data[0] |= (m_servo_ref[nr] & 0x100) >> (8 - i); data[i + 1] = m_servo_ref[nr] & 0xFF; } m_proto.sendCommand(CMD_SERVO_SET, data, c_servo_count + 1); if (!waitForCommand(CMD_SERVO_SET)) { setEntityState(IMC::EntityState::ESTA_ERROR, Status::CODE_COM_ERROR); throw RestartNeeded(DTR(Status::getString(Status::CODE_COM_ERROR)), 5); } else { setEntityState(IMC::EntityState::ESTA_NORMAL, Status::CODE_ACTIVE); } if (!m_last_adc.overflow()) return; m_last_adc.reset(); // Request state. m_proto.sendCommand(CMD_STATE); if (!waitForCommand(CMD_STATE)) { setEntityState(IMC::EntityState::ESTA_ERROR, Status::CODE_COM_ERROR); } else { setEntityState(IMC::EntityState::ESTA_NORMAL, Status::CODE_ACTIVE); } }
NameLookupError(const std::string& msg): Exception(Utils::String::str(DTR("unable to resolve hostname '%s'"), msg.c_str())) { }
//! Constructor. //! @param[in] feature feature name. NotImplemented(const std::string& feature): Exception(Utils::String::str(DTR("feature not implemented '%s'"), feature.c_str())) { }
InvalidAddress(const std::string& addr): Exception(Utils::String::str(DTR("invalid network address '%s'"), addr.c_str())) { }
void Maneuver::signalNoAltitude(void) { signalError(DTR("no valid value for altitude has been received yet," "maneuver will not proceed")); }
NetworkUnreachable(const std::string& addr): Exception(Utils::String::str(DTR("network unreachable '%s'"), addr.c_str())) { }
void BasicDeviceDriver::updateStateMachine(void) { switch (dequeueState()) { // Wait for activation. case SM_IDLE: break; // Begin activation sequence. case SM_ACT_BEGIN: setEntityState(IMC::EntityState::ESTA_NORMAL, Status::CODE_ACTIVATING); m_wdog.setTop(getActivationTime()); queueState(SM_ACT_POWER_ON); break; // Turn power on. case SM_ACT_POWER_ON: turnPowerOn(); queueState(SM_ACT_POWER_WAIT); break; // Wait for power to be on. case SM_ACT_POWER_WAIT: if (isPowered(true)) { m_power_on_timer.setTop(m_post_power_on_delay); queueState(SM_ACT_DEV_WAIT); } if (m_wdog.overflow()) { failActivation(DTR("failed to turn power on")); queueState(SM_IDLE); } break; // Connect to device. case SM_ACT_DEV_WAIT: if (m_wdog.overflow()) { failActivation(DTR("failed to connect to device")); queueState(SM_IDLE); } else if (m_power_on_timer.overflow()) { if (connect()) queueState(SM_ACT_DEV_SYNC); } break; // Synchronize with device. case SM_ACT_DEV_SYNC: if (m_wdog.overflow()) { failActivation(DTR("failed to synchronize with device")); queueState(SM_IDLE); } else { if (synchronize()) { if (enableLogControl()) queueState(SM_ACT_LOG_REQUEST); else queueState(SM_ACT_DONE); } } break; // Request log name. case SM_ACT_LOG_REQUEST: if (m_wdog.overflow()) { failActivation(DTR("failed to request current log name")); queueState(SM_IDLE); } else { closeLog(); requestLogName(); queueState(SM_ACT_LOG_WAIT); } break; // Wait for log name. case SM_ACT_LOG_WAIT: if (m_wdog.overflow()) { failActivation(DTR("failed to retrieve current log name")); queueState(SM_IDLE); } else { if (m_log_opened) queueState(SM_ACT_DONE); } break; // Activation procedure is complete. case SM_ACT_DONE: activate(); queueState(SM_ACT_SAMPLE); break; // Read samples. case SM_ACT_SAMPLE: readSample(); break; // Start deactivation procedure. case SM_DEACT_BEGIN: setEntityState(IMC::EntityState::ESTA_NORMAL, Status::CODE_DEACTIVATING); m_wdog.setTop(getDeactivationTime()); queueState(SM_DEACT_DISCONNECT); break; // Gracefully disconnect from device. case SM_DEACT_DISCONNECT: disconnect(); if (enableLogControl()) closeLog(); m_power_off_timer.setTop(m_power_off_delay); queueState(SM_DEACT_POWER_OFF); break; // Turn power off. case SM_DEACT_POWER_OFF: if (m_power_off_timer.overflow() || m_wdog.overflow()) { turnPowerOff(); queueState(SM_DEACT_POWER_WAIT); } break; // Wait for power to be turned off. case SM_DEACT_POWER_WAIT: if (isPowered(false)) queueState(SM_DEACT_DONE); break; // Deactivation is complete. case SM_DEACT_DONE: deactivate(); queueState(SM_IDLE); break; } }
HostUnreachable(const std::string& addr): Exception(Utils::String::str(DTR("host unreachable '%s'"), addr.c_str())) { }
//! Constructor. //! @param[in] str invalid command/response. InvalidFormat(const std::string& str): std::runtime_error(DUNE::Utils::String::str(DTR("invalid format: %s"), str.c_str())) { }
//! Constructor. //! @param[in] r received checksum. //! @param[in] c computed checksum. InvalidChecksum(uint8_t* r, uint8_t* c): std::runtime_error(DUNE::Utils::String::str(DTR("invalid checksum: should be %02X%02X but received %02X%02X"), c[0], c[1], r[0], r[1])) { }