From 9ba0fc471ba7be5ea8bffe06d3ab5fd688549049 Mon Sep 17 00:00:00 2001 From: bresch Date: Fri, 9 Aug 2024 14:12:23 +0200 Subject: [PATCH] comander: trigger failsafe when navigator reports failure --- msg/FailsafeFlags.msg | 1 + .../HealthAndArmingChecks/CMakeLists.txt | 1 + .../HealthAndArmingChecks.hpp | 3 ++ .../checks/navigatorCheck.cpp | 47 +++++++++++++++++ .../checks/navigatorCheck.hpp | 51 +++++++++++++++++++ src/modules/commander/failsafe/failsafe.cpp | 8 +++ src/modules/navigator/rtl_direct.cpp | 4 +- 7 files changed, 113 insertions(+), 2 deletions(-) create mode 100644 src/modules/commander/HealthAndArmingChecks/checks/navigatorCheck.cpp create mode 100644 src/modules/commander/HealthAndArmingChecks/checks/navigatorCheck.hpp diff --git a/msg/FailsafeFlags.msg b/msg/FailsafeFlags.msg index 2cd31bf83598..de514fb2db4b 100644 --- a/msg/FailsafeFlags.msg +++ b/msg/FailsafeFlags.msg @@ -49,6 +49,7 @@ bool vtol_fixed_wing_system_failure # vehicle in fixed-wing system failure fai bool wind_limit_exceeded # Wind limit exceeded bool flight_time_limit_exceeded # Maximum flight time exceeded bool local_position_accuracy_low # Local position estimate has dropped below threshold, but is currently still declared valid +bool navigator_failure # Navigator failed to execute a mode # Failure detector bool fd_critical_failure # Critical failure (attitude/altitude limit exceeded, or external ATS) diff --git a/src/modules/commander/HealthAndArmingChecks/CMakeLists.txt b/src/modules/commander/HealthAndArmingChecks/CMakeLists.txt index 7c78c47c8534..7265e042b4c0 100644 --- a/src/modules/commander/HealthAndArmingChecks/CMakeLists.txt +++ b/src/modules/commander/HealthAndArmingChecks/CMakeLists.txt @@ -55,6 +55,7 @@ px4_add_library(health_and_arming_checks checks/manualControlCheck.cpp checks/missionCheck.cpp checks/modeCheck.cpp + checks/navigatorCheck.cpp checks/offboardCheck.cpp checks/openDroneIDCheck.cpp checks/parachuteCheck.cpp diff --git a/src/modules/commander/HealthAndArmingChecks/HealthAndArmingChecks.hpp b/src/modules/commander/HealthAndArmingChecks/HealthAndArmingChecks.hpp index af0fd9aec5f0..fdf38f5d5d77 100644 --- a/src/modules/commander/HealthAndArmingChecks/HealthAndArmingChecks.hpp +++ b/src/modules/commander/HealthAndArmingChecks/HealthAndArmingChecks.hpp @@ -49,6 +49,7 @@ #include "checks/escCheck.hpp" #include "checks/estimatorCheck.hpp" #include "checks/failureDetectorCheck.hpp" +#include "checks/navigatorCheck.hpp" #include "checks/gyroCheck.hpp" #include "checks/imuConsistencyCheck.hpp" #include "checks/loggerCheck.hpp" @@ -129,6 +130,7 @@ class HealthAndArmingChecks : public ModuleParams EscChecks _esc_checks; EstimatorChecks _estimator_checks; FailureDetectorChecks _failure_detector_checks; + NavigatorChecks _navigator_checks; GyroChecks _gyro_checks; ImuConsistencyChecks _imu_consistency_checks; LoggerChecks _logger_checks; @@ -167,6 +169,7 @@ class HealthAndArmingChecks : public ModuleParams &_esc_checks, &_estimator_checks, &_failure_detector_checks, + &_navigator_checks, &_gyro_checks, &_imu_consistency_checks, &_logger_checks, diff --git a/src/modules/commander/HealthAndArmingChecks/checks/navigatorCheck.cpp b/src/modules/commander/HealthAndArmingChecks/checks/navigatorCheck.cpp new file mode 100644 index 000000000000..8c7d0e314af4 --- /dev/null +++ b/src/modules/commander/HealthAndArmingChecks/checks/navigatorCheck.cpp @@ -0,0 +1,47 @@ +/**************************************************************************** + * + * Copyright (c) 2024 PX4 Development Team. All rights reserved. + * + * Redistribution and use in source and binary forms, with or without + * modification, are permitted provided that the following conditions + * are met: + * + * 1. Redistributions of source code must retain the above copyright + * notice, this list of conditions and the following disclaimer. + * 2. Redistributions in binary form must reproduce the above copyright + * notice, this list of conditions and the following disclaimer in + * the documentation and/or other materials provided with the + * distribution. + * 3. Neither the name PX4 nor the names of its contributors may be + * used to endorse or promote products derived from this software + * without specific prior written permission. + * + * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS + * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT + * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS + * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE + * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, + * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, + * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS + * OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED + * AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT + * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN + * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE + * POSSIBILITY OF SUCH DAMAGE. + * + ****************************************************************************/ + +#include "navigatorCheck.hpp" + +void NavigatorChecks::checkAndReport(const Context &context, Report &reporter) +{ + navigator_status_s status; + + if (!_navigator_status_sub.copy(&status)) { + status = {}; + } + + if (context.status().nav_state == status.nav_state) { + reporter.failsafeFlags().navigator_failure = status.failure; + } +} diff --git a/src/modules/commander/HealthAndArmingChecks/checks/navigatorCheck.hpp b/src/modules/commander/HealthAndArmingChecks/checks/navigatorCheck.hpp new file mode 100644 index 000000000000..48c6965deaf8 --- /dev/null +++ b/src/modules/commander/HealthAndArmingChecks/checks/navigatorCheck.hpp @@ -0,0 +1,51 @@ +/**************************************************************************** + * + * Copyright (c) 2024 PX4 Development Team. All rights reserved. + * + * Redistribution and use in source and binary forms, with or without + * modification, are permitted provided that the following conditions + * are met: + * + * 1. Redistributions of source code must retain the above copyright + * notice, this list of conditions and the following disclaimer. + * 2. Redistributions in binary form must reproduce the above copyright + * notice, this list of conditions and the following disclaimer in + * the documentation and/or other materials provided with the + * distribution. + * 3. Neither the name PX4 nor the names of its contributors may be + * used to endorse or promote products derived from this software + * without specific prior written permission. + * + * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS + * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT + * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS + * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE + * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, + * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, + * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS + * OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED + * AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT + * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN + * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE + * POSSIBILITY OF SUCH DAMAGE. + * + ****************************************************************************/ + +#pragma once + +#include "../Common.hpp" +#include +#include + + +class NavigatorChecks : public HealthAndArmingCheckBase +{ +public: + NavigatorChecks() = default; + ~NavigatorChecks() = default; + + void checkAndReport(const Context &context, Report &reporter) override; + +private: + uORB::Subscription _navigator_status_sub{ORB_ID(navigator_status)}; +}; diff --git a/src/modules/commander/failsafe/failsafe.cpp b/src/modules/commander/failsafe/failsafe.cpp index fef6e2136bf3..c4bf59786177 100644 --- a/src/modules/commander/failsafe/failsafe.cpp +++ b/src/modules/commander/failsafe/failsafe.cpp @@ -472,6 +472,14 @@ void Failsafe::checkStateAndMode(const hrt_abstime &time_us, const State &state, CHECK_FAILSAFE(status_flags, local_position_accuracy_low, ActionOptions(Action::RTL)); } + if (state.user_intended_mode == vehicle_status_s::NAVIGATION_STATE_AUTO_TAKEOFF || + state.user_intended_mode == vehicle_status_s::NAVIGATION_STATE_AUTO_RTL) { + CHECK_FAILSAFE(status_flags, navigator_failure, ActionOptions(Action::Land).clearOn(ClearCondition::OnModeChangeOrDisarm)); + + } else { + CHECK_FAILSAFE(status_flags, navigator_failure, ActionOptions(Action::Hold).clearOn(ClearCondition::OnModeChangeOrDisarm)); + } + CHECK_FAILSAFE(status_flags, geofence_breached, fromGfActParam(_param_gf_action.get()).cannotBeDeferred()); // Battery flight time remaining failsafe diff --git a/src/modules/navigator/rtl_direct.cpp b/src/modules/navigator/rtl_direct.cpp index 216a94c858b4..9be8ce24a937 100644 --- a/src/modules/navigator/rtl_direct.cpp +++ b/src/modules/navigator/rtl_direct.cpp @@ -161,8 +161,8 @@ void RtlDirect::set_rtl_item() if (_global_pos_sub.get().terrain_alt_valid && ((_rtl_alt - _global_pos_sub.get().terrain_alt) > _navigator->get_local_position()->hagl_max)) { // Handle case where the RTL altidude is above the maximum HAGL and land in place instead of RTL - mavlink_log_info(_navigator->get_mavlink_log_pub(), "RTL: return alt higher than max HAGL, landing\t"); - events::send(events::ID("rtl_fail_max_hagl"), events::Log::Warning, "RTL: return alt higher than max HAGL, landing"); + mavlink_log_info(_navigator->get_mavlink_log_pub(), "RTL: return alt higher than max HAGL\t"); + events::send(events::ID("rtl_fail_max_hagl"), events::Log::Error, "RTL: return alt higher than max HAGL"); _navigator->trigger_failsafe(getNavigatorStateId()); _rtl_state = RTLState::IDLE;