Add static mcu reset fct

Author: Myzhar

examples/zed_oc_sensors_example.cpp

@@ -32,6 +32,8 @@ int main(int argc, char *argv[])
     // Set the verbose level
     sl_oc::VERBOSITY verbose = sl_oc::VERBOSITY::INFO;
 
+    sl_oc::sensors::SensorCapture::resetSensorModule();
+
     // Create a SensorCapture object
     sl_oc::sensors::SensorCapture sens(verbose);
 
@@ -52,7 +54,7 @@ int main(int argc, char *argv[])
         return EXIT_FAILURE;
     }
 
-    std::cout << "Video Capture connected to camera sn: " << sens.getSerialNumber() << std::endl;
+    std::cout << "Sensor Capture connected to camera sn: " << sens.getSerialNumber() << std::endl;
     // <---- Inizialize the sensors
 
     // ----> Get FW version information

include/sensorcapture.hpp

@@ -201,6 +201,29 @@ class SL_OC_EXPORT SensorCapture
      */
     const data::Temperature& getLastCameraTemperatureData(uint64_t timeout_usec=100);
 
+    /*!
+     * \brief Perform a SW reset of the Sensors Module. To be called in case one of the sensors stops to work correctly.
+     *
+     * \param serial_number The serial number of the device to be reset (0 to reset the first available)
+     * \return true if successful
+     *
+     * \note After the reset a new \ref SensorCapture connection is required
+     * \note The Sensors Module reset automatically performs a reset of the Video Module, so a new \ref VideoCapture
+     *  connection is required
+     */
+    static bool resetSensorModule(int serial_number=0);
+
+    /*!
+     * \brief Perform a reset of the video module without resetting the sensor module. To be called in case the Video
+     * module stops to work correctly.
+     *
+     * \param \param serial_number The serial number of the device to be reset (0 to reset the first available)
+     * \return true if successful
+     *
+     * \note After the reset a new \ref VideoCapture connection is required
+     */
+    static bool resetVideoModule(int serial_number=0);
+
 #ifdef VIDEO_MOD_AVAILABLE
     void updateTimestampOffset(uint64_t frame_ts);                                 //!< Called by  VideoCapture to update timestamp offset
     inline void setStartTimestamp(uint64_t start_ts){mStartSysTs=start_ts;}        //!< Called by  VideoCapture to sync timestamps reference point
@@ -221,10 +244,6 @@ class SL_OC_EXPORT SensorCapture
     bool enableDataStream(bool enable); //!< Enable/Disable the data stream
     bool isDataStreamEnabled();         //!< Check if the data stream is enabled
     bool sendPing();                    //!< Send a ping  each second (before 6 seconds) to keep data streaming alive
-
-    //bool resetMCU(); TODO
-    //bool resetUVC(); TODO
-
     // ----> USB commands to MCU
 
 private:

include/sensorcapture_def.hpp

