//! 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
BasicRemoteOperation::consume(const IMC::ControlLoops* msg)
{
if (!(msg->mask & IMC::CL_TELEOPERATION))
return;
// If this scope is obsolete, ignore message
if (msg->scope_ref < m_scope_ref)
return;
m_scope_ref = msg->scope_ref;
if (msg->enable == isActive())
return;
if (msg->enable == IMC::ControlLoops::CL_ENABLE)
{
requestActivation();
setEntityState(IMC::EntityState::ESTA_NORMAL, Status::CODE_ACTIVE);
}
else
{
requestDeactivation();
setEntityState(IMC::EntityState::ESTA_NORMAL, Status::CODE_IDLE);
}
}
void
onMain(void)
{
char bfr[32];
while (!stopping())
{
consumeMessages();
if (m_wdog.overflow())
{
setEntityState(IMC::EntityState::ESTA_ERROR, Status::CODE_COM_ERROR);
throw RestartNeeded(DTR(Status::getString(CODE_COM_ERROR)), 5);
}
if (!Poll::poll(*m_uart, 1.0))
continue;
size_t rv = m_uart->readString(bfr, sizeof(bfr));
if (rv == 0)
throw RestartNeeded(DTR("I/O error"), 5);
if (std::sscanf(bfr, "%f", &m_sspeed.value) != 1)
continue;
if ((m_sspeed.value < c_min_speed) || (m_sspeed.value > c_max_speed))
m_sspeed.value = -1;
setEntityState(IMC::EntityState::ESTA_NORMAL, Status::CODE_ACTIVE);
m_wdog.reset();
dispatch(m_sspeed);
}
}
void
Daemon::measureCpuUsage(void)
{
// Measure CPU usage per task.
m_tman->measureCpuUsage();
// Dispatch global CPU usage.
IMC::CpuUsage cpu_usage;
int value = m_sys_resources.getProcessorUsage();
if (value >= 0 && value <= 100)
{
cpu_usage.value = value;
dispatch(cpu_usage);
double cpu_avg = m_cpu_avg->update(value);
if (cpu_avg >= m_cpu_max_usage)
{
setEntityState(IMC::EntityState::ESTA_ERROR, Status::CODE_CPU_TOO_HIGH);
m_tman->adjustPriorities();
}
else
{
setEntityState(IMC::EntityState::ESTA_NORMAL, Status::CODE_ACTIVE);
}
}
}
void
BasicRemoteOperation::consume(const IMC::ControlLoops* msg)
{
if (!(msg->mask & IMC::CL_TELEOPERATION))
return;
// If this scope is obsolete, ignore message
if (msg->scope_ref < m_scope_ref)
return;
m_scope_ref = msg->scope_ref;
if (msg->enable == isActive())
return;
if (msg->enable == IMC::ControlLoops::CL_ENABLE)
{
requestActivation();
if (m_teleop_src != 0)
{
std::string state = Utils::String::str(DTR("teleoperation by %s"), m_ctx.resolver.resolve(m_teleop_src));
setEntityState(IMC::EntityState::ESTA_NORMAL, state);
}
else
setEntityState(IMC::EntityState::ESTA_NORMAL, Status::CODE_ACTIVE);
}
else
{
requestDeactivation();
setEntityState(IMC::EntityState::ESTA_NORMAL, Status::CODE_IDLE);
}
}
bool
setup(void)
{
clear();
setEntityState(IMC::EntityState::ESTA_BOOT, Status::CODE_INIT);
// Stop continuous mode.
setContinuousMode();
Delay::wait(1.0);
m_uart->flushInput();
// Stop continuous mode and read answer.
setContinuousMode();
if (!getMessage(MSG_CONTMODE))
{
setEntityState(IMC::EntityState::ESTA_FAILURE,
DTR("failed to stop device"));
return false;
}
// Read GyroGainScale.
queryEEPROM(130);
if (!getMessage(MSG_EEPROM))
{
setEntityState(IMC::EntityState::ESTA_FAILURE,
DTR("failed to read EEPROM#130"));
return false;
}
m_gyro_scale = (32768000.0 / m_eeprom);
// Read AccelGainScale.
queryEEPROM(230);
if (!getMessage(MSG_EEPROM))
{
setEntityState(IMC::EntityState::ESTA_FAILURE,
DTR("failed to read EEPROM#230"));
return false;
}
m_accel_scale = (32768000.0 / m_eeprom);
// Set continuous mode for gyro-stabilized euler angles.
setContinuousMode(MSG_GS_EULER);
// Device is configured.
setEntityState(IMC::EntityState::ESTA_NORMAL, Status::CODE_ACTIVE);
return true;
}
void
updateBeacons(void)
{
m_beacons.clear();
IMC::MessageList<IMC::LblBeacon>::const_iterator itr = m_lbl_cfg->beacons.begin();
for (unsigned i = 0; itr != m_lbl_cfg->beacons.end(); ++itr, ++i)
addBeacon(i, *itr);
m_beacon = m_beacons.begin();
m_ping_time.reset();
if (isActive())
setEntityState(IMC::EntityState::ESTA_NORMAL, Status::CODE_ACTIVE);
else
setEntityState(IMC::EntityState::ESTA_NORMAL, Status::CODE_IDLE);
}
//! Signal a bad vertical mode or incompatible
//! @param[in] desc description of bad vertical mode
void
signalBadVertical(const char* desc = DTR("vertical control mode %d not supported"))
{
setEntityState(IMC::EntityState::ESTA_ERROR, Utils::String::str(DTR(desc), m_vertical_mode));
err("%s", Utils::String::str(DTR(desc), m_vertical_mode).c_str());
requestDeactivation();
}
BasicAutopilot::BasicAutopilot(const std::string& name, Tasks::Context& ctx,
const uint32_t controllable_loops, const uint32_t required_loops):
Tasks::Task(name, ctx),
m_vertical_mode(VERTICAL_MODE_NONE),
m_yaw_mode(YAW_MODE_NONE),
m_aloops(0),
m_controllable_loops(controllable_loops),
m_required_loops(required_loops),
m_scope_ref(0)
{
param("Heading Rate Bypass", m_hrate_bypass)
.defaultValue("false")
.description("Bypass heading rate controller and use reference directly on torques");
m_ctx.config.get("General", "Absolute Maximum Depth", "50.0", m_max_depth);
// Initialize entity state.
setEntityState(IMC::EntityState::ESTA_NORMAL, Status::CODE_IDLE);
// Register handler routines.
bind<IMC::EstimatedState>(this);
bind<IMC::DesiredHeadingRate>(this);
bind<IMC::DesiredHeading>(this);
bind<IMC::DesiredZ>(this);
bind<IMC::DesiredPitch>(this);
bind<IMC::DesiredVelocity>(this);
bind<IMC::ControlLoops>(this);
}
void
onActivation(void)
{
inf("%s", DTR(Status::getString(Status::CODE_ACTIVE)));
setEntityState(IMC::EntityState::ESTA_NORMAL, Status::CODE_ACTIVE);
m_activating = false;
}
void
BasicDeviceDriver::onActivation(void)
{
initializeDevice();
setEntityState(IMC::EntityState::ESTA_NORMAL, Status::CODE_ACTIVE);
debug("activation took %0.2f s", m_wdog.getElapsed());
}
//! Initialize resources.
void
onResourceInitialization(void)
{
if (!getConstantParameters())
throw RestartNeeded(DTR("failed to get constant parameters"), c_restart_delay);
setConfig();
std::map<std::string, LED*>::iterator itr = m_led_by_name.begin();
for (unsigned i = 0; i < c_led_count; ++i)
setBrightness(itr->second, 0);
if (!m_args.led_patterns.empty())
{
uint8_t count = m_args.led_patterns.size();
UCTK::Frame frame;
frame.setId(PKT_ID_LED_PATTERN);
frame.setPayloadSize(1 + (count * 2));
frame.set(count, 0);
for (size_t i = 0; i < count; ++i)
frame.set<uint16_t>(m_args.led_patterns[i], 1 + i * 2);
if (!m_ctl->sendFrame(frame))
throw RestartNeeded(DTR("failed to set LED patterns"), c_restart_delay);
}
m_wdog.reset();
setEntityState(IMC::EntityState::ESTA_NORMAL, Status::CODE_ACTIVE);
}
//! Request device to ping.
//! @param[in] data_point data index.
void
ping(unsigned data_point)
{
m_sdata[SD_PACKET_NUM] = (uint8_t)data_point;
m_sock.write((char*)m_sdata, c_sdata_size);
int rv = m_sock.read((char*)m_rdata_hdr, c_rdata_hdr_size);
if (rv != c_rdata_hdr_size)
throw std::runtime_error(DTR("failed to read header"));
unsigned dat_idx = data_point * c_rdata_dat_size;
if (m_args.save_in_837)
rv = m_sock.read((char*)(m_frame.getMessageData() + dat_idx), c_rdata_dat_size);
else
rv = m_sock.read(&m_ping.data[dat_idx], c_rdata_dat_size);
if (rv != c_rdata_dat_size)
throw std::runtime_error(DTR("failed to read data"));
rv = m_sock.read((char*)m_rdata_ftr, c_rdata_ftr_size);
if (rv != c_rdata_ftr_size)
throw std::runtime_error(DTR("failed to read footer"));
setEntityState(IMC::EntityState::ESTA_NORMAL, Status::CODE_ACTIVE);
}
void
consume(const IMC::LblConfig* msg)
{
if (msg->op == IMC::LblConfig::OP_SET_CFG)
{
// Save message to cache.
IMC::CacheControl cop;
cop.op = IMC::CacheControl::COP_STORE;
cop.message.set(*msg);
dispatch(cop);
Memory::replace(m_lbl_cfg, new IMC::LblConfig(*msg));
if (hasOrigin())
updateBeacons();
else
setEntityState(IMC::EntityState::ESTA_BOOT, Status::CODE_WAIT_GPS_FIX);
}
else if (msg->op == IMC::LblConfig::OP_GET_CFG)
{
IMC::LblConfig cfg;
cfg.op = IMC::LblConfig::OP_CUR_CFG;
if (m_lbl_cfg != NULL)
cfg.beacons = m_lbl_cfg->beacons;
dispatch(cfg);
}
}
void
consume(const IMC::GpsFix* msg)
{
if (msg->getSourceEntity() != m_gps_eid)
return;
if ((msg->validity & IMC::GpsFix::GFV_VALID_POS) == 0)
{
m_gps_rej.reason = IMC::GpsFixRejection::RR_INVALID;
dispatch(m_gps_rej, DF_KEEP_TIME);
return;
}
// Received valid GPS data.
setEntityState(IMC::EntityState::ESTA_NORMAL, Status::CODE_ACTIVE);
m_time_without_gps.reset();
m_estate.lat = msg->lat;
m_estate.lon = msg->lon;
// Decompose velocity vector.
m_estate.vx = std::cos(msg->cog) * msg->sog;
m_estate.vy = std::sin(msg->cog) * msg->sog;
m_estate.u = msg->sog;
dispatch(m_estate);
}
void
onVersion(unsigned major, unsigned minor, unsigned patch)
{
m_hw_major = major;
inf(DTR("version: %u.%u.%u"), major, minor, patch);
setEntityState(IMC::EntityState::ESTA_NORMAL, Status::CODE_ACTIVE);
}
void
BasicDeviceDriver::failActivation(const std::string& message)
{
activationFailed(message);
turnPowerOff();
setEntityState(IMC::EntityState::ESTA_NORMAL, Status::CODE_IDLE);
}
//! On activation enter active entity state
//! Method from parent class
void
onActivation(void)
{
setEntityState(IMC::EntityState::ESTA_NORMAL, Status::CODE_ACTIVE);
reset();
onAutopilotActivation();
}
//! On deactivation leave error or active entity state
//! Method from parent class
void
onDeactivation(void)
{
setEntityState(IMC::EntityState::ESTA_NORMAL, Status::CODE_IDLE);
reset();
onAutopilotDeactivation();
}
void
setAndSendState(EntityStates state)
{
m_state = state;
setEntityState((IMC::EntityState::StateEnum)m_states[m_state].state,
m_states[m_state].description);
}
void
consume(const IMC::SimulatedState* msg)
{
setEntityState(IMC::EntityState::ESTA_NORMAL, Status::CODE_ACTIVE);
m_active = true;
m_sstate = *msg;
}
void
openDB(void)
{
if (m_db != NULL)
return;
setEntityState(IMC::EntityState::ESTA_BOOT, Status::CODE_INIT);
Path db_file = m_ctx.dir_db / "Plan.db";
debug("database file: '%s'", db_file.c_str());
m_db = new Database::Connection(db_file.c_str(), true);
m_get_plan_stmt = new Database::Statement(c_get_plan_stmt, *m_db);
setEntityState(IMC::EntityState::ESTA_NORMAL, Status::CODE_ACTIVE);
}
void
Daemon::onResourceInitialization(void)
{
m_ctx.mbus.resume();
m_tman->start();
m_periodic_counter.setTop(1.0);
setEntityState(IMC::EntityState::ESTA_NORMAL, Status::CODE_ACTIVE);
}
void
onActivation(void)
{
connect();
setupScanList();
configAnalogueInputs();
setEntityState(IMC::EntityState::ESTA_NORMAL, Status::CODE_ACTIVE);
}
void
onResourceAcquisition(void)
{
setEntityState(IMC::EntityState::ESTA_BOOT, Status::CODE_INIT);
m_uart = new SerialPort(m_args.uart_dev, m_args.uart_baud);
m_uart->setCanonicalInput(true);
m_uart->flush();
}
void
BasicRemoteOperation::consume(const IMC::ControlLoops* msg)
{
if (!(msg->mask & IMC::CL_TELEOPERATION) || msg->enable == isActive())
return;
if (msg->enable == IMC::ControlLoops::CL_ENABLE)
{
activate();
setEntityState(IMC::EntityState::ESTA_NORMAL, Status::CODE_ACTIVE);
}
else
{
deactivate();
setEntityState(IMC::EntityState::ESTA_NORMAL, Status::CODE_IDLE);
}
}
void
Maneuver::onDeactivation(void)
{
onManeuverDeactivation();
setEntityState(IMC::EntityState::ESTA_NORMAL, Status::CODE_IDLE);
debug("disabling");
unlock();
}
void
consume(const IMC::SimulatedState* msg)
{
if (!isActive())
{
m_vel[0] = msg->u;
m_vel[1] = msg->v;
m_vel[2] = msg->w;
requestActivation();
}
// Compute time delta.
double tstep = m_delta.getDelta();
// Check if we have a valid time delta.
if (tstep <= 0)
return;
// Define Euler Angles variables and add gaussian noise component.
if (m_args.euler)
{
m_euler.phi = Angles::normalizeRadian(msg->phi + m_prng->gaussian() * Angles::radians(m_args.stdev_euler));
m_euler.theta = Angles::normalizeRadian(msg->theta + m_prng->gaussian() * Angles::radians(m_args.stdev_euler));
m_euler.psi_magnetic = Angles::normalizeRadian(msg->psi + m_prng->gaussian() * Angles::radians(m_args.stdev_euler));
m_euler.psi = Angles::normalizeRadian(m_euler.psi_magnetic + m_heading_offset);
// Heading offset will increment through time according with gyro rate bias.
m_heading_offset += Angles::radians(m_args.gyro_bias / 3600) * tstep;
m_euler.setTimeStamp(msg->getTimeStamp());
dispatch(m_euler, DF_KEEP_TIME);
}
// Define Angular Velocity variables and add gaussian noise component.
m_agvel.x = Angles::normalizeRadian(msg->p + m_prng->gaussian() * Angles::radians(m_args.stdev_agvel));
m_agvel.y = Angles::normalizeRadian(msg->q + m_prng->gaussian() * Angles::radians(m_args.stdev_agvel));
m_agvel.z = Angles::normalizeRadian(msg->r + m_prng->gaussian() * Angles::radians(m_args.stdev_agvel));
// Compute acceleration values using simulated state velocity fields.
m_accel.x = (msg->u - m_vel[0]) / tstep;
m_accel.y = (msg->v - m_vel[1]) / tstep;
m_accel.z = (msg->w - m_vel[2]) / tstep;
// Store velocity for next iteration.
m_vel[0] = msg->u;
m_vel[1] = msg->v;
m_vel[2] = msg->w;
// Define messages timestamp and dispatch them to the bus.
m_agvel.setTimeStamp(msg->getTimeStamp());
m_accel.setTimeStamp(msg->getTimeStamp());
dispatch(m_agvel, DF_KEEP_TIME);
dispatch(m_accel, DF_KEEP_TIME);
setEntityState(IMC::EntityState::ESTA_NORMAL, Status::CODE_ACTIVE);
}
//! Initialize resources.
void
onResourceInitialization(void)
{
// Initialize RPM values.
m_rpm.value = 0;
m_rpm_new = 0;
m_avg_motor = new MovingAverage<double>(m_args.avg_samples);
setEntityState(IMC::EntityState::ESTA_NORMAL, Status::CODE_ACTIVE);
}
void
consume(const IMC::EntityControl* msg)
{
if (msg->getDestinationEntity() != getEntityId())
return;
m_active = (msg->op == IMC::EntityControl::ECO_ACTIVATE);
if (m_active)
{
setEntityState(IMC::EntityState::ESTA_NORMAL, Status::CODE_ACTIVE);
if (!m_log_file.is_open())
m_log_file.open((m_ctx.dir_log / m_log_filename / "Data.837").c_str(), std::ios::binary);
}
else
{
setEntityState(IMC::EntityState::ESTA_NORMAL, Status::CODE_IDLE);
}
}
