Add function to reset the OV580 module without resetting the MCU

Author: Myzhar

examples/zed_oc_sensors_example.cpp

@@ -32,8 +32,6 @@ 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);
 

examples/zed_oc_sync_example.cpp

@@ -51,8 +51,11 @@ uint64_t mcu_sync_ts=0;
 // The main function
 int main(int argc, char *argv[])
 {
+    sl_oc::sensors::SensorCapture::resetSensorModule();
+    sl_oc::sensors::SensorCapture::resetVideoModule();
+
     // Set the verbose level
-    sl_oc::VERBOSITY verbose = sl_oc::VERBOSITY::INFO;
+    sl_oc::VERBOSITY verbose = sl_oc::VERBOSITY::ERROR;
 
     // ----> Set the video parameters
     sl_oc::video::VideoParams params;

include/sensorcapture.hpp

@@ -231,6 +231,8 @@ class SL_OC_EXPORT SensorCapture
 #endif
 
 private:
+    static bool searchForConnectedDev(int* serial_number, unsigned short* found_pid); //!< Search for a device and returns its pid and serial number
+
     void grabThreadFunc();              //!< The sensor data grabbing thread function
 
     bool startCapture();                //!< Start data capture thread

include/sensorcapture_def.hpp

@@ -50,19 +50,23 @@ typedef enum CUSTOMHID_REPORT_ID {
     REP_ID_SENSOR_DATA          = 0x01,  //!< Sensor data report ID
 
     // Generic commands
-    REP_ID_REQUEST_SET          = 0x21, //!< USB Request report ID
+    REP_ID_REQUEST_SET          = 0x21,  //!< USB Request report ID
+
+    // OV580
+    REP_ID_OV580_CMD            = 0x22,	 //!< OV580 control request
 
     // Features Reports
-    REP_ID_SENSOR_STREAM_STATUS = 0x32, //!< Stream Status report ID
+    REP_ID_SENSOR_STREAM_STATUS = 0x32,  //!< Stream Status 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
+    RQ_CMD_PING         = 0xF2, //!< Command to ping the MCU to communicate that host is alive
+    RQ_CMD_RST          = 0xE1, //!< Command to reset the MCU
+    OV580_CMD_RESET		= 0x02  //!< Command to reset the OV580 using the MCU
 } CUSTOMHID_REQUEST_ID;
 
 #pragma pack(push)  // push current alignment to stack
@@ -109,6 +113,15 @@ typedef struct StreamStatus {
     uint8_t stream_status;	//!< Status of the USB streaming
 } StreamStatus;
 
+/*!
+ *  \brief OV580 control using the MCU
+ */
+typedef struct _ov580_cmd_struct {
+    uint8_t struct_id; 	//!< struct identifier for HID comm
+    uint8_t cmd;		//!< command to be sent to OV580: OV580_RESET
+    uint16_t info;		//!< NOT USED
+} OV580CmdStruct;
+
 #pragma pack(pop) // Restore previous saved alignment
 
 }

src/sensorcapture.cpp

@@ -604,13 +604,20 @@ bool SensorCapture::sendPing() {
     return true;
 }
 
