diff --git a/libraries/AP_Motors/AP_MotorsCoax.cpp b/libraries/AP_Motors/AP_MotorsCoax.cpp index e65699f95d..5a92488817 100644 --- a/libraries/AP_Motors/AP_MotorsCoax.cpp +++ b/libraries/AP_Motors/AP_MotorsCoax.cpp @@ -136,11 +136,12 @@ void AP_MotorsCoax::output_armed_stabilizing() float actuator_allowed = 0.0f; // amount of yaw we can fit in // apply voltage and air pressure compensation - roll_thrust = _roll_in * get_compensation_gain(); - pitch_thrust = _pitch_in * get_compensation_gain(); - yaw_thrust = _yaw_in * get_compensation_gain(); - throttle_thrust = get_throttle() * get_compensation_gain(); - throttle_avg_max = _throttle_avg_max * get_compensation_gain(); + const float compensation_gain = get_compensation_gain(); + roll_thrust = _roll_in * compensation_gain; + pitch_thrust = _pitch_in * compensation_gain; + yaw_thrust = _yaw_in * compensation_gain; + throttle_thrust = get_throttle() * compensation_gain; + throttle_avg_max = _throttle_avg_max * compensation_gain; // sanity check throttle is above zero and below current limited throttle if (throttle_thrust <= 0.0f) { diff --git a/libraries/AP_Motors/AP_MotorsMatrix.cpp b/libraries/AP_Motors/AP_MotorsMatrix.cpp index da514fea18..3092bff1b9 100644 --- a/libraries/AP_Motors/AP_MotorsMatrix.cpp +++ b/libraries/AP_Motors/AP_MotorsMatrix.cpp @@ -146,11 +146,12 @@ void AP_MotorsMatrix::output_armed_stabilizing() float thr_adj; // the difference between the pilot's desired throttle and throttle_thrust_best_rpy // apply voltage and air pressure compensation - roll_thrust = _roll_in * get_compensation_gain(); - pitch_thrust = _pitch_in * get_compensation_gain(); - yaw_thrust = _yaw_in * get_compensation_gain(); - throttle_thrust = get_throttle() * get_compensation_gain(); - throttle_avg_max = _throttle_avg_max * get_compensation_gain(); + const float compensation_gain = get_compensation_gain(); + roll_thrust = _roll_in * compensation_gain; + pitch_thrust = _pitch_in * compensation_gain; + yaw_thrust = _yaw_in * compensation_gain; + throttle_thrust = get_throttle() * compensation_gain; + throttle_avg_max = _throttle_avg_max * compensation_gain; // sanity check throttle is above zero and below current limited throttle if (throttle_thrust <= 0.0f) { diff --git a/libraries/AP_Motors/AP_MotorsSingle.cpp b/libraries/AP_Motors/AP_MotorsSingle.cpp index 94bfc63da9..7b7096f132 100644 --- a/libraries/AP_Motors/AP_MotorsSingle.cpp +++ b/libraries/AP_Motors/AP_MotorsSingle.cpp @@ -146,11 +146,12 @@ void AP_MotorsSingle::output_armed_stabilizing() float actuator_max = 0.0f; // maximum actuator value // apply voltage and air pressure compensation - roll_thrust = _roll_in * get_compensation_gain(); - pitch_thrust = _pitch_in * get_compensation_gain(); - yaw_thrust = _yaw_in * get_compensation_gain(); - throttle_thrust = get_throttle() * get_compensation_gain(); - throttle_avg_max = _throttle_avg_max * get_compensation_gain(); + const float compensation_gain = get_compensation_gain(); + roll_thrust = _roll_in * compensation_gain; + pitch_thrust = _pitch_in * compensation_gain; + yaw_thrust = _yaw_in * compensation_gain; + throttle_thrust = get_throttle() * compensation_gain; + throttle_avg_max = _throttle_avg_max * compensation_gain; // sanity check throttle is above zero and below current limited throttle if (throttle_thrust <= 0.0f) { diff --git a/libraries/AP_Motors/AP_MotorsTri.cpp b/libraries/AP_Motors/AP_MotorsTri.cpp index 63dff1fa82..ad7d712292 100644 --- a/libraries/AP_Motors/AP_MotorsTri.cpp +++ b/libraries/AP_Motors/AP_MotorsTri.cpp @@ -134,11 +134,12 @@ void AP_MotorsTri::output_armed_stabilizing() _yaw_servo_angle_max_deg = constrain_float(_yaw_servo_angle_max_deg, AP_MOTORS_TRI_SERVO_RANGE_DEG_MIN, AP_MOTORS_TRI_SERVO_RANGE_DEG_MAX); // apply voltage and air pressure compensation - roll_thrust = _roll_in * get_compensation_gain(); - pitch_thrust = _pitch_in * get_compensation_gain(); - yaw_thrust = _yaw_in * get_compensation_gain()*sinf(radians(_yaw_servo_angle_max_deg)); // we scale this so a thrust request of 1.0f will ask for full servo deflection at full rear throttle - throttle_thrust = get_throttle() * get_compensation_gain(); - throttle_avg_max = _throttle_avg_max * get_compensation_gain(); + const float compensation_gain = get_compensation_gain(); + roll_thrust = _roll_in * compensation_gain; + pitch_thrust = _pitch_in * compensation_gain; + yaw_thrust = _yaw_in * compensation_gain * sinf(radians(_yaw_servo_angle_max_deg)); // we scale this so a thrust request of 1.0f will ask for full servo deflection at full rear throttle + throttle_thrust = get_throttle() * compensation_gain; + throttle_avg_max = _throttle_avg_max * compensation_gain; // calculate angle of yaw pivot _pivot_angle = safe_asin(yaw_thrust);