From 5bb78b9b903abbea25be57b61a1260643d4b1ae6 Mon Sep 17 00:00:00 2001
From: Andrii Fil <root.fi36@gmail.com>
Date: Wed, 28 Aug 2024 17:38:47 +0300
Subject: [PATCH] support MAV_CMD_EXTERNAL_WIND_ESTIMATE with autotest

---
 Tools/autotest/arduplane.py          | 41 ++++++++++++++++++++++++++++
 libraries/AP_AHRS/AP_AHRS.h          |  4 +--
 libraries/AP_AHRS/AP_AHRS_DCM.cpp    | 12 +++-----
 libraries/AP_AHRS/AP_AHRS_DCM.h      |  2 +-
 libraries/GCS_MAVLink/GCS_Common.cpp |  2 +-
 5 files changed, 49 insertions(+), 12 deletions(-)

diff --git a/Tools/autotest/arduplane.py b/Tools/autotest/arduplane.py
index 3ca7780e5ff453..9416f762a9bf17 100644
--- a/Tools/autotest/arduplane.py
+++ b/Tools/autotest/arduplane.py
@@ -6052,6 +6052,46 @@ def ForceArm(self):
             )
             self.disarm_vehicle()
 
+    def _MAV_CMD_EXTERNAL_WIND_ESTIMATE(self, command):
+        cmp_with_variance = lambda a, b, p: abs(a - b) < p
+
+        self.set_parameters({
+            "SIM_WIND_SPD": 0,
+            "SIM_WIND_DIR": 180,
+        })
+
+        m = self.assert_receive_message("WIND")
+        start_speed = m.speed
+        start_direction = m.direction
+
+        def check_eq(speed, direction, ret_dir):
+            command(mavutil.mavlink.MAV_CMD_EXTERNAL_WIND_ESTIMATE, p1=speed, p3=direction)
+            for i in range(10):
+                m = self.assert_receive_message("WIND")
+
+                if cmp_with_variance(m.speed, speed, 0.5) and cmp_with_variance(m.direction, ret_dir, 5):
+                    break
+            else:
+                raise NotAchievedException("Wind is not equal to given values {} != {} or {} != {}".format(
+                    m.speed, speed, m.direction, ret_dir))
+
+        check_eq(1, 45, 45)
+        check_eq(2, 90, 90)
+        check_eq(3, 120, 120)
+        check_eq(4, 180, -180)
+        check_eq(5, 240, -120)
+        check_eq(6, 320, -40)
+        check_eq(7, 360, 0)
+
+        command(mavutil.mavlink.MAV_CMD_EXTERNAL_WIND_ESTIMATE, p1=-2, p3=90, want_result=mavutil.mavlink.MAV_RESULT_DENIED)
+        command(mavutil.mavlink.MAV_CMD_EXTERNAL_WIND_ESTIMATE, p1=2, p3=-90, want_result=mavutil.mavlink.MAV_RESULT_DENIED)
+        command(mavutil.mavlink.MAV_CMD_EXTERNAL_WIND_ESTIMATE, p1=-2, p3=-90, want_result=mavutil.mavlink.MAV_RESULT_DENIED)
+
+    def MAV_CMD_EXTERNAL_WIND_ESTIMATE(self):
+        '''test _MAV_CMD_EXTERNAL_WIND_ESTIMATE as a mavlink command'''
+        self._MAV_CMD_EXTERNAL_WIND_ESTIMATE(self.run_cmd)
+        self._MAV_CMD_EXTERNAL_WIND_ESTIMATE(self.run_cmd_int)
+
     def tests(self):
         '''return list of all tests'''
         ret = super(AutoTestPlane, self).tests()
@@ -6183,6 +6223,7 @@ def tests(self):
             self.GPSPreArms,
             self.SetHomeAltChange,
             self.ForceArm,
+            self.MAV_CMD_EXTERNAL_WIND_ESTIMATE,
         ])
         return ret
 
diff --git a/libraries/AP_AHRS/AP_AHRS.h b/libraries/AP_AHRS/AP_AHRS.h
index 716c2d7ea07e47..e6d40a6fc5fa47 100644
--- a/libraries/AP_AHRS/AP_AHRS.h
+++ b/libraries/AP_AHRS/AP_AHRS.h
@@ -132,8 +132,8 @@ class AP_AHRS {
     }
 
 #if AP_AHRS_EXTERNAL_WIND_ESTIMATE_ENABLED
-    void external_wind_estimate(float speed, float direction) {
-        dcm.external_wind_estimate(speed, direction);
+    void set_external_wind_estimate(float speed, float direction) {
+        dcm.set_external_wind_estimate(speed, direction);
     }
 #endif
 
diff --git a/libraries/AP_AHRS/AP_AHRS_DCM.cpp b/libraries/AP_AHRS/AP_AHRS_DCM.cpp
index 04a70ad693e57a..ad7f01e7be2d46 100644
--- a/libraries/AP_AHRS/AP_AHRS_DCM.cpp
+++ b/libraries/AP_AHRS/AP_AHRS_DCM.cpp
@@ -1029,14 +1029,10 @@ void AP_AHRS_DCM::estimate_wind(void)
 }
 
 #ifdef AP_AHRS_EXTERNAL_WIND_ESTIMATE_ENABLED
-void AP_AHRS_DCM::external_wind_estimate(float speed, float direction) {
-    Vector3f wind = Vector3f();
-    wind.z = 0;
-
-    wind.x = -cosf(radians(direction)) * speed;
-    wind.y = -sinf(radians(direction)) * speed;
-
-    _wind = wind;
+void AP_AHRS_DCM::set_external_wind_estimate(float speed, float direction) {
+    _wind.x = -cosf(radians(direction)) * speed;
+    _wind.y = -sinf(radians(direction)) * speed;
+    _wind.z = 0;
 }
 #endif
 
diff --git a/libraries/AP_AHRS/AP_AHRS_DCM.h b/libraries/AP_AHRS/AP_AHRS_DCM.h
index 2644850998a9ea..aed6f0a4ba8727 100644
--- a/libraries/AP_AHRS/AP_AHRS_DCM.h
+++ b/libraries/AP_AHRS/AP_AHRS_DCM.h
@@ -78,7 +78,7 @@ class AP_AHRS_DCM : public AP_AHRS_Backend {
         return true;
     }
 
-    void external_wind_estimate(float speed, float direction);
+    void set_external_wind_estimate(float speed, float direction);
 
     // return an airspeed estimate if available. return true
     // if we have an estimate
diff --git a/libraries/GCS_MAVLink/GCS_Common.cpp b/libraries/GCS_MAVLink/GCS_Common.cpp
index 1a2be9f5700af7..e6318c9e2a15f2 100644
--- a/libraries/GCS_MAVLink/GCS_Common.cpp
+++ b/libraries/GCS_MAVLink/GCS_Common.cpp
@@ -5243,7 +5243,7 @@ MAV_RESULT GCS_MAVLINK::handle_command_int_external_wind_estimate(const mavlink_
         return MAV_RESULT_DENIED;
     }
 
-    AP::ahrs().external_wind_estimate(packet.param1, packet.param3);
+    AP::ahrs().set_external_wind_estimate(packet.param1, packet.param3);
     return MAV_RESULT_ACCEPTED;
 }
 #endif // AP_AHRS_EXTERNAL_WIND_ESTIMATE_ENABLED