-bool SensorCapture::resetSensorModule(int serial_number)
+bool SensorCapture::searchForConnectedDev(int* serial_number, unsigned short* found_pid)
 {
+    int in_serial_number;
+    if(serial_number==nullptr)
+         in_serial_number = 0;
+    else
+        in_serial_number = *serial_number;
+    int found_serial_number = 0;
+
     // ----> Search for connected device
     struct hid_device_info *devs, *cur_dev;
 
     if (hid_init()==-1)
-        return 0;
+        return false;
 
     devs = hid_enumerate(SL_USB_VENDOR, 0x0);
     cur_dev = devs;
@@ -626,19 +633,20 @@ bool SensorCapture::resetSensorModule(int serial_number)
         sn_str = wstr2str( cur_dev->serial_number );
         int sn = std::stoi( sn_str );
 
-        if(serial_number==0 || sn==serial_number)
+        if(in_serial_number==0 || sn==in_serial_number)
         {
             if( pid==SL_USB_PROD_MCU_ZED2_REVA || pid==SL_USB_PROD_MCU_ZED2i_REVA)
             {
                 found = true;
+                found_serial_number = sn;
                 break;
             }
             else
             {
                 std::string msg = "The reset function works only for ZED2/ZED2i camera models.";
                 std::cerr << msg << std::endl;
 
-                if(serial_number==0)
+                if(in_serial_number==0)
                     continue;
                 else
                     return false;
@@ -651,24 +659,41 @@ bool SensorCapture::resetSensorModule(int serial_number)
     hid_free_enumeration(devs);
     // <---- Search for connected device
 
-    if(!found)
+    if(!found) {
+        return false;
+    }
+
+    if(serial_number)
+        *serial_number = found_serial_number;
+    if(found_pid)
+        *found_pid = pid;
+    return true;
+}
+
+bool SensorCapture::resetSensorModule(int serial_number)
+{
+    int found_sn = serial_number;
+    unsigned short pid;
+    bool res = searchForConnectedDev(&found_sn, &pid);
+    if(!res)
     {
         std::string msg;
         if(serial_number!=0)
         {
-            msg = "Unable to find the Sensor Module with serial number ";
+            msg = "[sl_oc::sensors::SensorCapture] WARNING: Sensors Module reset failed. 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.";
+            msg = "[sl_oc::sensors::SensorCapture] WARNING: Sensors Module reset failed. Unable to find the Sensor Module of a ZED2 camera. Please verify the USB connection.";
         }
 
         std::cerr << msg << std::endl;
 
         return false;
     }
 
+    std::string sn_str = std::to_string(found_sn);
     std::wstring wide_sn_string = std::wstring(sn_str.begin(), sn_str.end());
     const wchar_t* wsn = wide_sn_string.c_str();
 
@@ -687,14 +712,72 @@ bool SensorCapture::resetSensorModule(int serial_number)
     buf[1] = static_cast<unsigned char>(usb::RQ_CMD_RST);
 
     hid_send_feature_report(devHandle, buf, 2);
-    sleep(2); // Wait for MCU to reboot
+    // Note: cannot verify the return value of the `hid_send_feature_report` command because the MCU is suddenly reset
+    // and it cannot return a valid value
+
+    sleep(2); // Wait for MCU and OV580 to reboot
+
+    std::cerr << "[sl_oc::sensors::SensorCapture] INFO: Sensors Module reset successful" << std::endl;
 
     return true;
 }
 
 bool SensorCapture::resetVideoModule(int serial_number)
 {
+    int found_sn = serial_number;
+    unsigned short pid;
+    bool res = searchForConnectedDev(&found_sn, &pid);
+    if(!res)
+    {
+        std::string msg;
+        if(serial_number!=0)
+        {
+            msg = "[sl_oc::sensors::SensorCapture] WARNING: Video Module reset failed. Unable to find the Sensor Module with serial number ";
+            msg += std::to_string(serial_number);
+        }
+        else
+        {
+            msg = "[sl_oc::sensors::SensorCapture] WARNING: Video Module reset failed. Unable to find the Sensor Module of a ZED2 camera. Please verify the USB connection.";
+        }
+
+        std::cerr << msg << std::endl;
+
+        return false;
+    }
+
+    std::string sn_str = std::to_string(found_sn);
+    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;
+    }
+
+    usb::OV580CmdStruct cmd;
+    cmd.struct_id = usb::REP_ID_OV580_CMD;
+    cmd.cmd = usb::OV580_CMD_RESET;
+    cmd.info=0;
+
+    unsigned char buf[65];
+    memcpy(buf, &(cmd.struct_id), sizeof(usb::OV580CmdStruct));
+
+    int ret = hid_send_feature_report(devHandle, buf, sizeof(usb::OV580CmdStruct));
+    hid_close(devHandle);
+
+    if(ret!=sizeof(usb::OV580CmdStruct)) {
+        std::cerr << "[sl_oc::sensors::SensorCapture] INFO: Video Module reset failed" << std::endl;
+        return false;
+    }
+
+    sleep(2); // Wait for OV580 to reboot
 
+    std::cerr << "[sl_oc::sensors::SensorCapture] INFO: Video Module reset successful" << std::endl;
     return true;
 }