add esc feedback

RaccoonlabDev · Oct 8, 2023 · da85ade · da85ade
1 parent 7b243f9
commit da85ade
Show file tree

Hide file tree

Showing 2 changed files with 26 additions and 0 deletions.
diff --git 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
@@ -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_