ALSA(unsigned channels, unsigned samplerate, const std::string& device = "default") : runnable(true), pcm(nullptr), params(nullptr), fps(samplerate)
{
int rc = snd_pcm_open(&pcm, device.c_str(), SND_PCM_STREAM_PLAYBACK, 0);
if (rc < 0)
{
runnable = false;
throw DeviceException(General::join("Unable to open PCM device ", snd_strerror(rc)));
}
snd_pcm_format_t fmt = type_to_format(T());
if (snd_pcm_hw_params_malloc(¶ms) < 0)
{
runnable = false;
throw DeviceException("Failed to allocate memory.");
}
runnable = false;
if (
(snd_pcm_hw_params_any(pcm, params) < 0) ||
(snd_pcm_hw_params_set_access(pcm, params, SND_PCM_ACCESS_RW_INTERLEAVED) < 0) ||
(snd_pcm_hw_params_set_channels(pcm, params, channels) < 0) ||
(snd_pcm_hw_params_set_format(pcm, params, fmt) < 0) ||
(snd_pcm_hw_params_set_rate(pcm, params, samplerate, 0) < 0) ||
((rc = snd_pcm_hw_params(pcm, params)) < 0)
)
throw DeviceException(General::join("Unable to install HW params: ", snd_strerror(rc)));
runnable = true;
}
std::shared_ptr<CameraDriverInterface> GetDevice(const Uri& uri)
{
std::vector<Uri> suburis = splitUri(uri.url);
std::vector<std::shared_ptr<CameraDriverInterface>> cameras;
cameras.reserve(suburis.size());
for( const Uri& uri : suburis ) {
try {
std::cout << "Creating stream from uri: " << uri.ToString()
<< std::endl;
cameras.emplace_back
(DeviceRegistry<hal::CameraDriverInterface>::I().Create(uri));
} catch ( std::exception& e ) {
throw DeviceException(std::string("Error creating driver from uri \"") +
uri.ToString() + "\": " + e.what());
}
}
if( cameras.empty() ) {
throw DeviceException("No input cameras given to join");
}
JoinCameraDriver* pDriver = new JoinCameraDriver(cameras);
return std::shared_ptr<CameraDriverInterface>( pDriver );
}
TCPClientProxy::TCPClientProxy(Beetle &beetle, SSL *ssl, int sockfd, std::string clientGateway_,
struct sockaddr_in clientGatewaySockAddr_, device_t localProxyFor_) :
TCPConnection(beetle, ssl, sockfd, clientGatewaySockAddr_, false, new SingleAllocator(localProxyFor_)) {
name = "Proxy for " + std::to_string(localProxyFor_) + " to " + clientGateway_;
type = TCP_CLIENT_PROXY;
clientGateway = clientGateway_;
localProxyFor = localProxyFor_;
/*
* Make sure the device exists locally.
*/
boost::shared_lock<boost::shared_mutex> devicesLk(beetle.devicesMutex);
if (beetle.devices.find(localProxyFor_) == beetle.devices.end()) {
throw DeviceException("no device for " + std::to_string(localProxyFor_));
}
switch (beetle.devices[localProxyFor_]->getType()) {
case LE_PERIPHERAL:
case LE_CENTRAL:
case TCP_CLIENT:
case TCP_SERVER_PROXY:
case IPC_APPLICATION:
case BEETLE_INTERNAL:
break;
default:
throw DeviceException("cannot proxy to device type");
}
}
LEDevice *LEDevice::newPeripheral(Beetle &beetle, HCI &hci, bdaddr_t bdaddr,
AddrType addrType) {
struct sockaddr_l2 loc_addr;
memset(&loc_addr, 0, sizeof(struct sockaddr_l2));
loc_addr.l2_family = AF_BLUETOOTH;
hci_dev_info devInfo;
hci_devinfo(hci.getDeviceId(), &devInfo);
bacpy(&loc_addr.l2_bdaddr, &devInfo.bdaddr);
loc_addr.l2_psm = 0;
loc_addr.l2_cid = htobs(ATT_CID);
loc_addr.l2_bdaddr_type = devInfo.type;
struct sockaddr_l2 rem_addr;
memset(&rem_addr, 0, sizeof(struct sockaddr_l2));
rem_addr.l2_family = AF_BLUETOOTH;
bacpy(&rem_addr.l2_bdaddr, &bdaddr);
rem_addr.l2_psm = 0;
rem_addr.l2_cid = htobs(ATT_CID);
rem_addr.l2_bdaddr_type = (addrType == PUBLIC) ? BDADDR_LE_PUBLIC : BDADDR_LE_RANDOM;
int sockfd = socket(AF_BLUETOOTH, SOCK_SEQPACKET, BTPROTO_L2CAP);
if (sockfd < 0) {
throw DeviceException("could not create socket");
}
if (bind(sockfd, (struct sockaddr *) &loc_addr, sizeof(loc_addr)) < 0) {
close(sockfd);
throw DeviceException("could not bind");
}
if (connect(sockfd, (struct sockaddr *) &rem_addr, sizeof(rem_addr)) < 0) {
close(sockfd);
throw DeviceException("could not connect");
}
struct l2cap_conninfo connInfo;
unsigned int connInfoLen = sizeof(connInfo);
if (getsockopt(sockfd, SOL_L2CAP, L2CAP_CONNINFO, &connInfo, &connInfoLen) < 0) {
close(sockfd);
throw DeviceException("could not get l2cap conn info");
}
if (debug_socket) {
std::stringstream ss;
ss << "peripheral hci handle: " << connInfo.hci_handle;
pdebug(ss.str());
}
std::string name;
std::list<delayed_packet_t> delayedPackets;
if (!request_device_name(sockfd, name, delayedPackets)) {
close(sockfd);
throw DeviceException("could not discover device name");
}
return new LEDevice(beetle, hci, sockfd, rem_addr, connInfo, LE_PERIPHERAL,
name, delayedPackets);
}
void
SchmersalLss300::setMonitoring(const Monitoring& monitoring)
{
assert(this->isConnected());
::std::array< ::std::uint8_t, 1013> buf;
buf[4] = 0x20;
switch (monitoring)
{
case MONITORING_CONTINUOUS:
buf[5] = 0x24;
break;
case MONITORING_SINGLE:
buf[5] = 0x25;
break;
default:
break;
}
do
{
this->send(buf.data(), 1 + 1 + 2 + 1 + 1 + 2);
}
while (!this->waitAck());
this->recv(buf.data(), 1 + 1 + 2 + 1 + 1 + 2, 0xA0);
switch (buf[5])
{
case 0x10:
throw DeviceException("mode switchover not possible due to incorrect operating mode");
break;
case 0x12:
throw DeviceException("mode switchover not possible due to incorrect password");
break;
case 0x13:
throw DeviceException("mode switchover not possible due to not rectangular field");
break;
case 0x80:
throw DeviceException("mode switchover not possible due to a sensor fault");
break;
default:
break;
}
this->monitoring = monitoring;
}
std::shared_ptr<CameraDriverInterface> GetDevice(const Uri& uri)
{
const Uri input_uri = Uri(uri.url);
// Create input camera
std::shared_ptr<CameraDriverInterface> input =
DeviceRegistry<hal::CameraDriverInterface>::I().Create(input_uri);
std::string filename = ExpandTildePath(
uri.properties.Get<std::string>("file", "cameras.xml")
);
if(!FileExists(filename))
{
std::string dir = input->GetDeviceProperty(hal::DeviceDirectory);
while(!dir.empty() && !FileExists(dir+"/"+filename)) {
dir = DirUp(dir);
}
filename = (dir.empty() ? "" : dir + "/") + filename;
}
calibu::CameraRig rig = calibu::ReadXmlRig( filename );
if(rig.cameras.size() != 2) {
throw DeviceException("Unable to find 2 cameras in file '" + filename + "'");
}
RectifyDriver* rectify = new RectifyDriver( input, rig );
return std::shared_ptr<CameraDriverInterface>( rectify );
}
void
SickLms200::setVariant(const Variant& variant)
{
assert(this->isConnected());
uint8_t buf[812];
buf[4] = 0x3B;
switch (variant)
{
case VARIANT_100_25:
buf[5] = 0x64;
buf[6] = 0x0;
buf[7] = 0x19;
buf[8] = 0x0;
break;
case VARIANT_100_50:
buf[5] = 0x64;
buf[6] = 0x0;
buf[7] = 0x32;
buf[8] = 0x0;
break;
case VARIANT_100_100:
buf[5] = 0x64;
buf[6] = 0x0;
buf[7] = 0x64;
buf[8] = 0x0;
break;
case VARIANT_180_50:
buf[5] = 0xB4;
buf[6] = 0x0;
buf[7] = 0x32;
buf[8] = 0x0;
break;
case VARIANT_180_100:
buf[5] = 0xB4;
buf[6] = 0x0;
buf[7] = 0x64;
buf[8] = 0x0;
break;
default:
break;
}
do
{
this->send(buf, 11);
}
while (!this->waitAck());
this->recv(buf, 1 + 1 + 2 + 1 + 5 + 1 + 2, 0xBB);
if (0x00 == buf[5])
{
throw DeviceException("switchover aborted, previous variant still active");
}
this->variant = variant;
}
void
SchmersalLss300::send(::std::uint8_t* buf, const ::std::size_t& len)
{
assert(this->isConnected());
assert(len > 6);
buf[0] = 0x02;
buf[1] = 0x00;
::std::uint16_t length = len - 6;
buf[2] = Endian::hostLowByte(length);
buf[3] = Endian::hostHighByte(length);
::std::uint16_t checksum = this->crc(buf, len - 2);
buf[len - 2] = Endian::hostLowByte(checksum);
buf[len - 1] = Endian::hostHighByte(checksum);
if (len != this->serial.write(buf, len))
{
throw DeviceException("could not send complete data");
}
this->serial.flush(true, false);
}
/**
* @brief Ft245sync::getWriteChunkSize
* @return returns write chunk size
*/
unsigned int Ft245sync::getWriteChunkSize()
{
unsigned int value;
if(ftdi_write_data_get_chunksize(ftdic, &value) != 0)
{
throw DeviceException("Wrong ftdi context. FTDI not initilized?", FTDI_ERROR);
}
return value;
}
void
SickLms200::setMonitoring(const Monitoring& monitoring)
{
assert(this->isConnected());
uint8_t buf[812];
buf[4] = 0x20;
switch (monitoring)
{
case MONITORING_CONTINUOUS:
buf[5] = 0x24;
break;
case MONITORING_SINGLE:
buf[5] = 0x25;
break;
default:
break;
}
do
{
this->send(buf, 1 + 1 + 2 + 1 + 1 + 2);
}
while (!this->waitAck());
this->recv(buf, 1 + 1 + 2 + 1 + 1 + 1 + 2, 0xA0);
switch (buf[5])
{
case 0x01:
throw DeviceException("mode switchover not possible due to incorrect password");
break;
case 0x02:
throw DeviceException("mode switchover not possible due to a fault in the LMS2xx");
break;
default:
break;
}
this->monitoring = monitoring;
}
LEDevice *LEDevice::newCentral(Beetle &beetle, HCI &hci, int sockfd,
struct sockaddr_l2 addr, std::function<void()> onDisconnect) {
struct l2cap_conninfo connInfo;
unsigned int connInfoLen = sizeof(connInfo);
if (getsockopt(sockfd, SOL_L2CAP, L2CAP_CONNINFO, &connInfo, &connInfoLen) < 0) {
close(sockfd);
throw DeviceException("could not get l2cap conn info");
}
std::string name;
std::list<delayed_packet_t> delayedPackets;
if (!request_device_name(sockfd, name, delayedPackets)) {
close(sockfd);
throw DeviceException("could not discover device name");
}
return new LEDevice(beetle, hci, sockfd, addr, connInfo, LE_CENTRAL,
name, delayedPackets, onDisconnect);
}
ConvertDriver::ConvertDriver(
std::shared_ptr<CameraDriverInterface> Input,
const std::string& sFormat,
double dRange,
ImageDim dims,
int channel)
: m_Input(Input),
m_sFormat(sFormat),
m_nOutCvType(-1),
m_nNumChannels(Input->NumChannels()),
m_dRange(dRange),
m_Dims(dims),
m_iChannel(channel)
{
// Set the correct image size on the output interface, considering the request
// to resize the images
for(size_t i = 0; i < Input->NumChannels(); ++i) {
m_nImgWidth.push_back(Input->Width(i));
m_nImgHeight.push_back(Input->Height(i));
m_nOrigImgWidth.push_back(Input->Width(i));
m_nOrigImgHeight.push_back(Input->Height(i));
if (m_iChannel != -1) {
if (m_iChannel != (int)i) {
continue;
}
}
const bool resize_requested = (m_Dims.x != 0 || m_Dims.y != 0);
if (resize_requested) {
m_nImgWidth[i] = m_Dims.x;
m_nImgHeight[i] = m_Dims.y;
}
}
// Guess output color coding
if( m_sFormat == "MONO8" ) {
m_nOutCvType = CV_8UC1;
m_nOutPbType = hal::Format::PB_LUMINANCE;
} else if( m_sFormat == "RGB8" ) {
m_nOutCvType = CV_8UC3;
m_nOutPbType = hal::Format::PB_RGB;
} else if( m_sFormat == "BGR8" ) {
m_nOutCvType = CV_8UC3;
m_nOutPbType = hal::Format::PB_BGR;
}
if( m_nOutCvType == -1 )
throw DeviceException("HAL: Error! Unknown target format: " + m_sFormat);
}
void
LeuzeRs4::step()
{
assert(this->isConnected());
if (2 == this->type)
{
uint8_t buf[1099];
buf[2] = 0x24;
buf[3] = 0x01;
this->send(buf, 2 + 1 + 1 + 1 + 3);
}
this->recv(this->data, 1099);
if (0x23 != this->data[2])
{
throw DeviceException("incorrect reply");
}
}
std::shared_ptr<BaseDevice> DeviceRegistry<BaseDevice>::Create(
const Uri& uri )
{
// Check for aliases
std::map<std::string,std::string>::const_iterator iAlias= m_aliases.find( uri.scheme );
if( iAlias != m_aliases.end() ){
std::string sAlias = iAlias->second;
std::ostringstream oss;
oss << sAlias << uri.url;
return Create(oss.str());
}
else{
auto pf = m_factories.find(uri.scheme);
if(pf != m_factories.end()) {
std::shared_ptr<BaseDevice> dev = pf->second->GetDevice(uri);
return dev;
}
else{
throw DeviceException("Scheme '" + uri.scheme + "' not registered for factory");
}
}
}
::std::size_t
LeuzeRs4::recv(uint8_t* buf, const ::std::size_t& len)
{
assert(this->isConnected());
assert(len > 7);
uint8_t* ptr;
::std::size_t sumbytes;
::std::size_t numbytes;
do
{
do
{
ptr = buf;
sumbytes = 0;
do
{
numbytes = this->serial->read(ptr, 1);
}
while (buf[0] != 0x00);
ptr += numbytes;
sumbytes += numbytes;
numbytes = this->serial->read(ptr, 1);
}
while (buf[1] != 0x00);
ptr += numbytes;
sumbytes += numbytes;
numbytes = this->serial->read(ptr, 1);
}
while (buf[2] == 0x00 || buf[2] == 0xFF);
ptr += numbytes;
sumbytes += numbytes;
do
{
numbytes = this->serial->read(ptr, 1);
ptr += numbytes;
sumbytes += numbytes;
if (sumbytes > 1099 - 1)
{
throw DeviceException("invalid data length");
return -1;
}
}
while (*(ptr - 1) != 0x00 || *(ptr - 2) != 0x00 || *(ptr - 3) != 0x00);
if (this->crc(buf, sumbytes - 4) != buf[sumbytes - 4])
{
throw DeviceException("checksum error");
}
switch (buf[2])
{
case 0x17:
throw DeviceException("not acknowledged");
break;
case 0x53:
throw DeviceException("error");
break;
case 0x54:
throw DeviceException("warning");
break;
default:
break;
}
// Option 1
if (buf[3] & 16)
{
throw DeviceException("error / malfunction");
}
if (buf[3] & 2)
{
// Option 2
this->near1 = (buf[4] & 1) ? true : false;
this->far1 = (buf[4] & 2) ? true : false;
if (buf[4] & 8)
{
throw DeviceException("malfunction");
}
if (buf[4] & 16)
{
throw DeviceException("restart inhibit");
}
this->near2 = (buf[4] & 32) ? true : false;
this->far2 = (buf[4] & 64) ? true : false;
if ((buf[3] & 3) && (buf[4] & 128))
{
// Option 3
this->fn1Fn2 = (buf[5] & 64) ? true : false;
}
}
return sumbytes;
}
void
LeuzeRs4::open()
{
this->serial->open();
this->setConnected(true);
#if 0
uint8_t buf[1099];
// synchronize baud rates
buf[4] = 0x20;
buf[5] = 0x42;
Serial::BaudRate baudRates[6] = {
// Serial::BAUDRATE_625000BPS,
// Serial::BAUDRATE_345600BPS,
Serial::BAUDRATE_115200BPS,
Serial::BAUDRATE_57600BPS,
Serial::BAUDRATE_38400BPS,
Serial::BAUDRATE_19200BPS,
Serial::BAUDRATE_9600BPS,
Serial::BAUDRATE_4800BPS
};
for (::std::size_t i = 0; i < 6; ++i)
{
this->serial->setBaudRate(baudRates[i]);
this->serial->changeParameters();
this->send(buf, 1 + 1 + 2 + 1 + 1 + 2);
if (this->waitAck())
{
break;
}
if (5 == i)
{
throw DeviceException("could not sync baud rate.");
}
}
this->recv(buf, 1 + 1 + 2 + 1 + 1 + 1 + 2, 0xA0);
switch (buf[5])
{
case 0x01:
throw DeviceException("mode switchover not possible due to incorrect password");
break;
case 0x02:
throw DeviceException("mode switchover not possible due to a fault in the LMS2xx");
break;
default:
break;
}
// status
buf[4] = 0x31;
do
{
this->send(buf, 1 + 1 + 2 + 1 + 2);
}
while (!this->waitAck());
this->recv(buf, 1 + 1 + 2 + 1 + 152 + 1 + 2, 0xB1);
// baud rate
switch (buf[120])
{
#ifndef WIN32
case 0x01:
this->baudRate = BAUDRATE_500000BPS;
break;
#endif // WIN32
case 0x19:
this->baudRate = BAUDRATE_38400BPS;
break;
case 0x33:
this->baudRate = BAUDRATE_19200BPS;
break;
case 0x67:
this->baudRate = BAUDRATE_9600BPS;
break;
default:
break;
}
if (this->desired != this->baudRate)
{
this->setBaudRate(this->desired);
}
// monitoring
if (0x25 != buf[12])
{
this->setMonitoring(MONITORING_SINGLE);
}
#endif
}
::std::size_t
SickS300::recv(uint8_t* buf)
{
uint8_t* ptr;
::std::size_t sumbytes;
::std::size_t numbytes;
do
{
do
{
ptr = buf;
sumbytes = 0;
do
{
numbytes = serial.read(ptr, 1);
printf("buf[0] %02x\n", buf[0]);
}
while (0x00 != buf[0]); // 0x00
ptr += numbytes;
sumbytes += numbytes;
numbytes = serial.read(ptr, 1);
printf("buf[1] %02x\n", buf[1]);
}
while (0x00 != buf[1]); // 0x00 0x00
ptr += numbytes;
sumbytes += numbytes;
numbytes = serial.read(ptr, 1);
printf("buf[2] %02x\n", buf[2]);
}
while (0x00 != buf[2]); // 0x00 0x00 0x00
ptr += numbytes;
sumbytes += numbytes;
numbytes = serial.read(ptr, 1);
printf("buf[3] %02x\n", buf[3]);
ptr += numbytes;
sumbytes += numbytes;
if (0x00 != buf[3])
{
switch (buf[3])
{
case 0x01:
throw DeviceException("The current device status does not permit access to the data block");
break;
case 0x02:
throw DeviceException("Access to the data block by the current user group is not permitted");
break;
case 0x03:
throw DeviceException("Incorrect password");
break;
case 0x04:
throw DeviceException("System token is occupied");
break;
case 0x05:
throw DeviceException("Incorrect parameter");
break;
case 0x0A:
throw DeviceException("One of the communication monitoring processes failed (e.g. timeout of EFI-RK512 packages/EFI-RK512 acknowledgement or failed transmission of EFI-RK512 packages/EFI-RK512 acknowledgement)");
break;
case 0x0C:
throw DeviceException("The data word number of the destination address or source address in command telegram (byte 6) is impermissible (not defined in interface register) | The co-ordination flag (byte number) in command telegram (byte 9) does not equal 0xFF | The device code in the command telegram (byte 10, bits 0 to 3) is invalid (i.e. equals 0) | The CPU number in the command telegram (byte 10, bits 5 to 7) is impermissible");
break;
case 0x10:
throw DeviceException("The telegram identifier in the command telegram (byte 1) is not equal to 0x00 or 0xFF or is not followed by a further 0x00 byte (byte 2) | The command data type in the command telegram (byte 4) is impermissible");
break;
case 0x14:
throw DeviceException("The data block number of the destination address or source address in the command telegram (byte 5) is impermissible (not defined in the interface register)");
break;
case 0x16:
throw DeviceException("The command telegram type in the command telegram (byte 3) is impermissible");
break;
case 0x34:
throw DeviceException("Telegram format error");
break;
case 0x36:
throw DeviceException("A command telegram has been received though no reply telegram has been received yet");
break;
default:
throw DeviceException("Unknown error");
break;
}
}
for (::std::size_t i = 0; i < 6; ++i)
{
numbytes = serial.read(ptr, 1);
printf("buf[%zi] %02x\n", i + 4, buf[i + 4]);
ptr += numbytes;
sumbytes += numbytes;
}
uint16_t length = Endian::hostEndianWord(buf[6], buf[7]);
std::cout << "length " << length << std::endl;
if (length != 552)
{
throw DeviceException("Data length mismatch");
}
printf("coordinationFlag %02x\n", buf[8]);
printf("deviceAddress %02x\n", buf[9]);
for (::std::size_t i = 0; i < 1094; ++i)
{
numbytes = serial.read(ptr, 1);
ptr += numbytes;
sumbytes += numbytes;
}
printf("version %02x%02x\n", buf[11], buf[10]);
if (0x01 != buf[11] || 0x02 != buf[10])
{
throw DeviceException("Protocol version mismatch");
}
if (0x00 != buf[13] || 0x00 != buf[12])
{
throw DeviceException("Incorrect status");
}
uint32_t scanNumber = Endian::hostEndianDoubleWord(
Endian::hostEndianWord(buf[17], buf[16]),
Endian::hostEndianWord(buf[15], buf[14])
);
std::cout << "scanNumber " << scanNumber << std::endl;
uint16_t telegramNumber = Endian::hostEndianWord(buf[19], buf[18]);
std::cout << "telegramNumber " << telegramNumber << std::endl;
for (::std::size_t i = 0; i < 2; ++i)
{
numbytes = serial.read(ptr, 1);
ptr += numbytes;
sumbytes += numbytes;
}
for (::std::size_t i = 0; i < 2; ++i)
{
numbytes = serial.read(ptr, 1);
ptr += numbytes;
sumbytes += numbytes;
}
if (this->crc(buf + 4, sumbytes - 2 - 4) != Endian::hostEndianWord(buf[sumbytes - 1], buf[sumbytes - 2]))
{
throw DeviceException("Checksum error");
}
for (::std::size_t i = 0; i < sumbytes; ++i) { printf("%02x ", buf[i]); } printf("\n");
return sumbytes;
}
void
SickLms200::getDistances(::rl::math::Vector& distances) const
{
assert(this->isConnected());
assert(distances.size() >= this->getDistancesCount());
if (this->data[6] & 32)
{
throw DeviceException("partial scan");
}
::rl::math::Real scale;
switch (this->data[6] & 192)
{
case 0:
scale = 0.01f;
break;
case 64:
scale = 0.001f;
break;
default:
throw DeviceException("unknown scale");
break;
}
uint16_t count = Endian::hostEndianWord(this->data[6] & 11, this->data[5]);
uint8_t mask;
switch (this->configuration)
{
case 0x00:
case 0x01:
case 0x02:
mask = 0x1F;
break;
case 0x03:
case 0x04:
case 0x05:
mask = 0x3F;
break;
case 0x06:
mask = 0x7F;
break;
default:
mask = 0x00;
break;
}
uint16_t value;
for (::std::size_t i = 0; i < count; ++i)
{
value = Endian::hostEndianWord(this->data[8 + i * 2] & mask, this->data[7 + i * 2]);
switch (this->configuration)
{
case 0x00:
case 0x01:
case 0x02:
switch (value)
{
case 0x1FFF:
::std::cerr << "Measured value not valid" << ::std::endl;
case 0x1FFE:
::std::cerr << "Dazzling" << ::std::endl;
case 0x1FFD:
::std::cerr << "Operation overflow" << ::std::endl;
case 0x1FFB:
::std::cerr << "Signal-to-noise ratio too small" << ::std::endl;
case 0x1FFA:
::std::cerr << "Error when reading channel 1" << ::std::endl;
distances(i) = ::std::numeric_limits< ::rl::math::Real >::quiet_NaN();
break;
case 0x1FF7:
::std::cerr << "Measured value > Maximum value" << ::std::endl;
distances(i) = ::std::numeric_limits< ::rl::math::Real >::infinity();
break;
default:
distances(i) = value;
distances(i) *= scale;
break;
}
break;
case 0x03:
case 0x04:
case 0x05:
switch (value)
{
case 0x3FFF:
::std::cerr << "Measured value not valid" << ::std::endl;
case 0x3FFE:
::std::cerr << "Dazzling" << ::std::endl;
case 0x3FFD:
::std::cerr << "Operation overflow" << ::std::endl;
case 0x3FFB:
::std::cerr << "Signal-to-noise ratio too small" << ::std::endl;
case 0x3FFA:
::std::cerr << "Error when reading channel 1" << ::std::endl;
distances(i) = ::std::numeric_limits< ::rl::math::Real >::quiet_NaN();
break;
case 0x3FF7:
::std::cerr << "Measured value > Maximum value" << ::std::endl;
distances(i) = ::std::numeric_limits< ::rl::math::Real >::infinity();
break;
default:
distances(i) = value;
distances(i) *= scale;
break;
}
break;
case 0x06:
switch (value)
{
case 0x7FFF:
::std::cerr << "Measured value not valid" << ::std::endl;
case 0x7FFE:
::std::cerr << "Dazzling" << ::std::endl;
case 0x7FFD:
::std::cerr << "Operation overflow" << ::std::endl;
case 0x7FFB:
::std::cerr << "Signal-to-noise ratio too small" << ::std::endl;
case 0x7FFA:
::std::cerr << "Error when reading channel 1" << ::std::endl;
distances(i) = ::std::numeric_limits< ::rl::math::Real >::quiet_NaN();
break;
case 0x7FF7:
::std::cerr << "Measured value > Maximum value" << ::std::endl;
distances(i) = ::std::numeric_limits< ::rl::math::Real >::infinity();
break;
default:
distances(i) = value;
distances(i) *= scale;
break;
}
default:
distances(i) = value;
distances(i) *= scale;
break;
}
}
}
void
SickLms200::open()
{
this->serial.open();
this->setConnected(true);
uint8_t buf[812];
// synchronize baud rates
buf[4] = 0x20;
buf[5] = 0x42;
#if defined(WIN32) || defined(__QNX__)
Serial::BaudRate baudRates[3] = {
#else // defined(WIN32) || defined(__QNX__)
Serial::BaudRate baudRates[4] = {
Serial::BAUDRATE_500000BPS,
#endif // defined(WIN32) || defined(__QNX__)
Serial::BAUDRATE_38400BPS,
Serial::BAUDRATE_19200BPS,
Serial::BAUDRATE_9600BPS
};
#if defined(WIN32) || defined(__QNX__)
for (::std::size_t i = 0; i < 3; ++i)
#else // defined(WIN32) || defined(__QNX__)
for (::std::size_t i = 0; i < 4; ++i)
#endif // defined(WIN32) || defined(__QNX__)
{
this->serial.setBaudRate(baudRates[i]);
this->serial.changeParameters();
this->send(buf, 1 + 1 + 2 + 1 + 1 + 2);
if (this->waitAck())
{
break;
}
#if defined(WIN32) || defined(__QNX__)
if (3 == i)
#else // defined(WIN32) || defined(__QNX__)
if (4 == i)
#endif // defined(WIN32) || defined(__QNX__)
{
throw DeviceException("could not sync baud rate.");
}
}
this->recv(buf, 1 + 1 + 2 + 1 + 1 + 1 + 2, 0xA0);
switch (buf[5])
{
case 0x01:
throw DeviceException("mode switchover not possible due to incorrect password");
break;
case 0x02:
throw DeviceException("mode switchover not possible due to a fault in the LMS2xx");
break;
default:
break;
}
// status
buf[4] = 0x31;
do
{
this->send(buf, 1 + 1 + 2 + 1 + 2);
}
while (!this->waitAck());
this->recv(buf, 1 + 1 + 2 + 1 + 152 + 1 + 2, 0xB1);
// baud rate
switch (buf[120])
{
#if !(defined(WIN32) || defined(__QNX__))
case 0x01:
this->baudRate = BAUDRATE_500000BPS;
break;
#endif // !(defined(WIN32) || defined(__QNX__))
case 0x19:
this->baudRate = BAUDRATE_38400BPS;
break;
case 0x33:
this->baudRate = BAUDRATE_19200BPS;
break;
case 0x67:
this->baudRate = BAUDRATE_9600BPS;
break;
default:
break;
}
if (this->desired != this->baudRate)
{
this->setBaudRate(this->desired);
}
// monitoring
if (0x25 != buf[12])
{
this->setMonitoring(MONITORING_SINGLE);
}
// measuring
this->setMeasuring(this->measuring);
// variant
uint16_t angle = Endian::hostEndianWord(buf[112], buf[111]);
uint16_t resolution = Endian::hostEndianWord(buf[114], buf[113]);
Variant variant;
switch (resolution)
{
case 25:
variant = VARIANT_100_25;
break;
case 50:
switch (angle)
{
case 100:
variant = VARIANT_100_50;
break;
case 180:
variant = VARIANT_180_50;
break;
default:
throw DeviceException("unknown variant");
break;
}
break;
case 100:
switch (angle)
{
case 100:
variant = VARIANT_100_100;
break;
case 180:
variant = VARIANT_180_100;
break;
default:
throw DeviceException("unknown variant");
break;
}
break;
default:
throw DeviceException("unknown variant");
break;
}
if (variant != this->variant)
{
this->setVariant(this->variant);
}
}
::std::size_t
SickLms200::recv(uint8_t* buf, const ::std::size_t& len, const uint8_t& command)
{
assert(this->isConnected());
assert(len > 7);
uint8_t* ptr;
::std::size_t sumbytes;
::std::size_t numbytes;
do
{
do
{
ptr = buf;
sumbytes = 0;
do
{
numbytes = this->serial.read(ptr, 1);
}
while (0x02 != buf[0]);
ptr += numbytes;
sumbytes += numbytes;
numbytes = this->serial.read(ptr, 1);
}
while (0x80 != buf[1]);
ptr += numbytes;
sumbytes += numbytes;
for (::std::size_t i = 0; i < 4; ++i)
{
numbytes = this->serial.read(ptr, 1);
ptr += numbytes;
sumbytes += numbytes;
}
if ((0x02 == buf[2]) && (0x00 == buf[3]) && (0x92 == buf[4]))
{
throw DeviceException("not acknowledge, incorrect command");
}
}
while (command != buf[4]);
uint16_t length = Endian::hostEndianWord(buf[3], buf[2]);
if (len != static_cast< ::std::size_t >(length) + 6)
{
throw DeviceException("data length mismatch in command " + command);
}
while (sumbytes < len)
{
numbytes = this->serial.read(ptr, len - sumbytes);
ptr += numbytes;
sumbytes += numbytes;
}
if (this->crc(buf, sumbytes - 2) != Endian::hostEndianWord(buf[sumbytes - 1], buf[sumbytes - 2]))
{
throw DeviceException("checksum error");
}
switch (buf[sumbytes - 3] & 7)
{
case 1:
throw DeviceException("info");
break;
case 2:
throw DeviceException("warning");
break;
case 3:
throw DeviceException("error");
break;
case 4:
throw DeviceException("fatal error");
break;
default:
break;
}
if (buf[sumbytes - 3] & 64)
{
throw DeviceException("implausible measured values");
}
if (buf[sumbytes - 3] & 128)
{
throw DeviceException("pollution");
}
return sumbytes;
}
::std::size_t
SchmersalLss300::recv(::std::uint8_t* buf, const ::std::size_t& len, const ::std::uint8_t& command)
{
assert(this->isConnected());
assert(len > 6);
::std::uint8_t* ptr;
::std::size_t sumbytes;
::std::size_t numbytes;
do
{
do
{
ptr = buf;
sumbytes = 0;
do
{
numbytes = this->serial.read(ptr, 1);
}
while (0x02 != buf[0]);
ptr += numbytes;
sumbytes += numbytes;
numbytes = this->serial.read(ptr, 1);
}
while (0x80 != buf[1]);
ptr += numbytes;
sumbytes += numbytes;
for (::std::size_t i = 0; i < 4; ++i)
{
numbytes = this->serial.read(ptr, 1);
ptr += numbytes;
sumbytes += numbytes;
}
}
while (command != buf[4]);
::std::uint16_t length = Endian::hostWord(buf[3], buf[2]);
if (len != static_cast< ::std::size_t>(length) + 6)
{
throw DeviceException("data length mismatch in command " + ::std::to_string(command));
}
while (sumbytes < len)
{
numbytes = this->serial.read(ptr, len - sumbytes);
ptr += numbytes;
sumbytes += numbytes;
}
if (this->crc(buf, sumbytes - 2) != Endian::hostWord(buf[sumbytes - 1], buf[sumbytes - 2]))
{
throw DeviceException("checksum error");
}
return sumbytes;
}
/*
* Should never get called. All reads and writes are serviced by the cache.
*/
void BeetleInternal::writeResponse(uint8_t *buf, int len) {
throw DeviceException(getName() + " does not make requests");
}
void
SickLms200::setBaudRate(const BaudRate& baudRate)
{
assert(this->isConnected());
uint8_t buf[812];
buf[4] = 0x20;
switch (baudRate)
{
case BAUDRATE_9600BPS:
buf[5] = 0x42;
break;
case BAUDRATE_19200BPS:
buf[5] = 0x41;
break;
case BAUDRATE_38400BPS:
buf[5] = 0x40;
break;
#if !(defined(WIN32) || defined(__QNX__))
case BAUDRATE_500000BPS:
buf[5] = 0x48;
break;
#endif // !(defined(WIN32) || defined(__QNX__))
default:
break;
}
do
{
this->send(buf, 1 + 1 + 2 + 1 + 1 + 2);
}
while (!this->waitAck());
this->recv(buf, 1 + 1 + 2 + 1 + 1 + 1 + 2, 0xA0);
switch (buf[5])
{
case 0x01:
throw DeviceException("mode switchover not possible due to incorrect password");
break;
case 0x02:
throw DeviceException("mode switchover not possible due to a fault in the LMS2xx");
break;
default:
break;
}
switch (baudRate)
{
case BAUDRATE_9600BPS:
this->serial.setBaudRate(Serial::BAUDRATE_9600BPS);
break;
case BAUDRATE_19200BPS:
this->serial.setBaudRate(Serial::BAUDRATE_19200BPS);
break;
case BAUDRATE_38400BPS:
this->serial.setBaudRate(Serial::BAUDRATE_38400BPS);
break;
#if !(defined(WIN32) || defined(__QNX__))
case BAUDRATE_500000BPS:
this->serial.setBaudRate(Serial::BAUDRATE_500000BPS);
break;
#endif // !(defined(WIN32) || defined(__QNX__))
default:
break;
}
this->serial.changeParameters();
this->baudRate = baudRate;
}
/**
* @brief Ft245sync::Ft245sync constructor initializes FTDI comunication first in MPSSE mode to
* make FPGA load design from FLASH and reset design loaded in FPGA. Then switches mode to SYNC FT245
* and sends cleanup command to initialize communication with device.
* @param chunkSizeRead
* @param chunkSizeWrite
* @param gpio
* @param vftdic
* @param vftdic2
*/
Ft245sync::Ft245sync(unsigned int chunkSizeRead,
unsigned int chunkSizeWrite,
uint8_t gpio,
struct ftdi_context * vftdic,
struct ftdi_context * vftdic2)
{
if(vftdic == NULL)
{
this->ftdic = static_cast<struct ftdi_context*>(malloc(sizeof(struct ftdi_context)));
}
else
{
this->ftdic = vftdic;
}
if(vftdic2 == NULL)
{
this->ftdic2 = static_cast<struct ftdi_context*>(malloc(sizeof(struct ftdi_context)));
}
else
{
this->ftdic2 = vftdic2;
}
int f;
// Init 1. channel
if (ftdi_init(ftdic) < 0)
{
throw DeviceException("ftdi_init failure\n", FTDI_ERROR);
}
ftdi_set_interface(ftdic, INTERFACE_A);
f = ftdi_usb_open(ftdic, 0x0403, PID);
if (f < 0 && f != -5)
{
qDebug() << "Error code: " << f;
throw DeviceException("Unable to open FTDI device, channel A\n", FTDI_ERROR);
}
// Init 2. channel
if (ftdi_init(ftdic2) < 0)
{
throw DeviceException("ftdi_init failure\n", FTDI_ERROR);
}
ftdi_usb_reset(ftdic);
ftdi_usb_reset(ftdic2);
ftdi_set_interface(ftdic2, INTERFACE_B);
f = ftdi_usb_open(ftdic2, 0x0403, PID);
if (f < 0 && f != -5)
{
qDebug() << "Error code: " << f;
throw DeviceException("Unable to open FTDI device, channel B\n", FTDI_ERROR);
}
ftdi_write_data_set_chunksize(ftdic2, 512);
ftdi_set_interface(ftdic2, INTERFACE_B);
ftdi_usb_reset(ftdic2);
ftdi_set_latency_timer(ftdic2, 2);
ftdi_setflowctrl(ftdic2, SIO_RTS_CTS_HS);
ftdi_set_bitmode(ftdic2, 0, BBMODE_SPI);
uint8_t buf[3];
buf[0] = SET_BITS_LOW;
buf[1] = 8;
buf[2] = BIT_DIR; //holding programming of FPGA*/
ftdi_write_data(ftdic2, buf, 3);
buf[0] = SET_BITS_HIGH;
buf[1] = 0xFF; //lighting leds
buf[2] = BIT_DIR;
ftdi_write_data(ftdic2, buf, 3);
buf[0] = SET_BITS_HIGH;
buf[1] = 0x00; //lighting leds
buf[2] = BIT_DIR;
ftdi_write_data(ftdic2, buf, 3);
buf[0] = SET_BITS_LOW;
buf[1] = gpio;
buf[2] = BIT_DIR; //releasing programming of FPGA
ftdi_write_data(ftdic2, buf, 3);
usleep(300);
buf[0] = SET_BITS_LOW;
buf[1] = 0xFF; //reseting design in FPGA
buf[2] = BIT_DIR;
ftdi_write_data(ftdic2, buf, 3);
usleep(300);
buf[0] = SET_BITS_LOW;
buf[1] = 0xDD; //releasing reset
buf[2] = BIT_DIR;
ftdi_write_data(ftdic2, buf, 3);
usleep(300);
buf[0] = SET_BITS_HIGH;
buf[1] = 0xFF; //lighting leds
buf[2] = BIT_DIR;
ftdi_write_data(ftdic2, buf, 3);
if (ftdi_usb_purge_buffers(ftdic2))
{
throw DeviceException("Purging buffers failed\n", FTDI_ERROR);
}
ftdi_usb_close(ftdic2); // close channel 2
ftdi_deinit(ftdic2); // close channel 2
ftdic->usb_read_timeout = READ_TIMEOUT;
ftdic->usb_write_timeout = WRITE_TIMEOUT;
ftdi_read_data_set_chunksize(ftdic, chunkSizeRead);
ftdi_write_data_set_chunksize(ftdic, chunkSizeWrite);
if (ftdi_usb_reset(ftdic))
{
throw DeviceException("Reset failed\n", FTDI_ERROR);
}
usleep(1000);
if(ftdi_usb_purge_buffers(ftdic) < 0)
{
throw DeviceException("Setting FT2232 synchronous bitmode failed\n", FTDI_ERROR);
}
if(ftdi_set_bitmode(ftdic, 0xFF, 0x00) < 0)
{
throw DeviceException("Setting FT2232 synchronous bitmode failed\n", FTDI_ERROR);
}
if(ftdi_set_bitmode(ftdic, 0xFF, 0x40) < 0)
{
throw DeviceException("Setting FT2232 synchronous bitmode failed\n", FTDI_ERROR);
}
if (ftdi_set_latency_timer(ftdic, 2)) /* AN_130 */
{
throw DeviceException("Set latency failed failed\n", FTDI_ERROR);
}
//SetUSBParameters(ftHandle,0x10000, 0x10000);
if (ftdi_setflowctrl(ftdic, SIO_RTS_CTS_HS)) // AN_130
{
throw DeviceException("Set RTS_CTS failed\n", FTDI_ERROR);
}
if(ftdi_usb_purge_buffers(ftdic) < 0)
{
throw DeviceException("Setting FT2232 synchronous bitmode failed\n", FTDI_ERROR);
}
//fixes unalignment of first read (should be fixed in cleaner manner)
usleep(800);
unsigned char cleanup[10] = { 0xBB, 0xBB, 0xBB, 0xAA, 0xAA, 0xAA, 0xAA, 0xAA, 0xAA, 0xAA };
ftdi_write_data(ftdic, cleanup, 10);
unsigned char recvbuf[4000];
read(recvbuf);
}
void
SchmersalLss300::open()
{
this->serial.open();
this->setConnected(true);
::std::array< ::std::uint8_t, 1013> buf;
// synchronize baud rates
buf[4] = 0x66;
buf[5] = 0x00;
Serial::BaudRate baudRates[4] = {
Serial::BAUDRATE_57600BPS,
Serial::BAUDRATE_38400BPS,
Serial::BAUDRATE_19200BPS,
Serial::BAUDRATE_9600BPS
};
for (::std::size_t i = 0; i < 4; ++i)
{
this->serial.setBaudRate(baudRates[i]);
this->serial.changeParameters();
this->send(buf.data(), 1 + 1 + 2 + 1 + 1 + 2);
if (this->waitAck())
{
break;
}
if (3 == i)
{
throw DeviceException("could not sync baud rate.");
}
}
this->recv(buf.data(), 1 + 1 + 2 + 1 + 1 + 2, 0xE6);
if (0x81 == buf[5])
{
throw DeviceException("mode switchover not possible due to internal processing error");
}
// status
buf[4] = 0x31;
do
{
this->send(buf.data(), 1 + 1 + 2 + 1 + 2);
}
while (!this->waitAck());
this->recv(buf.data(), 1 + 1 + 2 + 1 + 1 + 1 + 9 + 17 + 17 + 17 + 17 + 1 + 1 + 4 + 1 + 1 + 1 + 1 + 1 + 1 + 1 + 1 + 2 + 4 + 50 + 2, 0xB1);
// baud rate
if (this->desired != this->baudRate)
{
this->setBaudRate(this->desired);
}
// monitoring
if (0x25 != buf[5])
{
this->setMonitoring(MONITORING_SINGLE);
}
}
void
SickLms200::setMeasuring(const Measuring& measuring)
{
assert(this->isConnected());
uint8_t buf[812];
// transmit password
buf[4] = 0x20;
buf[5] = 0x00;
for (::std::size_t i = 0; i < 8; ++i)
{
buf[6 + i] = this->password[i];
}
do
{
this->send(buf, 16);
}
while (!this->waitAck());
this->recv(buf, 1 + 1 + 2 + 1 + 1 + 1 + 2, 0xA0);
if (0x00 != buf[5])
{
throw DeviceException("could not switch mode");
}
// get current configuration
buf[4] = 0x74;
do
{
this->send(buf, 1 + 1 + 2 + 1 + 2);
}
while (!this->waitAck());
this->recv(buf, 1 + 1 + 2 + 1 + 32 + 1 + 2, 0xF4);
this->configuration = buf[10];
// set new configuration
buf[4] = 0x77;
switch (measuring)
{
case MEASURING_8M:
buf[10] = 0x00;
buf[11] = 0x01;
break;
case MEASURING_16M:
buf[10] = 0x04;
buf[11] = 0x01;
break;
case MEASURING_32M:
buf[10] = 0x06;
buf[11] = 0x01;
break;
case MEASURING_80M:
buf[10] = 0x00;
buf[11] = 0x00;
break;
case MEASURING_160M:
buf[10] = 0x04;
buf[11] = 0x00;
break;
case MEASURING_320M:
buf[10] = 0x06;
buf[11] = 0x00;
break;
default:
break;
}
uint8_t configuration = buf[10];
do
{
this->send(buf, 40);
}
while (!this->waitAck());
this->recv(buf, 1 + 1 + 2 + 1 + 33 + 1 + 2, 0xF7);
if (0x00 == buf[5])
{
throw DeviceException("LMS configuration not accepted");
}
this->configuration = configuration;
this->measuring = measuring;
}
void UvcDriver::Start(int vid, int pid, char* sn)
{
width_ = 640;
height_ = 480;
fps_ = 30;
if(ctx_) {
Stop();
}
uvc_init(&ctx_, NULL);
if(!ctx_) {
throw DeviceException("Unable to open UVC Context");
}
uvc_error_t find_err = uvc_find_device(ctx_, &dev_, vid, pid, sn );
if (find_err != UVC_SUCCESS) {
uvc_perror(find_err, "uvc_find_device");
throw DeviceException("Unable to open UVC Device");
}
if(!dev_) {
throw DeviceException("Unable to open UVC Device - no pointer returned.");
}
uvc_error_t open_err = uvc_open(dev_, &devh_);
if (open_err != UVC_SUCCESS) {
uvc_perror(open_err, "uvc_open");
uvc_unref_device(dev_);
throw DeviceException("Unable to open device");
}
// uvc_set_status_callback(devh_, &CameraDriver::AutoControlsCallbackAdapter, this);
uvc_stream_ctrl_t ctrl;
uvc_error_t mode_err = uvc_get_stream_ctrl_format_size(
devh_, &ctrl,
UVC_COLOR_FORMAT_YUYV,
width_, height_,
fps_);
pbtype = hal::PB_UNSIGNED_BYTE;
pbformat = hal::PB_LUMINANCE;
if (mode_err != UVC_SUCCESS) {
uvc_perror(mode_err, "uvc_get_stream_ctrl_format_size");
uvc_close(devh_);
uvc_unref_device(dev_);
throw DeviceException("Unable to device mode.");
}
// uvc_error_t stream_err = uvc_start_iso_streaming(devh_, &ctrl, &UvcDriver::ImageCallbackAdapter, this);
//uvc_error_t stream_err = uvc_start_iso_streaming(devh_, &ctrl, NULL, this);
uvc_error_t stream_err = uvc_start_streaming(devh_, &ctrl, NULL, this, 0);
if (stream_err != UVC_SUCCESS) {
uvc_perror(stream_err, "uvc_start_iso_streaming");
uvc_close(devh_);
uvc_unref_device(dev_);
throw DeviceException("Unable to start iso streaming.");
}
if (frame_)
uvc_free_frame(frame_);
frame_ = uvc_allocate_frame(ctrl.dwMaxVideoFrameSize);
if(!frame_) {
throw DeviceException("Unable to allocate frame.");
}
}
const char* getCommandLineOption(unsigned int index)
{
if ((index+1)<argCount) return argValues[index+1];
throw DeviceException("index out of bounds",__FILE__,__LINE__);
}
void
SchmersalLss300::setBaudRate(const BaudRate& baudRate)
{
assert(this->isConnected());
::std::array< ::std::uint8_t, 1013> buf;
buf[4] = 0x66;
switch (baudRate)
{
case BAUDRATE_9600BPS:
buf[5] = 0x00;
break;
case BAUDRATE_19200BPS:
buf[5] = 0x01;
break;
case BAUDRATE_38400BPS:
buf[5] = 0x02;
break;
case BAUDRATE_57600BPS:
buf[5] = 0x03;
break;
default:
break;
}
do
{
this->send(buf.data(), 1 + 1 + 2 + 1 + 1 + 2);
}
while (!this->waitAck());
this->recv(buf.data(), 1 + 1 + 2 + 1 + 1 + 2, 0xE6);
if (0x81 == buf[5])
{
throw DeviceException("mode switchover not possible due to internal processing error");
}
switch (baudRate)
{
case BAUDRATE_9600BPS:
this->serial.setBaudRate(Serial::BAUDRATE_9600BPS);
break;
case BAUDRATE_19200BPS:
this->serial.setBaudRate(Serial::BAUDRATE_19200BPS);
break;
case BAUDRATE_38400BPS:
this->serial.setBaudRate(Serial::BAUDRATE_38400BPS);
break;
case BAUDRATE_57600BPS:
this->serial.setBaudRate(Serial::BAUDRATE_57600BPS);
break;
default:
break;
}
this->serial.changeParameters();
this->baudRate = baudRate;
}
void DeviceException::launch(const std::string &reason) {
throw DeviceException(reason);
}
const char* getCommandLineProgram()
{
if (argCount>0) return argValues[0];
throw DeviceException("no parameter saved",__FILE__,__LINE__);
}
