diff --git a/ROMFS/px4fmu_common/init.d/rc.fw_defaults b/ROMFS/px4fmu_common/init.d/rc.fw_defaults index e6271d8144..9d0db56e7c 100644 --- a/ROMFS/px4fmu_common/init.d/rc.fw_defaults +++ b/ROMFS/px4fmu_common/init.d/rc.fw_defaults @@ -13,8 +13,6 @@ param set-default MAV_TYPE 1 # # Default parameters for fixed wing UAVs. # -# there is a 2.5 factor applied on the _FS thresholds if for invalidation -param set-default COM_POS_FS_EPH 50 param set-default COM_VEL_FS_EVH 3 param set-default COM_POS_LOW_EPH 50 diff --git a/ROMFS/px4fmu_common/init.d/rc.vtol_defaults b/ROMFS/px4fmu_common/init.d/rc.vtol_defaults index b666d59bb8..b4b4b665e6 100644 --- a/ROMFS/px4fmu_common/init.d/rc.vtol_defaults +++ b/ROMFS/px4fmu_common/init.d/rc.vtol_defaults @@ -10,9 +10,6 @@ set VEHICLE_TYPE vtol # MAV_TYPE_VTOL_FIXEDROTOR 22 param set-default MAV_TYPE 22 -# there is a 2.5 factor applied on COM_POS_FS_EPH if for invalidation -param set-default COM_POS_FS_EPH 50 - param set-default COM_POS_LOW_EPH 50 param set-default MIS_TAKEOFF_ALT 20 diff --git a/docs/en/config/safety.md b/docs/en/config/safety.md index e20991146f..af6c309cd4 100644 --- a/docs/en/config/safety.md +++ b/docs/en/config/safety.md @@ -187,7 +187,23 @@ The following settings also apply, but are not displayed in the QGC UI. ## Position (GNSS) Loss Failsafe -The _Position Loss Failsafe_ is triggered if the quality of the PX4 global position estimate falls below acceptable levels (this might be caused by GPS loss) while in a mode that requires an acceptable position estimate. +The _Position Loss Failsafe_ is triggered if the quality of the PX4 position estimate falls below acceptable levels (this might be caused by GPS loss) while in a mode that requires an acceptable position estimate. +The sections below cover first the trigger and then the failsafe action taken by the controller. + +### Position Loss Failsafe Trigger + +There are basically two mechanisms in PX4 to trigger position failsafes: +- A timeout since the last sensor data was fused that provides direct speed or horizontal position measurements. Sensors that fall into that category are: GNSS, optical flow, airspeed, VIO, auxiliary global position. +- The estimated horizontal position accuracy exceeds a certain threshold. This check is only done on hovering systems (rotary wing vehicles or VTOLs in hover phase). + +The relevant parameters shown below. + +| Parameter | Description | +| -------------------------------------------------------------------------------------------------------- | ------------------------------------------------------------------------------------------------------------- | +| [EKF2_NOAID_TOUT](../advanced_config/parameter_reference.md#EKF2_NOAID_TOUT) | Maximum inertial dead-reckoning time, so the time after the last data sample was received of any sensor that constrains the velocity drift [microseconds]. | +| [COM_POS_FS_EPH](../advanced_config/parameter_reference.md#COM_POS_FS_EPH) | Horizontal position error threshold for hovering vehicles (Multicopters and VTOLs in hover). Fixed-wing vehicles have this value set to infinity.| + +### Position Loss Failsafe Action The failure action is controlled by [COM_POSCTL_NAVL](../advanced_config/parameter_reference.md#COM_POSCTL_NAVL), based on whether RC control is assumed to be available (and altitude information): diff --git a/docs/en/msg_docs/FailsafeFlags.md b/docs/en/msg_docs/FailsafeFlags.md index 164f8bb216..c0763f555c 100644 --- a/docs/en/msg_docs/FailsafeFlags.md +++ b/docs/en/msg_docs/FailsafeFlags.md @@ -58,7 +58,7 @@ bool mission_failure # Mission failure bool vtol_fixed_wing_system_failure # vehicle in fixed-wing system failure failsafe mode (after quad-chute) 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 position_accuracy_low # Position estimate has dropped below threshold, but is currently still declared valid bool navigator_failure # Navigator failed to execute a mode # Failure detector diff --git a/docs/ko/msg_docs/FailsafeFlags.md b/docs/ko/msg_docs/FailsafeFlags.md index 164f8bb216..c0763f555c 100644 --- a/docs/ko/msg_docs/FailsafeFlags.md +++ b/docs/ko/msg_docs/FailsafeFlags.md @@ -58,7 +58,7 @@ bool mission_failure # Mission failure bool vtol_fixed_wing_system_failure # vehicle in fixed-wing system failure failsafe mode (after quad-chute) 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 position_accuracy_low # Position estimate has dropped below threshold, but is currently still declared valid bool navigator_failure # Navigator failed to execute a mode # Failure detector diff --git a/docs/uk/msg_docs/FailsafeFlags.md b/docs/uk/msg_docs/FailsafeFlags.md index 844ea20226..5877ada4d4 100644 --- a/docs/uk/msg_docs/FailsafeFlags.md +++ b/docs/uk/msg_docs/FailsafeFlags.md @@ -58,7 +58,7 @@ bool mission_failure # Mission failure bool vtol_fixed_wing_system_failure # vehicle in fixed-wing system failure failsafe mode (after quad-chute) 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 position_accuracy_low # Position estimate has dropped below threshold, but is currently still declared valid bool navigator_failure # Navigator failed to execute a mode # Failure detector diff --git a/docs/zh/msg_docs/FailsafeFlags.md b/docs/zh/msg_docs/FailsafeFlags.md index 164f8bb216..c0763f555c 100644 --- a/docs/zh/msg_docs/FailsafeFlags.md +++ b/docs/zh/msg_docs/FailsafeFlags.md @@ -58,7 +58,7 @@ bool mission_failure # Mission failure bool vtol_fixed_wing_system_failure # vehicle in fixed-wing system failure failsafe mode (after quad-chute) 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 position_accuracy_low # Position estimate has dropped below threshold, but is currently still declared valid bool navigator_failure # Navigator failed to execute a mode # Failure detector diff --git a/msg/CMakeLists.txt b/msg/CMakeLists.txt index a0cc5ab1ec..e07888adb3 100644 --- a/msg/CMakeLists.txt +++ b/msg/CMakeLists.txt @@ -81,7 +81,7 @@ set(msg_files EstimatorStates.msg EstimatorStatus.msg EstimatorStatusFlags.msg - Event.msg + versioned/Event.msg FigureEightStatus.msg FailsafeFlags.msg FailureDetectorStatus.msg diff --git a/msg/FailsafeFlags.msg b/msg/FailsafeFlags.msg index 33240b0759..243a4b5c9e 100644 --- a/msg/FailsafeFlags.msg +++ b/msg/FailsafeFlags.msg @@ -12,6 +12,7 @@ uint32 mode_req_local_alt uint32 mode_req_local_position uint32 mode_req_local_position_relaxed uint32 mode_req_global_position +uint32 mode_req_global_position_relaxed uint32 mode_req_mission uint32 mode_req_offboard_signal uint32 mode_req_home_position @@ -29,6 +30,7 @@ bool local_position_invalid # Local position estimate invalid bool local_position_invalid_relaxed # Local position with reduced accuracy requirements invalid (e.g. flying with optical flow) bool local_velocity_invalid # Local velocity estimate invalid bool global_position_invalid # Global position estimate invalid +bool global_position_invalid_relaxed # Global position estimate invalid with relaxed accuracy requirements bool auto_mission_missing # No mission available bool offboard_control_signal_lost # Offboard signal lost bool home_position_invalid # No home position available @@ -48,7 +50,7 @@ bool mission_failure # Mission failure bool vtol_fixed_wing_system_failure # vehicle in fixed-wing system failure failsafe mode (after quad-chute) 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 position_accuracy_low # Position estimate has dropped below threshold, but is currently still declared valid bool navigator_failure # Navigator failed to execute a mode # Failure detector diff --git a/msg/px4_msgs_old/msg/ArmingCheckReplyV0.msg b/msg/px4_msgs_old/msg/ArmingCheckReplyV0.msg new file mode 100644 index 0000000000..c417d6a4f0 --- /dev/null +++ b/msg/px4_msgs_old/msg/ArmingCheckReplyV0.msg @@ -0,0 +1,34 @@ +uint32 MESSAGE_VERSION = 0 + +uint64 timestamp # time since system start (microseconds) + +uint8 request_id +uint8 registration_id + +uint8 HEALTH_COMPONENT_INDEX_NONE = 0 + +uint8 health_component_index # HEALTH_COMPONENT_INDEX_* +bool health_component_is_present +bool health_component_warning +bool health_component_error + +bool can_arm_and_run # whether arming is possible, and if it's a navigation mode, if it can run + +uint8 num_events + +EventV0[5] events + +# Mode requirements +bool mode_req_angular_velocity +bool mode_req_attitude +bool mode_req_local_alt +bool mode_req_local_position +bool mode_req_local_position_relaxed +bool mode_req_global_position +bool mode_req_mission +bool mode_req_home_position +bool mode_req_prevent_arming +bool mode_req_manual_control + + +uint8 ORB_QUEUE_LENGTH = 4 diff --git a/msg/px4_msgs_old/msg/EventV0.msg b/msg/px4_msgs_old/msg/EventV0.msg new file mode 100644 index 0000000000..84351a36aa --- /dev/null +++ b/msg/px4_msgs_old/msg/EventV0.msg @@ -0,0 +1,14 @@ +# this message is required here in the msg_old folder because other msg are depending on it +# Events interface + +uint32 MESSAGE_VERSION = 0 + +uint64 timestamp # time since system start (microseconds) + +uint32 id # Event ID +uint16 event_sequence # Event sequence number +uint8[25] arguments # (optional) arguments, depend on event id + +uint8 log_levels # Log levels: 4 bits MSB: internal, 4 bits LSB: external + +uint8 ORB_QUEUE_LENGTH = 16 diff --git a/msg/translation_node/translations/all_translations.h b/msg/translation_node/translations/all_translations.h index 21b128c03c..22202033f1 100644 --- a/msg/translation_node/translations/all_translations.h +++ b/msg/translation_node/translations/all_translations.h @@ -13,3 +13,5 @@ #include "translation_vehicle_status_v1.h" #include "translation_airspeed_validated_v1.h" #include "translation_vehicle_attitude_setpoint_v1.h" +#include "translation_arming_check_reply_v1.h" +#include "translation_event_v1.h" diff --git a/msg/translation_node/translations/translation_arming_check_reply_v1.h b/msg/translation_node/translations/translation_arming_check_reply_v1.h new file mode 100644 index 0000000000..7753ce96c2 --- /dev/null +++ b/msg/translation_node/translations/translation_arming_check_reply_v1.h @@ -0,0 +1,84 @@ +/**************************************************************************** + * Copyright (c) 2025 PX4 Development Team. + * SPDX-License-Identifier: BSD-3-Clause + ****************************************************************************/ +#pragma once + +// Translate ArmingCheckReply v0 <--> v1 +#include +#include +#include "translation_event_v1.h" + +class ArmingCheckReplyV1Translation { +public: + using MessageOlder = px4_msgs_old::msg::ArmingCheckReplyV0; + static_assert(MessageOlder::MESSAGE_VERSION == 0); + + using MessageNewer = px4_msgs::msg::ArmingCheckReply; + static_assert(MessageNewer::MESSAGE_VERSION == 1); + + static constexpr const char* kTopic = "/fmu/in/arming_check_reply"; + + static void fromOlder(const MessageOlder &msg_older, MessageNewer &msg_newer) { + msg_newer.timestamp = msg_older.timestamp; + + msg_newer.request_id = msg_older.request_id; + msg_newer.registration_id = msg_older.registration_id; + + msg_newer.health_component_index = msg_older.health_component_index; + msg_newer.health_component_is_present = msg_older.health_component_is_present; + msg_newer.health_component_warning = msg_older.health_component_warning; + msg_newer.health_component_error = msg_older.health_component_error; + + msg_newer.can_arm_and_run = msg_older.can_arm_and_run; + + + for (std::size_t i = 0; i < msg_newer.events.size();i++) { + EventV1Translation::fromOlder(msg_older.events.at(i), msg_newer.events.at(i)); + } + + + msg_newer.mode_req_angular_velocity = msg_older.mode_req_angular_velocity; + msg_newer.mode_req_attitude = msg_older.mode_req_attitude; + msg_newer.mode_req_local_alt = msg_older.mode_req_local_alt; + msg_newer.mode_req_local_position = msg_older.mode_req_local_position; + msg_newer.mode_req_local_position_relaxed = msg_older.mode_req_local_position_relaxed; + msg_newer.mode_req_global_position = msg_older.mode_req_global_position; + msg_newer.mode_req_global_position_relaxed = false; + msg_newer.mode_req_mission = msg_older.mode_req_mission; + msg_newer.mode_req_home_position = msg_older.mode_req_home_position; + msg_newer.mode_req_prevent_arming = msg_older.mode_req_prevent_arming; + msg_newer.mode_req_manual_control = msg_older.mode_req_manual_control; + + } + + static void toOlder(const MessageNewer &msg_newer, MessageOlder &msg_older) { + msg_older.timestamp = msg_newer.timestamp; + + msg_older.request_id = msg_newer.request_id; + msg_older.registration_id = msg_newer.registration_id; + + msg_older.health_component_index = msg_newer.health_component_index; + msg_older.health_component_is_present = msg_newer.health_component_is_present; + msg_older.health_component_warning = msg_newer.health_component_warning; + msg_older.health_component_error = msg_newer.health_component_error; + + msg_older.can_arm_and_run = msg_newer.can_arm_and_run; + + for (std::size_t i = 0; i < msg_older.events.size();i++) { + EventV1Translation::toOlder(msg_newer.events.at(i), msg_older.events.at(i)); + } + + msg_older.mode_req_angular_velocity = msg_newer.mode_req_angular_velocity; + msg_older.mode_req_attitude = msg_newer.mode_req_attitude; + msg_older.mode_req_local_alt = msg_newer.mode_req_local_alt; + msg_older.mode_req_local_position = msg_newer.mode_req_local_position; + msg_older.mode_req_global_position = msg_newer.mode_req_global_position; + msg_older.mode_req_mission = msg_newer.mode_req_mission; + msg_older.mode_req_home_position = msg_newer.mode_req_home_position; + msg_older.mode_req_prevent_arming = msg_newer.mode_req_prevent_arming; + msg_older.mode_req_manual_control = msg_newer.mode_req_manual_control; + } +}; + +REGISTER_TOPIC_TRANSLATION_DIRECT(ArmingCheckReplyV1Translation); diff --git a/msg/translation_node/translations/translation_event_v1.h b/msg/translation_node/translations/translation_event_v1.h new file mode 100644 index 0000000000..edf2a74a9d --- /dev/null +++ b/msg/translation_node/translations/translation_event_v1.h @@ -0,0 +1,38 @@ +/**************************************************************************** + * Copyright (c) 2025 PX4 Development Team. + * SPDX-License-Identifier: BSD-3-Clause + ****************************************************************************/ +#pragma once + +// Translate Event v0 <--> v1 +#include +#include + +class EventV1Translation { +public: + using MessageOlder = px4_msgs_old::msg::EventV0; + static_assert(MessageOlder::MESSAGE_VERSION == 0); + + using MessageNewer = px4_msgs::msg::Event; + static_assert(MessageNewer::MESSAGE_VERSION == 1); + + static constexpr const char* kTopic = "fmu/out/event"; + + static void fromOlder(const MessageOlder &msg_older, MessageNewer &msg_newer) { + msg_newer.timestamp = msg_older.timestamp; + msg_newer.id = msg_older.id; + msg_newer.event_sequence = msg_older.event_sequence; + msg_newer.arguments = msg_older.arguments; + msg_newer.log_levels = msg_older.log_levels; + } + + static void toOlder(const MessageNewer &msg_newer, MessageOlder &msg_older) { + msg_older.timestamp = msg_newer.timestamp; + msg_older.id = msg_newer.id; + msg_older.event_sequence = msg_newer.event_sequence; + msg_older.arguments = msg_newer.arguments; + msg_older.log_levels = msg_newer.log_levels; + } +}; + +REGISTER_TOPIC_TRANSLATION_DIRECT(EventV1Translation); diff --git a/msg/versioned/ArmingCheckReply.msg b/msg/versioned/ArmingCheckReply.msg index 4c1616d849..0efcc21738 100644 --- a/msg/versioned/ArmingCheckReply.msg +++ b/msg/versioned/ArmingCheckReply.msg @@ -1,4 +1,4 @@ -uint32 MESSAGE_VERSION = 0 +uint32 MESSAGE_VERSION = 1 uint64 timestamp # time since system start (microseconds) @@ -25,6 +25,7 @@ bool mode_req_local_alt bool mode_req_local_position bool mode_req_local_position_relaxed bool mode_req_global_position +bool mode_req_global_position_relaxed bool mode_req_mission bool mode_req_home_position bool mode_req_prevent_arming diff --git a/msg/Event.msg b/msg/versioned/Event.msg similarity index 92% rename from msg/Event.msg rename to msg/versioned/Event.msg index df1dd4a97d..ed8d841ebd 100644 --- a/msg/Event.msg +++ b/msg/versioned/Event.msg @@ -1,4 +1,6 @@ # Events interface +uint32 MESSAGE_VERSION = 1 + uint64 timestamp # time since system start (microseconds) uint32 id # Event ID diff --git a/src/modules/commander/HealthAndArmingChecks/HealthAndArmingChecks.cpp b/src/modules/commander/HealthAndArmingChecks/HealthAndArmingChecks.cpp index f4f91d0450..ffb72e6d78 100644 --- a/src/modules/commander/HealthAndArmingChecks/HealthAndArmingChecks.cpp +++ b/src/modules/commander/HealthAndArmingChecks/HealthAndArmingChecks.cpp @@ -45,6 +45,7 @@ HealthAndArmingChecks::HealthAndArmingChecks(ModuleParams *parent, vehicle_statu _failsafe_flags.local_position_invalid_relaxed = true; _failsafe_flags.local_velocity_invalid = true; _failsafe_flags.global_position_invalid = true; + _failsafe_flags.global_position_invalid_relaxed = true; _failsafe_flags.auto_mission_missing = true; _failsafe_flags.offboard_control_signal_lost = true; _failsafe_flags.home_position_invalid = true; @@ -54,7 +55,10 @@ bool HealthAndArmingChecks::update(bool force_reporting, bool is_arming_request) { _reporter.reset(); - _reporter.prepare(_context.status().vehicle_type); + // treat VTOLs in transition mode as fixed-wing - this is not in line with what's published as vehicle_status_s::vehicle_type + const uint8_t vehicle_type = _context.status().in_transition_mode ? vehicle_status_s::VEHICLE_TYPE_FIXED_WING : + _context.status().vehicle_type; + _reporter.prepare(vehicle_type); _context.setIsArmingRequest(is_arming_request); @@ -78,7 +82,7 @@ bool HealthAndArmingChecks::update(bool force_reporting, bool is_arming_request) _reporter._mavlink_log_pub = &_mavlink_log_pub; _reporter.reset(); - _reporter.prepare(_context.status().vehicle_type); + _reporter.prepare(vehicle_type); for (unsigned i = 0; i < sizeof(_checks) / sizeof(_checks[0]); ++i) { if (!_checks[i]) { diff --git a/src/modules/commander/HealthAndArmingChecks/checks/estimatorCheck.cpp b/src/modules/commander/HealthAndArmingChecks/checks/estimatorCheck.cpp index 8d1dbbd2df..fa7def7727 100644 --- a/src/modules/commander/HealthAndArmingChecks/checks/estimatorCheck.cpp +++ b/src/modules/commander/HealthAndArmingChecks/checks/estimatorCheck.cpp @@ -633,10 +633,18 @@ void EstimatorChecks::checkGps(const Context &context, Report &reporter, const s void EstimatorChecks::lowPositionAccuracy(const Context &context, Report &reporter, const vehicle_local_position_s &lpos) const { - const bool local_position_valid_but_low_accuracy = !reporter.failsafeFlags().local_position_invalid - && (_param_com_low_eph.get() > FLT_EPSILON && lpos.eph > _param_com_low_eph.get()); - if (!reporter.failsafeFlags().local_position_accuracy_low && local_position_valid_but_low_accuracy + bool position_valid_but_low_accuracy = false; + + if ((reporter.failsafeFlags().mode_req_global_position && !reporter.failsafeFlags().global_position_invalid) || + (reporter.failsafeFlags().mode_req_global_position_relaxed + && !reporter.failsafeFlags().global_position_invalid_relaxed) || + (reporter.failsafeFlags().mode_req_local_position && !reporter.failsafeFlags().local_position_invalid)) { + + position_valid_but_low_accuracy = (_param_com_low_eph.get() > FLT_EPSILON && lpos.eph > _param_com_low_eph.get()); + } + + if (!reporter.failsafeFlags().position_accuracy_low && position_valid_but_low_accuracy && _param_com_pos_low_act.get()) { // only report if armed @@ -658,7 +666,7 @@ void EstimatorChecks::lowPositionAccuracy(const Context &context, Report &report } } - reporter.failsafeFlags().local_position_accuracy_low = local_position_valid_but_low_accuracy; + reporter.failsafeFlags().position_accuracy_low = position_valid_but_low_accuracy; } void EstimatorChecks::setModeRequirementFlags(const Context &context, bool pre_flt_fail_innov_heading, @@ -698,6 +706,12 @@ void EstimatorChecks::setModeRequirementFlags(const Context &context, bool pre_f !checkPosVelValidity(now, global_pos_valid, gpos.eph, lpos_eph_threshold, gpos.timestamp, _last_gpos_fail_time_us, !failsafe_flags.global_position_invalid); + // for relaxed global condition we don't have any accuracy requirement + const float pos_eph_relaxed_treshold = INFINITY; + failsafe_flags.global_position_invalid_relaxed = !checkPosVelValidity(now, global_pos_valid, gpos.eph, + pos_eph_relaxed_treshold, gpos.timestamp, _last_gpos_relaxed_fail_time_us, + !failsafe_flags.global_position_invalid_relaxed); + // Additional warning if the system is about to enter position-loss failsafe after dead-reckoning period const float eph_critical = 2.5f * lpos_eph_threshold; // threshold used to trigger the navigation failsafe const float gpos_critical_warning_thrld = math::max(0.9f * eph_critical, math::max(eph_critical - 10.f, 0.f)); @@ -712,6 +726,7 @@ void EstimatorChecks::setModeRequirementFlags(const Context &context, bool pre_f || estimator_status_flags.cs_wind_dead_reckoning; if (!failsafe_flags.global_position_invalid + && failsafe_flags.mode_req_global_position && !_nav_failure_imminent_warned && gpos.eph > gpos_critical_warning_thrld && dead_reckoning) { diff --git a/src/modules/commander/HealthAndArmingChecks/checks/estimatorCheck.hpp b/src/modules/commander/HealthAndArmingChecks/checks/estimatorCheck.hpp index 0e10484023..13b351c986 100644 --- a/src/modules/commander/HealthAndArmingChecks/checks/estimatorCheck.hpp +++ b/src/modules/commander/HealthAndArmingChecks/checks/estimatorCheck.hpp @@ -96,6 +96,7 @@ private: uORB::Subscription _vehicle_gps_position_sub{ORB_ID(vehicle_gps_position)}; hrt_abstime _last_gpos_fail_time_us{0}; ///< Last time that the global position validity recovery check failed (usec) + hrt_abstime _last_gpos_relaxed_fail_time_us{0}; ///< Last time that the global position relaxed validity recovery check failed (usec) hrt_abstime _last_lpos_fail_time_us{0}; ///< Last time that the local position validity recovery check failed (usec) hrt_abstime _last_lpos_relaxed_fail_time_us{0}; ///< Last time that the relaxed local position validity recovery check failed (usec) hrt_abstime _last_lvel_fail_time_us{0}; ///< Last time that the local velocity validity recovery check failed (usec) diff --git a/src/modules/commander/HealthAndArmingChecks/checks/externalChecks.cpp b/src/modules/commander/HealthAndArmingChecks/checks/externalChecks.cpp index 8ef3c5cfa9..b1f2abc064 100644 --- a/src/modules/commander/HealthAndArmingChecks/checks/externalChecks.cpp +++ b/src/modules/commander/HealthAndArmingChecks/checks/externalChecks.cpp @@ -173,6 +173,8 @@ void ExternalChecks::checkAndReport(const Context &context, Report &reporter) reporter.failsafeFlags().mode_req_local_position_relaxed); setOrClearRequirementBits(reply.mode_req_global_position, nav_mode_id, replaces_nav_state, reporter.failsafeFlags().mode_req_global_position); + setOrClearRequirementBits(reply.mode_req_global_position_relaxed, nav_mode_id, replaces_nav_state, + reporter.failsafeFlags().mode_req_global_position_relaxed); setOrClearRequirementBits(reply.mode_req_mission, nav_mode_id, replaces_nav_state, reporter.failsafeFlags().mode_req_mission); setOrClearRequirementBits(reply.mode_req_home_position, nav_mode_id, replaces_nav_state, diff --git a/src/modules/commander/HealthAndArmingChecks/checks/modeCheck.cpp b/src/modules/commander/HealthAndArmingChecks/checks/modeCheck.cpp index 7352a6bbfe..d13a4a19ae 100644 --- a/src/modules/commander/HealthAndArmingChecks/checks/modeCheck.cpp +++ b/src/modules/commander/HealthAndArmingChecks/checks/modeCheck.cpp @@ -86,16 +86,27 @@ void ModeChecks::checkAndReport(const Context &context, Report &reporter) reporter.clearCanRunBits(local_position_modes); } + NavModes global_position_modes = NavModes::None; + if (reporter.failsafeFlags().global_position_invalid && reporter.failsafeFlags().mode_req_global_position != 0) { + global_position_modes = (NavModes)reporter.failsafeFlags().mode_req_global_position; + } + + if (reporter.failsafeFlags().global_position_invalid_relaxed + && reporter.failsafeFlags().mode_req_global_position_relaxed != 0) { + global_position_modes = global_position_modes | (NavModes)reporter.failsafeFlags().mode_req_global_position_relaxed; + } + + if (global_position_modes != NavModes::None) { /* EVENT * @description * The available positioning data is not sufficient to execute the selected mode. */ - reporter.armingCheckFailure((NavModes)reporter.failsafeFlags().mode_req_global_position, + reporter.armingCheckFailure(global_position_modes, health_component_t::global_position_estimate, events::ID("check_modes_global_pos"), events::Log::Error, "Navigation error: No valid global position estimate"); - reporter.clearCanRunBits((NavModes)reporter.failsafeFlags().mode_req_global_position); + reporter.clearCanRunBits(global_position_modes); } if (reporter.failsafeFlags().local_altitude_invalid && reporter.failsafeFlags().mode_req_local_alt != 0) { diff --git a/src/modules/commander/ModeUtil/mode_requirements.cpp b/src/modules/commander/ModeUtil/mode_requirements.cpp index 555b564d97..5c6fa30295 100644 --- a/src/modules/commander/ModeUtil/mode_requirements.cpp +++ b/src/modules/commander/ModeUtil/mode_requirements.cpp @@ -50,6 +50,7 @@ void getModeRequirements(uint8_t vehicle_type, failsafe_flags_s &flags) flags.mode_req_local_position = 0; flags.mode_req_local_position_relaxed = 0; flags.mode_req_global_position = 0; + flags.mode_req_global_position_relaxed = 0; flags.mode_req_local_alt = 0; flags.mode_req_mission = 0; flags.mode_req_offboard_signal = 0; @@ -85,8 +86,16 @@ void getModeRequirements(uint8_t vehicle_type, failsafe_flags_s &flags) // NAVIGATION_STATE_AUTO_MISSION setRequirement(vehicle_status_s::NAVIGATION_STATE_AUTO_MISSION, flags.mode_req_angular_velocity); setRequirement(vehicle_status_s::NAVIGATION_STATE_AUTO_MISSION, flags.mode_req_attitude); - setRequirement(vehicle_status_s::NAVIGATION_STATE_AUTO_MISSION, flags.mode_req_local_position); - setRequirement(vehicle_status_s::NAVIGATION_STATE_AUTO_MISSION, flags.mode_req_global_position); + + if (vehicle_type == vehicle_status_s::VEHICLE_TYPE_FIXED_WING) { + setRequirement(vehicle_status_s::NAVIGATION_STATE_AUTO_MISSION, flags.mode_req_global_position_relaxed); + setRequirement(vehicle_status_s::NAVIGATION_STATE_AUTO_MISSION, flags.mode_req_local_position_relaxed); + + } else { + setRequirement(vehicle_status_s::NAVIGATION_STATE_AUTO_MISSION, flags.mode_req_global_position); + setRequirement(vehicle_status_s::NAVIGATION_STATE_AUTO_MISSION, flags.mode_req_local_position); + } + setRequirement(vehicle_status_s::NAVIGATION_STATE_AUTO_MISSION, flags.mode_req_local_alt); setRequirement(vehicle_status_s::NAVIGATION_STATE_AUTO_MISSION, flags.mode_req_mission); setRequirement(vehicle_status_s::NAVIGATION_STATE_AUTO_MISSION, flags.mode_req_wind_and_flight_time_compliance); @@ -94,16 +103,32 @@ void getModeRequirements(uint8_t vehicle_type, failsafe_flags_s &flags) // NAVIGATION_STATE_AUTO_LOITER setRequirement(vehicle_status_s::NAVIGATION_STATE_AUTO_LOITER, flags.mode_req_angular_velocity); setRequirement(vehicle_status_s::NAVIGATION_STATE_AUTO_LOITER, flags.mode_req_attitude); - setRequirement(vehicle_status_s::NAVIGATION_STATE_AUTO_LOITER, flags.mode_req_local_position); - setRequirement(vehicle_status_s::NAVIGATION_STATE_AUTO_LOITER, flags.mode_req_global_position); + + if (vehicle_type == vehicle_status_s::VEHICLE_TYPE_FIXED_WING) { + setRequirement(vehicle_status_s::NAVIGATION_STATE_AUTO_LOITER, flags.mode_req_global_position_relaxed); + setRequirement(vehicle_status_s::NAVIGATION_STATE_AUTO_LOITER, flags.mode_req_local_position_relaxed); + + } else { + setRequirement(vehicle_status_s::NAVIGATION_STATE_AUTO_LOITER, flags.mode_req_global_position); + setRequirement(vehicle_status_s::NAVIGATION_STATE_AUTO_LOITER, flags.mode_req_local_position); + } + setRequirement(vehicle_status_s::NAVIGATION_STATE_AUTO_LOITER, flags.mode_req_local_alt); setRequirement(vehicle_status_s::NAVIGATION_STATE_AUTO_LOITER, flags.mode_req_wind_and_flight_time_compliance); // NAVIGATION_STATE_AUTO_RTL setRequirement(vehicle_status_s::NAVIGATION_STATE_AUTO_RTL, flags.mode_req_angular_velocity); setRequirement(vehicle_status_s::NAVIGATION_STATE_AUTO_RTL, flags.mode_req_attitude); - setRequirement(vehicle_status_s::NAVIGATION_STATE_AUTO_RTL, flags.mode_req_local_position); - setRequirement(vehicle_status_s::NAVIGATION_STATE_AUTO_RTL, flags.mode_req_global_position); + + if (vehicle_type == vehicle_status_s::VEHICLE_TYPE_FIXED_WING) { + setRequirement(vehicle_status_s::NAVIGATION_STATE_AUTO_RTL, flags.mode_req_global_position_relaxed); + setRequirement(vehicle_status_s::NAVIGATION_STATE_AUTO_RTL, flags.mode_req_local_position_relaxed); + + } else { + setRequirement(vehicle_status_s::NAVIGATION_STATE_AUTO_RTL, flags.mode_req_global_position); + setRequirement(vehicle_status_s::NAVIGATION_STATE_AUTO_RTL, flags.mode_req_local_position); + } + setRequirement(vehicle_status_s::NAVIGATION_STATE_AUTO_RTL, flags.mode_req_local_alt); setRequirement(vehicle_status_s::NAVIGATION_STATE_AUTO_RTL, flags.mode_req_home_position); setRequirement(vehicle_status_s::NAVIGATION_STATE_AUTO_RTL, flags.mode_req_prevent_arming); diff --git a/src/modules/commander/commander_params.c b/src/modules/commander/commander_params.c index 3f350618b6..c1b77b6721 100644 --- a/src/modules/commander/commander_params.c +++ b/src/modules/commander/commander_params.c @@ -513,12 +513,13 @@ PARAM_DEFINE_INT32(COM_ARM_AUTH_MET, 0); PARAM_DEFINE_FLOAT(COM_ARM_AUTH_TO, 1); /** - * Horizontal position error threshold. + * Horizontal position error threshold for hovering systems * * This is the horizontal position error (EPH) threshold that will trigger a failsafe. - * The default is appropriate for a multicopter. Can be increased for a fixed-wing. * If the previous position error was below this threshold, there is an additional * factor of 2.5 applied (threshold for invalidation 2.5 times the one for validation). + * Only used for multicopters and VTOLs in hover mode. + * Independent from estimator positioning data timeout threshold (see EKF2_NOAID_TOUT). * * Set to -1 to disable. * diff --git a/src/modules/commander/failsafe/failsafe.cpp b/src/modules/commander/failsafe/failsafe.cpp index 6d2bc83d83..905d24491c 100644 --- a/src/modules/commander/failsafe/failsafe.cpp +++ b/src/modules/commander/failsafe/failsafe.cpp @@ -532,7 +532,7 @@ void Failsafe::checkStateAndMode(const hrt_abstime &time_us, const State &state, // trigger Low Position Accuracy Failsafe (only in auto mission and auto loiter) if (state.user_intended_mode == vehicle_status_s::NAVIGATION_STATE_AUTO_MISSION || state.user_intended_mode == vehicle_status_s::NAVIGATION_STATE_AUTO_LOITER) { - CHECK_FAILSAFE(status_flags, local_position_accuracy_low, fromPosLowActParam(_param_com_pos_low_act.get())); + CHECK_FAILSAFE(status_flags, position_accuracy_low, fromPosLowActParam(_param_com_pos_low_act.get())); } if (state.user_intended_mode == vehicle_status_s::NAVIGATION_STATE_AUTO_TAKEOFF || diff --git a/src/modules/commander/failsafe/framework.cpp b/src/modules/commander/failsafe/framework.cpp index 3de8305702..0632c0681e 100644 --- a/src/modules/commander/failsafe/framework.cpp +++ b/src/modules/commander/failsafe/framework.cpp @@ -697,6 +697,7 @@ bool FailsafeBase::modeCanRun(const failsafe_flags_s &status_flags, uint8_t mode (!status_flags.local_position_invalid || ((status_flags.mode_req_local_position & mode_mask) == 0)) && (!status_flags.local_position_invalid_relaxed || ((status_flags.mode_req_local_position_relaxed & mode_mask) == 0)) && (!status_flags.global_position_invalid || ((status_flags.mode_req_global_position & mode_mask) == 0)) && + (!status_flags.global_position_invalid_relaxed || ((status_flags.mode_req_global_position_relaxed & mode_mask) == 0)) && (!status_flags.local_altitude_invalid || ((status_flags.mode_req_local_alt & mode_mask) == 0)) && (!status_flags.auto_mission_missing || ((status_flags.mode_req_mission & mode_mask) == 0)) && (!status_flags.offboard_control_signal_lost || ((status_flags.mode_req_offboard_signal & mode_mask) == 0)) &&