Commander: remove dynamic position velocity probation period

ROMFS/px4fmu_common/init.d/rc.fw_defaults

@@ -16,8 +16,6 @@ param set-default MAV_TYPE 1
 param set-default COM_POS_FS_DELAY 5
 param set-default COM_POS_FS_EPH 15
 param set-default COM_POS_FS_EPV 30
-param set-default COM_POS_FS_GAIN 0
-param set-default COM_POS_FS_PROB 1
 param set-default COM_VEL_FS_EVH 5
 # Disable preflight disarm to not interfere with external launching
 param set-default COM_DISARM_PRFLT -1

src/modules/commander/Commander.cpp

@@ -955,7 +955,6 @@ Commander::handle_command(const vehicle_command_s &cmd)
 			}
 
 			if (desired_main_state != commander_state_s::MAIN_STATE_MAX) {
-				reset_posvel_validity();
 				main_ret = main_state_transition(_status, desired_main_state, _status_flags, _internal_state);
 			}
 
@@ -2164,13 +2163,6 @@ Commander::run()
 					events::send(events::ID("commander_takeoff_detected"), events::Log::Info, "Takeoff detected");
 					_status.takeoff_time = hrt_absolute_time();
 					_have_taken_off_since_arming = true;
-
-					// Set all position and velocity test probation durations to takeoff value
-					// This is a larger value to give the vehicle time to complete a failsafe landing
-					// if faulty sensors cause loss of navigation shortly after takeoff.
-					_gpos_probation_time_us = _param_com_pos_fs_prob.get() * 1_s;
-					_lpos_probation_time_us = _param_com_pos_fs_prob.get() * 1_s;
-					_lvel_probation_time_us = _param_com_pos_fs_prob.get() * 1_s;
 				}
 
 				// automatically set or update home position
@@ -3244,46 +3236,21 @@ Commander::control_status_leds(bool changed, const uint8_t battery_warning)
 	_leds_counter++;
 }
 
