diff --git a/src/drivers/uavcan_v1/Actuators/EscClient.hpp b/src/drivers/uavcan_v1/Actuators/EscClient.hpp index 3be32fac9a..348680de71 100644 --- a/src/drivers/uavcan_v1/Actuators/EscClient.hpp +++ b/src/drivers/uavcan_v1/Actuators/EscClient.hpp @@ -129,10 +129,19 @@ public: if (_port_id > 0) { reg_drone_service_actuator_common_sp_Vector31_0_1 msg_sp {0}; - for (uint8_t i = 0; i < num_outputs; i++) { - msg_sp.value[i] = outputs[i]; + for (uint8_t i = 0; i < MAX_ACTUATORS; i++) { + if (i < num_outputs) { + msg_sp.value[i] = static_cast(outputs[i]); + + } else { + // "unset" values published as NaN + msg_sp.value[i] = NAN; + } } + PX4_INFO("Publish %d values %f, %f, %f, %f", num_outputs, (double)msg_sp.value[0], (double)msg_sp.value[1], + (double)msg_sp.value[2], (double)msg_sp.value[3]); + uint8_t esc_sp_payload_buffer[reg_drone_service_actuator_common_sp_Vector31_0_1_SERIALIZATION_BUFFER_SIZE_BYTES_]; CanardTransfer transfer = { diff --git a/src/drivers/uavcan_v1/ParamManager.hpp b/src/drivers/uavcan_v1/ParamManager.hpp index 2d3b212ce6..6d99174230 100644 --- a/src/drivers/uavcan_v1/ParamManager.hpp +++ b/src/drivers/uavcan_v1/ParamManager.hpp @@ -67,7 +67,7 @@ private: {"uavcan.pub.esc.0.id", "UCAN1_ESC_PUB"}, {"uavcan.pub.servo.0.id", "UCAN1_SERVO_PUB"}, {"uavcan.pub.gps.0.id", "UCAN1_GPS_PUB"}, - {"uavcan.sub.esc.0.id", "UCAN1_ESC_PID"}, + {"uavcan.sub.esc.0.id", "UCAN1_ESC0_PID"}, {"uavcan.sub.gps.0.id", "UCAN1_GPS0_PID"}, {"uavcan.sub.gps.1.id", "UCAN1_GPS1_PID"}, {"uavcan.sub.bms.0.id", "UCAN1_BMS0_PID"}, diff --git a/src/drivers/uavcan_v1/Subscribers/Esc.hpp b/src/drivers/uavcan_v1/Subscribers/Esc.hpp index 59c3e2940e..36e4dafa64 100644 --- a/src/drivers/uavcan_v1/Subscribers/Esc.hpp +++ b/src/drivers/uavcan_v1/Subscribers/Esc.hpp @@ -42,8 +42,12 @@ #pragma once +/// For use with PR-16808 once merged +// #include + // DS-15 Specification Messages #include +#include #include "Subscriber.hpp" @@ -63,15 +67,20 @@ public: CANARD_DEFAULT_TRANSFER_ID_TIMEOUT_USEC, &_canard_sub); - /** TODO: Add additional ESC-service messages: reg.drone.service.common.Readiness - */ + // Subscribe to messages reg.drone.service.common.Readiness.0.1 + canardRxSubscribe(&_canard_instance, + CanardTransferKindMessage, + static_cast(static_cast(_port_id) + 1), + reg_drone_service_common_Readiness_0_1_EXTENT_BYTES_, + CANARD_DEFAULT_TRANSFER_ID_TIMEOUT_USEC, + &_canard_sub_readiness); }; void callback(const CanardTransfer &receive) override { // Test with Yakut: // export YAKUT_TRANSPORT="pyuavcan.transport.can.CANTransport(pyuavcan.transport.can.media.slcan.SLCANMedia('/dev/serial/by-id/usb-Zubax_Robotics_Zubax_Babel_23002B000E514E413431302000000000-if00', 8, 115200), 42)" - // yakut pub 22.reg.drone.service.actuator.common.sp.Vector8.0.1 '{value: {1000, 2000, 3000, 4000}, longitude: 2.34, altitude: {meter: 0.5}}' + // yakut pub 22.reg.drone.service.actuator.common.sp.Vector8.0.1 '{value: [1000, 2000, 3000, 4000, 0, 0, 0, 0]}' PX4_INFO("EscCallback"); reg_drone_service_actuator_common_sp_Vector8_0_1 esc {}; @@ -86,7 +95,19 @@ public: PX4_INFO("values[0-3] = {%f, %f, %f, %f}", val1, val2, val3, val4); /// do something with the data - /// TODO: Publish to output_control_mc + /// For use with PR-16808 once merged + // output_control_s outputs; + + // for (uint8_t i = 0; i < 8; i++) { + // outputs.value[i] = 2.f * (esc.value[i] / 8191.f) - 1.f; + // } + + // _output_pub.publish(outputs); }; +private: + /// For use with PR-16808 once merged + // uORB::Publication _output_pub{ORB_ID(output_control_mc)}; + + CanardRxSubscription _canard_sub_readiness; }; diff --git a/src/drivers/uavcan_v1/Uavcan.cpp b/src/drivers/uavcan_v1/Uavcan.cpp index 23ac0b311d..d22f6b4753 100644 --- a/src/drivers/uavcan_v1/Uavcan.cpp +++ b/src/drivers/uavcan_v1/Uavcan.cpp @@ -90,6 +90,8 @@ UavcanNode::UavcanNode(CanardInterface *interface, uint32_t node_id) : for (auto &subscriber : _subscribers) { subscriber->updateParam(); } + + _mixing_output.mixingOutput().updateSubscriptions(false, false); } UavcanNode::~UavcanNode() @@ -245,6 +247,10 @@ void UavcanNode::Run() // Setting the port-id to 0 disables the subscription subscriber->updateParam(); } + + _mixing_output.updateParams(); + + _mixing_output.mixingOutput().updateSubscriptions(false, false); } perf_begin(_cycle_perf); @@ -341,7 +347,7 @@ void UavcanNode::Run() } else if (result == 1) { // A transfer has been received, process it. - PX4_INFO("received Port ID: %d", receive.port_id); + // PX4_INFO("received Port ID: %d", receive.port_id); if (receive.port_id == uavcan_pnp_NodeIDAllocationData_1_0_FIXED_PORT_ID_) { result = handlePnpNodeIDAllocationData(receive); @@ -495,15 +501,6 @@ void UavcanNode::sendHeartbeat() } _uavcan_node_heartbeat_last = transfer.timestamp_usec; - - /// TESTING -- Remove before flight! - // Since we don't have a mixer file here yet - uint16_t outputs[8] {}; - outputs[0] = 1000; - outputs[1] = 2000; - outputs[2] = 3000; - outputs[3] = 4000; - _mixing_output.updateOutputs(false, outputs, 4, 1); } } @@ -690,9 +687,6 @@ int UavcanNode::handleRegisterAccess(const CanardTransfer &receive) } - /// TODO: I'm getting the following error: - /// "error: the frame size of 2448 bytes is larger than 2048 bytes [-Werror=frame-larger-than=]" - /// TODO: Access_Response uavcan_register_Access_Response_1_0 response {}; response.value = value; @@ -764,6 +758,6 @@ void UavcanMixingInterface::Run() { pthread_mutex_lock(&_node_mutex); _mixing_output.update(); - _mixing_output.updateSubscriptions(false); + _mixing_output.updateSubscriptions(false, false); pthread_mutex_unlock(&_node_mutex); } diff --git a/src/drivers/uavcan_v1/Uavcan.hpp b/src/drivers/uavcan_v1/Uavcan.hpp index 85e83f8ebc..a62366a50d 100644 --- a/src/drivers/uavcan_v1/Uavcan.hpp +++ b/src/drivers/uavcan_v1/Uavcan.hpp @@ -114,6 +114,9 @@ public: MixingOutput &mixingOutput() { return _mixing_output; } + /// For use with PR-16808 once merged + // const char *get_param_prefix() override { return "UCAN1_ACT"; } + protected: void Run() override; private: diff --git a/src/drivers/uavcan_v1/parameters.c b/src/drivers/uavcan_v1/parameters.c index 6c5335a62d..2b0eba1c62 100644 --- a/src/drivers/uavcan_v1/parameters.c +++ b/src/drivers/uavcan_v1/parameters.c @@ -87,6 +87,7 @@ PARAM_DEFINE_INT32(UAVCAN_V1_BAT_MD, 0); PARAM_DEFINE_INT32(UAVCAN_V1_BAT_ID, 4242); // Subscription Port IDs +PARAM_DEFINE_INT32(UCAN1_ESC0_PID, 0); PARAM_DEFINE_INT32(UCAN1_GPS0_PID, 0); PARAM_DEFINE_INT32(UCAN1_GPS1_PID, 0); PARAM_DEFINE_INT32(UCAN1_BMS0_PID, 0);