Skip to content

Commit

Permalink
AC_AttitudeControl: clean up to read easier
Browse files Browse the repository at this point in the history
Co-authored-by: Peter Hall <[email protected]>
  • Loading branch information
bnsgeyer and IamPete1 authored Apr 1, 2024
1 parent f2cfc24 commit e40e253
Showing 1 changed file with 2 additions and 1 deletion.
3 changes: 2 additions & 1 deletion libraries/AC_AttitudeControl/AC_AttitudeControl_Heli.cpp
Original file line number Diff line number Diff line change
Expand Up @@ -567,7 +567,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;
}
Expand Down

0 comments on commit e40e253

Please sign in to comment.