Legacy/details/channel.cc

Go to the documentation of this file.

 
#include "MultiSense/details/channel.hh"
#include "MultiSense/details/query.hh"
 
#include <wire/DisparityMessage.hh>
#include <wire/SysMtuMessage.hh>
#include <wire/SysGetMtuMessage.hh>
#include <wire/StatusRequestMessage.hh>
#include <wire/StatusResponseMessage.hh>
#include <wire/PtpStatusRequestMessage.hh>
#include <wire/PtpStatusResponseMessage.hh>
#include <wire/VersionRequestMessage.hh>
#include <wire/SysDeviceInfoMessage.hh>
 
#include "MultiSense/details/utility/Functional.hh"
 
#ifdef WIN32
#include <ws2tcpip.h>
#else
#include <netdb.h>
#endif
#include <errno.h>
#include <fcntl.h>
 
namespace crl {
namespace multisense {
namespace details {
 
//
// Implementation constructor
 
impl::impl(const std::string& address, const RemoteHeadChannel& cameraId, const std::string& ifName) :
    m_serverSocket(INVALID_SOCKET),
    m_serverSocketPort(0),
    m_sensorAddress(),
    m_sensorMtu(MAX_MTU_SIZE),
    m_incomingBuffer(MAX_MTU_SIZE),
    m_txSeqId(0),
    m_lastRxSeqId(-1),
    m_unWrappedRxSeqId(0),
    m_lastUnexpectedSequenceId(-1),
    m_udpTrackerCache(UDP_TRACKER_CACHE_DEPTH),
    m_rxLargeBufferPool(),
    m_rxSmallBufferPool(),
    m_imageMetaCache(IMAGE_META_CACHE_DEPTH),
    m_secondaryAppMetaCache(SECONDARY_APP_META_CACHE_DEPTH),
    m_udpAssemblerMap(),
    m_dispatchLock(),
    m_streamLock(),
    m_threadsRunning(false),
    m_rxThreadP(NULL),
    m_rxLock(),
    m_statusThreadP(NULL),
    m_imageListeners(),
    m_lidarListeners(),
    m_ppsListeners(),
    m_imuListeners(),
    m_compressedImageListeners(),
    m_secondaryAppListeners(),
    m_watch(),
    m_messages(),
    m_streamsEnabled(0),
    m_timeLock(),
    m_timeOffsetInit(false),
    m_timeOffset(0),
    m_networkTimeSyncEnabled(true),
    m_ptpTimeSyncEnabled(false),
    m_sensorVersion()
{
 
#if WIN32
    WSADATA wsaData;
    int result = WSAStartup (MAKEWORD (0x02, 0x02), &wsaData);
    if (result != 0)
        CRL_EXCEPTION("WSAStartup() failed: %d", result);
 
#endif
 
    struct addrinfo hints, *res;
    memset(&hints, 0, sizeof(hints));
    hints.ai_family = AF_INET;
    hints.ai_socktype = 0;
 
    const int addrstatus = getaddrinfo(address.c_str(), NULL, &hints, &res);
    if (addrstatus != 0 || res == NULL)
        CRL_EXCEPTION("unable to resolve \"%s\": %s", address.c_str(), strerror(errno));
 
    in_addr addr;
    memcpy(&addr, &(((struct sockaddr_in *)(res->ai_addr))->sin_addr), sizeof(in_addr));
 
    memset(&m_sensorAddress, 0, sizeof(m_sensorAddress));
 
    m_sensorAddress.sin_family = AF_INET;
    m_sensorAddress.sin_port   = htons(DEFAULT_SENSOR_TX_PORT + static_cast<uint16_t>(cameraId + 1));
    m_sensorAddress.sin_addr   = addr;
 
    freeaddrinfo(res);
 
    //
    // Create a pool of RX buffers
 
    uint32_t largeBufferRetry = 0;
    for(uint32_t i=0; i<RX_POOL_LARGE_BUFFER_COUNT;)
    {
        try {
            m_rxLargeBufferPool.push_back(new utility::BufferStreamWriter(RX_POOL_LARGE_BUFFER_SIZE));
            i++;
            largeBufferRetry = 0;
        }
        catch (const std::exception &e) {
            CRL_DEBUG("Failed to allocate memory (will sleep and try again): %s", e.what());
            usleep(static_cast<unsigned int> (10000));
            largeBufferRetry++;
 
            if (largeBufferRetry >= MAX_BUFFER_ALLOCATION_RETRIES)
                throw e;
        }
    }
 
    uint32_t smallBufferRetry = 0;
    for(uint32_t i=0; i<RX_POOL_SMALL_BUFFER_COUNT;)
    {
        try {
            m_rxSmallBufferPool.push_back(new utility::BufferStreamWriter(RX_POOL_SMALL_BUFFER_SIZE));
            i++;
            smallBufferRetry = 0;
        }
        catch (const std::exception &e) {
            CRL_DEBUG("Failed to allocate memory (will sleep and try again): %s", e.what());
            usleep(static_cast<unsigned int> (10000));
            smallBufferRetry++;
 
            if (smallBufferRetry >= MAX_BUFFER_ALLOCATION_RETRIES)
                throw e;
        }
    }
 
    //
    // Bind to the port
 
    try {
        bind(ifName);
    } catch (const std::exception& e) {
        CRL_DEBUG("exception: %s\n", e.what());
        cleanup();
        throw e;
    }
 
    //
    // Register any special UDP reassemblers
 
    m_udpAssemblerMap[MSG_ID(wire::Disparity::ID)] = wire::Disparity::assembler;
 
    //
    // Create UDP reception thread
 
    m_threadsRunning = true;
    m_rxThreadP      = new utility::Thread(rxThread, this);
 
    //
    // Request the current operating MTU of the device
 
    wire::SysMtu mtu;
 
    Status status = waitData(wire::SysGetMtu(), mtu);
    if (Status_Ok != status) {
        cleanup();
        CRL_EXCEPTION("failed to establish comms with the sensor at \"%s\", with remote head enum %d",
                      address.c_str(), cameraId);
    } else {
 
        //
        // Use the same MTU for TX
 
        m_sensorMtu = mtu.mtu;
    }
 
    //
    // Request version info from the device
 
    status = waitData(wire::VersionRequest(), m_sensorVersion);
    if (Status_Ok != status) {
        cleanup();
        CRL_EXCEPTION("failed to request version info from sensor at \"%s\"",
                      address.c_str());
    }
 
    //
    // Create status thread
 
    m_statusThreadP = new utility::Thread(statusThread, this);
}
 
//
// Implementation cleanup
 
void impl::cleanup()
{
    m_threadsRunning = false;
 
    if (m_rxThreadP)
        delete m_rxThreadP;
    if (m_statusThreadP)
        delete m_statusThreadP;
 
    std::list<ImageListener*>::const_iterator iti;
    for(iti  = m_imageListeners.begin();
        iti != m_imageListeners.end();
        ++ iti)
        delete *iti;
    std::list<LidarListener*>::const_iterator itl;
    for(itl  = m_lidarListeners.begin();
        itl != m_lidarListeners.end();
        ++ itl)
        delete *itl;
    std::list<PpsListener*>::const_iterator itp;
    for(itp  = m_ppsListeners.begin();
        itp != m_ppsListeners.end();
        ++ itp)
        delete *itp;
    std::list<ImuListener*>::const_iterator itm;
    for(itm  = m_imuListeners.begin();
        itm != m_imuListeners.end();
        ++ itm)
        delete *itm;
    std::list<CompressedImageListener*>::const_iterator itc;
    for(itc  = m_compressedImageListeners.begin();
        itc != m_compressedImageListeners.end();
        ++ itc)
        delete *itc;
    std::list<SecondaryAppListener*>::const_iterator its;
    for(its  = m_secondaryAppListeners.begin();
        its != m_secondaryAppListeners.end();
        its ++)
        delete *its;
    BufferPool::const_iterator it;
    for(it  = m_rxLargeBufferPool.begin();
        it != m_rxLargeBufferPool.end();
        ++it)
        delete *it;
    for(it  = m_rxSmallBufferPool.begin();
        it != m_rxSmallBufferPool.end();
        ++it)
        delete *it;
 
    m_imageListeners.clear();
    m_lidarListeners.clear();
    m_ppsListeners.clear();
    m_imuListeners.clear();
    m_secondaryAppListeners.clear();
    m_compressedImageListeners.clear();
    m_rxLargeBufferPool.clear();
    m_rxSmallBufferPool.clear();
 
    if (m_serverSocket > 0)
        closesocket(m_serverSocket);
 
#if WIN32
    WSACleanup ();
#endif
}
 
//
// Implementation destructor
 
impl::~impl()
{
    cleanup();
}
 
//
// Binds the communications channel, preparing it to send/receive data
// over the network.
 
void impl::bind(const std::string& ifName)
{
    //
    // Create the socket.
 
    m_serverSocket = socket(AF_INET, SOCK_DGRAM, IPPROTO_UDP);
#if defined(__MINGW64__)
    if (m_serverSocket == INVALID_SOCKET)
#else
    if (m_serverSocket < 0)
#endif
        CRL_EXCEPTION("failed to create the UDP socket: %s",
                      strerror(errno));
 
    #if __linux__
        //
        // Bind to specific interface if specified
        if (!ifName.empty()){
            if (0 != setsockopt(m_serverSocket, SOL_SOCKET, SO_BINDTODEVICE,  ifName.c_str(), ifName.size())){
                CRL_EXCEPTION("Failed to bind to device %s. Error: %s", ifName.c_str(),
                              strerror(errno));
            }
        }
    #else
        if (!ifName.empty())
            CRL_DEBUG("User specified binding to adapter %s, but this feature is only supported under linux. Ignoring bind to specific adapter", ifName.c_str());
    #endif
 
    //
    // Turn non-blocking on.
#if WIN32
    u_long ioctl_arg = 1;
    if (0 != ioctlsocket(m_serverSocket, FIONBIO, &ioctl_arg))
        CRL_EXCEPTION("failed to make a socket non-blocking: %d",WSAGetLastError ());
#else
    const int flags = fcntl(m_serverSocket, F_GETFL, 0);
 
    if (0 != fcntl(m_serverSocket, F_SETFL, flags | O_NONBLOCK))
        CRL_EXCEPTION("failed to make a socket non-blocking: %s",
                      strerror(errno));
#endif
 
    //
    // Allow reusing sockets.
 
    int reuseSocket = 1;
 
    if (0 != setsockopt(m_serverSocket, SOL_SOCKET, SO_REUSEADDR, (char*) &reuseSocket,
                        sizeof(reuseSocket)))
        CRL_EXCEPTION("failed to turn on socket reuse flag: %s",
                      strerror(errno));
 
    //
    // We want very large buffers to store several images
 
#if __APPLE__
    // MacOS cannot reliably allocate a buffer larger than this
    int bufferSize = 4 * 1024 * 1024;
#else
    int bufferSize = 48 * 1024 * 1024;
#endif
 
    if (0 != setsockopt(m_serverSocket, SOL_SOCKET, SO_RCVBUF, (char*) &bufferSize,
                        sizeof(bufferSize)) ||
        0 != setsockopt(m_serverSocket, SOL_SOCKET, SO_SNDBUF, (char*) &bufferSize,
                        sizeof(bufferSize)))
        CRL_EXCEPTION("failed to adjust socket buffer sizes (%d bytes): %s",
                      bufferSize, strerror(errno));
 
    //
    // Bind the connection to the port.
 
    struct sockaddr_in address;
 
    address.sin_family      = AF_INET;
    address.sin_port        = htons(0); // system assigned
    address.sin_addr.s_addr = htonl(INADDR_ANY);
 
    if (0 != ::bind(m_serverSocket, (struct sockaddr*) &address, sizeof(address)))
        CRL_EXCEPTION("failed to bind the server socket to system-assigned port: %s",
                      strerror(errno));
 
    //
    // Retrieve the system assigned local UDP port
#if WIN32
    int len = sizeof(address);
#else
    socklen_t len = sizeof(address);
#endif
    if (0 != getsockname(m_serverSocket, (struct sockaddr*) &address, &len))
        CRL_EXCEPTION("getsockname() failed: %s", strerror(errno));
    m_serverSocketPort = htons(address.sin_port);
}
 
//
// Publish a stream to the sensor
 
void impl::publish(const utility::BufferStreamWriter& stream)
{
    //
    // Install the header
 
    wire::Header& header = *(reinterpret_cast<wire::Header*>(stream.data()));
 
    header.magic              = wire::HEADER_MAGIC;
    header.version            = wire::HEADER_VERSION;
    header.group              = wire::HEADER_GROUP;
    header.flags              = 0;
#if WIN32
    // TBD: This returns the post-incremented value
    header.sequenceIdentifier = InterlockedIncrement16((short*)&m_txSeqId);
#else
    // TBD: This returns the pre-incremented value
    header.sequenceIdentifier = __sync_fetch_and_add(&m_txSeqId, 1);
#endif
    header.messageLength      = static_cast<uint32_t> (stream.tell() - sizeof(wire::Header));
    header.byteOffset         = 0;
 
    //
    // Send the packet along
 
// disable MSVC warning for narrowing conversion.
#if defined(WIN32) && !defined(__MINGW64__)
#pragma warning (push)
#pragma warning (disable : 4267)
#endif
    const int32_t ret = sendto(m_serverSocket, (char*)stream.data(), stream.tell(), 0,
                               (struct sockaddr *) &m_sensorAddress,
                               sizeof(m_sensorAddress));
#if defined(WIN32) && !defined(__MINGW64__)
#pragma warning (pop)
#endif
 
    if (static_cast<size_t>(ret) != stream.tell())
        CRL_EXCEPTION("error sending data to sensor, %d/%d bytes written: %s",
                      ret, stream.tell(), strerror(errno));
}
 
//
// Convert data source types from wire<->API. These match 1:1 right now, but we
// want the freedom to change the wire protocol as we see fit.
 
wire::SourceType impl::sourceApiToWire(DataSource mask)
{
    wire::SourceType wire_mask = 0;
 
    if (mask & Source_Raw_Left)                         wire_mask |= wire::SOURCE_RAW_LEFT;
    if (mask & Source_Raw_Right)                        wire_mask |= wire::SOURCE_RAW_RIGHT;
    if (mask & Source_Raw_Aux)                          wire_mask |= wire::SOURCE_RAW_AUX;
    if (mask & Source_Luma_Left)                        wire_mask |= wire::SOURCE_LUMA_LEFT;
    if (mask & Source_Luma_Right)                       wire_mask |= wire::SOURCE_LUMA_RIGHT;
    if (mask & Source_Luma_Aux)                         wire_mask |= wire::SOURCE_LUMA_AUX;
    if (mask & Source_Luma_Rectified_Left)              wire_mask |= wire::SOURCE_LUMA_RECT_LEFT;
    if (mask & Source_Luma_Rectified_Right)             wire_mask |= wire::SOURCE_LUMA_RECT_RIGHT;
    if (mask & Source_Luma_Rectified_Aux)               wire_mask |= wire::SOURCE_LUMA_RECT_AUX;
    if (mask & Source_Chroma_Left)                      wire_mask |= wire::SOURCE_CHROMA_LEFT;
    if (mask & Source_Chroma_Right)                     wire_mask |= wire::SOURCE_CHROMA_RIGHT;
    if (mask & Source_Chroma_Rectified_Aux)             wire_mask |= wire::SOURCE_CHROMA_RECT_AUX;
    if (mask & Source_Chroma_Aux)                       wire_mask |= wire::SOURCE_CHROMA_AUX;
    if (mask & Source_Disparity)                        wire_mask |= wire::SOURCE_DISPARITY;
    if (mask & Source_Disparity_Right)                  wire_mask |= wire::SOURCE_DISPARITY_RIGHT;
    if (mask & Source_Disparity_Aux)                    wire_mask |= wire::SOURCE_DISPARITY_AUX;
    if (mask & Source_Disparity_Cost)                   wire_mask |= wire::SOURCE_DISPARITY_COST;
    if (mask & Source_Jpeg_Left)                        wire_mask |= wire::SOURCE_JPEG_LEFT;
    if (mask & Source_Rgb_Left)                         wire_mask |= wire::SOURCE_RGB_LEFT;
    if (mask & Source_Lidar_Scan)                       wire_mask |= wire::SOURCE_LIDAR_SCAN;
    if (mask & Source_Imu)                              wire_mask |= wire::SOURCE_IMU;
    if (mask & Source_Pps)                              wire_mask |= wire::SOURCE_PPS;
    if (mask & Source_Compressed_Left)                  wire_mask |= wire::SOURCE_COMPRESSED_LEFT;
    if (mask & Source_Compressed_Rectified_Left)        wire_mask |= wire::SOURCE_COMPRESSED_RECTIFIED_LEFT;
    if (mask & Source_Compressed_Right)                 wire_mask |= wire::SOURCE_COMPRESSED_RIGHT;
    if (mask & Source_Compressed_Rectified_Right)       wire_mask |= wire::SOURCE_COMPRESSED_RECTIFIED_RIGHT;
    if (mask & Source_Compressed_Aux)                   wire_mask |= wire::SOURCE_COMPRESSED_AUX;
    if (mask & Source_Compressed_Rectified_Aux)         wire_mask |= wire::SOURCE_COMPRESSED_RECTIFIED_AUX;
    if (mask & Source_Ground_Surface_Spline_Data)       wire_mask |= wire::SOURCE_GROUND_SURFACE_SPLINE_DATA;
    if (mask & Source_Ground_Surface_Class_Image)       wire_mask |= wire::SOURCE_GROUND_SURFACE_CLASS_IMAGE;
    if (mask & Source_AprilTag_Detections)              wire_mask |= wire::SOURCE_APRILTAG_DETECTIONS;
    if (mask & Source_Secondary_App_Data_0)             wire_mask |= wire::SOURCE_SECONDARY_APP_DATA_0;
    if (mask & Source_Secondary_App_Data_1)             wire_mask |= wire::SOURCE_SECONDARY_APP_DATA_1;
    if (mask & Source_Secondary_App_Data_2)             wire_mask |= wire::SOURCE_SECONDARY_APP_DATA_2;
    if (mask & Source_Secondary_App_Data_3)             wire_mask |= wire::SOURCE_SECONDARY_APP_DATA_3;
    if (mask & Source_Secondary_App_Data_4)             wire_mask |= wire::SOURCE_SECONDARY_APP_DATA_4;
    if (mask & Source_Secondary_App_Data_5)             wire_mask |= wire::SOURCE_SECONDARY_APP_DATA_5;
 
    return wire_mask;
}
 
DataSource impl::sourceWireToApi(wire::SourceType mask)
{
    DataSource api_mask = 0;
 
    if (mask & wire::SOURCE_RAW_LEFT)                       api_mask |= Source_Raw_Left;
    if (mask & wire::SOURCE_RAW_RIGHT)                      api_mask |= Source_Raw_Right;
    if (mask & wire::SOURCE_RAW_AUX)                        api_mask |= Source_Raw_Aux;
    if (mask & wire::SOURCE_LUMA_LEFT)                      api_mask |= Source_Luma_Left;
    if (mask & wire::SOURCE_LUMA_RIGHT)                     api_mask |= Source_Luma_Right;
    if (mask & wire::SOURCE_LUMA_AUX)                       api_mask |= Source_Luma_Aux;
    if (mask & wire::SOURCE_LUMA_RECT_LEFT)                 api_mask |= Source_Luma_Rectified_Left;
    if (mask & wire::SOURCE_LUMA_RECT_RIGHT)                api_mask |= Source_Luma_Rectified_Right;
    if (mask & wire::SOURCE_LUMA_RECT_AUX)                  api_mask |= Source_Luma_Rectified_Aux;
    if (mask & wire::SOURCE_CHROMA_LEFT)                    api_mask |= Source_Chroma_Left;
    if (mask & wire::SOURCE_CHROMA_RIGHT)                   api_mask |= Source_Chroma_Right;
    if (mask & wire::SOURCE_CHROMA_AUX)                     api_mask |= Source_Chroma_Aux;
    if (mask & wire::SOURCE_CHROMA_RECT_AUX)                api_mask |= Source_Chroma_Rectified_Aux;
    if (mask & wire::SOURCE_DISPARITY)                      api_mask |= Source_Disparity;
    if (mask & wire::SOURCE_DISPARITY_RIGHT)                api_mask |= Source_Disparity_Right;
    if (mask & wire::SOURCE_DISPARITY_AUX)                  api_mask |= Source_Disparity_Aux;
    if (mask & wire::SOURCE_DISPARITY_COST)                 api_mask |= Source_Disparity_Cost;
    if (mask & wire::SOURCE_JPEG_LEFT)                      api_mask |= Source_Jpeg_Left;
    if (mask & wire::SOURCE_RGB_LEFT)                       api_mask |= Source_Rgb_Left;
    if (mask & wire::SOURCE_LIDAR_SCAN)                     api_mask |= Source_Lidar_Scan;
    if (mask & wire::SOURCE_IMU)                            api_mask |= Source_Imu;
    if (mask & wire::SOURCE_PPS)                            api_mask |= Source_Pps;
    if (mask & wire::SOURCE_GROUND_SURFACE_SPLINE_DATA)     api_mask |= Source_Ground_Surface_Spline_Data;
    if (mask & wire::SOURCE_GROUND_SURFACE_CLASS_IMAGE)     api_mask |= Source_Ground_Surface_Class_Image;
    if (mask & wire::SOURCE_APRILTAG_DETECTIONS)            api_mask |= Source_AprilTag_Detections;
    if (mask & wire::SOURCE_COMPRESSED_LEFT)                api_mask |= Source_Compressed_Left;
    if (mask & wire::SOURCE_COMPRESSED_RECTIFIED_LEFT)      api_mask |= Source_Compressed_Rectified_Left;
    if (mask & wire::SOURCE_COMPRESSED_RIGHT)               api_mask |= Source_Compressed_Right;
    if (mask & wire::SOURCE_COMPRESSED_RECTIFIED_RIGHT)     api_mask |= Source_Compressed_Rectified_Right;
    if (mask & wire::SOURCE_COMPRESSED_AUX)                 api_mask |= Source_Compressed_Aux;
    if (mask & wire::SOURCE_COMPRESSED_RECTIFIED_AUX)       api_mask |= Source_Compressed_Rectified_Aux;
    if (mask & wire::SOURCE_SECONDARY_APP_DATA_0)           api_mask |= Source_Secondary_App_Data_0;
    if (mask & wire::SOURCE_SECONDARY_APP_DATA_1)           api_mask |= Source_Secondary_App_Data_1;
    if (mask & wire::SOURCE_SECONDARY_APP_DATA_2)           api_mask |= Source_Secondary_App_Data_2;
    if (mask & wire::SOURCE_SECONDARY_APP_DATA_3)           api_mask |= Source_Secondary_App_Data_3;
    if (mask & wire::SOURCE_SECONDARY_APP_DATA_4)           api_mask |= Source_Secondary_App_Data_4;
    if (mask & wire::SOURCE_SECONDARY_APP_DATA_5)           api_mask |= Source_Secondary_App_Data_5;
 
    return api_mask;
}
 
uint32_t impl::hardwareApiToWire(uint32_t a)
{
    switch(a) {
    case system::DeviceInfo::HARDWARE_REV_MULTISENSE_SL:                  return wire::SysDeviceInfo::HARDWARE_REV_MULTISENSE_SL;
    case system::DeviceInfo::HARDWARE_REV_MULTISENSE_S7:                  return wire::SysDeviceInfo::HARDWARE_REV_MULTISENSE_S7;
    case system::DeviceInfo::HARDWARE_REV_MULTISENSE_M:                   return wire::SysDeviceInfo::HARDWARE_REV_MULTISENSE_M;
    case system::DeviceInfo::HARDWARE_REV_MULTISENSE_S7S:                 return wire::SysDeviceInfo::HARDWARE_REV_MULTISENSE_S7S;
    case system::DeviceInfo::HARDWARE_REV_MULTISENSE_S21:                 return wire::SysDeviceInfo::HARDWARE_REV_MULTISENSE_S21;
    case system::DeviceInfo::HARDWARE_REV_MULTISENSE_ST21:                return wire::SysDeviceInfo::HARDWARE_REV_MULTISENSE_ST21;
    case system::DeviceInfo::HARDWARE_REV_MULTISENSE_C6S2_S27:            return wire::SysDeviceInfo::HARDWARE_REV_MULTISENSE_C6S2_S27;
    case system::DeviceInfo::HARDWARE_REV_MULTISENSE_S30:                 return wire::SysDeviceInfo::HARDWARE_REV_MULTISENSE_S30;
    case system::DeviceInfo::HARDWARE_REV_MULTISENSE_S7AR:                return wire::SysDeviceInfo::HARDWARE_REV_MULTISENSE_S7AR;
    case system::DeviceInfo::HARDWARE_REV_MULTISENSE_KS21:                return wire::SysDeviceInfo::HARDWARE_REV_MULTISENSE_KS21;
    case system::DeviceInfo::HARDWARE_REV_MULTISENSE_MONOCAM:             return wire::SysDeviceInfo::HARDWARE_REV_MULTISENSE_MONOCAM;
    case system::DeviceInfo::HARDWARE_REV_MULTISENSE_REMOTE_HEAD_VPB:     return wire::SysDeviceInfo::HARDWARE_REV_MULTISENSE_REMOTE_HEAD_VPB;
    case system::DeviceInfo::HARDWARE_REV_MULTISENSE_REMOTE_HEAD_STEREO:  return wire::SysDeviceInfo::HARDWARE_REV_MULTISENSE_REMOTE_HEAD_STEREO;
    case system::DeviceInfo::HARDWARE_REV_MULTISENSE_REMOTE_HEAD_MONOCAM: return wire::SysDeviceInfo::HARDWARE_REV_MULTISENSE_REMOTE_HEAD_MONOCAM;
    case system::DeviceInfo::HARDWARE_REV_BCAM:                           return wire::SysDeviceInfo::HARDWARE_REV_BCAM;
    case system::DeviceInfo::HARDWARE_REV_MONO:                           return wire::SysDeviceInfo::HARDWARE_REV_MONO;
    case system::DeviceInfo::HARDWARE_REV_MULTISENSE_KS21_SILVER:         return wire::SysDeviceInfo::HARDWARE_REV_MULTISENSE_KS21_SILVER;
    case system::DeviceInfo::HARDWARE_REV_MULTISENSE_ST25:                return wire::SysDeviceInfo::HARDWARE_REV_MULTISENSE_ST25;
    case system::DeviceInfo::HARDWARE_REV_MULTISENSE_KS21i:               return wire::SysDeviceInfo::HARDWARE_REV_MULTISENSE_KS21i;
    default:
        CRL_DEBUG("unknown API hardware type \"%d\"\n", a);
        return a; // pass through
    }
}
uint32_t impl::hardwareWireToApi(uint32_t w)
{
    switch(w) {
    case wire::SysDeviceInfo::HARDWARE_REV_MULTISENSE_SL:                  return system::DeviceInfo::HARDWARE_REV_MULTISENSE_SL;
    case wire::SysDeviceInfo::HARDWARE_REV_MULTISENSE_S7:                  return system::DeviceInfo::HARDWARE_REV_MULTISENSE_S7;
    case wire::SysDeviceInfo::HARDWARE_REV_MULTISENSE_M:                   return system::DeviceInfo::HARDWARE_REV_MULTISENSE_M;
    case wire::SysDeviceInfo::HARDWARE_REV_MULTISENSE_S7S:                 return system::DeviceInfo::HARDWARE_REV_MULTISENSE_S7S;
    case wire::SysDeviceInfo::HARDWARE_REV_MULTISENSE_S21:                 return system::DeviceInfo::HARDWARE_REV_MULTISENSE_S21;
    case wire::SysDeviceInfo::HARDWARE_REV_MULTISENSE_ST21:                return system::DeviceInfo::HARDWARE_REV_MULTISENSE_ST21;
    case wire::SysDeviceInfo::HARDWARE_REV_MULTISENSE_C6S2_S27:            return system::DeviceInfo::HARDWARE_REV_MULTISENSE_C6S2_S27;
    case wire::SysDeviceInfo::HARDWARE_REV_MULTISENSE_S30:                 return system::DeviceInfo::HARDWARE_REV_MULTISENSE_S30;
    case wire::SysDeviceInfo::HARDWARE_REV_MULTISENSE_S7AR:                return system::DeviceInfo::HARDWARE_REV_MULTISENSE_S7AR;
    case wire::SysDeviceInfo::HARDWARE_REV_MULTISENSE_KS21:                return system::DeviceInfo::HARDWARE_REV_MULTISENSE_KS21;
    case wire::SysDeviceInfo::HARDWARE_REV_MULTISENSE_MONOCAM:             return system::DeviceInfo::HARDWARE_REV_MULTISENSE_MONOCAM;
    case wire::SysDeviceInfo::HARDWARE_REV_MULTISENSE_REMOTE_HEAD_VPB:     return system::DeviceInfo::HARDWARE_REV_MULTISENSE_REMOTE_HEAD_VPB;
    case wire::SysDeviceInfo::HARDWARE_REV_MULTISENSE_REMOTE_HEAD_STEREO:  return system::DeviceInfo::HARDWARE_REV_MULTISENSE_REMOTE_HEAD_STEREO;
    case wire::SysDeviceInfo::HARDWARE_REV_MULTISENSE_REMOTE_HEAD_MONOCAM: return system::DeviceInfo::HARDWARE_REV_MULTISENSE_REMOTE_HEAD_MONOCAM;
    case wire::SysDeviceInfo::HARDWARE_REV_BCAM:                           return system::DeviceInfo::HARDWARE_REV_BCAM;
    case wire::SysDeviceInfo::HARDWARE_REV_MONO:                           return system::DeviceInfo::HARDWARE_REV_MONO;
    case wire::SysDeviceInfo::HARDWARE_REV_MULTISENSE_KS21_SILVER:         return system::DeviceInfo::HARDWARE_REV_MULTISENSE_KS21_SILVER;
    case wire::SysDeviceInfo::HARDWARE_REV_MULTISENSE_ST25:                return system::DeviceInfo::HARDWARE_REV_MULTISENSE_ST25;
    case wire::SysDeviceInfo::HARDWARE_REV_MULTISENSE_KS21i:               return system::DeviceInfo::HARDWARE_REV_MULTISENSE_KS21i;
    default:
        CRL_DEBUG("unknown WIRE hardware type \"%d\"\n", w);
        return w; // pass through
    }
}
uint32_t impl::imagerApiToWire(uint32_t a)
{
    switch(a) {
    case system::DeviceInfo::IMAGER_TYPE_CMV2000_GREY:  return wire::SysDeviceInfo::IMAGER_TYPE_CMV2000_GREY;
    case system::DeviceInfo::IMAGER_TYPE_CMV2000_COLOR: return wire::SysDeviceInfo::IMAGER_TYPE_CMV2000_COLOR;
    case system::DeviceInfo::IMAGER_TYPE_CMV4000_GREY:  return wire::SysDeviceInfo::IMAGER_TYPE_CMV4000_GREY;
    case system::DeviceInfo::IMAGER_TYPE_CMV4000_COLOR: return wire::SysDeviceInfo::IMAGER_TYPE_CMV4000_COLOR;
    case system::DeviceInfo::IMAGER_TYPE_IMX104_COLOR:  return wire::SysDeviceInfo::IMAGER_TYPE_IMX104_COLOR;
    case system::DeviceInfo::IMAGER_TYPE_AR0234_GREY:   return wire::SysDeviceInfo::IMAGER_TYPE_AR0234_GREY;
    case system::DeviceInfo::IMAGER_TYPE_AR0239_COLOR:  return wire::SysDeviceInfo::IMAGER_TYPE_AR0239_COLOR;
    case system::DeviceInfo::IMAGER_TYPE_FLIR_TAU2:     return wire::SysDeviceInfo::IMAGER_TYPE_FLIR_TAU2;
    default:
        CRL_DEBUG("unknown API imager type \"%d\"\n", a);
        return a; // pass through
    }
}
uint32_t impl::imagerWireToApi(uint32_t w)
{
    switch(w) {
    case wire::SysDeviceInfo::IMAGER_TYPE_CMV2000_GREY:  return system::DeviceInfo::IMAGER_TYPE_CMV2000_GREY;
    case wire::SysDeviceInfo::IMAGER_TYPE_CMV2000_COLOR: return system::DeviceInfo::IMAGER_TYPE_CMV2000_COLOR;
    case wire::SysDeviceInfo::IMAGER_TYPE_CMV4000_GREY:  return system::DeviceInfo::IMAGER_TYPE_CMV4000_GREY;
    case wire::SysDeviceInfo::IMAGER_TYPE_CMV4000_COLOR: return system::DeviceInfo::IMAGER_TYPE_CMV4000_COLOR;
    case wire::SysDeviceInfo::IMAGER_TYPE_IMX104_COLOR:  return system::DeviceInfo::IMAGER_TYPE_IMX104_COLOR;
    case wire::SysDeviceInfo::IMAGER_TYPE_AR0234_GREY:   return system::DeviceInfo::IMAGER_TYPE_AR0234_GREY;
    case wire::SysDeviceInfo::IMAGER_TYPE_AR0239_COLOR:  return system::DeviceInfo::IMAGER_TYPE_AR0239_COLOR;
    case wire::SysDeviceInfo::IMAGER_TYPE_FLIR_TAU2:     return system::DeviceInfo::IMAGER_TYPE_FLIR_TAU2;
    default:
        CRL_DEBUG("unknown WIRE imager type \"%d\"\n", w);
        return w; // pass through
    }
}
 
//
// Apply a time offset correction
 
void impl::applySensorTimeOffset(const utility::TimeStamp& offset)
{
    utility::ScopedLock lock(m_timeLock);
 
    //
    // Reseed on startup or if there is a large jump in time
    //
    CRL_CONSTEXPR int TIME_SYNC_THRESH_SECONDS = 100;
    const bool seed_offset = (false == m_timeOffsetInit) ||
                             (abs(m_timeOffset.getSeconds() - offset.getSeconds()) > TIME_SYNC_THRESH_SECONDS);
 
    if (seed_offset)
    {
        m_timeOffset = offset; // seed
        m_timeOffsetInit = true;
        return;
    }
 
    //
    // Use doubles to compute offsets to prevent overflow
 
    const double samples = static_cast<double>(TIME_SYNC_OFFSET_DECAY);
 
    const double currentOffset = m_timeOffset.getSeconds() + m_timeOffset.getMicroSeconds() * 1e-6;
    const double measuredOffset = offset.getSeconds() + offset.getMicroSeconds() * 1e-6;
 
    const double newOffset = utility::decayedAverage(currentOffset, samples, measuredOffset);
 
    m_timeOffset = utility::TimeStamp(static_cast<int64_t>(newOffset * 1e9));
}
 
//
// Return the corrected time
 
utility::TimeStamp impl::sensorToLocalTime(const utility::TimeStamp& sensorTime)
{
    utility::ScopedLock lock(m_timeLock);
    return m_timeOffset + sensorTime;
}
 
//
// Correct the time, populate seconds/microseconds
 
void impl::sensorToLocalTime(const utility::TimeStamp& sensorTime,
                             uint32_t&     seconds,
                             uint32_t&     microseconds)
{
    const utility::TimeStamp corrected = sensorToLocalTime(sensorTime);
    seconds          = corrected.getSeconds();
    microseconds     = corrected.getMicroSeconds();
}
 
//
// An internal thread for status/time-synchronization
 
#ifdef WIN32
DWORD impl::statusThread(void *userDataP)
#else
void *impl::statusThread(void *userDataP)
#endif
{
    impl *selfP = reinterpret_cast<impl*>(userDataP);
 
    //
    // Loop until shutdown
 
    while(selfP->m_threadsRunning) {
 
        //
        // Try to get device status message
        try {
 
            //
            // Setup handler for the status response
 
            ScopedWatch ack(wire::StatusResponse::ID, selfP->m_watch);
 
            //
            // Send the status request, recording the (approx) local time
 
            const utility::TimeStamp ping = utility::TimeStamp::getCurrentTime();
            selfP->publish(wire::StatusRequest());
 
            //
            // Wait for the response
 
            Status status;
            if (ack.wait(status, DEFAULT_ACK_TIMEOUT())) {
 
                //
                // Record (approx) time of response
 
                const utility::TimeStamp pong = utility::TimeStamp::getCurrentTime();
 
                //
                // Extract the response payload
 
                wire::StatusResponse msg;
                selfP->m_messages.extract(msg);
 
 
                //
                // Estimate 'msg.uptime' capture using half of the round trip period
 
                const utility::TimeStamp latency((pong.getNanoSeconds() - ping.getNanoSeconds()) / 2);
 
                //
                // If it took less than 5ms each direction for transmission consider this for a valid time offset
 
                if (latency.getNanoSeconds() < 5000000) {
 
                    //
                    // Compute and apply the estimated time offset
 
                    const utility::TimeStamp offset = ping + latency - msg.uptime;
 
                    selfP->applySensorTimeOffset(offset);
                }
 
                //
                // Cache the status message
 
                selfP->m_statusResponseMessage = msg;
                selfP->m_getStatusReturnStatus = Status_Ok;
            } else {
                selfP->m_getStatusReturnStatus = Status_TimedOut;
            }
 
        } catch (const std::exception& e) {
 
            CRL_DEBUG("exception: %s\n", e.what());
 
        } catch (...) {
 
            CRL_DEBUG_RAW("unknown exception\n");
        }
 
        //
        // Try to get device PTP status if FW supports it
        if (selfP->m_sensorVersion.firmwareVersion >= 0x60A /*FW release v6.10*/ ){
            try {
 
                //
                // Setup handler for the PTP status response
 
                wire::PtpStatusResponse ptpStatusResponse;
                Status status = selfP->waitData(wire::PtpStatusRequest(), ptpStatusResponse, DEFAULT_ACK_TIMEOUT(), 1);
 
                //
                // Cache the PTP status message
 
                if (status == Status_Ok) {
                    selfP->m_ptpStatusResponseMessage = ptpStatusResponse;
                    selfP->m_getPtpStatusReturnStatus = Status_Ok;
                } else if (status == Status_Unknown){
                    selfP->m_getPtpStatusReturnStatus = Status_Unsupported;
                } else {
                    selfP->m_getPtpStatusReturnStatus = status;
                }
 
            } catch (const std::exception& e) {
 
                CRL_DEBUG("exception: %s\n", e.what());
 
            } catch (...) {
 
                CRL_DEBUG_RAW("unknown exception\n");
            }
        }
        //
        // Recompute offset at ~1Hz
 
        usleep(static_cast<unsigned int> (1e6));
    }
 
#if defined(__MINGW64__)
    return 0;
#else
    return NULL;
#endif
}
 
} // namespace details
 
Channel* Channel::Create(const std::string& address)
{
    try {
 
        return new details::impl(address, Remote_Head_VPB, "");
 
    } catch (const std::exception& e) {
 
        CRL_DEBUG("exception: %s\n", e.what());
        return NULL;
    }
}
 
Channel* Channel::Create(const std::string& address, const RemoteHeadChannel &cameraId)
{
    try {
 
        return new details::impl(address, cameraId, "");
 
    } catch (const std::exception& e) {
 
        CRL_DEBUG("exception: %s\n", e.what());
        return NULL;
    }
}
 
Channel* Channel::Create(const std::string &address, const std::string& ifName)
    {
        try {
            return new details::impl(address, Remote_Head_VPB, ifName);
        } catch (const std::exception& e) {
            CRL_DEBUG("exception: %s\n", e.what());
            return NULL;
        }
    }
 
Channel* Channel::Create(const std::string &address, const RemoteHeadChannel &cameraId, const std::string &ifName)
    {
        try {
            return new details::impl(address, cameraId, ifName);
        } catch (const std::exception& e) {
            CRL_DEBUG("exception: %s\n", e.what());
            return NULL;
        }
    }
 
void Channel::Destroy(Channel *instanceP)
{
    try {
 
        if (instanceP)
            delete static_cast<details::impl*>(instanceP);
 
    } catch (const std::exception& e) {
 
        CRL_DEBUG("exception: %s\n", e.what());
    }
}
 
const char *Channel::statusString(Status status)
{
    switch(status) {
    case Status_Ok:          return "Ok";
    case Status_TimedOut:    return "Timed out";
    case Status_Error:       return "Error";
    case Status_Failed:      return "Failed";
    case Status_Unsupported: return "Unsupported";
    case Status_Unknown:     return "Unknown command";
    case Status_Exception:   return "Exception";
    }
 
    return "Unknown Error";
}
 
} // namespace multisense
} // namespace crl