From eee4aaee4f4ff77e5929bb4f73a5f943fdb21334 Mon Sep 17 00:00:00 2001 From: Matthias Grob Date: Tue, 22 Mar 2022 21:35:35 +0100 Subject: [PATCH] Commander: remove dynamic position velocity probation period --- ROMFS/px4fmu_common/init.d/rc.fw_defaults | 2 - src/modules/commander/Commander.cpp | 63 +++++------------------ src/modules/commander/Commander.hpp | 16 +----- src/modules/commander/commander_params.c | 27 ---------- 4 files changed, 14 insertions(+), 94 deletions(-) diff --git a/ROMFS/px4fmu_common/init.d/rc.fw_defaults b/ROMFS/px4fmu_common/init.d/rc.fw_defaults index 2fcd351db2..6cae25306e 100644 --- a/ROMFS/px4fmu_common/init.d/rc.fw_defaults +++ b/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 diff --git a/src/modules/commander/Commander.cpp b/src/modules/commander/Commander.cpp index f1268fe976..53b1adf666 100644 --- a/src/modules/commander/Commander.cpp +++ b/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)) { diff --git a/src/modules/commander/Commander.hpp b/src/modules/commander/Commander.hpp index 2ab2742a99..356543d9c7 100644 --- a/src/modules/commander/Commander.hpp +++ b/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) _param_com_posctl_navl, /* failsafe response to loss of navigation accuracy */ (ParamInt) _param_com_pos_fs_delay, - (ParamInt) _param_com_pos_fs_prob, - (ParamInt) _param_com_pos_fs_gain, (ParamInt) _param_com_low_bat_act, (ParamFloat) _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 */ diff --git a/src/modules/commander/commander_params.c b/src/modules/commander/commander_params.c index 34a632d625..f36ad0c096 100644 --- a/src/modules/commander/commander_params.c +++ b/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. *