Mirror
/
ardupilot
mirror of https://github.com/ArduPilot/ardupilot
5
0
Fork 0
Code Issues Packages Projects Releases Wiki Activity

AP_Motors: reduce unnecessary multiple call of get_compensation_gain()

This commit is contained in:
night-ghost 2018-01-24 18:42:10 +11:00 committed by Randy Mackay
parent 60c3ae1ec2
commit 3165d72647
4 changed files with 24 additions and 20 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

11
libraries/AP_Motors/AP_MotorsCoax.cpp
View File

@ -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) {

11
libraries/AP_Motors/AP_MotorsMatrix.cpp
View File

@ -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) {

11
libraries/AP_Motors/AP_MotorsSingle.cpp
View File

@ -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) {

11
libraries/AP_Motors/AP_MotorsTri.cpp
View File

@ -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);