From f588e9a23003f2e3378be7abadda7446fce08362 Mon Sep 17 00:00:00 2001
From: Peter Barker <pbarker@barker.dropbear.id.au>
Date: Tue, 8 Oct 2024 12:50:54 +1100
Subject: [PATCH] autotest: add simple test that wind estimates from DCM and
 EKF3 converge

---
 Tools/autotest/quadplane.py | 53 +++++++++++++++++++++++++++++++++++++
 1 file changed, 53 insertions(+)

diff --git a/Tools/autotest/quadplane.py b/Tools/autotest/quadplane.py
index a5a61fe763f79..932b674bd0536 100644
--- a/Tools/autotest/quadplane.py
+++ b/Tools/autotest/quadplane.py
@@ -1858,6 +1858,58 @@ def RTL_AUTOLAND_1_FROM_GUIDED(self):
 
         self.fly_home_land_and_disarm()
 
+    def WindEstimateConsistency(self):
+        '''test that DCM and EKF3 roughly agree on wind speed and direction'''
+        self.set_parameters({
+            'SIM_WIND_SPD': 10,   # metres/second
+            'SIM_WIND_DIR': 315,  # from the North-West
+        })
+        self.change_mode('TAKEOFF')
+        self.wait_ready_to_arm()
+        self.arm_vehicle()
+        self.delay_sim_time(180)
+        mlog = self.dfreader_for_current_onboard_log()
+        self.fly_home_land_and_disarm()
+
+        self.progress("Inspecting dataflash log")
+        match_start_time = None
+        dcm = None
+        xkf2 = None
+        while True:
+            m = mlog.recv_match(
+                type=['DCM', 'XKF2'],
+                blocking=True,
+            )
+            if m is None:
+                raise NotAchievedException("Did not see wind estimates match")
+
+            m_type = m.get_type()
+            if m_type == 'DCM':
+                dcm = m
+            else:
+                xkf2 = m
+            if dcm is None or xkf2 is None:
+                continue
+
+            now = m.TimeUS * 1e-6
+
+            matches_east = abs(dcm.VWE-xkf2.VWE) < 1.5
+            matches_north = abs(dcm.VWN-xkf2.VWN) < 1.5
+
+            matches = matches_east and matches_north
+
+            if not matches:
+                match_start_time = None
+                continue
+
+            if match_start_time is None:
+                match_start_time = now
+                continue
+
+            if now - match_start_time > 60:
+                self.progress("Wind estimates correlated")
+                break
+
     def tests(self):
         '''return list of all tests'''
 
@@ -1885,6 +1937,7 @@ def tests(self):
             self.GUIDEDToAUTO,
             self.BootInAUTO,
             self.Ship,
+            self.WindEstimateConsistency,
             self.MAV_CMD_NAV_LOITER_TO_ALT,
             self.LoiterAltQLand,
             self.VTOLLandSpiral,