update cyphal feedback

Src/modules/feedback/cyphal/feedback.cpp

@@ -9,29 +9,49 @@
 #include "cyphalNode/cyphal.hpp"
 #include "params.hpp"
 #include "peripheral/pwm/pwm.hpp"
+#include "peripheral/adc/circuit_periphery.hpp"
 
 REGISTER_MODULE(CyphalFeedbackModule)
 
-void FeedbackPublisher::publish() {
-    uint8_t buffer[reg_udral_service_actuator_common_Feedback_0_1_EXTENT_BYTES_];
-    size_t size = reg_udral_service_actuator_common_Feedback_0_1_EXTENT_BYTES_;
-    int32_t res = reg_udral_service_actuator_common_Feedback_0_1_serialize_(&msg, buffer, &size);
-    if (NUNAVUT_SUCCESS == res) {
-        push(size, buffer);
-    }
-}
 
 void CyphalFeedbackModule::update_params() {
-    auto port_id = static_cast<uint16_t>(paramsGetIntegerValue(PARAM_PUB_FEEDBACK_1_ID));
-    pub.setPortId(port_id);
+    udral_feedbacks[0].setPortId(static_cast<uint16_t>(paramsGetIntegerValue(PARAM_PUB_FEEDBACK_1_ID)));
+    udral_feedbacks[1].setPortId(static_cast<uint16_t>(paramsGetIntegerValue(PARAM_PUB_FEEDBACK_2_ID)));
+    udral_feedbacks[2].setPortId(static_cast<uint16_t>(paramsGetIntegerValue(PARAM_PUB_FEEDBACK_3_ID)));
+    udral_feedbacks[3].setPortId(static_cast<uint16_t>(paramsGetIntegerValue(PARAM_PUB_FEEDBACK_4_ID)));
+
+    compact_feedbacks[0].setPortId(static_cast<uint16_t>(paramsGetIntegerValue(PARAM_PUB_COMPACT_FEEDBACK_1_ID)));
+    compact_feedbacks[1].setPortId(static_cast<uint16_t>(paramsGetIntegerValue(PARAM_PUB_COMPACT_FEEDBACK_2_ID)));
+    compact_feedbacks[2].setPortId(static_cast<uint16_t>(paramsGetIntegerValue(PARAM_PUB_COMPACT_FEEDBACK_3_ID)));
+    compact_feedbacks[3].setPortId(static_cast<uint16_t>(paramsGetIntegerValue(PARAM_PUB_COMPACT_FEEDBACK_4_ID)));
 }
 
 void CyphalFeedbackModule::spin_once() {
-    pub.msg.heartbeat.health.value = uavcan_node_Health_1_0_NOMINAL;
-    pub.msg.heartbeat.readiness.value = reg_udral_service_common_Readiness_0_1_ENGAGED;
-    uint32_t pwm_ccr_reg_value = HAL::Pwm::get_duration(HAL::PwmPin::PWM_1);
-    uint32_t pwm_duration = std::clamp(pwm_ccr_reg_value, (uint32_t)1000, (uint32_t)2000);
-    pub.msg.demand_factor_pct = static_cast<int8_t>((pwm_duration - 1000) / 10);
+    for (size_t pin_idx = 0; pin_idx < PWMModule::get_pins_amount(); pin_idx++) {
+        auto& udral_feedback = udral_feedbacks[pin_idx];
+        if (!udral_feedback.isEnabled()) {
+            continue;
+        }
 
-    pub.publish();
+        udral_feedback.msg.heartbeat.health.value = uavcan_node_Health_1_0_NOMINAL;
+        udral_feedback.msg.heartbeat.readiness.value = reg_udral_service_common_Readiness_0_1_ENGAGED;
+        udral_feedback.msg.demand_factor_pct = PWMModule::get_pin_percent(pin_idx);
+        udral_feedback.publish();
+    }
+
+    for (size_t pin_idx = 0; pin_idx < PWMModule::get_pins_amount(); pin_idx++) {
+        auto& compact_feedback = compact_feedbacks[pin_idx];
+        if (!compact_feedback.isEnabled()) {
+            continue;
+        }
+
+        compact_feedback.msg = {
+            .dc_voltage = CircuitPeriphery::voltage_vin(),
+            .dc_current = CircuitPeriphery::current(),
+            .phase_current_amplitude = CircuitPeriphery::current(),
+            .velocity = 0,
+            .demand_factor_pct = (int8_t)PWMModule::get_pin_percent(pin_idx),
+        };
+        compact_feedback.publish();
+    }
 }

