dark0707
/
px4-firmware
forked from Archive/PX4-Autopilot
1
0
Fork 0
Code Pull Requests Activity

Commander: remove dynamic position velocity probation period

This commit is contained in:
Matthias Grob 2022-03-22 21:35:35 +01:00 committed by Daniel Agar
parent 9fe2dfc2e3
commit eee4aaee4f
4 changed files with 14 additions and 94 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

2
ROMFS/px4fmu_common/init.d/rc.fw_defaults
View File

@ -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

63
src/modules/commander/Commander.cpp
View File

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

16
src/modules/commander/Commander.hpp
View File

@ -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 */

27
src/modules/commander/commander_params.c
View File

@ -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.
*