From 2d3557ee8dcbf4c43e577578aa686edd6e48b851 Mon Sep 17 00:00:00 2001
From: Wolfgang Hoenig <whoenig@usc.edu>
Date: Wed, 16 Jan 2019 12:12:55 -0800
Subject: [PATCH] improve USB behavior

---
 include/crazyflie_cpp/Crazyflie.h |  2 ++
 src/Crazyflie.cpp                 |  8 ++++++++
 src/CrazyflieUSB.cpp              | 23 +++++++++++++++++++----
 3 files changed, 29 insertions(+), 4 deletions(-)

diff --git a/include/crazyflie_cpp/Crazyflie.h b/include/crazyflie_cpp/Crazyflie.h
index 882b793..71e0a63 100644
--- a/include/crazyflie_cpp/Crazyflie.h
+++ b/include/crazyflie_cpp/Crazyflie.h
@@ -110,6 +110,8 @@ class Crazyflie
     const std::string& link_uri,
     Logger& logger = EmptyLogger);
 
+  virtual ~Crazyflie();
+
   void logReset();
 
   void sendSetpoint(
diff --git a/src/Crazyflie.cpp b/src/Crazyflie.cpp
index cc47e21..2f2d7ea 100644
--- a/src/Crazyflie.cpp
+++ b/src/Crazyflie.cpp
@@ -124,6 +124,14 @@ Crazyflie::Crazyflie(
 
 }
 
+Crazyflie::~Crazyflie()
+{
+  for (size_t i = 0; i < MAX_USB; ++i) {
+    delete g_crazyflieUSB[i];
+  }
+
+}
+
 void Crazyflie::logReset()
 {
   crtpLogResetRequest request;
diff --git a/src/CrazyflieUSB.cpp b/src/CrazyflieUSB.cpp
index 0620f55..1578440 100644
--- a/src/CrazyflieUSB.cpp
+++ b/src/CrazyflieUSB.cpp
@@ -71,13 +71,28 @@ void CrazyflieUSB::sendPacket(
         (unsigned char*)&result.data[0],
         sizeof(result) - 2,
         &transferred,
-        /*timeout*/ 1000);
-    if (status != LIBUSB_SUCCESS) {
+        /*timeout*/ 1);
+    if (   status != LIBUSB_SUCCESS
+        && status != LIBUSB_ERROR_TIMEOUT) {
         throw std::runtime_error(libusb_error_name(status));
     }
-    result.ack = true;
+    if (status == LIBUSB_SUCCESS) {
+        result.ack = true;
+        result.size = transferred;
+    }
+
+    // std::cout << "sendPacket: ";
+    // for (uint32_t i = 0; i < length; ++i) {
+    //     std::cout << std::hex << (int)data[i] << " ";
+    // }
+    // std::cout << std::endl;
+    // std::cout << " ack: " << (int)result.ack << " " << (int)result.size << " ";
+    // for (uint32_t i = 0; i < result.size; ++i) {
+    //     std::cout << std::hex << (int)result.data[i] << " ";
+    // }
+    // std::cout << std::endl;
+
 
-    result.size = transferred;
 }
 
 void CrazyflieUSB::sendPacketNoAck(