-void
-Commander::reset_posvel_validity()
-{
-	// reset all the check probation times back to the minimum value
-	_gpos_probation_time_us = POSVEL_PROBATION_MIN;
-	_lpos_probation_time_us = POSVEL_PROBATION_MIN;
-	_lvel_probation_time_us = POSVEL_PROBATION_MIN;
-
-	// recheck validity (force update)
-	estimator_check(true);
-}
-
-bool
-Commander::check_posvel_validity(const bool data_valid, const float data_accuracy, const float required_accuracy,
-				 const hrt_abstime &data_timestamp_us, hrt_abstime *last_fail_time_us, hrt_abstime *probation_time_us,
-				 const bool was_valid)
+bool Commander::check_posvel_validity(const bool data_valid, const float data_accuracy, const float required_accuracy,
+				      const hrt_abstime &data_timestamp_us, hrt_abstime *last_fail_time_us,
+				      const bool was_valid)
 {
 	bool valid = was_valid;
-
-	// constrain probation times
-	if (_vehicle_land_detected.landed) {
-		*probation_time_us = POSVEL_PROBATION_MIN;
-	}
-
 	const bool data_stale = ((hrt_elapsed_time(&data_timestamp_us) > _param_com_pos_fs_delay.get() * 1_s)
 				 || (data_timestamp_us == 0));
 	const float req_accuracy = (was_valid ? required_accuracy * 2.5f : required_accuracy);
-
 	const bool level_check_pass = data_valid && !data_stale && (data_accuracy < req_accuracy);
 
 	// Check accuracy with hysteresis in both test level and time
 	if (level_check_pass) {
-		if (was_valid) {
-			// still valid, continue to decrease probation time
-			const int64_t probation_time_new = *probation_time_us - hrt_elapsed_time(last_fail_time_us);
-			*probation_time_us = math::constrain(probation_time_new, POSVEL_PROBATION_MIN, POSVEL_PROBATION_MAX);
-
-		} else {
+		if (!was_valid) {
 			// check if probation period has elapsed
-			if (hrt_elapsed_time(last_fail_time_us) > *probation_time_us) {
+			if (hrt_elapsed_time(last_fail_time_us) > 1_s) {
 				valid = true;
 			}
 		}
@@ -3293,12 +3260,6 @@ Commander::check_posvel_validity(const bool data_valid, const float data_accurac
 		if (was_valid) {
 			// FAILURE! no longer valid
 			valid = false;
-
-		} else {
-			// failed again, increase probation time
-			const int64_t probation_time_new = *probation_time_us + hrt_elapsed_time(last_fail_time_us) *
-							   _param_com_pos_fs_gain.get();
-			*probation_time_us = math::constrain(probation_time_new, POSVEL_PROBATION_MIN, POSVEL_PROBATION_MAX);
 		}
 
 		*last_fail_time_us = hrt_absolute_time();
@@ -3970,7 +3931,7 @@ void Commander::battery_status_check()
 	}
 }
 
-void Commander::estimator_check(bool force)
+void Commander::estimator_check()
 {
 	// Check if quality checking of position accuracy and consistency is to be performed
 	const bool run_quality_checks = !_status_flags.circuit_breaker_engaged_posfailure_check;
@@ -3992,7 +3953,7 @@ void Commander::estimator_check(bool force)
 	const bool gnss_heading_fault_prev = _estimator_status_flags_sub.get().cs_gps_yaw_fault;
 
 	// use primary estimator_status
-	if (_estimator_selector_status_sub.updated() || force) {
+	if (_estimator_selector_status_sub.updated()) {
 		estimator_selector_status_s estimator_selector_status;
 
 		if (_estimator_selector_status_sub.copy(&estimator_selector_status)) {
@@ -4047,7 +4008,7 @@ void Commander::estimator_check(bool force)
 	bool pre_flt_fail_innov_heading = false;
 	bool pre_flt_fail_innov_vel_horiz = false;
 
-	if (_estimator_status_sub.updated() || force) {
+	if (_estimator_status_sub.updated()) {
 
 		estimator_status_s estimator_status;
 
@@ -4136,15 +4097,15 @@ void Commander::estimator_check(bool force)
 
 		_status_flags.global_position_valid =
 			check_posvel_validity(xy_valid, gpos.eph, _param_com_pos_fs_eph.get(), gpos.timestamp,
-					      &_last_gpos_fail_time_us, &_gpos_probation_time_us, _status_flags.global_position_valid);
+					      &_last_gpos_fail_time_us, _status_flags.global_position_valid);
 
 		_status_flags.local_position_valid =
 			check_posvel_validity(xy_valid, lpos.eph, lpos_eph_threshold_adj, lpos.timestamp,
-					      &_last_lpos_fail_time_us, &_lpos_probation_time_us, _status_flags.local_position_valid);
+					      &_last_lpos_fail_time_us, _status_flags.local_position_valid);
 
 		_status_flags.local_velocity_valid =
 			check_posvel_validity(v_xy_valid, lpos.evh, _param_com_vel_fs_evh.get(), lpos.timestamp,
-					      &_last_lvel_fail_time_us, &_lvel_probation_time_us, _status_flags.local_velocity_valid);
+					      &_last_lvel_fail_time_us, _status_flags.local_velocity_valid);
 	}
 
 
@@ -4200,7 +4161,7 @@ void Commander::estimator_check(bool force)
 	// gps
 	const bool condition_gps_position_was_valid = _status_flags.gps_position_valid;
 
-	if (_vehicle_gps_position_sub.updated() || force) {
+	if (_vehicle_gps_position_sub.updated()) {
 		vehicle_gps_position_s vehicle_gps_position;
 
 		if (_vehicle_gps_position_sub.copy(&vehicle_gps_position)) {

src/modules/commander/Commander.hpp

@@ -132,7 +132,7 @@ private:
 	void battery_status_check();
 
 	bool check_posvel_validity(const bool data_valid, const float data_accuracy, const float required_accuracy,
-				   const hrt_abstime &data_timestamp_us, hrt_abstime *last_fail_time_us, hrt_abstime *probation_time_us,
+				   const hrt_abstime &data_timestamp_us, hrt_abstime *last_fail_time_us,
 				   const bool was_valid);
 
 	void control_status_leds(bool changed, const uint8_t battery_warning);
@@ -146,7 +146,7 @@ private:
 
 	void esc_status_check();
 
-	void estimator_check(bool force = false);
+	void estimator_check();
 
 	bool handle_command(const vehicle_command_s &cmd);
 
@@ -159,8 +159,6 @@ private:
 
 	void print_reject_mode(uint8_t main_state);
 
-	void reset_posvel_validity();
-
 	bool set_home_position();
 	bool set_home_position_alt_only();
 	bool set_in_air_home_position();
@@ -207,8 +205,6 @@ private:
 		(ParamInt<px4::params::COM_POSCTL_NAVL>) _param_com_posctl_navl,	/* failsafe response to loss of navigation accuracy */
 
 		(ParamInt<px4::params::COM_POS_FS_DELAY>) _param_com_pos_fs_delay,
-		(ParamInt<px4::params::COM_POS_FS_PROB>) _param_com_pos_fs_prob,
-		(ParamInt<px4::params::COM_POS_FS_GAIN>) _param_com_pos_fs_gain,
 
 		(ParamInt<px4::params::COM_LOW_BAT_ACT>) _param_com_low_bat_act,
 		(ParamFloat<px4::params::COM_BAT_ACT_T>) _param_com_bat_act_t,
@@ -291,9 +287,6 @@ private:
 	static constexpr uint64_t HOTPLUG_SENS_TIMEOUT{8_s};	/**< wait for hotplug sensors to come online for upto 8 seconds */
 	static constexpr uint64_t INAIR_RESTART_HOLDOFF_INTERVAL{500_ms};
 
-	const int64_t POSVEL_PROBATION_MIN = 1_s;	/**< minimum probation duration (usec) */
-	const int64_t POSVEL_PROBATION_MAX = 100_s;	/**< maximum probation duration (usec) */
-
 	PreFlightCheck::arm_requirements_t	_arm_requirements{};
 
 	hrt_abstime	_valid_distance_sensor_time_us{0}; /**< Last time that distance sensor data arrived (usec) */
@@ -302,11 +295,6 @@ private:
 	hrt_abstime	_last_lpos_fail_time_us{0};	/**< Last time that the 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) */
 
-	// Probation times for position and velocity validity checks to pass if failed
-	hrt_abstime	_gpos_probation_time_us = POSVEL_PROBATION_MIN;
-	hrt_abstime	_lpos_probation_time_us = POSVEL_PROBATION_MIN;
-	hrt_abstime	_lvel_probation_time_us = POSVEL_PROBATION_MIN;
-
 	/* class variables used to check for navigation failure after takeoff */
 	hrt_abstime	_time_last_innov_pass{0};	/**< last time velocity and position innovations passed */
 	hrt_abstime	_time_last_innov_fail{0};	/**< last time velocity and position innovations failed */

src/modules/commander/commander_params.c

@@ -796,33 +796,6 @@ PARAM_DEFINE_FLOAT(COM_ARM_AUTH_TO, 1);
  */
 PARAM_DEFINE_INT32(COM_POS_FS_DELAY, 1);
 
-/**
- * Loss of position probation delay at takeoff.
- *
- * The probation delay is the number of seconds that the EKF innovation checks need to pass for the position to be declared good after it has been declared bad.
- * The probation delay will be reset to this parameter value when takeoff is detected.
- * After takeoff, if position checks are passing, the probation delay will reduce by one second for every lapsed second of valid position down to a minimum of 1 second.
- * If position checks are failing, the probation delay will increase by COM_POS_FS_GAIN seconds for every lapsed second up to a maximum of 100 seconds.
- * The default value has been optimised for rotary wing applications. For fixed wing applications, a value of 1 should be used.
- *
- * @unit s
- * @group Commander
- * @min 1
- * @max 100
- */
-PARAM_DEFINE_INT32(COM_POS_FS_PROB, 30);
-
-/**
- * Loss of position probation gain factor.
- *
- * This sets the rate that the loss of position probation time grows when position checks are failing.
- * The default value has been optimised for rotary wing applications. For fixed wing applications a value of 0 should be used.
- *
- * @reboot_required true
- * @group Commander
- */
-PARAM_DEFINE_INT32(COM_POS_FS_GAIN, 10);
-
 /**
  * Horizontal position error threshold.
  *