diff --git a/libraries/AC_AttitudeControl/AC_AttitudeControl_Heli.cpp b/libraries/AC_AttitudeControl/AC_AttitudeControl_Heli.cpp index f865266a0bbb6..39c9ad277e982 100644 --- a/libraries/AC_AttitudeControl/AC_AttitudeControl_Heli.cpp +++ b/libraries/AC_AttitudeControl/AC_AttitudeControl_Heli.cpp @@ -569,7 +569,8 @@ float AC_AttitudeControl_Heli::get_throttle_boosted(float throttle_in) float boost_factor = 1.0f / constrain_float(cos_tilt_target, 0.1f, 1.0f); // angle boost and inverted factor applied about the zero thrust collective - float throttle_out = (throttle_in - ((AP_MotorsHeli&)_motors).get_coll_mid()) * inverted_factor * boost_factor + ((AP_MotorsHeli&)_motors).get_coll_mid(); + const float coll_mid = ((AP_MotorsHeli&)_motors).get_coll_mid(); + float throttle_out = ((throttle_in - coll_mid) * inverted_factor * boost_factor) + coll_mid; _angle_boost = constrain_float(throttle_out - throttle_in, -1.0f, 1.0f); return throttle_out; } diff --git a/libraries/AC_AttitudeControl/AC_AttitudeControl_Heli.h b/libraries/AC_AttitudeControl/AC_AttitudeControl_Heli.h index 996439f631f91..b2695b59cfa03 100644 --- a/libraries/AC_AttitudeControl/AC_AttitudeControl_Heli.h +++ b/libraries/AC_AttitudeControl/AC_AttitudeControl_Heli.h @@ -90,10 +90,10 @@ class AC_AttitudeControl_Heli : public AC_AttitudeControl { void input_thrust_vector_heading(const Vector3f& thrust_vector, float heading_angle_cd, float heading_rate_cds) override; // enable/disable inverted flight - void set_inverted_flight(bool inverted) override { _inverted_flight = inverted;} + void set_inverted_flight(bool inverted) override { _inverted_flight = inverted; } // accessor for inverted flight flag - bool get_inverted_flight() override { return _inverted_flight;} + bool get_inverted_flight() override { return _inverted_flight; } // set the PID notch sample rates void set_notch_sample_rate(float sample_rate) override; @@ -136,7 +136,7 @@ class AC_AttitudeControl_Heli : public AC_AttitudeControl { float _passthrough_yaw; // get_roll_trim - angle in centi-degrees to be added to roll angle. Used by helicopter to counter tail rotor thrust in hover - float get_roll_trim_rad() override { return constrain_float(radians(get_roll_trim_cd() * 0.01f), -radians(10.0f),radians(10.0f));} + float get_roll_trim_rad() override { return radians(get_roll_trim_cd() * 0.01); } // internal variables float _hover_roll_trim_scalar = 0; // scalar used to suppress Hover Roll Trim