Src/modules/feedback/cyphal/feedback.hpp

@@ -8,35 +8,23 @@
 #define SRC_CYPHAL_APPLICATION_FEEDBACK_HPP_
 
 #include "cyphalNode/cyphal.hpp"
-#include "reg/udral/service/actuator/common/Feedback_0_1.h"
+#include <array>
+#include "cyphalNode/udral/feedback.hpp"
+#include "cyphalNode/zubax/compact_feedback.hpp"
+#include "modules/pwm/main.hpp"
 #include "module.hpp"
 
-#ifdef __cplusplus
-extern "C" {
-#endif
-
-struct FeedbackPublisher: public cyphal::CyphalPublisher {
-    FeedbackPublisher() : CyphalPublisher(cyphal::Cyphal::get_instance(), 65535) {}
-    int8_t init();
-    void publish();
-
-    reg_udral_service_actuator_common_Feedback_0_1 msg;
-};
-
 class CyphalFeedbackModule : public Module {
 public:
-    CyphalFeedbackModule() : Module(1, Protocol::CYPHAL) {}
+    CyphalFeedbackModule() : Module(10, Protocol::CYPHAL) {}
 
 protected:
     void update_params() override;
     void spin_once() override;
 
 private:
-    FeedbackPublisher pub;
+    std::array<udral::FeedbackPublisher, PWMModule::get_pins_amount()> udral_feedbacks;
+    std::array<zubax::telega::CompactFeedbackPublisher, PWMModule::get_pins_amount()> compact_feedbacks;
 };
 
-#ifdef __cplusplus
-}
-#endif
-
 #endif  // SRC_CYPHAL_APPLICATION_FEEDBACK_HPP_

Src/modules/feedback/cyphal/params.yaml

@@ -1,9 +1,39 @@
-# This is a short form that generates Integer & String registers related to the subscriber with PORT_NAME=feedback:
-# - Integer register with name `uavcan.pub.feedback.id`
-# - String register with name `uavcan.pub.feedback.type`
-# The generated registers have proper flags, data type and min, max and default values.
-# They correspond the standard: https://github.com/OpenCyphal/public_regulated_data_types/blob/master/uavcan/register/384.Access.1.0.dsdl
-uavcan.pub.feedback:
+uavcan.pub.udral_feedback1:
   type: Port
   data_type: reg.udral.service.actuator.common.Feedback.0.1
   enum_base: PARAM_PUB_FEEDBACK_1
+
+uavcan.pub.udral_feedback2:
+  type: Port
+  data_type: reg.udral.service.actuator.common.Feedback.0.1
+  enum_base: PARAM_PUB_FEEDBACK_2
+
+uavcan.pub.udral_feedback3:
+  type: Port
+  data_type: reg.udral.service.actuator.common.Feedback.0.1
+  enum_base: PARAM_PUB_FEEDBACK_3
+
+uavcan.pub.udral_feedback4:
+  type: Port
+  data_type: reg.udral.service.actuator.common.Feedback.0.1
+  enum_base: PARAM_PUB_FEEDBACK_4
+
+uavcan.pub.zubax_feedback1:
+  type: Port
+  data_type: zubax.telega.CompactFeedback.0.1
+  enum_base: PARAM_PUB_COMPACT_FEEDBACK_1
+
+uavcan.pub.zubax_feedback2:
+  type: Port
+  data_type: zubax.telega.CompactFeedback.0.1
+  enum_base: PARAM_PUB_COMPACT_FEEDBACK_2
+
+uavcan.pub.zubax_feedback3:
+  type: Port
+  data_type: zubax.telega.CompactFeedback.0.1
+  enum_base: PARAM_PUB_COMPACT_FEEDBACK_3
+
+uavcan.pub.zubax_feedback4:
+  type: Port
+  data_type: zubax.telega.CompactFeedback.0.1
+  enum_base: PARAM_PUB_COMPACT_FEEDBACK_4

