diff --git a/src/modules/commander/commander.cpp b/src/modules/commander/commander.cpp index 2e262f36b5..2fff1c686a 100644 --- a/src/modules/commander/commander.cpp +++ b/src/modules/commander/commander.cpp @@ -146,7 +146,7 @@ static constexpr uint8_t COMMANDER_MAX_GPS_NOISE = 60; /**< Maximum percentage #define STICK_ON_OFF_LIMIT 0.9f -#define POSITION_TIMEOUT (1 * 1000 * 1000) /**< consider the local or global position estimate invalid after 1000ms */ +#define POSITION_TIMEOUT 1 /**< default number of seconds of position health check failure required to declare the position invalid */ #define FAILSAFE_DEFAULT_TIMEOUT (3 * 1000 * 1000) /**< hysteresis time - the failsafe will trigger after 3 seconds in this state */ #define OFFBOARD_TIMEOUT 500000 #define DIFFPRESS_TIMEOUT 2000000 @@ -159,14 +159,14 @@ static constexpr uint8_t COMMANDER_MAX_GPS_NOISE = 60; /**< Maximum percentage #define INAIR_RESTART_HOLDOFF_INTERVAL 500000 /* Controls the probation period which is the amount of time required for position and velocity checks to pass before the validity can be changed from false to true*/ -#define POSVEL_PROBATION_TAKEOFF 30E6 /**< probation duration set at takeoff (usec) */ +#define POSVEL_PROBATION_TAKEOFF 30 /**< probation duration set at takeoff (sec) */ #define POSVEL_PROBATION_MIN 1E6 /**< minimum probation duration (usec) */ #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_delay = POSITION_TIMEOUT * (1000 * 1000); +static int32_t posctl_nav_loss_prob = POSVEL_PROBATION_TAKEOFF * (1000 * 1000); static int32_t posctl_nav_loss_gain = POSVEL_VALID_PROBATION_FACTOR; /* @@ -1701,8 +1701,14 @@ int commander_thread_main(int argc, char *argv[]) control_status_leds(&status, &armed, true, &battery, &cpuload); + /* Get parameter values controlloing activation of position failure failsafe and convert to required units*/ + const int32_t sec_to_usec = (1000 * 1000); + int32_t init_param_val = POSITION_TIMEOUT; param_get(param_find("COM_POS_FS_DELAY"), &posctl_nav_loss_delay); + posctl_nav_loss_delay = init_param_val * sec_to_usec; // convert to uSec + init_param_val = POSVEL_PROBATION_TAKEOFF; param_get(param_find("COM_POS_FS_PROB"), &posctl_nav_loss_prob); + posctl_nav_loss_prob = init_param_val * sec_to_usec; // convert to uSec param_get(param_find("COM_POS_FS_GAIN"), &posctl_nav_loss_gain); /* now initialized */ diff --git a/src/modules/commander/commander_params.c b/src/modules/commander/commander_params.c index f9e5467e90..020cd2eb1d 100644 --- a/src/modules/commander/commander_params.c +++ b/src/modules/commander/commander_params.c @@ -641,37 +641,36 @@ PARAM_DEFINE_INT32(COM_ARM_AUTH, 256010); /** * Loss of position failsafe activation delay. * - * This sets number of uSec that the position checks need to be failed before the failsafe will activate. - * The default value has been optimised for rotary wing applications. For fixed wing applications, a larger value between 5E6 and 10E6 should be used. + * This sets number of second that the position checks need to be failed before the failsafe will activate. + * The default value has been optimised for rotary wing applications. For fixed wing applications, a larger value between 5 and 10 should be used. * - * @unit uSec + * @unit sec * @reboot_required true * @group Commander */ -PARAM_DEFINE_INT32(COM_POS_FS_DELAY, 1E6); +PARAM_DEFINE_INT32(COM_POS_FS_DELAY, 1); /** * Loss of position probation delay at takeoff. * - * The probation delay is number of uSec that the EKF innovation checks need to pass for the positon to be declared good once it has been declared bad. + * The probation delay is number of seconds that the EKF innovation checks need to pass for the positon 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 uSec for every lapsed uSec of valid position down to a minimum of 1E6 uSec. - * If position checks are failing, the probation delay will increase by COM_POS_FS_FACT micro seconds for every lapsed uSec up to a maximum of 100E6 uSec. - * The default value has been optimised for rotary wing applications. For fixed wing applications, a value of 1E6 should be used. + * 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 uSec + * @unit sec * @reboot_required true * @group Commander */ -PARAM_DEFINE_INT32(COM_POS_FS_PROB, 30E6); +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 2 should be used. + * The default value has been optimised for rotary wing applications. For fixed wing applications a value of 0 should be used. * - * @unit uSec * @reboot_required true * @group Commander */