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]))
{ }
