diff --git a/Src/modules/feedback/cyphal/feedback.cpp b/Src/modules/feedback/cyphal/feedback.cpp index 34bb488..9911150 100644 --- a/Src/modules/feedback/cyphal/feedback.cpp +++ b/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(paramsGetIntegerValue(PARAM_PUB_FEEDBACK_1_ID)); - pub.setPortId(port_id); + udral_feedbacks[0].setPortId(static_cast(paramsGetIntegerValue(PARAM_PUB_FEEDBACK_1_ID))); + udral_feedbacks[1].setPortId(static_cast(paramsGetIntegerValue(PARAM_PUB_FEEDBACK_2_ID))); + udral_feedbacks[2].setPortId(static_cast(paramsGetIntegerValue(PARAM_PUB_FEEDBACK_3_ID))); + udral_feedbacks[3].setPortId(static_cast(paramsGetIntegerValue(PARAM_PUB_FEEDBACK_4_ID))); + + compact_feedbacks[0].setPortId(static_cast(paramsGetIntegerValue(PARAM_PUB_COMPACT_FEEDBACK_1_ID))); + compact_feedbacks[1].setPortId(static_cast(paramsGetIntegerValue(PARAM_PUB_COMPACT_FEEDBACK_2_ID))); + compact_feedbacks[2].setPortId(static_cast(paramsGetIntegerValue(PARAM_PUB_COMPACT_FEEDBACK_3_ID))); + compact_feedbacks[3].setPortId(static_cast(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((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(); + } } diff --git a/Src/modules/feedback/cyphal/feedback.hpp b/Src/modules/feedback/cyphal/feedback.hpp index 129623a..ad0ac9d 100644 --- a/Src/modules/feedback/cyphal/feedback.hpp +++ b/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 +#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_feedbacks; + std::array compact_feedbacks; }; -#ifdef __cplusplus -} -#endif - #endif // SRC_CYPHAL_APPLICATION_FEEDBACK_HPP_ diff --git a/Src/modules/feedback/cyphal/params.yaml b/Src/modules/feedback/cyphal/params.yaml index 67e6c5d..aa4b444 100644 --- a/Src/modules/feedback/cyphal/params.yaml +++ b/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 diff --git a/Src/modules/pwm/main.hpp b/Src/modules/pwm/main.hpp index 678d011..45c83b1 100644 --- a/Src/modules/pwm/main.hpp +++ b/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(HAL::PwmPin::PWM_AMOUNT); } diff --git a/docs/cyphal/README.md b/docs/cyphal/README.md index 4bd092f..2fac9b5 100644 --- a/docs/cyphal/README.md +++ b/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)
crct.5v | Voltage 5V (after DC-DC).
The normal values are within [4.5, 5.5] Volts.
| | [uavcan.si.sample.voltage.Scalar](https://github.com/OpenCyphal/public_regulated_data_types/blob/master/uavcan/si/sample/voltage/Scalar.1.0.dsdl)
crct.vin | Voltage Vin (before DC-DC).
It make sense only when you power up the node from Molex 6-pin connector.
The normal values are within [5.5, 55] Volts.
| | [uavcan.si.sample.temperature.Scalar](https://github.com/OpenCyphal/public_regulated_data_types/blob/master/uavcan/si/sample/temperature/Scalar.1.0.dsdl)
crct.temperature | STM32 internal temperature.
It it expected that the temperature is a little bit higher then your room environment.
| -| [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)
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)
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)
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)
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)
udral_feedback4 | | +| [zubax.telega.CompactFeedback.0.1](zubax.telega.CompactFeedback.0.1)
zubax_feedback1 | | +| [zubax.telega.CompactFeedback.0.1](zubax.telega.CompactFeedback.0.1)
zubax_feedback2 | | +| [zubax.telega.CompactFeedback.0.1](zubax.telega.CompactFeedback.0.1)
zubax_feedback3 | | +| [zubax.telega.CompactFeedback.0.1](zubax.telega.CompactFeedback.0.1)
zubax_feedback4 | | Cyphal Subscribers: | Data type and topic name | Description |