fly316/PX4-Autopilot
7
0
Fork 0
mirror of https://gitee.com/mirrors_PX4/PX4-Autopilot.git synced 2026-10-16 22:33:37 +08:00
Code Issues Packages Projects Releases Wiki Activity

commander: add wind or flight time limit exceeded mode requirement

Browse Source 
This prevents switching into any of these modes once the condition is set.
This commit is contained in:
Beat Küng
2023-02-01 08:48:09 +01:00
committed by Silvan Fuhrer
parent a8628c9d9c
commit b1709743f7
5 changed files with 14 additions and 2 deletions
Download Patch File Download Diff File Expand all files Collapse all files
msg/FailsafeFlags.msg
+1
View File
@@ -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)
src/modules/commander/HealthAndArmingChecks/checks/modeCheck.cpp
+6
View File
@@ -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)
src/modules/commander/ModeUtil/mode_requirements.cpp
+5
View File
@@ -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);
src/modules/commander/failsafe/failsafe.cpp
+1 -2
View File
@@ -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()));
src/modules/commander/failsafe/framework.cpp
+1
View File
@@ -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)) &&