Merge branch 'dev-crazyflie-link-cpp' into dev-coltrans

whoenig · Jan 30, 2024 · b34a43d · b34a43d
2 parents 8891b9b + 96cd78a
commit b34a43d
Show file tree

Hide file tree

Showing 3 changed files with 68 additions and 8 deletions.
diff --git a/crazyflie-link-cpp b/crazyflie-link-cpp
diff --git a/include/crazyflie_cpp/Crazyflie.h b/include/crazyflie_cpp/Crazyflie.h
@@ -131,11 +131,18 @@ class Crazyflie
   static std::vector<std::string> scan(
     uint64_t address = 0xE7E7E7E7E7);
 
+  // returns the URI that can be used for broadcast communication (or empty string if there is none)
+  static std::string broadcastUriFromUnicastUri(const std::string& link_uri);
+
   const bitcraze::crazyflieLinkCpp::Connection::Statistics connectionStats() const
   {
     return m_connection.statistics();
   }
 
+  bitcraze::crazyflieLinkCpp::Connection::Statistics connectionStatsDelta()
+  {
+    return m_connection.statisticsDelta();
+  }
 
   // returns the URI for this Crazyflie
   std::string uri() const;
@@ -742,11 +749,21 @@ class CrazyflieBroadcaster
   CrazyflieBroadcaster(
     const std::string& link_uri);
 
+  const std::string& uri() const
+  {
+    return m_connection.uri();
+  }
+
   const bitcraze::crazyflieLinkCpp::Connection::Statistics connectionStats() const
   {
     return m_connection.statistics();
   }
 
+  bitcraze::crazyflieLinkCpp::Connection::Statistics connectionStatsDelta()
+  {
+    return m_connection.statisticsDelta();
+  }
+
   // High-Level setpoints
   void takeoff(float height, float duration, uint8_t groupMask = 0);
 

diff --git a/src/Crazyflie.cpp b/src/Crazyflie.cpp
@@ -46,16 +46,12 @@ std::vector<std::string> Crazyflie::scan(
   return bitcraze::crazyflieLinkCpp::Connection::scan(address);
 }
 
-std::string Crazyflie::uri() const
-{
-  return m_connection.uri();
-}
-
-std::string Crazyflie::broadcastUri() const
+std::string Crazyflie::broadcastUriFromUnicastUri(
+  const std::string& link_uri)
 {
   const std::regex uri_regex("radio:\\/\\/(\\d+|\\*)\\/(\\d+)\\/(250K|1M|2M)\\/([a-fA-F0-9]+)");
   std::smatch match;
-  if (!std::regex_match(m_connection.uri(), match, uri_regex))
+  if (!std::regex_match(link_uri, match, uri_regex))
   {
     // unsupported for broadcast
     return std::string();
@@ -64,6 +60,16 @@ std::string Crazyflie::broadcastUri() const
   return "radiobroadcast://*/" + match[2].str() + "/" + match[3].str();
 }
 
+std::string Crazyflie::uri() const
+{
+  return m_connection.uri();
+}
+
+std::string Crazyflie::broadcastUri() const
+{
+  return Crazyflie::broadcastUriFromUnicastUri(uri());
+}
+
 uint64_t Crazyflie::address() const
 {
   const std::regex uri_regex("radio:\\/\\/(\\d+|\\*)\\/(\\d+)\\/(250K|1M|2M)\\/([a-fA-F0-9]+)");
@@ -1101,12 +1107,49 @@ void Crazyflie::uploadTrajectory(
         req.setDataAt(0, reinterpret_cast<const uint8_t *>(pieces.data()) + i * 24, size);
         req.setDataSize(size);
         m_connection.send(req);
+
+        // wait for the response
+        using res = crtpMemoryWriteResponse;
+        auto p = waitForResponse(&res::valid);
+        if (   res::id(p) != entry.id
+            || res::address(p) != pieceOffset * sizeof(poly4d) + i*24
+            || res::status(p) != 0) {
+            m_logger.error("uploadTrajectory: unexpected response!" + std::to_string(res::status(p)));
+        }
+
         remainingBytes -= size;
       }
       // define trajectory
       crtpCommanderHighLevelDefineTrajectoryRequest req(trajectoryId);
       req.setPoly4d(pieceOffset * sizeof(poly4d), (uint8_t)pieces.size());
       m_connection.send(req);
+
+      // // verify
+      // remainingBytes = sizeof(poly4d) * pieces.size();
+      // numRequests = ceil(remainingBytes / 24.0f);
+      // for (size_t i = 0; i < numRequests; ++i) {
+      //   size_t size = std::min<size_t>(remainingBytes, 24);
+      //   crtpMemoryReadRequest req(entry.id, pieceOffset * sizeof(poly4d) + i*24, size);
+
+      //   m_connection.send(req);
+      //   using res = crtpMemoryReadResponse;
+      //   auto p = waitForResponse(&res::valid);
+      //   if (   res::id(p) != entry.id
+      //       || res::address(p) != pieceOffset * sizeof(poly4d) + i*24
+      //       || res::dataSize(p) != size
+      //       || res::status(p) != 0) {
+      //       m_logger.error("uploadTrajectory: unexpected response!");
+      //       return;
+      //   }
+
+      //   if (memcmp(reinterpret_cast<const uint8_t *>(pieces.data()) + i * 24, res::data(p), res::dataSize(p)) != 0) {
+      //       m_logger.error("uploadTrajectory: verify failed!");
+      //   }
+
+      //   remainingBytes -= size;
+      // }
+      // m_logger.info("upload & verify done!");
+
       return;
     }
   }