@@ -41,10 +41,6 @@ namespace sl_oc {
 namespace sensors {
 
 namespace usb {
-// ----> Command to be used with the REPORT ID "REP_ID_REQUEST_SET"
-// Command to ping the MCU to communicate that host is alive
-const uint8_t RQ_CMD_PING = 0xF2;
-// <---- Command to be used with the REPORT ID "REP_ID_REQUEST_SET"
 
 /*!
  * \brief USB HID communication report IDs
@@ -61,6 +57,14 @@ typedef enum CUSTOMHID_REPORT_ID {
 
 } CUSTOMHID_REPORT_ID;
 
+/*!
+ * \brief USB HID requests IDs
+ */
+typedef enum CUSTOMHID_REQUEST_ID {
+    RQ_CMD_PING     = 0xF2, //!< Command to ping the MCU to communicate that host is alive
+    RQ_CMD_RST      = 0xE1, //!< Command to reset the MCU
+} CUSTOMHID_REQUEST_ID;
+
 #pragma pack(push)  // push current alignment to stack
 #pragma pack(1)     // set alignment to 1 byte boundary
 
@@ -163,6 +167,9 @@ const size_t TS_SHIFT_VAL_COUNT = 50; //!< Number of sensor data to use to updat
  */
 inline std::string wstr2str( const wchar_t* wstr)
 {
+    if(wstr==NULL)
+        return std::string();
+
     try
     {
         std::wstring ws( wstr );

src/sensorcapture.cpp

@@ -69,6 +69,11 @@ int SensorCapture::enumerateDevices()
         int fw_major = cur_dev->release_number>>8;
         int fw_minor = cur_dev->release_number&0x00FF;
         uint16_t pid = cur_dev->product_id;
+        if(!cur_dev->serial_number)
+        {
+            cur_dev = cur_dev->next;
+            continue;
+        }
         std::string sn_str = wstr2str( cur_dev->serial_number );
         int sn = std::stoi( sn_str );
 
@@ -157,7 +162,7 @@ bool SensorCapture::initializeSensors( int sn )
     if(!open( pid,sn))
     {
         std::string msg = "Connection to device with sn ";
-        msg += sn_str;
+        msg += std::to_string(sn);
         msg += " failed";
 
         ERROR_OUT(mVerbose,msg);
@@ -171,7 +176,7 @@ bool SensorCapture::initializeSensors( int sn )
     if(mVerbose)
     {
         std::string msg = "Connected to device with sn ";
-        msg += sn_str;
+        msg += std::to_string(sn);
 
         INFO_OUT(mVerbose,msg);
     }
@@ -599,6 +604,100 @@ bool SensorCapture::sendPing() {
     return true;
 }
 
+bool SensorCapture::resetSensorModule(int serial_number)
+{
+    // ----> Search for connected device
+    struct hid_device_info *devs, *cur_dev;
+
+    if (hid_init()==-1)
+        return 0;
+
+    devs = hid_enumerate(SL_USB_VENDOR, 0x0);
+    cur_dev = devs;
+
+    bool found = false;
+    uint16_t pid=0;
+    std::string sn_str;
+
+    while (cur_dev) {
+        int fw_major = cur_dev->release_number>>8;
+        int fw_minor = cur_dev->release_number&0x00FF;
+        pid = cur_dev->product_id;
+        sn_str = wstr2str( cur_dev->serial_number );
+        int sn = std::stoi( sn_str );
+
+        if(serial_number==0 || sn==serial_number)
+        {
+            if( pid==SL_USB_PROD_MCU_ZED2_REVA || pid==SL_USB_PROD_MCU_ZED2i_REVA)
+            {
+                found = true;
+                break;
+            }
+            else
+            {
+                std::string msg = "The reset function works only for ZED2/ZED2i camera models.";
+                std::cerr << msg << std::endl;
+
+                if(serial_number==0)
+                    continue;
+                else
+                    return false;
+            }
+        }
+
+        cur_dev = cur_dev->next;
+    }
+
+    hid_free_enumeration(devs);
+    // <---- Search for connected device
+
+    if(!found)
+    {
+        std::string msg;
+        if(serial_number!=0)
+        {
+            msg = "Unable to find the Sensor Module with serial number ";
+            msg += std::to_string(serial_number);
+        }
+        else
+        {
+            msg = "Unable to find the Sensor Module of a ZED2 camera. Please verify the USB connection.";
+        }
+
+        std::cerr << msg << std::endl;
+
+        return false;
+    }
+
+    std::wstring wide_sn_string = std::wstring(sn_str.begin(), sn_str.end());
+    const wchar_t* wsn = wide_sn_string.c_str();
+
+    hid_device* devHandle = hid_open(SL_USB_VENDOR, pid, wsn );
+
+    if(!devHandle)
+    {
+        std::string msg = "Unable to open the MCU HID device";
+        std::cerr << msg << std::endl;
+
+        return false;
+    }
+
+    unsigned char  buf[65];
+    buf[0] = static_cast<unsigned char>(usb::REP_ID_REQUEST_SET);
+    buf[1] = static_cast<unsigned char>(usb::RQ_CMD_RST);
+
+    hid_send_feature_report(devHandle, buf, 2);
+    sleep(2); // Wait for MCU to reboot
+
+    return true;
+}
+
+bool SensorCapture::resetVideoModule(int serial_number)
+{
+
+    return true;
+}
+
 const data::Imu& SensorCapture::getLastIMUData(uint64_t timeout_usec)
 {
     // ----> Wait for a new frame

src/videocapture.cpp

@@ -2013,6 +2013,7 @@ void VideoCapture::setColorBars(int side, bool c)
 
 
     unsigned long long ulAddr2 = 0x80181080;
+
     if (side==1)
          ulAddr2 = 0x80181880;