redundancy: Add communication timeout information

Signed-off-by: Jukka Laitinen <[email protected]>
tiiuae · Aug 9, 2024 · 8fb2ed4 · 8fb2ed4
1 parent b7c8be4
commit 8fb2ed4
Show file tree

Hide file tree

Showing 2 changed files with 18 additions and 3 deletions.
diff --git a/src/modules/redundancy/redundancy.cpp b/src/modules/redundancy/redundancy.cpp
@@ -177,10 +177,14 @@ void Redundancy::manage_spare_disarming()
 		landed--;
 	}
 
-	/* We have been landed the needed time, allowed to disarm as soon as primary is disarmed */
+	/* We have been landed the needed time, allowed to disarm as soon as primary is disarmed.
+	 * Primary FC timeout is handled as if it was disarmed. This enables auto-disarm
+	 * in case primary FC is completely died
+	 */
 
 	if (landed == 0 && armed &&
-	    _status[PRIMARY_FC_IDX].arming_state != vehicle_status_s::ARMING_STATE_ARMED) {
+	    (_status[PRIMARY_FC_IDX].arming_state != vehicle_status_s::ARMING_STATE_ARMED ||
+	     _autopilot_timeout[PRIMARY_FC_IDX])) {
 		PX4_INFO("Disarming as landed and primary disarmed");
 		force_disarm();
 		landed = -1;
@@ -200,6 +204,8 @@ void Redundancy::manage_spare()
 
 void Redundancy::Run()
 {
+	hrt_abstime now = hrt_absolute_time();
+
 	_landed_sub.copy(&_landed);
 
 	for (int i = 0; i < _n_autopilots; i++) {
@@ -211,6 +217,14 @@ void Redundancy::Run()
 			// This controller
 			_vehicle_status_sub.copy(&_status[i]);
 		}
+
+		if (_status[i].timestamp < now + 1_s) {
+			if (!_autopilot_timeout[i]) {
+				PX4_ERR("Controller %d timed out!\n", i);
+			}
+
+			_autopilot_timeout[i] = true;
+		}
 	}
 
 	if (_controller_idx == PRIMARY_FC_IDX) {

diff --git a/src/modules/redundancy/redundancy.hpp b/src/modules/redundancy/redundancy.hpp
@@ -68,11 +68,12 @@ class Redundancy final : public ModuleBase<Redundancy>, public px4::ScheduledWor
 	uORB::Subscription _redundant_status_sub[vehicle_status_s::MAX_REDUNDANT_CONTROLLERS] {ORB_ID(redundant_status0), ORB_ID(redundant_status1)};
 	vehicle_status_s _status[vehicle_status_s::MAX_REDUNDANT_CONTROLLERS];
 
-	uORB::Subscription _landed_sub{ORB_ID(vehicle_land_detected)};;
+	uORB::Subscription _landed_sub{ORB_ID(vehicle_land_detected)};
 	vehicle_land_detected_s _landed;
 
 	uORB::Publication<vehicle_command_s> _pub_vehicle_command{ORB_ID(vehicle_command)};
 
+	bool _autopilot_timeout[vehicle_status_s::MAX_REDUNDANT_CONTROLLERS] {};
 	int _n_autopilots;
 	int _controller_idx;
 	int _auto_disarm_min_time;