fly316/PX4-Autopilot
7
0
Fork 0
mirror of https://gitee.com/mirrors_PX4/PX4-Autopilot.git synced 2026-07-17 06:40:35 +08:00
Code Issues Packages Projects Releases Wiki Activity

commander: Add parameters to control position failsafe behaviour

Browse Source 
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.
This commit is contained in:
Paul Riseborough
2017-08-22 20:57:42 +10:00
committed by Lorenz Meier
parent f67ac8ba00
commit 491ba08af1
2 changed files with 52 additions and 11 deletions
Download Patch File Download Diff File Expand all files Collapse all files
src/modules/commander/commander.cpp
+20 -11
View File
@@ -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) {