improve USB behavior

whoenig · Jan 16, 2019 · 2d3557e · 2d3557e
1 parent 08921f9
commit 2d3557e
Show file tree

Hide file tree

Showing 3 changed files with 29 additions and 4 deletions.
diff --git 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
@@ -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
@@ -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(