Browse Source

Commander: Relax pre-arm check for EKF

The previous testing ratios could lead to extremely tight pre-arm acceptance.
release/1.12
Lorenz Meier 4 years ago committed by Daniel Agar
parent
commit
90f5d7338c
  1. 4
      src/modules/commander/Arming/PreFlightCheck/checks/ekf2Check.cpp

4
src/modules/commander/Arming/PreFlightCheck/checks/ekf2Check.cpp

@ -242,7 +242,7 @@ bool PreFlightCheck::ekf2CheckSensorBias(orb_advert_t *mavlink_log_pub, const bo @@ -242,7 +242,7 @@ bool PreFlightCheck::ekf2CheckSensorBias(orb_advert_t *mavlink_log_pub, const bo
// check accelerometer bias estimates
if (bias.accel_bias_valid) {
const float ekf_ab_test_limit = 0.5f * bias.accel_bias_limit;
const float ekf_ab_test_limit = 0.75f * bias.accel_bias_limit;
for (uint8_t axis_index = 0; axis_index < 3; axis_index++) {
// allow for higher uncertainty in estimates for axes that are less observable to prevent false positives
@ -263,7 +263,7 @@ bool PreFlightCheck::ekf2CheckSensorBias(orb_advert_t *mavlink_log_pub, const bo @@ -263,7 +263,7 @@ bool PreFlightCheck::ekf2CheckSensorBias(orb_advert_t *mavlink_log_pub, const bo
// check gyro bias estimates
if (bias.gyro_bias_valid) {
const float ekf_gb_test_limit = 0.5f * bias.gyro_bias_limit;
const float ekf_gb_test_limit = 0.75f * bias.gyro_bias_limit;
for (uint8_t axis_index = 0; axis_index < 3; axis_index++) {
// allow for higher uncertainty in estimates for axes that are less observable to prevent false positives

Loading…
Cancel
Save