From 3344dba6271dfcc0f87eee7ef90b2402f929d184 Mon Sep 17 00:00:00 2001
From: Andrii Fil <root.fi36@gmail.com>
Date: Tue, 3 Sep 2024 19:12:18 +0300
Subject: [PATCH] AP_AHRS: support MAV_CMD_EXTERNAL_WIND_ESTIMATE

---
 libraries/AP_AHRS/AP_AHRS.h        | 6 ++++++
 libraries/AP_AHRS/AP_AHRS_DCM.cpp  | 7 +++++++
 libraries/AP_AHRS/AP_AHRS_DCM.h    | 2 ++
 libraries/AP_AHRS/AP_AHRS_config.h | 3 +++
 4 files changed, 18 insertions(+)

diff --git a/libraries/AP_AHRS/AP_AHRS.h b/libraries/AP_AHRS/AP_AHRS.h
index 16ec1009e0..e6d40a6fc5 100644
--- a/libraries/AP_AHRS/AP_AHRS.h
+++ b/libraries/AP_AHRS/AP_AHRS.h
@@ -131,6 +131,12 @@ public:
 #endif
     }
 
+#if AP_AHRS_EXTERNAL_WIND_ESTIMATE_ENABLED
+    void set_external_wind_estimate(float speed, float direction) {
+        dcm.set_external_wind_estimate(speed, direction);
+    }
+#endif
+
     // return the parameter AHRS_WIND_MAX in metres per second
     uint8_t get_max_wind() const {
         return _wind_max;
diff --git a/libraries/AP_AHRS/AP_AHRS_DCM.cpp b/libraries/AP_AHRS/AP_AHRS_DCM.cpp
index b26bdeb472..ad7f01e7be 100644
--- a/libraries/AP_AHRS/AP_AHRS_DCM.cpp
+++ b/libraries/AP_AHRS/AP_AHRS_DCM.cpp
@@ -1028,6 +1028,13 @@ void AP_AHRS_DCM::estimate_wind(void)
 #endif
 }
 
+#ifdef AP_AHRS_EXTERNAL_WIND_ESTIMATE_ENABLED
+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
 
 // return our current position estimate using
 // dead-reckoning or GPS
diff --git a/libraries/AP_AHRS/AP_AHRS_DCM.h b/libraries/AP_AHRS/AP_AHRS_DCM.h
index 0d3dedc5b5..aed6f0a4ba 100644
--- a/libraries/AP_AHRS/AP_AHRS_DCM.h
+++ b/libraries/AP_AHRS/AP_AHRS_DCM.h
@@ -78,6 +78,8 @@ public:
         return true;
     }
 
+    void set_external_wind_estimate(float speed, float direction);
+
     // return an airspeed estimate if available. return true
     // if we have an estimate
     bool airspeed_estimate(float &airspeed_ret) const override;
diff --git a/libraries/AP_AHRS/AP_AHRS_config.h b/libraries/AP_AHRS/AP_AHRS_config.h
index b0535c1f2e..c7b96306bc 100644
--- a/libraries/AP_AHRS/AP_AHRS_config.h
+++ b/libraries/AP_AHRS/AP_AHRS_config.h
@@ -41,3 +41,6 @@
 #define AP_AHRS_POSITION_RESET_ENABLED (BOARD_FLASH_SIZE>1024 && AP_AHRS_ENABLED)
 #endif
 
+#ifndef AP_AHRS_EXTERNAL_WIND_ESTIMATE_ENABLED
+#define AP_AHRS_EXTERNAL_WIND_ESTIMATE_ENABLED (BOARD_FLASH_SIZE>1024 && AP_AHRS_DCM_ENABLED)
+#endif