diff --git a/libraries/AP_Motors/AP_MotorsHeli_Quad.cpp b/libraries/AP_Motors/AP_MotorsHeli_Quad.cpp index f2a173d628..f1d4461694 100644 --- a/libraries/AP_Motors/AP_MotorsHeli_Quad.cpp +++ b/libraries/AP_Motors/AP_MotorsHeli_Quad.cpp @@ -239,6 +239,8 @@ void AP_MotorsHeli_Quad::move_actuators(float roll_out, float pitch_out, float c limit.throttle_lower = true; } + float collective_range = (_collective_max - _collective_min) / 1000.0f; + if (_heliflags.inverted_flight) { collective_out = 1 - collective_out; } @@ -249,6 +251,9 @@ void AP_MotorsHeli_Quad::move_actuators(float roll_out, float pitch_out, float c // scale collective to -1 to 1 collective_out = collective_out*2-1; + // reserve some collective for attitude control + collective_out *= collective_range; + if (collective_out < 0) { // with negative collective yaw torque is reversed yaw_out = -yaw_out;