diff --git a/msg/FailsafeFlags.msg b/msg/FailsafeFlags.msg index 7455313732..8271298f11 100644 --- a/msg/FailsafeFlags.msg +++ b/msg/FailsafeFlags.msg @@ -15,6 +15,7 @@ uint32 mode_req_global_position uint32 mode_req_mission uint32 mode_req_offboard_signal uint32 mode_req_home_position +uint32 mode_req_wind_and_flight_time_compliance # if set, mode cannot be entered if wind or flight time limit exceeded uint32 mode_req_prevent_arming # if set, cannot arm while in this mode uint32 mode_req_other # other requirements, not covered above (for external modes) diff --git a/src/modules/commander/HealthAndArmingChecks/checks/modeCheck.cpp b/src/modules/commander/HealthAndArmingChecks/checks/modeCheck.cpp index 820d2e27d8..7ae7614819 100644 --- a/src/modules/commander/HealthAndArmingChecks/checks/modeCheck.cpp +++ b/src/modules/commander/HealthAndArmingChecks/checks/modeCheck.cpp @@ -147,6 +147,12 @@ void ModeChecks::checkAndReport(const Context &context, Report &reporter) // Here we expect there is already an event reported for the failing check (this is for external modes) reporter.clearCanRunBits((NavModes)reporter.failsafeFlags().mode_req_other); } + + if ((reporter.failsafeFlags().flight_time_limit_exceeded || reporter.failsafeFlags().wind_limit_exceeded) + && reporter.failsafeFlags().mode_req_wind_and_flight_time_compliance != 0) { + // Already reported + reporter.clearCanRunBits((NavModes)reporter.failsafeFlags().mode_req_wind_and_flight_time_compliance); + } } void ModeChecks::checkArmingRequirement(const Context &context, Report &reporter) diff --git a/src/modules/commander/ModeUtil/mode_requirements.cpp b/src/modules/commander/ModeUtil/mode_requirements.cpp index ba29627f87..fc48038867 100644 --- a/src/modules/commander/ModeUtil/mode_requirements.cpp +++ b/src/modules/commander/ModeUtil/mode_requirements.cpp @@ -54,6 +54,7 @@ void getModeRequirements(uint8_t vehicle_type, failsafe_flags_s &flags) flags.mode_req_mission = 0; flags.mode_req_offboard_signal = 0; flags.mode_req_home_position = 0; + flags.mode_req_wind_and_flight_time_compliance = 0; flags.mode_req_prevent_arming = 0; flags.mode_req_other = 0; @@ -82,6 +83,7 @@ void getModeRequirements(uint8_t vehicle_type, failsafe_flags_s &flags) 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_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); // NAVIGATION_STATE_AUTO_LOITER setRequirement(vehicle_status_s::NAVIGATION_STATE_AUTO_LOITER, flags.mode_req_angular_velocity); @@ -89,6 +91,7 @@ void getModeRequirements(uint8_t vehicle_type, failsafe_flags_s &flags) 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); 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); @@ -139,6 +142,7 @@ void getModeRequirements(uint8_t vehicle_type, failsafe_flags_s &flags) setRequirement(vehicle_status_s::NAVIGATION_STATE_AUTO_FOLLOW_TARGET, flags.mode_req_local_position); setRequirement(vehicle_status_s::NAVIGATION_STATE_AUTO_FOLLOW_TARGET, flags.mode_req_local_alt); setRequirement(vehicle_status_s::NAVIGATION_STATE_AUTO_FOLLOW_TARGET, flags.mode_req_prevent_arming); + setRequirement(vehicle_status_s::NAVIGATION_STATE_AUTO_FOLLOW_TARGET, flags.mode_req_wind_and_flight_time_compliance); // NAVIGATION_STATE_AUTO_PRECLAND setRequirement(vehicle_status_s::NAVIGATION_STATE_AUTO_PRECLAND, flags.mode_req_angular_velocity); @@ -153,6 +157,7 @@ void getModeRequirements(uint8_t vehicle_type, failsafe_flags_s &flags) setRequirement(vehicle_status_s::NAVIGATION_STATE_ORBIT, flags.mode_req_local_position); setRequirement(vehicle_status_s::NAVIGATION_STATE_ORBIT, flags.mode_req_local_alt); setRequirement(vehicle_status_s::NAVIGATION_STATE_ORBIT, flags.mode_req_prevent_arming); + setRequirement(vehicle_status_s::NAVIGATION_STATE_ORBIT, flags.mode_req_wind_and_flight_time_compliance); // NAVIGATION_STATE_AUTO_VTOL_TAKEOFF setRequirement(vehicle_status_s::NAVIGATION_STATE_AUTO_VTOL_TAKEOFF, flags.mode_req_angular_velocity); diff --git a/src/modules/commander/failsafe/failsafe.cpp b/src/modules/commander/failsafe/failsafe.cpp index dd969e02c2..69747868f1 100644 --- a/src/modules/commander/failsafe/failsafe.cpp +++ b/src/modules/commander/failsafe/failsafe.cpp @@ -393,8 +393,7 @@ void Failsafe::checkStateAndMode(const hrt_abstime &time_us, const State &state, CHECK_FAILSAFE(status_flags, wind_limit_exceeded, ActionOptions(Action::RTL).clearOn(ClearCondition::OnModeChangeOrDisarm)); - CHECK_FAILSAFE(status_flags, flight_time_limit_exceeded, - ActionOptions(Action::RTL).allowUserTakeover(UserTakeoverAllowed::Never)); + CHECK_FAILSAFE(status_flags, flight_time_limit_exceeded, ActionOptions(Action::RTL)); CHECK_FAILSAFE(status_flags, primary_geofence_breached, fromGfActParam(_param_gf_action.get())); diff --git a/src/modules/commander/failsafe/framework.cpp b/src/modules/commander/failsafe/framework.cpp index a8cbcd2cbc..e8e98a51cb 100644 --- a/src/modules/commander/failsafe/framework.cpp +++ b/src/modules/commander/failsafe/framework.cpp @@ -614,6 +614,7 @@ uint8_t FailsafeBase::modeFromAction(const Action &action, uint8_t user_intended bool FailsafeBase::modeCanRun(const failsafe_flags_s &status_flags, uint8_t mode) { uint32_t mode_mask = 1u << mode; + // mode_req_wind_and_flight_time_compliance: does not need to be handled here (these are separate failsafe triggers) return (!status_flags.angular_velocity_invalid || ((status_flags.mode_req_angular_velocity & mode_mask) == 0)) && (!status_flags.attitude_invalid || ((status_flags.mode_req_attitude & mode_mask) == 0)) &&