Merge pull request #494 from xlz/mostly-usb-fixes

Mostly usb fixes

Author: floe

examples/Protonect.cpp

@@ -48,6 +48,27 @@ void sigint_handler(int s)
   protonect_shutdown = true;
 }
 
+bool protonect_paused = false;
+libfreenect2::Freenect2Device *devtopause;
+
+//Doing non-trivial things in signal handler is bad. If you want to pause,
+//do it in another thread.
+//Though libusb operations are generally thread safe, I cannot guarantee
+//everything above is thread safe when calling start()/stop() while
+//waitForNewFrame().
+void sigusr1_handler(int s)
+{
+  if (devtopause == 0)
+    return;
+/// [pause]
+  if (protonect_paused)
+    devtopause->start();
+  else
+    devtopause->stop();
+  protonect_paused = !protonect_paused;
+/// [pause]
+}
+
 //The following demostrates how to create a custom logger
 /// [logger]
 #include <fstream>
@@ -91,6 +112,7 @@ int main(int argc, char *argv[])
   std::string program_path(argv[0]);
   std::cerr << "Environment variables: LOGFILE=<protonect.log>" << std::endl;
   std::cerr << "Usage: " << program_path << " [gl | cl | cpu] [<device serial>] [-noviewer]" << std::endl;
+  std::cerr << "To pause and unpause: pkill -USR1 Protonect" << std::endl;
   size_t executable_name_idx = program_path.rfind("Protonect");
 
   std::string binpath = "/";
@@ -193,7 +215,12 @@ int main(int argc, char *argv[])
     return -1;
   }
 
+  devtopause = dev;
+
   signal(SIGINT,sigint_handler);
+#ifdef SIGUSR1
+  signal(SIGUSR1, sigusr1_handler);
+#endif
   protonect_shutdown = false;
 
 /// [listeners]

include/internal/libfreenect2/protocol/command.h

@@ -31,7 +31,7 @@
 
 #define KCMD_READ_FIRMWARE_VERSIONS 0x02
 #define KCMD_INIT_STREAMS 0x09
-#define KCMD_READ_DATA_0x14 0x14
+#define KCMD_READ_HARDWARE_INFO 0x14
 #define KCMD_READ_STATUS 0x16
 #define KCMD_READ_DATA_PAGE 0x22
 #define KCMD_READ_DATA_0x26 0x26
@@ -43,7 +43,8 @@
 #define KCMD_0x47 0x47
 
 // observed in sensor stop/shutdown sequence
