diff --git a/libraries/AP_AHRS/AP_AHRS.h b/libraries/AP_AHRS/AP_AHRS.h index 16ec1009e03bf1..ed52980623ef51 100644 --- 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 index b26bdeb472714a..e48e2657fa9a23 100644 --- 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 index 0d3dedc5b54d96..2644850998a9ea 100644 --- 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 index b0535c1f2ed34c..77cdb26adbe0b9 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 AP_AHRS_DCM_ENABLED +#endif diff --git a/libraries/GCS_MAVLink/GCS.h b/libraries/GCS_MAVLink/GCS.h index 71725275b3e9ed..4463ae1b1ca8d0 100644 --- 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 index 4e22558349cbbc..1a2be9f5700af7 100644 --- 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);