diff --git a/libraries/AP_WindVane/AP_WindVane.cpp b/libraries/AP_WindVane/AP_WindVane.cpp index 79f3312120..db283e4770 100644 --- a/libraries/AP_WindVane/AP_WindVane.cpp +++ b/libraries/AP_WindVane/AP_WindVane.cpp @@ -243,6 +243,22 @@ bool AP_WindVane::start_calibration() return false; } +// send mavlink wind message +void AP_WindVane::send_wind(mavlink_channel_t chan) +{ + // exit immediately if not enabled + if (!enabled()) { + return; + } + + // send wind + mavlink_msg_wind_send( + chan, + wrap_360(degrees(get_absolute_wind_direction_rad())), + get_true_wind_speed(), + 0); +} + // read an analog port and calculate the wind direction in earth-frame in radians // assumes voltage increases as wind vane moves clockwise float AP_WindVane::read_analog_direction_ef() diff --git a/libraries/AP_WindVane/AP_WindVane.h b/libraries/AP_WindVane/AP_WindVane.h index aa1cef898f..6d94180a4d 100644 --- a/libraries/AP_WindVane/AP_WindVane.h +++ b/libraries/AP_WindVane/AP_WindVane.h @@ -67,6 +67,9 @@ public: // start calibration routine bool start_calibration(); + // send mavlink wind message + void send_wind(mavlink_channel_t chan); + // parameter block static const struct AP_Param::GroupInfo var_info[];