-#define KCMD_0x0A 0x0A
+#define KCMD_STOP 0x0A
+#define KCMD_SHUTDOWN 0x00
 
 namespace libfreenect2
 {
@@ -166,7 +167,7 @@ struct CommandWith4Params : public Command<CommandId, MaxResponseLength, 4>
 
 typedef CommandWith0Params<0x02, 0x200> ReadFirmwareVersionsCommand;
 
-typedef CommandWith0Params<KCMD_READ_DATA_0x14, 0x5C> ReadData0x14Command;
+typedef CommandWith0Params<KCMD_READ_HARDWARE_INFO, 0x5C> ReadHardwareInfoCommand;
 
 typedef CommandWith0Params<KCMD_INIT_STREAMS, 0x00> InitStreamsCommand;
 
@@ -192,7 +193,8 @@ typedef CommandWith1Param<KCMD_SET_STREAMING, 0x00, 0x01> SetStreamEnabledComman
 typedef CommandWith4Params<KCMD_0x46, 0x00, 0x00, 0x00003840, 0x00000037, 0x00> Unknown0x46Command;
 typedef CommandWith0Params<KCMD_0x47, 0x10> Unknown0x47Command;
 
-typedef CommandWith0Params<KCMD_0x0A, 0x00> Unknown0x0ACommand;
+typedef CommandWith0Params<KCMD_STOP, 0x00> StopCommand;
+typedef CommandWith0Params<KCMD_SHUTDOWN, 0x00> ShutdownCommand;
 
 typedef CommandWith4Params<KCMD_SET_MODE, 0x00, 0x00> SetModeDisabledCommand;
 typedef CommandWith4Params<KCMD_SET_MODE, 0x00, 0x01> SetModeEnabledCommand;

include/internal/libfreenect2/protocol/response.h

@@ -46,7 +46,7 @@ class SerialNumberResponse
 public:
   SerialNumberResponse(const unsigned char *data, int length)
   {
-    char *c = new char[length / 2];
+    char *c = new char[length / 2 + 1]();
 
     for(int i = 0, j = 0; i < length; i += 2, ++j)
     {
@@ -70,18 +70,16 @@ class FirmwareVersionResponse
 private:
   struct FWSubsystemVersion
   {
-    uint16_t minor;
-    uint16_t major;
-    uint16_t build;
-    uint16_t revision;
-    uint16_t reserved0[4];
+    uint32_t maj_min;
+    uint32_t revision;
+    uint32_t build;
+    uint32_t reserved0;
 
     FWSubsystemVersion()
     {
-      major = 0;
-      minor = 0;
-      build = 0;
+      maj_min = 0;
       revision = 0;
+      build = 0;
     }
   };
 
@@ -92,24 +90,23 @@ class FirmwareVersionResponse
     int n = length / sizeof(FWSubsystemVersion);
     const FWSubsystemVersion *sv = reinterpret_cast<const FWSubsystemVersion *>(data);
 
-    for(int i = 0; i < n && sv->major > 0; ++i, ++sv)
+    for(int i = 0; i < 7 && i < n; ++i)
     {
-      versions_.push_back(*sv);
+      versions_.push_back(sv[i]);
     }
   }
 
   std::string toString()
   {
     FWSubsystemVersion max;
-    for(int i = 0; i < versions_.size(); ++i)
+    std::stringstream version_string;
+    // the main blob's index
+    int i = 3;
+    if (i < versions_.size())
     {
-      max.major = std::max<uint16_t>(max.major, versions_[i].major);
-      max.minor = std::max<uint16_t>(max.minor, versions_[i].minor);
-      max.build = std::max<uint16_t>(max.build, versions_[i].build);
-      max.revision = std::max<uint16_t>(max.revision, versions_[i].revision);
+      const FWSubsystemVersion &ver = versions_[i];
+      version_string << (ver.maj_min >> 16) << "." << (ver.maj_min & 0xffff) << "." << ver.revision << "." << ver.build;
     }
-    std::stringstream version_string;
-    version_string << max.major << "." << max.minor << "." << max.build << "." << max.revision << "." << versions_.size();
 
     return version_string.str();
   }

include/libfreenect2/libfreenect2.hpp

@@ -224,6 +224,20 @@ If you are finished and no longer need to receive more frames, you can stop
 the device and exit.
 
 @snippet Protonect.cpp stop
+
+Pause the Device
+----------------
+
+You can also temporarily pause the device with
+[stop()](@ref libfreenect2::Freenect2Device::stop) and
+[start()](@ref libfreenect2::Freenect2Device::start).
+
+@snippet Protonect.cpp pause
+
+Doing this during `waitForNewFrame()` should be thread safe, and tests also
+show well. But a guarantee of thread safety has not been checked yet.
+
+THE END.
 */
 
 namespace libfreenect2

src/command_transaction.cpp

@@ -49,7 +49,7 @@ CommandTransaction::Result::~Result()
 
 void CommandTransaction::Result::allocate(size_t size)
 {
-  if (capacity < size)
+  if (capacity != size)
   {
     deallocate();
     data = new unsigned char[size];

src/libfreenect2.cpp

@@ -683,9 +683,9 @@ void Freenect2DeviceImpl::start()
   command_tx_.execute(ReadFirmwareVersionsCommand(nextCommandSeq()), firmware_result);
   firmware_ = FirmwareVersionResponse(firmware_result.data, firmware_result.length).toString();
 
-  command_tx_.execute(ReadData0x14Command(nextCommandSeq()), result);
-  LOG_DEBUG << "ReadData0x14 response";
-  LOG_DEBUG << GenericResponse(result.data, result.length).toString();
+  command_tx_.execute(ReadHardwareInfoCommand(nextCommandSeq()), result);
+  //The hardware version is currently useless.  It is only used to select the
+  //IR normalization table, but we don't have that.
 
   command_tx_.execute(ReadSerialNumberCommand(nextCommandSeq()), serial_result);
   std::string new_serial = SerialNumberResponse(serial_result.data, serial_result.length).toString();
@@ -749,17 +749,28 @@ void Freenect2DeviceImpl::start()
   rgb_camera_params_.my_x0y0 = rgb_p->my_x0y0; // 1
   setColorCameraParams(rgb_camera_params_);
 
-  command_tx_.execute(ReadStatus0x090000Command(nextCommandSeq()), result);
-  LOG_DEBUG << "ReadStatus0x090000 response";
-  LOG_DEBUG << GenericResponse(result.data, result.length).toString();
+  command_tx_.execute(SetModeEnabledWith0x00640064Command(nextCommandSeq()), result);
+  command_tx_.execute(SetModeDisabledCommand(nextCommandSeq()), result);
+
+  for (uint32_t status = 0, last = 0; (status & 1) == 0; last = status)
+  {
+    command_tx_.execute(ReadStatus0x090000Command(nextCommandSeq()), result);
+    if (result.length < sizeof(uint32_t))
+      continue; //TODO should report error
+    status = *reinterpret_cast<const uint32_t *>(result.data);
+    if (status != last)
+      LOG_DEBUG << "status 0x090000: " << status;
+    if ((status & 1) == 0)
+      this_thread::sleep_for(chrono::milliseconds(100));
+  }
 
   command_tx_.execute(InitStreamsCommand(nextCommandSeq()), result);
 
   usb_control_.setIrInterfaceState(UsbControl::Enabled);
 
   command_tx_.execute(ReadStatus0x090000Command(nextCommandSeq()), result);
-  LOG_DEBUG << "ReadStatus0x090000 response";
-  LOG_DEBUG << GenericResponse(result.data, result.length).toString();
+  if (result.length >= sizeof(uint32_t))
+    LOG_DEBUG << "status 0x090000: " << *reinterpret_cast<const uint32_t *>(result.data);
 
   command_tx_.execute(SetStreamEnabledCommand(nextCommandSeq()), result);
 
@@ -813,8 +824,14 @@ void Freenect2DeviceImpl::stop()
   usb_control_.setIrInterfaceState(UsbControl::Disabled);
 
   CommandTransaction::Result result;
-  command_tx_.execute(Unknown0x0ACommand(nextCommandSeq()), result);
+  command_tx_.execute(SetModeEnabledWith0x00640064Command(nextCommandSeq()), result);
+  command_tx_.execute(SetModeDisabledCommand(nextCommandSeq()), result);
+  command_tx_.execute(StopCommand(nextCommandSeq()), result);
   command_tx_.execute(SetStreamDisabledCommand(nextCommandSeq()), result);
+  command_tx_.execute(SetModeEnabledCommand(nextCommandSeq()), result);
+  command_tx_.execute(SetModeDisabledCommand(nextCommandSeq()), result);
+  command_tx_.execute(SetModeEnabledCommand(nextCommandSeq()), result);
+  command_tx_.execute(SetModeDisabledCommand(nextCommandSeq()), result);
 
   usb_control_.setVideoTransferFunctionState(UsbControl::Disabled);
 
@@ -837,6 +854,11 @@ void Freenect2DeviceImpl::close()
     stop();
   }
 
+  CommandTransaction::Result result;
+  command_tx_.execute(SetModeEnabledWith0x00640064Command(nextCommandSeq()), result);
+  command_tx_.execute(SetModeDisabledCommand(nextCommandSeq()), result);
+  command_tx_.execute(ShutdownCommand(nextCommandSeq()), result);
+
   if(pipeline_->getRgbPacketProcessor() != 0)
     pipeline_->getRgbPacketProcessor()->setFrameListener(0);
 

src/opencl_depth_packet_processor.cpp

@@ -53,6 +53,8 @@
 #define REG_OPENCL_FILE ""
 #endif
 
+#include <cstdlib>
+
 namespace libfreenect2
 {
 
@@ -141,6 +143,11 @@ class OpenCLDepthPacketProcessorImpl: public WithPerfLogging
     , programBuilt(false)
     , programInitialized(false)
   {
+#if _BSD_SOURCE || _POSIX_C_SOURCE >= 200112L || _XOPEN_SOURCE >= 600
+    setenv("OCL_IGNORE_SELF_TEST", "1", 0);
+    setenv("OCL_STRICT_CONFORMANCE", "0", 0);
+#endif
+
     newIrFrame();
     newDepthFrame();