mirror of https://github.com/ArduPilot/ardupilot
Copter: rename EKFCHECK_COMPASS to EKFCHECK_THRESH
Renamed because this threshold is now used for both compass and velocity variance. Also minor reorganisation of ekfcheck but no functional change.
This commit is contained in:
parent
d9bb3965ac
commit
b52f8351e8
|
@ -117,7 +117,7 @@ public:
|
|||
k_param_serial2_baud,
|
||||
k_param_land_repositioning,
|
||||
k_param_sonar, // sonar object
|
||||
k_param_ekfcheck_compass_thresh,// 54
|
||||
k_param_ekfcheck_thresh, // 54
|
||||
k_param_terrain,
|
||||
|
||||
// 65: AP_Limits Library
|
||||
|
@ -388,7 +388,7 @@ public:
|
|||
AP_Int8 arming_check;
|
||||
|
||||
AP_Int8 land_repositioning;
|
||||
AP_Float ekfcheck_compass_thresh;
|
||||
AP_Float ekfcheck_thresh;
|
||||
|
||||
#if FRAME_CONFIG == HELI_FRAME
|
||||
// Heli
|
||||
|
|
|
@ -446,12 +446,12 @@ const AP_Param::Info var_info[] PROGMEM = {
|
|||
// @User: Advanced
|
||||
GSCALAR(land_repositioning, "LAND_REPOSITION", LAND_REPOSITION_DEFAULT),
|
||||
|
||||
// @Param: EKFCHECK_COMPASS
|
||||
// @DisplayName: EKF checker compass variance threshold
|
||||
// @Description: Allows setting the maximum acceptable compass/magnetometer variance (0 to disable check)
|
||||
// @Param: EKFCHECK_THRESH
|
||||
// @DisplayName: EKF checker compass and velocity variance threshold
|
||||
// @Description: Allows setting the maximum acceptable compass and velocity variance (0 to disable check)
|
||||
// @Values: 0:Disabled, 0.6:Default, 1.0:Relaxed
|
||||
// @User: Advanced
|
||||
GSCALAR(ekfcheck_compass_thresh, "EKFCHECK_COMPASS", EKFCHECK_COMPASS_THRESHOLD_DEFAULT),
|
||||
GSCALAR(ekfcheck_thresh, "EKFCHECK_THRESH", EKFCHECK_THRESHOLD_DEFAULT),
|
||||
|
||||
#if FRAME_CONFIG == HELI_FRAME
|
||||
// @Group: HS1_
|
||||
|
|
|
@ -302,8 +302,8 @@
|
|||
|
||||
//////////////////////////////////////////////////////////////////////////////
|
||||
// EKF Checker
|
||||
#ifndef EKFCHECK_COMPASS_THRESHOLD_DEFAULT
|
||||
# define EKFCHECK_COMPASS_THRESHOLD_DEFAULT 0.6f // EKF checker's default compass variance above which the EKF's horizontal position will be considered bad
|
||||
#ifndef EKFCHECK_THRESHOLD_DEFAULT
|
||||
# define EKFCHECK_THRESHOLD_DEFAULT 0.6f // EKF checker's default compass and velocity variance above which the EKF's horizontal position will be considered bad
|
||||
#endif
|
||||
|
||||
//////////////////////////////////////////////////////////////////////////////
|
||||
|
|
|
@ -35,7 +35,7 @@ static struct {
|
|||
void ekf_check()
|
||||
{
|
||||
// return immediately if motors are not armed, ekf check is disabled, no inertial-nav position yet or usb is connected
|
||||
if (!motors.armed() || g.ekfcheck_compass_thresh == 0.0f || !inertial_nav.position_ok() || ap.usb_connected) {
|
||||
if (!motors.armed() || g.ekfcheck_thresh == 0.0f || !inertial_nav.position_ok() || ap.usb_connected) {
|
||||
ekf_check_state.fail_count_compass = 0;
|
||||
ekf_check_state.bad_compass = 0;
|
||||
AP_Notify::flags.ekf_bad = ekf_check_state.bad_compass;
|
||||
|
@ -45,7 +45,7 @@ void ekf_check()
|
|||
|
||||
// variances
|
||||
float compass_variance = 0;
|
||||
float velVar;
|
||||
float vel_variance = 9.0; // default set high to enable failsafe trigger if not using EKF
|
||||
|
||||
#if AP_AHRS_NAVEKF_AVAILABLE
|
||||
if (ahrs.have_inertial_nav()) {
|
||||
|
@ -53,21 +53,19 @@ void ekf_check()
|
|||
float posVar, hgtVar, tasVar;
|
||||
Vector3f magVar;
|
||||
Vector2f offset;
|
||||
ahrs.get_NavEKF().getVariances(velVar, posVar, hgtVar, magVar, tasVar, offset);
|
||||
ahrs.get_NavEKF().getVariances(vel_variance, posVar, hgtVar, magVar, tasVar, offset);
|
||||
compass_variance = magVar.length();
|
||||
} else {
|
||||
// use complementary filter's acceleration corrections multiplied by conversion factor to make them general in the same range as the EKF's variances
|
||||
compass_variance = safe_sqrt(inertial_nav.accel_correction_hbf.x * inertial_nav.accel_correction_hbf.x + inertial_nav.accel_correction_hbf.y * inertial_nav.accel_correction_hbf.y) * EKF_CHECK_COMPASS_INAV_CONVERSION;
|
||||
// set velocity variance to a value that will enable failsafe trigger if not using EKF
|
||||
velVar = 2.0f;
|
||||
}
|
||||
#else
|
||||
// use complementary filter's acceleration corrections multiplied by conversion factor to make them general in the same range as the EKF's variances
|
||||
compass_variance = safe_sqrt(inertial_nav.accel_correction_hbf.x * inertial_nav.accel_correction_hbf.x + inertial_nav.accel_correction_hbf.y * inertial_nav.accel_correction_hbf.y) * EKF_CHECK_COMPASS_INAV_CONVERSION;
|
||||
#endif
|
||||
|
||||
// compare compass variance vs threshold
|
||||
if (compass_variance >= g.ekfcheck_compass_thresh) {
|
||||
// compare compass and velocity variance vs threshold
|
||||
if (compass_variance >= g.ekfcheck_thresh && vel_variance > g.ekfcheck_thresh) {
|
||||
// if compass is not yet flagged as bad
|
||||
if (!ekf_check_state.bad_compass) {
|
||||
// increase counter
|
||||
|
@ -84,14 +82,7 @@ void ekf_check()
|
|||
gcs_send_text_P(SEVERITY_HIGH,PSTR("EKF: compass variance"));
|
||||
ekf_check_state.last_warn_time = hal.scheduler->millis();
|
||||
}
|
||||
#if AP_AHRS_NAVEKF_AVAILABLE
|
||||
// trigger failsafe if there is also a large velocity variance indicating a divergence from GPS
|
||||
if (velVar > 1.0f) {
|
||||
failsafe_ekf_event();
|
||||
}
|
||||
#else
|
||||
failsafe_ekf_event();
|
||||
#endif
|
||||
}
|
||||
}
|
||||
} else {
|
||||
|
@ -126,7 +117,7 @@ static void failsafe_ekf_event()
|
|||
uint32_t last_gps_update_ms;
|
||||
|
||||
// return immediately if ekf failsafe already triggered or disabled
|
||||
if (failsafe.ekf || g.ekfcheck_compass_thresh <= 0.0f) {
|
||||
if (failsafe.ekf || g.ekfcheck_thresh <= 0.0f) {
|
||||
return;
|
||||
}
|
||||
|
||||
|
|
Loading…
Reference in New Issue