Copter: fix tuning knob rate control of winch

This commit is contained in:
Randy Mackay 2017-10-26 09:45:25 +09:00
parent 787954fa37
commit 47bbf49aa6

View File

@ -215,11 +215,11 @@ void Copter::tuning() {
case TUNING_WINCH: {
float desired_rate = 0.0f;
if (v > 0.8f) {
desired_rate = g2.winch.get_rate_max();
if (v > 0.6f) {
desired_rate = g2.winch.get_rate_max() * (v - 0.6f) / 0.4f;
}
if (v < 0.2f) {
desired_rate = -g2.winch.get_rate_max();
if (v < 0.4f) {
desired_rate = g2.winch.get_rate_max() * (v - 0.4) / 0.4f;
}
g2.winch.set_desired_rate(desired_rate);
break;