usb: Add proper warmup sequence

Author: xlz

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;

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);