add sendDesCableAnglesSetpoint

include/crazyflie_cpp/Crazyflie.h

@@ -776,8 +776,8 @@ class CrazyflieBroadcaster
 
   void stop(uint8_t groupMask = 0);
 
-  // This is always in relative coordinates
-  void goTo(float x, float y, float z, float yaw, float duration, uint8_t groupMask = 0);
+  // using relative = false only makes sense for rare cases such as formation control
+  void goTo(float x, float y, float z, float yaw, float duration, bool relative = true, uint8_t groupMask = 0);
 
   // This is always in relative coordinates
   void startTrajectory(
@@ -835,6 +835,30 @@ class CrazyflieBroadcaster
     float qx, float qy, float qz, float qw,
     float rollRate, float pitchRate, float yawRate);
 
+  struct desCableAngles
+  {
+    uint8_t id;
+    float az;
+    float el;
+  };
+
+  void sendDesCableAnglesSetpoint(
+    const std::vector<desCableAngles>& data);
+
+  struct desCableStates
+  {
+    uint8_t id;
+    float mu_ref_x;
+    float mu_ref_y;
+    float mu_ref_z;
+    float qid_ref_x;
+    float qid_ref_y;
+    float qid_ref_z;
+  };
+
+  void sendDesCableStatesSetpoint(
+    const std::vector<desCableStates>& data);
+
 private:
   bitcraze::crazyflieLinkCpp::Connection m_connection;
 };

include/crazyflie_cpp/crtp.h

@@ -824,6 +824,60 @@ class crtpNotifySetpointsStopRequest
   }
 };
 
+class crtpDesCableAnglesSetpointRequest
+    : public bitcraze::crazyflieLinkCpp::Packet
+{
+public:
+  crtpDesCableAnglesSetpointRequest()
+      : Packet(0x07, 0, 1)
+  {
+    setPayloadAt<uint8_t>(0, 8);          // type
+  }
+
+  void add(uint8_t id, float az, float el)
+  {
+    uint8_t idx = payloadSize();
+    setPayloadSize(idx + 5);
+    setPayloadAt<uint8_t>(idx, id);
+    setPayloadAt<int16_t>(idx+1, az * 1000);
+    setPayloadAt<int16_t>(idx+3, el * 1000);
+  }
+
+  void clear()
+  {
+    setPayloadSize(1);
+  }
+};
+
+class crtpDesCableStatesSetpointRequest
+    : public bitcraze::crazyflieLinkCpp::Packet
+{
+public:
+  crtpDesCableStatesSetpointRequest()
+      : Packet(0x07, 0, 1)
+  {
+    setPayloadAt<uint8_t>(0, 9);          // type
+  }
+
+  void add(uint8_t id, float mu_ref_x, float mu_ref_y, float mu_ref_z, float qid_ref_x, float qid_ref_y, float qid_ref_z)
+  {
+    uint8_t idx = payloadSize();
+    setPayloadSize(idx + 13);
+    setPayloadAt<uint8_t>(idx, id);
+    setPayloadAt<int16_t>(idx+1, mu_ref_x * 1000);
+    setPayloadAt<int16_t>(idx+3, mu_ref_y * 1000);
+    setPayloadAt<int16_t>(idx+5, mu_ref_z * 1000);
+    setPayloadAt<int16_t>(idx+7, qid_ref_x * 1000);
+    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)
 
 class crtpCommanderHighLevelSetGroupMaskRequest

src/Crazyflie.cpp

@@ -14,7 +14,7 @@
 #include <cmath>
 #include <inttypes.h>
 #include <regex>
-
+#include <iostream>
 #define FIRMWARE_BUGGY
 
 Logger EmptyLogger;
@@ -1246,10 +1246,9 @@ void CrazyflieBroadcaster::stop(uint8_t groupMask)
   m_connection.send(req);
 }
 
-// This is always in relative coordinates
-void CrazyflieBroadcaster::goTo(float x, float y, float z, float yaw, float duration, uint8_t groupMask)
+void CrazyflieBroadcaster::goTo(float x, float y, float z, float yaw, float duration, bool relative, uint8_t groupMask)
 {
-  crtpCommanderHighLevelGoToRequest req(groupMask, true, x, y, z, yaw, duration);
+  crtpCommanderHighLevelGoToRequest req(groupMask, relative, x, y, z, yaw, duration);
   m_connection.send(req);
 }
 
@@ -1337,4 +1336,44 @@ void CrazyflieBroadcaster::sendFullStateSetpoint(
       qx, qy, qz, qw,
       rollRate, pitchRate, yawRate);
   m_connection.send(req);
+}
+
+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);
+  }
+}
+
+void CrazyflieBroadcaster::sendDesCableStatesSetpoint(
+    const std::vector<desCableStates>& data)
+{
+  crtpDesCableStatesSetpointRequest req;
+  size_t j = 0;
+  for (const auto entry : data) {
+    // std::cout << "s " << (int)entry.id << std::endl;
+    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);
+      std::this_thread::sleep_for(std::chrono::milliseconds(1));
+      req.clear();
+      j = 0;
+    }
+  }
+  if (j > 0) {
+    m_connection.send(req);
+  }
 }