added mode_req_global_position_relaxed, which allows fixed wing auto …

…modes

to only require relaxed global / local position (no accuracy requirement)

Signed-off-by: RomanBapst <[email protected]>

ROMFS/px4fmu_common/init.d/rc.fw_defaults

@@ -14,7 +14,7 @@ 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_POS_R_FS_EPH 50
 param set-default COM_VEL_FS_EVH 3
 
 param set-default COM_POS_LOW_EPH 50

ROMFS/px4fmu_common/init.d/rc.vtol_defaults

@@ -11,7 +11,7 @@ set VEHICLE_TYPE vtol
 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_R_FS_EPH 50
 
 param set-default COM_POS_LOW_EPH 50
 

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

msg/versioned/ArmingCheckReply.msg

@@ -26,6 +26,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

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;

src/modules/commander/HealthAndArmingChecks/checks/estimatorCheck.cpp

@@ -693,10 +693,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().local_position_accuracy_low && position_valid_but_low_accuracy
 	    && _param_com_pos_low_act.get()) {
 
 		// only report if armed
@@ -718,7 +726,7 @@ void EstimatorChecks::lowPositionAccuracy(const Context &context, Report &report
 		}
 	}
 
-	reporter.failsafeFlags().local_position_accuracy_low = local_position_valid_but_low_accuracy;
+	reporter.failsafeFlags().local_position_accuracy_low = position_valid_but_low_accuracy;
 }
 
 void EstimatorChecks::setModeRequirementFlags(const Context &context, bool pre_flt_fail_innov_heading,
@@ -758,6 +766,13 @@ 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 have a separate accuracy parameter
+	const float pos_eph_relaxed_treshold = (_param_com_pos_relaxed_fs_eph.get() < 0) ? INFINITY :
+					       _param_com_pos_relaxed_fs_eph.get();
+	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));
@@ -772,6 +787,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) {

src/modules/commander/HealthAndArmingChecks/checks/estimatorCheck.hpp

@@ -96,6 +96,7 @@ class EstimatorChecks : public HealthAndArmingCheckBase
 	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)
@@ -117,6 +118,7 @@ class EstimatorChecks : public HealthAndArmingCheckBase
 					(ParamInt<px4::params::COM_ARM_WO_GPS>) _param_com_arm_wo_gps,
 					(ParamBool<px4::params::SYS_HAS_GPS>) _param_sys_has_gps,
 					(ParamFloat<px4::params::COM_POS_FS_EPH>) _param_com_pos_fs_eph,
+					(ParamFloat<px4::params::COM_POS_R_FS_EPH>) _param_com_pos_relaxed_fs_eph,
 					(ParamFloat<px4::params::COM_VEL_FS_EVH>) _param_com_vel_fs_evh,
 					(ParamFloat<px4::params::COM_POS_LOW_EPH>) _param_com_low_eph,
 					(ParamInt<px4::params::COM_POS_LOW_ACT>) _param_com_pos_low_act

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,

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) {

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,25 +86,49 @@ 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);
 
 	// 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);

src/modules/commander/commander_params.c

@@ -513,14 +513,12 @@ PARAM_DEFINE_INT32(COM_ARM_AUTH_MET, 0);
 PARAM_DEFINE_FLOAT(COM_ARM_AUTH_TO, 1);
 
 /**
- * Horizontal position error threshold.
+ * Multicopter Horizontal position error threshold
  *
- * 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).
+ * Horizontal position error (EPH) threshold for multicopter modes which rely on local / global position.
+ * If mode is active, threshold is relaxed by factor 2.5.
  *
- * Set to -1 to disable.
+ * Set -1 to disable.
  *
  * @unit m
  * @min -1
@@ -530,6 +528,22 @@ PARAM_DEFINE_FLOAT(COM_ARM_AUTH_TO, 1);
  */
 PARAM_DEFINE_FLOAT(COM_POS_FS_EPH, 5.f);
 
+/**
+ * Fixed-wing Horizontal position error threshold.
+ *
+ * Relaxed Horizontal position error (EPH) threshold for fixed-wing modes which rely on local / global position.
+ * If mode is active, threshold is relaxed by factor 2.5.
+ *
+ * Set to -1 to disable.
+ *
+ * @unit m
+ * @min -1
+ * @max 400
+ * @decimal 1
+ * @group Commander
+ */
+PARAM_DEFINE_FLOAT(COM_POS_R_FS_EPH, 50.f);
+
 /**
  * Horizontal velocity error threshold.
  *

src/modules/commander/failsafe/framework.cpp

@@ -695,6 +695,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)) &&