diff --git a/libraries/AP_WindVane/AP_WindVane_ModernDevice.cpp b/libraries/AP_WindVane/AP_WindVane_ModernDevice.cpp index b5bcc70ca8..8a892f739f 100644 --- a/libraries/AP_WindVane/AP_WindVane_ModernDevice.cpp +++ b/libraries/AP_WindVane/AP_WindVane_ModernDevice.cpp @@ -52,3 +52,10 @@ void AP_WindVane_ModernDevice::update_speed() // simplified equation from data sheet, converted from mph to m/s speed_update_frontend(24.254896f * powf((analog_voltage / powf(temp_ambient, 0.115157f)), 3.009364f)); } + +void AP_WindVane_ModernDevice::calibrate() +{ + gcs().send_text(MAV_SEVERITY_INFO, "WindVane: rev P. zero wind voltage offset set to %.1f",double(_current_analog_voltage)); + _frontend._speed_sensor_voltage_offset.set_and_save(_current_analog_voltage); + _frontend._calibration.set_and_save(0); +}