sendDesCableStatesSetpoint: handle more than 2 CFs

whoenig · Feb 13, 2024 · f60cbe3 · f60cbe3
1 parent fcd6d14
commit f60cbe3
Show file tree

Hide file tree

Showing 2 changed files with 30 additions and 2 deletions.
diff --git 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
@@ -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);
 }