sensorcapture.hpp

Go to the documentation of this file.

//
// Copyright (c) 2021, STEREOLABS.
//
// All rights reserved.
//
// THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
// "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
// LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR
// A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT
// OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL,
// SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT
// LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS OF USE,
// DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND ON ANY
// THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT
// (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE
// OF THIS SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
//
 
#ifndef SENSORCAPTURE_HPP
#define SENSORCAPTURE_HPP
 
#include "defines.hpp"
 
#include <thread>
#include <vector>
#include <map>
#include <mutex>
 
#ifdef SENSORS_MOD_AVAILABLE
 
#include "sensorcapture_def.hpp"
#include "hidapi.h"
 
namespace sl_oc {
 
#ifdef VIDEO_MOD_AVAILABLE
namespace video {
class VideoCapture;
}
#endif
 
namespace sensors {
 
namespace data {
 
struct SL_OC_EXPORT Imu
{
    // Validity of the magnetometer sensor data
    typedef enum _imu_status {
        NOT_PRESENT = 0,
        OLD_VAL = 1,
        NEW_VAL = 2
    } ImuStatus;
 
    ImuStatus valid = NOT_PRESENT;     
    uint64_t timestamp = 0; 
    float aX;               
    float aY;               
    float aZ;               
    float gX;               
    float gY;               
    float gZ;               
    float temp;             
    bool sync;              
};
 
struct SL_OC_EXPORT Magnetometer
{
    // Validity of the magnetometer sensor data
    typedef enum _mag_status {
        NOT_PRESENT = 0,
        OLD_VAL = 1,
        NEW_VAL = 2
    } MagStatus;
 
    MagStatus valid = NOT_PRESENT;     
    uint64_t timestamp = 0; 
    float mX;               
    float mY;               
    float mZ;               
};
 
struct SL_OC_EXPORT Environment
{
    // Validity of the environmental sensor data
    typedef enum _env_status {
        NOT_PRESENT = 0,
        OLD_VAL = 1,
        NEW_VAL = 2
    } EnvStatus;
 
    EnvStatus valid = NOT_PRESENT;     
    uint64_t timestamp = 0; 
    float temp;             
    float press;            
    float humid;            
};
 
struct SL_OC_EXPORT Temperature
{
    typedef enum _temp_status {
        NOT_PRESENT = 0,
        OLD_VAL = 1,
        NEW_VAL = 2
    } TempStatus;
 
    TempStatus valid = NOT_PRESENT;     
    uint64_t timestamp = 0; 
    float temp_left;        
    float temp_right;       
};
 
}
 
class SL_OC_EXPORT SensorCapture
{
    ZED_OC_VERSION_ATTRIBUTE;
 
public:
    SensorCapture( sl_oc::VERBOSITY verbose_lvl=sl_oc::VERBOSITY::ERROR );
 
    virtual ~SensorCapture();
 
    std::vector<int> getDeviceList(bool refresh=false);
 
    bool initializeSensors( int sn=-1 );
 
    void getFirmwareVersion( uint16_t& fw_major, uint16_t& fw_minor );
 
    int getSerialNumber();
 
    const data::Imu& getLastIMUData(uint64_t timeout_usec=1500);
 
    const data::Magnetometer& getLastMagnetometerData(uint64_t timeout_usec=100);
 
    const data::Environment& getLastEnvironmentData(uint64_t timeout_usec=100);
 
    const data::Temperature& getLastCameraTemperatureData(uint64_t timeout_usec=100);
 
    static bool resetSensorModule(int serial_number=0);
 
    static bool resetVideoModule(int serial_number=0);
 
#ifdef VIDEO_MOD_AVAILABLE
    void updateTimestampOffset(uint64_t frame_ts);                                 
    inline void setStartTimestamp(uint64_t start_ts){mStartSysTs=start_ts;}        
    inline void setVideoPtr(video::VideoCapture* videoPtr){mVideoPtr=videoPtr;}    
#endif
 
private:
    static bool searchForConnectedDev(int* serial_number, unsigned short* found_pid); 
 
    void grabThreadFunc();              
 
    bool startCapture();                
 
    bool open(uint16_t pid, int serial_number); 
    void close();                       
 
    int enumerateDevices();             
 
    // ----> USB commands to MCU
    bool enableDataStream(bool enable); 
    bool isDataStreamEnabled();         
    bool sendPing();                    
    // ----> USB commands to MCU
 
private:
    // Flags
    int mVerbose=0;                
    bool mNewIMUData=false;             
    bool mNewMagData=false;             
    bool mNewEnvData=false;             
    bool mNewCamTempData=false;         
 
    bool mInitialized = false;          
    bool mStopCapture = false;          
    bool mGrabRunning = false;          
 
    std::map<int,uint16_t> mSlDevPid;   
    std::map<int,uint16_t> mSlDevFwVer; 
 
    hid_device* mDevHandle = nullptr;   
    int mDevSerial = -1;                
    int mDevFwVer = -1;                 
    unsigned short mDevPid = 0;         
 
    data::Imu mLastIMUData;             
    data::Magnetometer mLastMagData;    
    data::Environment mLastEnvData;     
    data::Temperature mLastCamTempData; 
 
    std::thread mGrabThread;            
 
    std::mutex mIMUMutex;               
    std::mutex mMagMutex;               
    std::mutex mEnvMutex;               
    std::mutex mCamTempMutex;           
 
    uint64_t mStartSysTs=0;             
    uint64_t mLastMcuTs=0;              
 
    bool mFirstImuData=true;            
 
    // ----> Timestamp synchronization
    uint64_t mLastFrameSyncCount=0;     
 
    std::vector<uint64_t> mMcuTsQueue;  
    std::vector<uint64_t> mSysTsQueue;  
 
    double mNTPTsScaling=1.0;           
    int mNTPAdjustedCount = 0;          
 
    int64_t mSyncOffset=0;              
    // <---- Timestamp synchronization
 
#ifdef VIDEO_MOD_AVAILABLE
    video::VideoCapture* mVideoPtr=nullptr;    
    uint64_t mSyncTs=0;                 
#endif
 
};
 
}
}
 
#endif
 
#endif // SENSORCAPTURE_HPP