AP_WindVane: add modern devices rev p cal

This commit is contained in:
Peter Hall 2019-05-13 20:48:25 +01:00 committed by Randy Mackay
parent 54165dbc9c
commit f26e1dc8e8
1 changed files with 7 additions and 0 deletions

View File

@ -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);
}