diff --git a/src/modules/commander/commander.cpp b/src/modules/commander/commander.cpp index fee2b9ad91..2e262f36b5 100644 --- a/src/modules/commander/commander.cpp +++ b/src/modules/commander/commander.cpp @@ -164,14 +164,19 @@ static constexpr uint8_t COMMANDER_MAX_GPS_NOISE = 60; /**< Maximum percentage #define POSVEL_PROBATION_MAX 100E6 /**< maximum probation duration (usec) */ #define POSVEL_VALID_PROBATION_FACTOR 10 /**< the rate at which the probation duration is increased while checks are failing */ +/* Parameters controlling the sensitivity of the position failsafe */ +static int32_t posctl_nav_loss_delay = POSITION_TIMEOUT; +static int32_t posctl_nav_loss_prob = POSVEL_PROBATION_TAKEOFF; +static int32_t posctl_nav_loss_gain = POSVEL_VALID_PROBATION_FACTOR; + /* * Probation times for position and velocity validity checks to pass if failed * Signed integers are used because these can become negative values before constraints are applied */ -static int64_t gpos_probation_time_us = POSVEL_PROBATION_TAKEOFF; -static int64_t gvel_probation_time_us = POSVEL_PROBATION_TAKEOFF; -static int64_t lpos_probation_time_us = POSVEL_PROBATION_TAKEOFF; -static int64_t lvel_probation_time_us = POSVEL_PROBATION_TAKEOFF; +static int64_t gpos_probation_time_us = POSVEL_PROBATION_MIN; +static int64_t gvel_probation_time_us = POSVEL_PROBATION_MIN; +static int64_t lpos_probation_time_us = POSVEL_PROBATION_MIN; +static int64_t lvel_probation_time_us = POSVEL_PROBATION_MIN; /* Mavlink log uORB handle */ static orb_advert_t mavlink_log_pub = nullptr; @@ -1696,6 +1701,10 @@ int commander_thread_main(int argc, char *argv[]) control_status_leds(&status, &armed, true, &battery, &cpuload); + param_get(param_find("COM_POS_FS_DELAY"), &posctl_nav_loss_delay); + param_get(param_find("COM_POS_FS_PROB"), &posctl_nav_loss_prob); + param_get(param_find("COM_POS_FS_GAIN"), &posctl_nav_loss_gain); + /* now initialized */ commander_initialized = true; thread_running = true; @@ -2216,7 +2225,7 @@ int commander_thread_main(int argc, char *argv[]) } /* update condition_local_altitude_valid */ - check_valid(local_position.timestamp, POSITION_TIMEOUT, local_position.z_valid, + check_valid(local_position.timestamp, posctl_nav_loss_delay, local_position.z_valid, &(status_flags.condition_local_altitude_valid), &status_changed); /* Update land detector */ @@ -2234,10 +2243,10 @@ int commander_thread_main(int argc, char *argv[]) // 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 = POSVEL_PROBATION_TAKEOFF; - gvel_probation_time_us = POSVEL_PROBATION_TAKEOFF; - lpos_probation_time_us = POSVEL_PROBATION_TAKEOFF; - lvel_probation_time_us = POSVEL_PROBATION_TAKEOFF; + gpos_probation_time_us = posctl_nav_loss_prob; + gvel_probation_time_us = posctl_nav_loss_prob; + lpos_probation_time_us = posctl_nav_loss_prob; + lvel_probation_time_us = posctl_nav_loss_prob; } } @@ -3814,7 +3823,7 @@ check_posvel_validity(bool data_valid, float data_accuracy, float required_accur } else if (!*valid_state) { bool level_check_pass = data_valid && data_accuracy < required_accuracy; if (!level_check_pass) { - *probation_time_us += (now - *last_fail_time_us) * POSVEL_VALID_PROBATION_FACTOR; + *probation_time_us += (now - *last_fail_time_us) * posctl_nav_loss_gain; *last_fail_time_us = now; } else if (now - *last_fail_time_us > *probation_time_us) { pos_inaccurate = false; @@ -3826,7 +3835,7 @@ check_posvel_validity(bool data_valid, float data_accuracy, float required_accur *last_fail_time_us = now; } - bool data_stale = (now - data_timestamp_us > POSITION_TIMEOUT); + bool data_stale = (now - data_timestamp_us > posctl_nav_loss_delay); // Set validity if (pos_status_changed) { diff --git a/src/modules/commander/commander_params.c b/src/modules/commander/commander_params.c index 4ea0a5c264..0e48c3fffa 100644 --- a/src/modules/commander/commander_params.c +++ b/src/modules/commander/commander_params.c @@ -637,3 +637,35 @@ PARAM_DEFINE_INT32(COM_POSCTL_NAVL, 0); * @group Commander */ PARAM_DEFINE_INT32(COM_ARM_AUTH, 256010); + +/** + * Loss of position failsafe activation delay. + * + * This sets number of micro seconds that the position checks need to be failed before the failsafe will activate. + * + * @reboot_required true + * @group Commander + */ +PARAM_DEFINE_INT32(COM_POS_FS_DELAY, 1E6); + +/** + * Loss of position probation delay at takeoff. + * + * This sets number of micro seconds at takeoff that the EKF innovation checks need to pass for the positon to be declared good, if it has been declared bad. + * The probation period reduces by one second for every lapsed second of valid position down to a minimum of one second. + * When checks fail, the probation period is increased by COM_POS_FS_FACT seconds for every lapsed second up to a maximum of 100 seconds + * + * @reboot_required true + * @group Commander + */ +PARAM_DEFINE_INT32(COM_POS_FS_PROB, 30E6); + +/** + * Loss of position probation gain factor. + * + * This sets the rate that the loss of position probation time grows when position checks are failing. + * + * @reboot_required true + * @group Commander + */ +PARAM_DEFINE_INT32(COM_POS_FS_GAIN, 10);