From 491ba08af1543587095fe5ebf4044b18be1b9c06 Mon Sep 17 00:00:00 2001 From: Paul Riseborough Date: Tue, 22 Aug 2017 20:57:42 +1000 Subject: [PATCH] commander: Add parameters to control position failsafe behaviour This is required because the hardcoded values were too sensitive for fixed wing use and bad initial mag heading could trigger the failsafe and lead to crashes on launch. The defaults have been left unchanged. Suitable values for fixed wing use will need to be implemented in the airframe specific config file. --- src/modules/commander/commander.cpp | 31 +++++++++++++++-------- src/modules/commander/commander_params.c | 32 ++++++++++++++++++++++++ 2 files changed, 52 insertions(+), 11 deletions(-) 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);