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) { diff --git a/src/modules/redundancy/redundancy.hpp b/src/modules/redundancy/redundancy.hpp index b7b8ba50dc55..aab61fe57c7d 100644 --- a/src/modules/redundancy/redundancy.hpp +++ b/src/modules/redundancy/redundancy.hpp @@ -68,11 +68,12 @@ class Redundancy final : public ModuleBase, 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 _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;