mavlink / redundancy: Add receiving actuator_outputs from other FCs

Signed-off-by: Jukka Laitinen <[email protected]>

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

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()
 {

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<vehicle_status_s>			_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_s>				_esc_status_pub{ORB_ID(esc_status)};
+	uORB::Publication<actuator_outputs_s>			_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_s>			_debug_array_pub {ORB_ID(debug_array)};