From bc397e61367ff359954007971ad5bf317c3bb617 Mon Sep 17 00:00:00 2001 From: Jukka Laitinen Date: Fri, 9 Aug 2024 14:51:45 +0300 Subject: [PATCH] redundancy: Add communication timeout information Signed-off-by: Jukka Laitinen --- src/modules/redundancy/redundancy.cpp | 18 ++++++++++++++++-- 1 file changed, 16 insertions(+), 2 deletions(-) diff --git a/src/modules/redundancy/redundancy.cpp b/src/modules/redundancy/redundancy.cpp index 513b0db053ae..ce1e50f2be08 100644 --- 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) {