From c673ed3d95dc6992d8e4364cc361a1bdd2276d71 Mon Sep 17 00:00:00 2001 From: Wolfgang Hoenig Date: Thu, 23 Mar 2023 07:45:54 +0100 Subject: [PATCH 1/7] add sendDesCableAnglesSetpoint --- include/crazyflie_cpp/Crazyflie.h | 10 ++++++++++ include/crazyflie_cpp/crtp.h | 20 ++++++++++++++++++++ src/Crazyflie.cpp | 10 ++++++++++ 3 files changed, 40 insertions(+) diff --git a/include/crazyflie_cpp/Crazyflie.h b/include/crazyflie_cpp/Crazyflie.h index 77578ba..ca662fe 100644 --- a/include/crazyflie_cpp/Crazyflie.h +++ b/include/crazyflie_cpp/Crazyflie.h @@ -800,6 +800,16 @@ 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& data); + private: bitcraze::crazyflieLinkCpp::Connection m_connection; }; diff --git a/include/crazyflie_cpp/crtp.h b/include/crazyflie_cpp/crtp.h index f8ceb1f..15a8f6d 100644 --- a/include/crazyflie_cpp/crtp.h +++ b/include/crazyflie_cpp/crtp.h @@ -819,6 +819,26 @@ class crtpNotifySetpointsStopRequest } }; +class crtpDesCableAnglesSetpointRequest + : public bitcraze::crazyflieLinkCpp::Packet +{ +public: + crtpDesCableAnglesSetpointRequest() + : Packet(0x07, 0, 1) + { + setPayloadAt(0, 8); // type + } + + void add(uint8_t id, float az, float el) + { + uint8_t idx = payloadSize(); + setPayloadSize(idx + 5); + setPayloadAt(idx, id); + setPayloadAt(idx+1, az * 1000); + setPayloadAt(idx+3, el * 1000); + } +}; + // Port 0x08 (High-level Setpoints) class crtpCommanderHighLevelSetGroupMaskRequest diff --git a/src/Crazyflie.cpp b/src/Crazyflie.cpp index 0409dd0..529539b 100644 --- a/src/Crazyflie.cpp +++ b/src/Crazyflie.cpp @@ -1249,4 +1249,14 @@ void CrazyflieBroadcaster::sendFullStateSetpoint( qx, qy, qz, qw, rollRate, pitchRate, yawRate); m_connection.send(req); +} + +void CrazyflieBroadcaster::sendDesCableAnglesSetpoint( + const std::vector& data) +{ + crtpDesCableAnglesSetpointRequest req; + for (const auto entry : data) { + req.add(entry.id, entry.az, entry.el); + } + m_connection.send(req); } \ No newline at end of file From e6b85c57f311f4ad96cd4231ff9600808cfeb057 Mon Sep 17 00:00:00 2001 From: Khaledwahba1994 Date: Tue, 5 Dec 2023 12:19:47 +0100 Subject: [PATCH 2/7] add sendDesCableStatesSetpoint --- include/crazyflie_cpp/Crazyflie.h | 14 ++++++++++++++ include/crazyflie_cpp/crtp.h | 28 ++++++++++++++++++++++++++-- src/Crazyflie.cpp | 10 ++++++++++ 3 files changed, 50 insertions(+), 2 deletions(-) diff --git a/include/crazyflie_cpp/Crazyflie.h b/include/crazyflie_cpp/Crazyflie.h index ca662fe..a782c4b 100644 --- a/include/crazyflie_cpp/Crazyflie.h +++ b/include/crazyflie_cpp/Crazyflie.h @@ -810,6 +810,20 @@ class CrazyflieBroadcaster void sendDesCableAnglesSetpoint( const std::vector& 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& data); + private: bitcraze::crazyflieLinkCpp::Connection m_connection; }; diff --git a/include/crazyflie_cpp/crtp.h b/include/crazyflie_cpp/crtp.h index 15a8f6d..f8121d3 100644 --- a/include/crazyflie_cpp/crtp.h +++ b/include/crazyflie_cpp/crtp.h @@ -834,8 +834,32 @@ class crtpDesCableAnglesSetpointRequest uint8_t idx = payloadSize(); setPayloadSize(idx + 5); setPayloadAt(idx, id); - setPayloadAt(idx+1, az * 1000); - setPayloadAt(idx+3, el * 1000); + setPayloadAt(idx+1, az * 1000); + setPayloadAt(idx+3, el * 1000); + } +}; + +class crtpDesCableStatesSetpointRequest + : public bitcraze::crazyflieLinkCpp::Packet +{ +public: + crtpDesCableStatesSetpointRequest() + : Packet(0x07, 0, 1) + { + setPayloadAt(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(idx, id); + setPayloadAt(idx+1, mu_ref_x * 1000); + setPayloadAt(idx+3, mu_ref_y * 1000); + setPayloadAt(idx+5, mu_ref_z * 1000); + setPayloadAt(idx+7, qid_ref_x * 1000); + setPayloadAt(idx+9, qid_ref_y * 1000); + setPayloadAt(idx+11, qid_ref_z * 1000); } }; diff --git a/src/Crazyflie.cpp b/src/Crazyflie.cpp index 529539b..89be451 100644 --- a/src/Crazyflie.cpp +++ b/src/Crazyflie.cpp @@ -1259,4 +1259,14 @@ void CrazyflieBroadcaster::sendDesCableAnglesSetpoint( req.add(entry.id, entry.az, entry.el); } m_connection.send(req); +} + +void CrazyflieBroadcaster::sendDesCableStatesSetpoint( + const std::vector& data) +{ + crtpDesCableStatesSetpointRequest req; + 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); + } + m_connection.send(req); } \ No newline at end of file From fcd6d149908f8c37657c4f16a4a77c71509c7647 Mon Sep 17 00:00:00 2001 From: Wolfgang Hoenig Date: Wed, 7 Feb 2024 16:35:22 +0100 Subject: [PATCH 3/7] update submodule --- crazyflie-link-cpp | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/crazyflie-link-cpp b/crazyflie-link-cpp index 6250dbb..e69c0e8 160000 --- a/crazyflie-link-cpp +++ b/crazyflie-link-cpp @@ -1 +1 @@ -Subproject commit 6250dbb13cd3f40914517aa49d9fabd9bec555e9 +Subproject commit e69c0e8f01b84f57898ace56c7e250c085ddf164 From f60cbe38b2798201f542588d1feedd135ff07dcc Mon Sep 17 00:00:00 2001 From: Wolfgang Hoenig Date: Tue, 13 Feb 2024 06:25:32 +0100 Subject: [PATCH 4/7] 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(idx+1, az * 1000); setPayloadAt(idx+3, el * 1000); } + + void clear() + { + setPayloadSize(1); + } }; class crtpDesCableStatesSetpointRequest @@ -866,6 +871,11 @@ class crtpDesCableStatesSetpointRequest setPayloadAt(idx+9, qid_ref_y * 1000); setPayloadAt(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& 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& 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 From 6afee2c67e0ebd6da5716b43648fa6d7dbbed495 Mon Sep 17 00:00:00 2001 From: Khaledwahba1994 Date: Wed, 14 Feb 2024 08:56:12 +0100 Subject: [PATCH 5/7] add delay before sending the message --- src/Crazyflie.cpp | 4 +++- 1 file changed, 3 insertions(+), 1 deletion(-) diff --git a/src/Crazyflie.cpp b/src/Crazyflie.cpp index 514f0df..ffddf3e 100644 --- a/src/Crazyflie.cpp +++ b/src/Crazyflie.cpp @@ -14,7 +14,7 @@ #include #include #include - +#include #define FIRMWARE_BUGGY Logger EmptyLogger; @@ -1351,10 +1351,12 @@ void CrazyflieBroadcaster::sendDesCableStatesSetpoint( 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; } From ca2bd07331ea87d0fe73d7c7ca8eaf6830d7cba4 Mon Sep 17 00:00:00 2001 From: Wolfgang Hoenig Date: Mon, 2 Dec 2024 21:52:14 +0100 Subject: [PATCH 6/7] update submodule --- crazyflie-link-cpp | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/crazyflie-link-cpp b/crazyflie-link-cpp index c595ac0..8ac2671 160000 --- a/crazyflie-link-cpp +++ b/crazyflie-link-cpp @@ -1 +1 @@ -Subproject commit c595ac09109b62d307088312fea93eed494739df +Subproject commit 8ac2671b6d283b5baee4d0da6a65840f8dfe5eb8 From 1b0e21c920d4873374babcdbba307088c6ebcdfd Mon Sep 17 00:00:00 2001 From: Wolfgang Hoenig Date: Mon, 2 Dec 2024 22:04:10 +0100 Subject: [PATCH 7/7] broadcaster goTo: allow absolute coordinates --- include/crazyflie_cpp/Crazyflie.h | 4 ++-- src/Crazyflie.cpp | 5 ++--- 2 files changed, 4 insertions(+), 5 deletions(-) diff --git a/include/crazyflie_cpp/Crazyflie.h b/include/crazyflie_cpp/Crazyflie.h index f397df9..78bda0f 100644 --- a/include/crazyflie_cpp/Crazyflie.h +++ b/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( diff --git a/src/Crazyflie.cpp b/src/Crazyflie.cpp index 48b657f..7e9fd9e 100644 --- a/src/Crazyflie.cpp +++ b/src/Crazyflie.cpp @@ -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); }