AP_NavEKF_Source: set compass_required if we require a compass

This commit is contained in:
Peter Barker 2020-11-23 09:22:37 +11:00 committed by Peter Barker
parent 9e20f4a68d
commit 8124c3fea8
1 changed files with 4 additions and 2 deletions

View File

@ -417,11 +417,13 @@ bool AP_NavEKF_Source::pre_arm_check(char *failure_msg, uint8_t failure_msg_len)
// check yaw // check yaw
switch ((SourceYaw)_source_set[i].yaw.get()) { switch ((SourceYaw)_source_set[i].yaw.get()) {
case SourceYaw::NONE: case SourceYaw::NONE:
case SourceYaw::COMPASS:
case SourceYaw::EXTERNAL: case SourceYaw::EXTERNAL:
case SourceYaw::EXTERNAL_COMPASS_FALLBACK:
// valid yaw value // valid yaw value
break; break;
case SourceYaw::COMPASS:
case SourceYaw::EXTERNAL_COMPASS_FALLBACK:
compass_required = true;
break;
default: default:
// invalid yaw value // invalid yaw value
hal.util->snprintf(failure_msg, failure_msg_len, "Check EK3_SRC%d_YAW", (int)i+1); hal.util->snprintf(failure_msg, failure_msg_len, "Check EK3_SRC%d_YAW", (int)i+1);