//! 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);
}
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;
m_wdog.reset();
dispatch(m_sspeed);
}
}
void
setConfig(void)
{
if (m_ctl != NULL)
{
if (!setExternalDriver(m_args.ext_drv))
throw RestartNeeded(DTR("failed to configure LED driver"), c_restart_delay);
if (!setExternalTrigger(m_args.ext_trg))
throw RestartNeeded(DTR("failed to configure LED driver"), c_restart_delay);
if (!setPatternPulseWidth(m_args.led_patterns_pw))
throw RestartNeeded(DTR("failed to configure LED pattern pulse width"), c_restart_delay);
}
}
//! 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
onResourceInitialization(void)
{
m_uart->writeString("\r");
Delay::wait(1.0);
m_uart->flush();
if (!sendCommand("\r", "\r\n"))
throw RestartNeeded(DTR("failed to enter command mode"), 5, false);
if (!sendCommand("SET SAMPLE 1 s\r", ">SET SAMPLE 1 s\r\n"))
throw RestartNeeded(DTR("failed to set sampling rate"), 5, false);
if (!sendCommand("MONITOR\r", ">MONITOR\r\n"))
throw RestartNeeded(DTR("failed to enter monitor mode"), 5, false);
m_wdog.setTop(m_args.input_timeout);
}
void
onResourceInitialization(void)
{
m_uart->writeString("\r");
Delay::wait(1.0);
m_uart->flush();
if (!sendCommand("\r", "\r\n"))
throw RestartNeeded(DTR("failed to enter command mode"), 5);
if (!sendCommand("SET SAMPLE 1 s\r", ">SET SAMPLE 1 s\r\n"))
throw RestartNeeded(DTR("failed to set sampling rate"), 5);
if (!sendCommand("MONITOR\r", ">MONITOR\r\n"))
throw RestartNeeded(DTR("failed to enter monitor mode"), 5);
setEntityState(IMC::EntityState::ESTA_NORMAL, Status::CODE_ACTIVE);
m_wdog.setTop(m_args.input_timeout);
}
void
onResourceAcquisition(void)
{
try
{
m_proto.setUART(m_args.uart_dev);
m_proto.open();
m_proto.requestVersion();
}
catch (std::runtime_error& e)
{
throw RestartNeeded(e.what(), 30);
}
}
void
findDevice(void)
{
debug("probing MCC device");
m_udev = usb_device_find_USB_MCC(USB1608G_PID, NULL);
if (m_udev != NULL)
return;
m_udev = usb_device_find_USB_MCC(c_1608g_alt_pid, NULL);
if (m_udev != NULL)
return;
throw RestartNeeded(DTR("failed to find valid device"), 5.0, false);
}
void
onResourceAcquisition(void)
{
setEntityState(IMC::EntityState::ESTA_BOOT, Status::CODE_INIT);
try
{
m_uart = new SerialPort(m_args.uart_dev, m_args.uart_baud);
m_uart->setCanonicalInput(true);
m_uart->flush();
}
catch (std::runtime_error& e)
{
throw RestartNeeded(e.what(), 30);
}
}
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();
}
}
}
//! Acquire resources.
void
onResourceAcquisition(void)
{
try
{
m_uart = new SerialPort(m_args.uart_dev, c_baud_rate);
m_ctl = new UCTK::Interface(m_uart);
UCTK::FirmwareInfo info = m_ctl->getFirmwareInfo();
if (info.isDevelopment())
war(DTR("device is using unstable firmware"));
else
inf(DTR("firmware version %u.%u.%u"), info.major,
info.minor, info.patch);
}
catch (std::runtime_error& e)
{
throw RestartNeeded(e.what(), c_restart_delay);
}
}
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);
}
}
}
void
connect(void)
{
debug("initializing USB library");
int rv = libusb_init(NULL);
if (rv < 0)
throw RestartNeeded(DTR("failed to initialize USB library"), 5.0, false);
findDevice();
debug("initializing MCC device");
usbInit_1608G(m_udev);
debug("building gain table");
usbBuildGainTable_USB1608G(m_udev, m_gain_table);
uint16_t version = 0xbeef;
usbFPGAVersion_USB1608G(m_udev, &version);
inf(DTR("FPGA version: %02x.%02x"), version >> 0x8, version & 0xff);
char serial[9] = {0};
usbGetSerialNumber_USB1608G(m_udev, serial);
inf(DTR("serial number: %s"), serial);
}
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);
}
}