From da85ade89b985153fdeec2540d22842493ccde1a Mon Sep 17 00:00:00 2001
From: PonomarevDA <ponomarevda96@gmail.com>
Date: Sun, 8 Oct 2023 19:10:19 +0300
Subject: [PATCH] add esc feedback

---
 Udral/actuator.cpp |  4 ++++
 Udral/actuator.hpp | 22 ++++++++++++++++++++++
 2 files changed, 26 insertions(+)

diff --git a/Udral/actuator.cpp b/Udral/actuator.cpp
index c1c8967..2ca8a14 100644
--- a/Udral/actuator.cpp
+++ b/Udral/actuator.cpp
@@ -71,3 +71,7 @@ void ReadinessSubscriber::callback(const CanardRxTransfer& transfer) {
     reg_udral_service_common_Readiness_0_1_deserialize_(&msg, payload, &payload_len);
     _last_recv_time_ms = HAL_GetTick();
 }
+
+void ZubaxCompactFeedbackPublisher::publish(const ZubaxCompactFeedback& msg) {
+    push(zubax_telega_CompactFeedback_0_1_SERIALIZATION_BUFFER_SIZE_BYTES, (const uint8_t*)&msg);
+}
diff --git a/Udral/actuator.hpp b/Udral/actuator.hpp
index 2738525..13d739f 100644
--- a/Udral/actuator.hpp
+++ b/Udral/actuator.hpp
@@ -10,6 +10,7 @@
 #include "reg/udral/service/actuator/common/sp/Vector4_0_1.h"
 #include "reg/udral/service/common/Readiness_0_1.h"
 
+
 struct SetpointSubscriber: public CyphalSubscriber {
     SetpointSubscriber(Cyphal* driver);
     int8_t init();
@@ -22,6 +23,7 @@ struct SetpointSubscriber: public CyphalSubscriber {
     uint32_t _recv_counter{0};
 };
 
+
 struct ReadinessSubscriber: public CyphalSubscriber {
     ReadinessSubscriber(Cyphal* driver_);
     int8_t init();
@@ -32,4 +34,24 @@ struct ReadinessSubscriber: public CyphalSubscriber {
     uint32_t _last_recv_time_ms{0};
 };
 
+
+// https://telega.zubax.com/interfaces/cyphal.html#compact
+#pragma pack(push, 1)
+struct ZubaxCompactFeedback {
+    uint32_t dc_voltage             : 11;
+    int32_t dc_current              : 12;
+    int32_t phase_current_amplitude : 12;
+    int32_t velocity                : 13;
+    int8_t demand_factor_pct        : 8;
+};
+#pragma pack(pop)
+
+struct ZubaxCompactFeedbackPublisher: public CyphalPublisher {
+    ZubaxCompactFeedbackPublisher(Cyphal* driver_, CanardPortID port_id) : CyphalPublisher(driver_, port_id) {};
+    void publish(const ZubaxCompactFeedback& msg);
+    static constexpr float RPM_TO_RAD_PER_SEC = 0.1047198;
+private:
+    static constexpr size_t zubax_telega_CompactFeedback_0_1_SERIALIZATION_BUFFER_SIZE_BYTES = 7;
+};
+
 #endif  // UDRAL_ACTUATOR_HPP_