support MAV_CMD_EXTERNAL_WIND_ESTIMATE

ArduPilot · Aug 15, 2024 · ec5c124 · ec5c124
1 parent 61bd5e9
commit ec5c124
Show file tree

Hide file tree

Showing 6 changed files with 40 additions and 0 deletions.
diff --git a/libraries/AP_AHRS/AP_AHRS.h b/libraries/AP_AHRS/AP_AHRS.h
@@ -131,6 +131,12 @@ class AP_AHRS {
 #endif
     }
 
+    void external_wind_estimate(float speed, float direction) {
+#if AP_AHRS_EXTERNAL_WIND_ESTIMATE_ENABLED
+        dcm.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
@@ -1028,6 +1028,15 @@ void AP_AHRS_DCM::estimate_wind(void)
 #endif
 }
 
+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;
+}
 
 // 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
@@ -78,6 +78,8 @@ class AP_AHRS_DCM : public AP_AHRS_Backend {
         return true;
     }
 
+    void 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
@@ -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 AP_AHRS_DCM_ENABLED
+#endif
diff --git a/libraries/GCS_MAVLink/GCS.h b/libraries/GCS_MAVLink/GCS.h
@@ -525,6 +525,7 @@ class GCS_MAVLINK
 
     virtual MAV_RESULT handle_command_int_packet(const mavlink_command_int_t &packet, const mavlink_message_t &msg);
     MAV_RESULT handle_command_int_external_position_estimate(const mavlink_command_int_t &packet);
+    MAV_RESULT handle_command_int_external_wind_estimate(const mavlink_command_int_t &packet);
 
 #if AP_HOME_ENABLED
     MAV_RESULT handle_command_do_set_home(const mavlink_command_int_t &packet);

diff --git a/libraries/GCS_MAVLink/GCS_Common.cpp b/libraries/GCS_MAVLink/GCS_Common.cpp
@@ -5233,6 +5233,21 @@ MAV_RESULT GCS_MAVLINK::handle_command_int_external_position_estimate(const mavl
 }
 #endif // AP_AHRS_POSITION_RESET_ENABLED
 
+#if AP_AHRS_EXTERNAL_WIND_ESTIMATE_ENABLED
+MAV_RESULT GCS_MAVLINK::handle_command_int_external_wind_estimate(const mavlink_command_int_t &packet)
+{
+    if (packet.param1 < 0) {
+        return MAV_RESULT_DENIED;
+    }
+    if (packet.param3 < 0 || packet.param3 > 360) {
+        return MAV_RESULT_DENIED;
+    }
+
+    AP::ahrs().external_wind_estimate(packet.param1, packet.param3);
+    return MAV_RESULT_ACCEPTED;
+}
+#endif // AP_AHRS_EXTERNAL_WIND_ESTIMATE_ENABLED
+
 MAV_RESULT GCS_MAVLINK::handle_command_do_set_roi(const mavlink_command_int_t &packet)
 {
     // be aware that this method is called for both MAV_CMD_DO_SET_ROI
@@ -5415,6 +5430,10 @@ MAV_RESULT GCS_MAVLINK::handle_command_int_packet(const mavlink_command_int_t &p
     case MAV_CMD_EXTERNAL_POSITION_ESTIMATE:
         return handle_command_int_external_position_estimate(packet);
 #endif
+#if AP_AHRS_EXTERNAL_WIND_ESTIMATE_ENABLED
+    case MAV_CMD_EXTERNAL_WIND_ESTIMATE:
+        return handle_command_int_external_wind_estimate(packet);
+#endif
 #if AP_ARMING_ENABLED
     case MAV_CMD_COMPONENT_ARM_DISARM:
         return handle_command_component_arm_disarm(packet);