From 32ef0be3b3275fd939cafb928cb248e1363a99d1 Mon Sep 17 00:00:00 2001 From: Jukka Laitinen Date: Fri, 16 Aug 2024 09:45:02 +0300 Subject: [PATCH] mavlink / redundancy: Add receiving actuator_outputs from other FCs Signed-off-by: Jukka Laitinen --- msg/ActuatorOutputs.msg | 1 + src/modules/mavlink/mavlink_receiver.cpp | 27 ++++++++++++++++++++++++ src/modules/mavlink/mavlink_receiver.h | 2 ++ 3 files changed, 30 insertions(+) diff --git a/msg/ActuatorOutputs.msg b/msg/ActuatorOutputs.msg index 3209e54e34cc..82836ace2742 100644 --- a/msg/ActuatorOutputs.msg +++ b/msg/ActuatorOutputs.msg @@ -6,3 +6,4 @@ float32[16] output # output data, in natural output units # actuator_outputs_sim is used for SITL, HITL & SIH (with an output range of [-1, 1]) # TOPICS actuator_outputs actuator_outputs_sim actuator_outputs_debug +# TOPICS redundant_actuator_outputs0 redundant_actuator_outputs1 diff --git a/src/modules/mavlink/mavlink_receiver.cpp b/src/modules/mavlink/mavlink_receiver.cpp index f8568819461e..168645e1b74a 100644 --- a/src/modules/mavlink/mavlink_receiver.cpp +++ b/src/modules/mavlink/mavlink_receiver.cpp @@ -317,6 +317,10 @@ MavlinkReceiver::handle_message(mavlink_message_t *msg) handle_message_esc_status(msg); break; + case MAVLINK_MSG_ID_ACTUATOR_OUTPUT_STATUS: + handle_message_actuator_output_status(msg); + break; + default: break; } @@ -3141,6 +3145,29 @@ MavlinkReceiver::handle_message_esc_status(mavlink_message_t *msg) _esc_status_pub.publish(esc_status); } +void +MavlinkReceiver::handle_message_actuator_output_status(mavlink_message_t *msg) +{ + int fc_idx = msg->compid - MAV_COMP_ID_AUTOPILOT1; + mavlink_actuator_output_status_t actuator_output_status_msg; + mavlink_msg_actuator_output_status_decode(msg, &actuator_output_status_msg); + actuator_outputs_s actuator_outputs{}; + + if (fc_idx < vehicle_status_s::MAX_REDUNDANT_CONTROLLERS) { + actuator_outputs.timestamp = hrt_absolute_time(); + actuator_outputs.noutputs = actuator_output_status_msg.active; + static constexpr size_t mavlink_actuator_output_status_size = sizeof(actuator_output_status_msg.actuator) / sizeof( + actuator_output_status_msg.actuator[0]); + static constexpr size_t actuator_outputs_size = sizeof(actuator_outputs.output) / sizeof(actuator_outputs.output[0]); + + for (size_t i = 0; i < math::min(mavlink_actuator_output_status_size, actuator_outputs_size); i++) { + actuator_outputs.output[i] = actuator_output_status_msg.actuator[i]; + } + + _redundant_actuator_outputs_pub[fc_idx].publish(actuator_outputs); + } +} + void MavlinkReceiver::run() { diff --git a/src/modules/mavlink/mavlink_receiver.h b/src/modules/mavlink/mavlink_receiver.h index f9a0a4009c3c..b6cf27ceb151 100644 --- a/src/modules/mavlink/mavlink_receiver.h +++ b/src/modules/mavlink/mavlink_receiver.h @@ -205,6 +205,7 @@ class MavlinkReceiver : public ModuleParams void handle_message_gimbal_device_information(mavlink_message_t *msg); void handle_message_gimbal_device_attitude_status(mavlink_message_t *msg); void handle_message_esc_status(mavlink_message_t *msg); + void handle_message_actuator_output_status(mavlink_message_t *msg); #if !defined(CONSTRAINED_FLASH) void handle_message_debug(mavlink_message_t *msg); @@ -328,6 +329,7 @@ class MavlinkReceiver : public ModuleParams uORB::Publication _redundant_status_pub[vehicle_status_s::MAX_REDUNDANT_CONTROLLERS] {ORB_ID(redundant_status0), ORB_ID(redundant_status1)}; vehicle_status_s _redundant_status[vehicle_status_s::MAX_REDUNDANT_CONTROLLERS] {}; uORB::Publication _esc_status_pub{ORB_ID(esc_status)}; + uORB::Publication _redundant_actuator_outputs_pub[vehicle_status_s::MAX_REDUNDANT_CONTROLLERS] {ORB_ID(redundant_actuator_outputs0), ORB_ID(redundant_actuator_outputs1)}; #if !defined(CONSTRAINED_FLASH) uORB::Publication _debug_array_pub {ORB_ID(debug_array)};