From f60cbe38b2798201f542588d1feedd135ff07dcc Mon Sep 17 00:00:00 2001
From: Wolfgang Hoenig <hoenig@tu-berlin.de>
Date: Tue, 13 Feb 2024 06:25:32 +0100
Subject: [PATCH] sendDesCableStatesSetpoint: handle more than 2 CFs

---
 include/crazyflie_cpp/crtp.h | 10 ++++++++++
 src/Crazyflie.cpp            | 22 ++++++++++++++++++++--
 2 files changed, 30 insertions(+), 2 deletions(-)

diff --git a/include/crazyflie_cpp/crtp.h b/include/crazyflie_cpp/crtp.h
index 5416eea..5484edb 100644
--- a/include/crazyflie_cpp/crtp.h
+++ b/include/crazyflie_cpp/crtp.h
@@ -842,6 +842,11 @@ class crtpDesCableAnglesSetpointRequest
     setPayloadAt<int16_t>(idx+1, az * 1000);
     setPayloadAt<int16_t>(idx+3, el * 1000);
   }
+
+  void clear()
+  {
+    setPayloadSize(1);
+  }
 };
 
 class crtpDesCableStatesSetpointRequest
@@ -866,6 +871,11 @@ class crtpDesCableStatesSetpointRequest
     setPayloadAt<int16_t>(idx+9, qid_ref_y * 1000);
     setPayloadAt<int16_t>(idx+11, qid_ref_z * 1000);
   }
+
+  void clear()
+  {
+    setPayloadSize(1);
+  }
 };
 
 // Port 0x08 (High-level Setpoints)
diff --git a/src/Crazyflie.cpp b/src/Crazyflie.cpp
index b957534..514f0df 100644
--- a/src/Crazyflie.cpp
+++ b/src/Crazyflie.cpp
@@ -1330,18 +1330,36 @@ void CrazyflieBroadcaster::sendDesCableAnglesSetpoint(
     const std::vector<desCableAngles>& data)
 {
   crtpDesCableAnglesSetpointRequest req;
+  size_t j = 0;
   for (const auto entry : data) {
     req.add(entry.id, entry.az, entry.el);
+    ++j;
+    if (j==5) {
+      m_connection.send(req);
+      req.clear();
+      j = 0;
+    }
+  }
+  if (j > 0) {
+    m_connection.send(req);
   }
-  m_connection.send(req);
 }
 
 void CrazyflieBroadcaster::sendDesCableStatesSetpoint(
     const std::vector<desCableStates>& data)
 {
   crtpDesCableStatesSetpointRequest req;
+  size_t j = 0;
   for (const auto entry : data) {
     req.add(entry.id, entry.mu_ref_x, entry.mu_ref_y, entry.mu_ref_z, entry.qid_ref_x, entry.qid_ref_y, entry.qid_ref_z);
+    ++j;
+    if (j==2) {
+      m_connection.send(req);
+      req.clear();
+      j = 0;
+    }
+  }
+  if (j > 0) {
+    m_connection.send(req);
   }
-  m_connection.send(req);
 }
\ No newline at end of file