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

AP_Motors: refactor apply_thrust_curve_and_volt_scaling

This commit is contained in:
Jonathan Challinger 2015-04-28 11:17:28 -07:00 committed by Randy Mackay
parent f381ef93e8
commit e2ba351149
1 changed files with 11 additions and 4 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

15
libraries/AP_Motors/AP_Motors_Class.cpp
View File

@ -289,12 +289,19 @@ void AP_Motors::current_limit_max_throttle()
// apply_thrust_curve_and_volt_scaling - returns throttle curve adjusted pwm value (i.e. 1000 ~ 2000)
int16_t AP_Motors::apply_thrust_curve_and_volt_scaling(int16_t pwm_out, int16_t pwm_min, int16_t pwm_max) const
{
float temp_out = ((float)(pwm_out-pwm_min))/((float)(pwm_max-pwm_min));
// convert to 0.0 to 1.0 ratio
float throttle_ratio = ((float)(pwm_out-pwm_min))/((float)(pwm_max-pwm_min));
// apply thrust curve - domain 0.0 to 1.0, range 0.0 to 1.0
if (_thrust_curve_expo > 0.0f){
temp_out = ((_thrust_curve_expo-1.0f) + safe_sqrt((1.0f-_thrust_curve_expo)*(1.0f-_thrust_curve_expo) + 4.0f*_thrust_curve_expo*_lift_max*temp_out))/(2.0f*_thrust_curve_expo*_batt_voltage_filt.get());
throttle_ratio = ((_thrust_curve_expo-1.0f) + safe_sqrt((1.0f-_thrust_curve_expo)*(1.0f-_thrust_curve_expo) + 4.0f*_thrust_curve_expo*_lift_max*throttle_ratio))/(2.0f*_thrust_curve_expo*_batt_voltage_filt.get());
}
temp_out = constrain_float(temp_out*_thrust_curve_max*(pwm_max-pwm_min)+pwm_min, pwm_min, pwm_max);
return (int16_t)temp_out;
// scale to maximum thrust point
throttle_ratio *= _thrust_curve_max;
// convert back to pwm range, constrain and return
return (int16_t)constrain_float(throttle_ratio*(pwm_max-pwm_min)+pwm_min, pwm_min, (pwm_max-pwm_min)*_thrust_curve_max+pwm_min);
}
// update_lift_max from battery voltage - used for voltage compensation