From 521b45c8dc62b0692b1d0eafc8f5c07f9d0e524e Mon Sep 17 00:00:00 2001
From: Jukka Laitinen <jukkax@ssrc.tii.ae>
Date: Tue, 6 Aug 2024 14:29:45 +0300
Subject: [PATCH] Add handling of heartbeats between redundant flight
 controllers

In case there are several FCs (autopilots) on a single drone, they can update
the basic status to each other via mavlink heartbeat messages

Signed-off-by: Jukka Laitinen <jukkax@ssrc.tii.ae>
---
 src/modules/mavlink/mavlink_receiver.cpp | 50 +++++++++++++++++++++++-
 src/modules/mavlink/mavlink_receiver.h   |  3 ++
 2 files changed, 52 insertions(+), 1 deletion(-)

diff --git a/src/modules/mavlink/mavlink_receiver.cpp b/src/modules/mavlink/mavlink_receiver.cpp
index 36958cbe69b2..95a690a3dd95 100644
--- a/src/modules/mavlink/mavlink_receiver.cpp
+++ b/src/modules/mavlink/mavlink_receiver.cpp
@@ -2078,8 +2078,56 @@ MavlinkReceiver::handle_message_heartbeat(mavlink_message_t *msg)
 		mavlink_msg_heartbeat_decode(msg, &hb);
 
 		const bool same_system = (msg->sysid == mavlink_system.sysid);
+		const bool redundant_fc = same_system && msg->compid >= MAV_COMP_ID_AUTOPILOT1
+					  && msg->compid < MAV_COMP_ID_AUTOPILOT1 + 4; // TODO: MAX_REDUNDANT_AUTOPILOTS
 
-		if (same_system || hb.type == MAV_TYPE_GCS) {
+		if (redundant_fc) {
+			int fc_idx = msg->compid - MAV_COMP_ID_AUTOPILOT1;
+
+			_redundant_status[fc_idx].system_id = msg->sysid;
+			_redundant_status[fc_idx].component_id = msg->compid;
+
+
+			/* publish the redundant status topic */
+
+			uint8_t arming_state;
+
+			switch (hb.system_status) {
+			case MAV_STATE_ACTIVE:
+			case MAV_STATE_CRITICAL:
+				arming_state = vehicle_status_s::ARMING_STATE_ARMED;
+				break;
+
+			case MAV_STATE_STANDBY:
+				arming_state = vehicle_status_s::ARMING_STATE_STANDBY;
+				break;
+
+			case MAV_STATE_EMERGENCY:
+			case MAV_STATE_FLIGHT_TERMINATION:
+				arming_state = vehicle_status_s::ARMING_STATE_STANDBY_ERROR;
+				break;
+
+			case MAV_STATE_POWEROFF:
+				arming_state = vehicle_status_s::ARMING_STATE_SHUTDOWN;
+				break;
+
+			default:
+				arming_state = vehicle_status_s::ARMING_STATE_INIT;
+				break;
+			}
+
+			_redundant_status[fc_idx].arming_state = arming_state;
+
+			_redundant_status[fc_idx].calibration_enabled = hb.system_status == MAV_STATE_CALIBRATING;
+
+			_redundant_status[fc_idx].hil_state = hb.base_mode & MAV_MODE_FLAG_HIL_ENABLED ? vehicle_status_s::HIL_STATE_ON :
+							      vehicle_status_s::HIL_STATE_OFF;
+
+			_redundant_status[fc_idx].timestamp = hrt_absolute_time();
+
+			_redundant_status_pub[fc_idx].publish(_redundant_status[fc_idx]);
+
+		} else if (same_system || hb.type == MAV_TYPE_GCS) {
 
 			camera_status_s camera_status{};
 
diff --git a/src/modules/mavlink/mavlink_receiver.h b/src/modules/mavlink/mavlink_receiver.h
index b47ca22c564b..48df60c3e59e 100644
--- a/src/modules/mavlink/mavlink_receiver.h
+++ b/src/modules/mavlink/mavlink_receiver.h
@@ -111,6 +111,7 @@
 #include <uORB/topics/vehicle_status.h>
 #include <uORB/topics/vehicle_trajectory_bezier.h>
 #include <uORB/topics/vehicle_trajectory_waypoint.h>
+#include <uORB/topics/vehicle_status.h>
 
 #if !defined(CONSTRAINED_FLASH)
 # include <uORB/topics/debug_array.h>
@@ -322,6 +323,8 @@ class MavlinkReceiver : public ModuleParams
 	uORB::Publication<vehicle_rates_setpoint_s>		_rates_sp_pub{ORB_ID(vehicle_rates_setpoint)};
 	uORB::Publication<vehicle_trajectory_bezier_s>		_trajectory_bezier_pub{ORB_ID(vehicle_trajectory_bezier)};
 	uORB::Publication<vehicle_trajectory_waypoint_s>	_trajectory_waypoint_pub{ORB_ID(vehicle_trajectory_waypoint)};
+	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] {};
 
 #if !defined(CONSTRAINED_FLASH)
 	uORB::Publication<debug_array_s>			_debug_array_pub {ORB_ID(debug_array)};