From c102270e722292accdae9914cefd4a10106b7557 Mon Sep 17 00:00:00 2001 From: Randy Mackay Date: Tue, 10 May 2022 13:17:04 +0900 Subject: [PATCH] Copter: ekf_has_relative_position may use wind estimate --- ArduCopter/system.cpp | 6 +++++- 1 file changed, 5 insertions(+), 1 deletion(-) diff --git a/ArduCopter/system.cpp b/ArduCopter/system.cpp index 2b9632b565..97467dd253 100644 --- a/ArduCopter/system.cpp +++ b/ArduCopter/system.cpp @@ -273,7 +273,7 @@ bool Copter::ekf_has_relative_position() const return false; } - // return immediately if neither optflow nor visual odometry is enabled + // return immediately if neither optflow nor visual odometry nor wind estimatation is enabled bool enabled = false; #if AP_OPTICALFLOW_ENABLED if (optflow.enabled()) { @@ -285,6 +285,10 @@ bool Copter::ekf_has_relative_position() const enabled = true; } #endif + Vector3f airspeed_vec_bf; + if (AP::ahrs().airspeed_vector_true(airspeed_vec_bf)) { + enabled = true; + } if (!enabled) { return false; }