From bb14e8f1383f42acd589bcb0793c3ed6e5e953ee Mon Sep 17 00:00:00 2001 From: bresch Date: Fri, 9 Aug 2024 11:15:15 +0200 Subject: [PATCH 1/4] navigator: add navigation state ID to every mode class --- src/modules/navigator/land.cpp | 4 ++-- src/modules/navigator/loiter.cpp | 2 +- src/modules/navigator/mission.cpp | 2 +- src/modules/navigator/mission_base.cpp | 6 +++--- src/modules/navigator/mission_base.h | 2 +- src/modules/navigator/mission_block.cpp | 4 ++-- src/modules/navigator/mission_block.h | 2 +- src/modules/navigator/navigator_mode.cpp | 5 +++-- src/modules/navigator/navigator_mode.h | 7 ++++++- src/modules/navigator/precland.cpp | 2 +- src/modules/navigator/rtl.cpp | 2 +- src/modules/navigator/rtl_base.h | 2 +- src/modules/navigator/rtl_direct.cpp | 4 ++-- src/modules/navigator/takeoff.cpp | 4 ++-- src/modules/navigator/vtol_takeoff.cpp | 4 ++-- 15 files changed, 29 insertions(+), 23 deletions(-) diff --git a/src/modules/navigator/land.cpp b/src/modules/navigator/land.cpp index b6a951d1b766..74f54fbb079a 100644 --- a/src/modules/navigator/land.cpp +++ b/src/modules/navigator/land.cpp @@ -42,7 +42,7 @@ #include "navigator.h" Land::Land(Navigator *navigator) : - MissionBlock(navigator) + MissionBlock(navigator, vehicle_status_s::NAVIGATION_STATE_AUTO_LAND) { } @@ -93,7 +93,7 @@ Land::on_active() if (_navigator->get_land_detected()->landed) { _navigator->get_mission_result()->finished = true; _navigator->set_mission_result_updated(); - _navigator->mode_completed(vehicle_status_s::NAVIGATION_STATE_AUTO_LAND); + _navigator->mode_completed(getNavigatorStateId()); set_idle_item(&_mission_item); struct position_setpoint_triplet_s *pos_sp_triplet = _navigator->get_position_setpoint_triplet(); diff --git a/src/modules/navigator/loiter.cpp b/src/modules/navigator/loiter.cpp index 15408baaff05..db61030528c1 100644 --- a/src/modules/navigator/loiter.cpp +++ b/src/modules/navigator/loiter.cpp @@ -43,7 +43,7 @@ #include "navigator.h" Loiter::Loiter(Navigator *navigator) : - MissionBlock(navigator), + MissionBlock(navigator, vehicle_status_s::NAVIGATION_STATE_AUTO_LOITER), ModuleParams(navigator) { } diff --git a/src/modules/navigator/mission.cpp b/src/modules/navigator/mission.cpp index 2949c88d62b3..ea7dae675217 100644 --- a/src/modules/navigator/mission.cpp +++ b/src/modules/navigator/mission.cpp @@ -65,7 +65,7 @@ using namespace time_literals; static constexpr int32_t DEFAULT_MISSION_CACHE_SIZE = 10; Mission::Mission(Navigator *navigator) : - MissionBase(navigator, DEFAULT_MISSION_CACHE_SIZE) + MissionBase(navigator, DEFAULT_MISSION_CACHE_SIZE, vehicle_status_s::NAVIGATION_STATE_AUTO_MISSION) { } diff --git a/src/modules/navigator/mission_base.cpp b/src/modules/navigator/mission_base.cpp index d41456ccce66..913bbd3e4591 100644 --- a/src/modules/navigator/mission_base.cpp +++ b/src/modules/navigator/mission_base.cpp @@ -46,8 +46,8 @@ #include "mission_feasibility_checker.h" #include "navigator.h" -MissionBase::MissionBase(Navigator *navigator, int32_t dataman_cache_size_signed) : - MissionBlock(navigator), +MissionBase::MissionBase(Navigator *navigator, int32_t dataman_cache_size_signed, uint8_t navigator_state_id) : + MissionBlock(navigator, navigator_state_id), ModuleParams(navigator), _dataman_cache_size_signed(dataman_cache_size_signed) { @@ -465,7 +465,7 @@ MissionBase::set_mission_items() if (set_end_of_mission) { setEndOfMissionItems(); - _navigator->mode_completed(vehicle_status_s::NAVIGATION_STATE_AUTO_MISSION); + _navigator->mode_completed(getNavigatorStateId()); } } diff --git a/src/modules/navigator/mission_base.h b/src/modules/navigator/mission_base.h index 2f416f29e206..d669943fed51 100644 --- a/src/modules/navigator/mission_base.h +++ b/src/modules/navigator/mission_base.h @@ -65,7 +65,7 @@ class Navigator; class MissionBase : public MissionBlock, public ModuleParams { public: - MissionBase(Navigator *navigator, int32_t dataman_cache_size_signed); + MissionBase(Navigator *navigator, int32_t dataman_cache_size_signed, uint8_t navigator_state_id); ~MissionBase() override = default; virtual void on_inactive() override; diff --git a/src/modules/navigator/mission_block.cpp b/src/modules/navigator/mission_block.cpp index 1f0c9f0dd021..f1f202231853 100644 --- a/src/modules/navigator/mission_block.cpp +++ b/src/modules/navigator/mission_block.cpp @@ -55,8 +55,8 @@ using matrix::wrap_pi; -MissionBlock::MissionBlock(Navigator *navigator) : - NavigatorMode(navigator) +MissionBlock::MissionBlock(Navigator *navigator, uint8_t navigator_state_id) : + NavigatorMode(navigator, navigator_state_id) { } diff --git a/src/modules/navigator/mission_block.h b/src/modules/navigator/mission_block.h index 13815b860211..edc1b0e843f5 100644 --- a/src/modules/navigator/mission_block.h +++ b/src/modules/navigator/mission_block.h @@ -64,7 +64,7 @@ class MissionBlock : public NavigatorMode /** * Constructor */ - MissionBlock(Navigator *navigator); + MissionBlock(Navigator *navigator, uint8_t navigator_state_id); virtual ~MissionBlock() = default; MissionBlock(const MissionBlock &) = delete; diff --git a/src/modules/navigator/navigator_mode.cpp b/src/modules/navigator/navigator_mode.cpp index b0ced9006c56..392ca6abb458 100644 --- a/src/modules/navigator/navigator_mode.cpp +++ b/src/modules/navigator/navigator_mode.cpp @@ -42,8 +42,9 @@ #include "navigator_mode.h" #include "navigator.h" -NavigatorMode::NavigatorMode(Navigator *navigator) : - _navigator(navigator) +NavigatorMode::NavigatorMode(Navigator *navigator, uint8_t navigator_state_id) : + _navigator(navigator), + _navigator_state_id(navigator_state_id) { /* set initial mission items */ on_inactivation(); diff --git a/src/modules/navigator/navigator_mode.h b/src/modules/navigator/navigator_mode.h index 60abdcd165e0..ca418e5b2d9a 100644 --- a/src/modules/navigator/navigator_mode.h +++ b/src/modules/navigator/navigator_mode.h @@ -41,12 +41,14 @@ #pragma once +#include + class Navigator; class NavigatorMode { public: - NavigatorMode(Navigator *navigator); + NavigatorMode(Navigator *navigator, uint8_t navigator_state_id); virtual ~NavigatorMode() = default; NavigatorMode(const NavigatorMode &) = delete; NavigatorMode &operator=(const NavigatorMode &) = delete; @@ -56,6 +58,8 @@ class NavigatorMode bool isActive() {return _active;}; + uint8_t getNavigatorStateId() const { return _navigator_state_id; } + /** * This function is called while the mode is inactive */ @@ -81,4 +85,5 @@ class NavigatorMode private: bool _active{false}; + uint8_t _navigator_state_id{0}; }; diff --git a/src/modules/navigator/precland.cpp b/src/modules/navigator/precland.cpp index 080491d37ca7..74c81569cf3d 100644 --- a/src/modules/navigator/precland.cpp +++ b/src/modules/navigator/precland.cpp @@ -62,7 +62,7 @@ static constexpr const char *LOST_TARGET_ERROR_MESSAGE = "Lost landing target while landing"; PrecLand::PrecLand(Navigator *navigator) : - MissionBlock(navigator), + MissionBlock(navigator, vehicle_status_s::NAVIGATION_STATE_AUTO_PRECLAND), ModuleParams(navigator) { _handle_param_acceleration_hor = param_find("MPC_ACC_HOR"); diff --git a/src/modules/navigator/rtl.cpp b/src/modules/navigator/rtl.cpp index 2b6bd55b3f91..6fef45f9df6e 100644 --- a/src/modules/navigator/rtl.cpp +++ b/src/modules/navigator/rtl.cpp @@ -55,7 +55,7 @@ static constexpr float MAX_DIST_FROM_HOME_FOR_LAND_APPROACHES{10.0f}; // [m] We static constexpr float MIN_DIST_THRESHOLD = 2.f; RTL::RTL(Navigator *navigator) : - NavigatorMode(navigator), + NavigatorMode(navigator, vehicle_status_s::NAVIGATION_STATE_AUTO_RTL), ModuleParams(navigator), _rtl_direct(navigator) { diff --git a/src/modules/navigator/rtl_base.h b/src/modules/navigator/rtl_base.h index dc698687fce6..a4c88e06b026 100644 --- a/src/modules/navigator/rtl_base.h +++ b/src/modules/navigator/rtl_base.h @@ -46,7 +46,7 @@ class RtlBase : public MissionBase { public: RtlBase(Navigator *navigator, int32_t dataman_cache_size_signed): - MissionBase(navigator, dataman_cache_size_signed) {}; + MissionBase(navigator, dataman_cache_size_signed, vehicle_status_s::NAVIGATION_STATE_AUTO_RTL) {}; virtual ~RtlBase() = default; virtual rtl_time_estimate_s calc_rtl_time_estimate() = 0; diff --git a/src/modules/navigator/rtl_direct.cpp b/src/modules/navigator/rtl_direct.cpp index 62a940f6c0cf..145b99d0d1db 100644 --- a/src/modules/navigator/rtl_direct.cpp +++ b/src/modules/navigator/rtl_direct.cpp @@ -51,7 +51,7 @@ using namespace math; RtlDirect::RtlDirect(Navigator *navigator) : - MissionBlock(navigator), + MissionBlock(navigator, vehicle_status_s::NAVIGATION_STATE_AUTO_RTL), ModuleParams(navigator) { _destination.lat = static_cast(NAN); @@ -318,7 +318,7 @@ void RtlDirect::set_rtl_item() case RTLState::IDLE: { set_idle_item(&_mission_item); - _navigator->mode_completed(vehicle_status_s::NAVIGATION_STATE_AUTO_RTL); + _navigator->mode_completed(getNavigatorStateId()); break; } diff --git a/src/modules/navigator/takeoff.cpp b/src/modules/navigator/takeoff.cpp index c9e59fec1e20..d9b22720f34a 100644 --- a/src/modules/navigator/takeoff.cpp +++ b/src/modules/navigator/takeoff.cpp @@ -43,7 +43,7 @@ #include Takeoff::Takeoff(Navigator *navigator) : - MissionBlock(navigator) + MissionBlock(navigator, vehicle_status_s::NAVIGATION_STATE_AUTO_TAKEOFF) { } @@ -68,7 +68,7 @@ Takeoff::on_active() } else if (is_mission_item_reached_or_completed() && !_navigator->get_mission_result()->finished) { _navigator->get_mission_result()->finished = true; _navigator->set_mission_result_updated(); - _navigator->mode_completed(vehicle_status_s::NAVIGATION_STATE_AUTO_TAKEOFF); + _navigator->mode_completed(getNavigatorStateId()); position_setpoint_triplet_s *pos_sp_triplet = _navigator->get_position_setpoint_triplet(); diff --git a/src/modules/navigator/vtol_takeoff.cpp b/src/modules/navigator/vtol_takeoff.cpp index 7b36201856d3..f9a552ef58fc 100644 --- a/src/modules/navigator/vtol_takeoff.cpp +++ b/src/modules/navigator/vtol_takeoff.cpp @@ -43,7 +43,7 @@ using matrix::wrap_pi; VtolTakeoff::VtolTakeoff(Navigator *navigator) : - MissionBlock(navigator), + MissionBlock(navigator, vehicle_status_s::NAVIGATION_STATE_AUTO_VTOL_TAKEOFF), ModuleParams(navigator) { } @@ -151,7 +151,7 @@ VtolTakeoff::on_active() // the VTOL takeoff is done _navigator->get_mission_result()->finished = true; _navigator->set_mission_result_updated(); - _navigator->mode_completed(vehicle_status_s::NAVIGATION_STATE_AUTO_VTOL_TAKEOFF); + _navigator->mode_completed(getNavigatorStateId()); break; } From 8f6489c2f114230e100763626f0797e78f7abcb1 Mon Sep 17 00:00:00 2001 From: bresch Date: Fri, 9 Aug 2024 11:19:08 +0200 Subject: [PATCH 2/4] navigator: publish navigator_state feedback to commander --- msg/CMakeLists.txt | 1 + msg/NavigatorStatus.msg | 6 ++++ src/modules/logger/logged_topics.cpp | 1 + src/modules/navigator/navigator.h | 14 ++++++++- src/modules/navigator/navigator_main.cpp | 37 ++++++++++++++++++++++++ src/modules/navigator/rtl_direct.cpp | 10 +++++++ 6 files changed, 68 insertions(+), 1 deletion(-) create mode 100644 msg/NavigatorStatus.msg diff --git a/msg/CMakeLists.txt b/msg/CMakeLists.txt index f0f6d520c701..3b31b7de554d 100644 --- a/msg/CMakeLists.txt +++ b/msg/CMakeLists.txt @@ -146,6 +146,7 @@ set(msg_files MountOrientation.msg ModeCompleted.msg NavigatorMissionItem.msg + NavigatorStatus.msg NormalizedUnsignedSetpoint.msg NpfgStatus.msg ObstacleDistance.msg diff --git a/msg/NavigatorStatus.msg b/msg/NavigatorStatus.msg new file mode 100644 index 000000000000..21517d61e7bb --- /dev/null +++ b/msg/NavigatorStatus.msg @@ -0,0 +1,6 @@ +# Current status of a Navigator mode +# The possible values of nav_state are defined in the VehicleStatus msg. +uint64 timestamp # time since system start (microseconds) + +uint8 nav_state # Source mode (values in VehicleStatus) +bool failure # True when the current mode cannot continue diff --git a/src/modules/logger/logged_topics.cpp b/src/modules/logger/logged_topics.cpp index 24b5004ee9af..5be39234e34f 100644 --- a/src/modules/logger/logged_topics.cpp +++ b/src/modules/logger/logged_topics.cpp @@ -91,6 +91,7 @@ void LoggedTopics::add_default_topics() add_topic("manual_control_switches"); add_topic("mission_result"); add_topic("navigator_mission_item"); + add_topic("navigator_status"); add_topic("npfg_status", 100); add_topic("offboard_control_mode", 100); add_topic("onboard_computer_status", 10); diff --git a/src/modules/navigator/navigator.h b/src/modules/navigator/navigator.h index 45c4f158b7c4..70068a9c515b 100644 --- a/src/modules/navigator/navigator.h +++ b/src/modules/navigator/navigator.h @@ -69,6 +69,7 @@ #include #include #include +#include #include #include #include @@ -283,8 +284,12 @@ class Navigator : public ModuleBase, public ModuleParams void mode_completed(uint8_t nav_state, uint8_t result = mode_completed_s::RESULT_SUCCESS); + void set_failsafe_status(uint8_t nav_state, bool failsafe); + void sendWarningDescentStoppedDueToTerrain(); + void trigger_failsafe(uint8_t nav_state); + private: int _local_pos_sub{-1}; @@ -305,6 +310,7 @@ class Navigator : public ModuleBase, public ModuleParams uORB::Publication _geofence_result_pub{ORB_ID(geofence_result)}; uORB::Publication _mission_result_pub{ORB_ID(mission_result)}; + uORB::Publication _navigator_status_pub{ORB_ID(navigator_status)}; uORB::Publication _pos_sp_triplet_pub{ORB_ID(position_setpoint_triplet)}; uORB::Publication _vehicle_cmd_ack_pub{ORB_ID(vehicle_command_ack)}; uORB::Publication _vehicle_cmd_pub{ORB_ID(vehicle_command)}; @@ -324,6 +330,7 @@ class Navigator : public ModuleBase, public ModuleParams // Publications geofence_result_s _geofence_result{}; + navigator_status_s _navigator_status{}; position_setpoint_triplet_s _pos_sp_triplet{}; /**< triplet of position setpoints */ position_setpoint_triplet_s _reposition_triplet{}; /**< triplet for non-mission direct position command */ position_setpoint_triplet_s _takeoff_triplet{}; /**< triplet for non-mission direct takeoff command */ @@ -333,7 +340,10 @@ class Navigator : public ModuleBase, public ModuleParams Geofence _geofence; /**< class that handles the geofence */ GeofenceBreachAvoidance _gf_breach_avoidance; - hrt_abstime _last_geofence_check = 0; + hrt_abstime _last_geofence_check{0}; + + bool _navigator_status_updated{false}; + hrt_abstime _last_navigator_status_publication{0}; hrt_abstime _wait_for_vehicle_status_timestamp{0}; /**< If non-zero, wait for vehicle_status update before processing next cmd */ @@ -386,6 +396,8 @@ class Navigator : public ModuleBase, public ModuleParams */ void publish_mission_result(); + void publish_navigator_status(); + void publish_vehicle_command_ack(const vehicle_command_s &cmd, uint8_t result); bool geofence_allows_position(const vehicle_global_position_s &pos); diff --git a/src/modules/navigator/navigator_main.cpp b/src/modules/navigator/navigator_main.cpp index f2694b595ab7..a00ca0e79c49 100644 --- a/src/modules/navigator/navigator_main.cpp +++ b/src/modules/navigator/navigator_main.cpp @@ -41,6 +41,7 @@ * @author Julian Oes * @author Anton Babushkin * @author Thomas Gubler + * and many more... */ #include "navigator.h" @@ -897,6 +898,8 @@ void Navigator::run() publish_mission_result(); } + publish_navigator_status(); + _geofence.run(); perf_end(_loop_perf); @@ -1355,6 +1358,40 @@ void Navigator::set_mission_failure_heading_timeout() } } +void Navigator::trigger_failsafe(const uint8_t nav_state) +{ + if (!_navigator_status.failure || _navigator_status.nav_state != nav_state) { + _navigator_status.failure = true; + _navigator_status.nav_state = nav_state; + + _navigator_status_updated = true; + } +} + +void Navigator::publish_navigator_status() +{ + uint8_t current_nav_state = _vstatus.nav_state; + + if (_navigation_mode != nullptr) { + current_nav_state = _navigation_mode->getNavigatorStateId(); + } + + if (_navigator_status.nav_state != current_nav_state) { + _navigator_status.nav_state = current_nav_state; + _navigator_status.failure = false; + _navigator_status_updated = true; + } + + if (_navigator_status_updated + || (hrt_elapsed_time(&_last_navigator_status_publication) > 500_ms)) { + _navigator_status.timestamp = hrt_absolute_time(); + _navigator_status_pub.publish(_navigator_status); + + _navigator_status_updated = false; + _last_navigator_status_publication = hrt_absolute_time(); + } +} + void Navigator::publish_vehicle_cmd(vehicle_command_s *vcmd) { vcmd->timestamp = hrt_absolute_time(); diff --git a/src/modules/navigator/rtl_direct.cpp b/src/modules/navigator/rtl_direct.cpp index 145b99d0d1db..216a94c858b4 100644 --- a/src/modules/navigator/rtl_direct.cpp +++ b/src/modules/navigator/rtl_direct.cpp @@ -158,6 +158,16 @@ void RtlDirect::set_rtl_item() { position_setpoint_triplet_s *pos_sp_triplet = _navigator->get_position_setpoint_triplet(); + 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"); + + _navigator->trigger_failsafe(getNavigatorStateId()); + _rtl_state = RTLState::IDLE; + } + const float destination_dist = get_distance_to_next_waypoint(_destination.lat, _destination.lon, _global_pos_sub.get().lat, _global_pos_sub.get().lon); const float loiter_altitude = math::min(_land_approach.height_m, _rtl_alt); From c6c24a73faa2ffb2b18913257ffb233073ee41c0 Mon Sep 17 00:00:00 2001 From: bresch Date: Fri, 9 Aug 2024 14:12:23 +0200 Subject: [PATCH 3/4] 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 | 10 ++++ src/modules/navigator/rtl_direct.cpp | 4 +- 7 files changed, 115 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..11285af40352 100644 --- a/src/modules/commander/failsafe/failsafe.cpp +++ b/src/modules/commander/failsafe/failsafe.cpp @@ -472,6 +472,16 @@ 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; From 1309c0fe77c6d94bdcffd2733209303f65424394 Mon Sep 17 00:00:00 2001 From: bresch Date: Tue, 13 Aug 2024 17:57:29 +0200 Subject: [PATCH 4/4] navigator: add failure enum --- msg/NavigatorStatus.msg | 5 ++++- .../HealthAndArmingChecks/checks/navigatorCheck.cpp | 13 ++++++++++++- src/modules/navigator/navigator.h | 2 +- src/modules/navigator/navigator_main.cpp | 8 ++++---- src/modules/navigator/rtl_direct.cpp | 2 +- 5 files changed, 22 insertions(+), 8 deletions(-) diff --git a/msg/NavigatorStatus.msg b/msg/NavigatorStatus.msg index 21517d61e7bb..da666533574f 100644 --- a/msg/NavigatorStatus.msg +++ b/msg/NavigatorStatus.msg @@ -3,4 +3,7 @@ uint64 timestamp # time since system start (microseconds) uint8 nav_state # Source mode (values in VehicleStatus) -bool failure # True when the current mode cannot continue +uint8 failure # Navigator failure enum + +uint8 FAILURE_NONE = 0 +uint8 FAILURE_HAGL = 1 # Target altitude exceeds maximum height above ground diff --git a/src/modules/commander/HealthAndArmingChecks/checks/navigatorCheck.cpp b/src/modules/commander/HealthAndArmingChecks/checks/navigatorCheck.cpp index 8c7d0e314af4..2a0245bb1fbd 100644 --- a/src/modules/commander/HealthAndArmingChecks/checks/navigatorCheck.cpp +++ b/src/modules/commander/HealthAndArmingChecks/checks/navigatorCheck.cpp @@ -42,6 +42,17 @@ void NavigatorChecks::checkAndReport(const Context &context, Report &reporter) } if (context.status().nav_state == status.nav_state) { - reporter.failsafeFlags().navigator_failure = status.failure; + + reporter.failsafeFlags().navigator_failure = (status.failure != navigator_status_s::FAILURE_NONE); + + if (status.failure == navigator_status_s::FAILURE_HAGL) { + /* EVENT + */ + reporter.armingCheckFailure(NavModes::All, + health_component_t::system, + events::ID("check_navigator_failure_hagl"), + events::Log::Error, + "Waypoint above maximum height"); + } } } diff --git a/src/modules/navigator/navigator.h b/src/modules/navigator/navigator.h index 70068a9c515b..ec72881fc527 100644 --- a/src/modules/navigator/navigator.h +++ b/src/modules/navigator/navigator.h @@ -288,7 +288,7 @@ class Navigator : public ModuleBase, public ModuleParams void sendWarningDescentStoppedDueToTerrain(); - void trigger_failsafe(uint8_t nav_state); + void trigger_hagl_failsafe(uint8_t nav_state); private: diff --git a/src/modules/navigator/navigator_main.cpp b/src/modules/navigator/navigator_main.cpp index a00ca0e79c49..15db1abdc44d 100644 --- a/src/modules/navigator/navigator_main.cpp +++ b/src/modules/navigator/navigator_main.cpp @@ -1358,10 +1358,10 @@ void Navigator::set_mission_failure_heading_timeout() } } -void Navigator::trigger_failsafe(const uint8_t nav_state) +void Navigator::trigger_hagl_failsafe(const uint8_t nav_state) { - if (!_navigator_status.failure || _navigator_status.nav_state != nav_state) { - _navigator_status.failure = true; + if ((_navigator_status.failure != navigator_status_s::FAILURE_HAGL) || _navigator_status.nav_state != nav_state) { + _navigator_status.failure = navigator_status_s::FAILURE_HAGL; _navigator_status.nav_state = nav_state; _navigator_status_updated = true; @@ -1378,7 +1378,7 @@ void Navigator::publish_navigator_status() if (_navigator_status.nav_state != current_nav_state) { _navigator_status.nav_state = current_nav_state; - _navigator_status.failure = false; + _navigator_status.failure = navigator_status_s::FAILURE_NONE; _navigator_status_updated = true; } diff --git a/src/modules/navigator/rtl_direct.cpp b/src/modules/navigator/rtl_direct.cpp index 9be8ce24a937..f26df56530d2 100644 --- a/src/modules/navigator/rtl_direct.cpp +++ b/src/modules/navigator/rtl_direct.cpp @@ -164,7 +164,7 @@ void RtlDirect::set_rtl_item() 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()); + _navigator->trigger_hagl_failsafe(getNavigatorStateId()); _rtl_state = RTLState::IDLE; }