Src/modules/pwm/main.hpp

@@ -39,7 +39,7 @@ class PWMModule : public Module {
     PWMModule() : Module(50, Protocol::CYPHAL_AND_DRONECAN) {}
     void init() override;
 
-    static inline constexpr uint8_t get_pins_amount() {
+    static constexpr uint8_t get_pins_amount() {
         return static_cast<uint8_t>(HAL::PwmPin::PWM_AMOUNT);
     }
 

docs/cyphal/README.md

@@ -6,7 +6,14 @@ Cyphal Publishers:
 | [uavcan.si.sample.voltage.Scalar](https://github.com/OpenCyphal/public_regulated_data_types/blob/master/uavcan/si/sample/voltage/Scalar.1.0.dsdl) </br> crct.5v | Voltage 5V (after DC-DC).</br>The normal values are within [4.5, 5.5] Volts.</br>|
 | [uavcan.si.sample.voltage.Scalar](https://github.com/OpenCyphal/public_regulated_data_types/blob/master/uavcan/si/sample/voltage/Scalar.1.0.dsdl) </br> crct.vin | Voltage Vin (before DC-DC).</br>It make sense only when you power up the node from Molex 6-pin connector.</br>The normal values are within [5.5, 55] Volts.</br>|
 | [uavcan.si.sample.temperature.Scalar](https://github.com/OpenCyphal/public_regulated_data_types/blob/master/uavcan/si/sample/temperature/Scalar.1.0.dsdl) </br> crct.temperature | STM32 internal temperature.</br>It it expected that the temperature is a little bit higher then your room environment.</br>|
-| [reg.udral.service.actuator.common.Feedback](https://github.com/OpenCyphal/public_regulated_data_types/blob/master/reg/udral/service/actuator/common/Feedback.0.1.dsdl) </br> feedback | |
+| [reg.udral.service.actuator.common.Feedback](https://github.com/OpenCyphal/public_regulated_data_types/blob/master/reg/udral/service/actuator/common/Feedback.0.1.dsdl) </br> udral_feedback1 | |
+| [reg.udral.service.actuator.common.Feedback](https://github.com/OpenCyphal/public_regulated_data_types/blob/master/reg/udral/service/actuator/common/Feedback.0.1.dsdl) </br> udral_feedback2 | |
+| [reg.udral.service.actuator.common.Feedback](https://github.com/OpenCyphal/public_regulated_data_types/blob/master/reg/udral/service/actuator/common/Feedback.0.1.dsdl) </br> udral_feedback3 | |
+| [reg.udral.service.actuator.common.Feedback](https://github.com/OpenCyphal/public_regulated_data_types/blob/master/reg/udral/service/actuator/common/Feedback.0.1.dsdl) </br> udral_feedback4 | |
+| [zubax.telega.CompactFeedback.0.1](zubax.telega.CompactFeedback.0.1) </br> zubax_feedback1 | |
+| [zubax.telega.CompactFeedback.0.1](zubax.telega.CompactFeedback.0.1) </br> zubax_feedback2 | |
+| [zubax.telega.CompactFeedback.0.1](zubax.telega.CompactFeedback.0.1) </br> zubax_feedback3 | |
+| [zubax.telega.CompactFeedback.0.1](zubax.telega.CompactFeedback.0.1) </br> zubax_feedback4 | |
 
 Cyphal Subscribers:
 | Data type and topic name  | Description |