Extend UVC retry behavior to F200/SR300 models

IntelRealSense · Jan 21, 2016 · 78b93c3 · 78b93c3
1 parent 05c8f21
commit 78b93c3
Show file tree

Hide file tree

Showing 4 changed files with 38 additions and 25 deletions.
diff --git a/src/f200-private.cpp b/src/f200-private.cpp
@@ -5,6 +5,7 @@
 
 #include <cstring>
 #include <algorithm>
+#include <thread>
 #include <cmath>
 
 #define IV_COMMAND_FIRMWARE_UPDATE_MODE     0x01
@@ -271,12 +272,12 @@ namespace rsimpl { namespace f200
     // single *potentially* useful XU on the color device, so let's ignore it for now.
     void xu_read(const uvc::device & device, uint8_t xu_ctrl, void * buffer, uint32_t length)
     {
-        get_control(device, 1, xu_ctrl, buffer, length);
+        uvc::get_control_with_retry(device, 1, static_cast<int>(xu_ctrl), buffer, length);
     }
 
     void xu_write(uvc::device & device, uint8_t xu_ctrl, void * buffer, uint32_t length)
     {
-        set_control(device, 1, xu_ctrl, buffer, length);
+        uvc::set_control_with_retry(device, 1, static_cast<int>(xu_ctrl), buffer, length);
     }
 
     void get_laser_power(const uvc::device & device, uint8_t & laser_power)

diff --git a/src/r200-private.cpp b/src/r200-private.cpp
@@ -37,24 +37,12 @@ namespace rsimpl { namespace r200
 {
     void xu_read(const uvc::device & device, control xu_ctrl, void * buffer, uint32_t length)
     {
-        // Try reading an XU control, if it fails, retry several times
-        for(int i=0; i<20; ++i)
-        {
-            try { get_control(device, 0, static_cast<int>(xu_ctrl), buffer, length); return; }
-            catch(...) { std::this_thread::sleep_for(std::chrono::milliseconds(50)); }
-        }
-        get_control(device, 0, static_cast<int>(xu_ctrl), buffer, length);
+        uvc::get_control_with_retry(device, 0, static_cast<int>(xu_ctrl), buffer, length);
     }
 
     void xu_write(uvc::device & device, control xu_ctrl, void * buffer, uint32_t length)
     {
-        // Try writing an XU control, if it fails, retry several times
-        for(int i=0; i<20; ++i)
-        {
-            try { set_control(device, 0, static_cast<int>(xu_ctrl), buffer, length); return; }
-            catch(...) { std::this_thread::sleep_for(std::chrono::milliseconds(50)); }
-        }
-        set_control(device, 0, static_cast<int>(xu_ctrl), buffer, length);
+        uvc::set_control_with_retry(device, 0, static_cast<int>(xu_ctrl), buffer, length);
     }
 
     struct CommandResponsePacket

diff --git a/src/uvc-v4l2.cpp b/src/uvc-v4l2.cpp
@@ -148,28 +148,28 @@ namespace rsimpl
             int get_pid() const { return pid; }
             int get_mi() const { return mi; }
 
-	    int get_xu_endpoint() const
+            int get_xu_endpoint() const
             {
-	        // Temporary workaround, it should be possible to query for this
-	        switch(get_pid())
-		{
+                // Temporary workaround, it should be possible to query for this
+                switch(get_pid())
+                {
                 case 2688: return 2; // R200
                 case 2662: return 6; // F200
                 case 2725: return 6; // SR300
-		default: return 0;
-		}
+                default: return 0;
+                }
             }
 
             void get_control(int control, void * data, size_t size)
             {
-	        uvc_xu_control_query q = {get_xu_endpoint(), control, UVC_GET_CUR, size, reinterpret_cast<uint8_t *>(data)};
+                uvc_xu_control_query q = {get_xu_endpoint(), control, UVC_GET_CUR, size, reinterpret_cast<uint8_t *>(data)};
                 if(xioctl(fd, UVCIOC_CTRL_QUERY, &q) < 0) throw_error("UVCIOC_CTRL_QUERY:UVC_GET_CUR");
             }
 
             void set_control(int control, void * data, size_t size)
             {
-	        uvc_xu_control_query q = {get_xu_endpoint(), control, UVC_SET_CUR, size, reinterpret_cast<uint8_t *>(data)};
-               if(xioctl(fd, UVCIOC_CTRL_QUERY, &q) < 0) throw_error("UVCIOC_CTRL_QUERY:UVC_SET_CUR");
+                uvc_xu_control_query q = {get_xu_endpoint(), control, UVC_SET_CUR, size, reinterpret_cast<uint8_t *>(data)};
+                if(xioctl(fd, UVCIOC_CTRL_QUERY, &q) < 0) throw_error("UVCIOC_CTRL_QUERY:UVC_SET_CUR");
             }
 
             void set_format(int width, int height, int fourcc, int fps, std::function<void(const void * data)> callback)

diff --git a/src/uvc.h b/src/uvc.h
@@ -8,6 +8,7 @@
 #include "types.h"
 
 #include <functional>
+#include <thread>
 
 namespace rsimpl
 {
@@ -40,6 +41,29 @@ namespace rsimpl
         void set_control(device & device, int subdevice, uint8_t ctrl, void * data, int len);
         void get_control(const device & device, int subdevice, uint8_t ctrl, void * data, int len);
 
+        inline void set_control_with_retry(device & device, int subdevice, uint8_t ctrl, void * data, int len)
+        {
+            // Try writing a control, if it fails, retry several times
+            // TODO: We may wish to tune the retry counts and sleep times based on camera, platform, firmware, etc.
+            for(int i=0; i<20; ++i)
+            {
+                try { set_control(device, subdevice, ctrl, data, len); return; }
+                catch(...) { std::this_thread::sleep_for(std::chrono::milliseconds(50)); }
+            }
+            set_control(device, subdevice, ctrl, data, len);
+        }
+
+        inline void get_control_with_retry(const device & device, int subdevice, uint8_t ctrl, void * data, int len)
+        {
+            // Try reading a control, if it fails, retry several times
+            for(int i=0; i<20; ++i)
+            {
+                try { get_control(device, subdevice, ctrl, data, len); return; }
+                catch(...) { std::this_thread::sleep_for(std::chrono::milliseconds(50)); }
+            }
+            get_control(device, subdevice, ctrl, data, len);
+        }
+
         // Control streaming
         void set_subdevice_mode(device & device, int subdevice_index, int width, int height, uint32_t fourcc, int fps, std::function<void(const void * frame)> callback);
         void start_streaming(device & device, int num_transfer_bufs);