Mirror/ardupilot
5
0
Fork 0
mirror of https://github.com/ArduPilot/ardupilot synced 2025-01-10 18:08:30 -04:00
Code Issues Packages Projects Releases Wiki Activity

AP_MotorsMulticopter: apply_thrust_curve_and_volt_scaling in 0 to 1 range

Browse Source 
existing equivalent pwm function becomes apply_thrust_curve_and_volt_scaling_pwm
This commit is contained in:
Leonard Hall 2015-12-03 15:49:19 +09:00 committed by Randy Mackay
parent cb39f8aab7
commit 65bbc23a08
2 changed files with 18 additions and 2 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

17
libraries/AP_Motors/AP_MotorsMulticopter.cpp
View File

@ -240,7 +240,7 @@ void AP_MotorsMulticopter::current_limit_max_throttle()
}
// apply_thrust_curve_and_volt_scaling - returns throttle curve adjusted pwm value (i.e. 1000 ~ 2000)
int16_t AP_MotorsMulticopter::apply_thrust_curve_and_volt_scaling(int16_t pwm_out, int16_t pwm_min, int16_t pwm_max) const
int16_t AP_MotorsMulticopter::apply_thrust_curve_and_volt_scaling_pwm(int16_t pwm_out, int16_t pwm_min, int16_t pwm_max) const
{
// convert to 0.0 to 1.0 ratio
float throttle_ratio = ((float)(pwm_out-pwm_min))/((float)(pwm_max-pwm_min));
@ -257,6 +257,21 @@ int16_t AP_MotorsMulticopter::apply_thrust_curve_and_volt_scaling(int16_t pwm_ou
return (int16_t)constrain_float(throttle_ratio*(pwm_max-pwm_min)+pwm_min, pwm_min, (pwm_max-pwm_min)*_thrust_curve_max+pwm_min);
}
// apply_thrust_curve_and_volt_scaling - returns throttle in the range 0 ~ 1
float AP_MotorsMulticopter::apply_thrust_curve_and_volt_scaling(float thrust) const
{
float throttle_ratio = thrust;
// apply thrust curve - domain 0.0 to 1.0, range 0.0 to 1.0
if (_thrust_curve_expo > 0.0f){
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*thrust))/(2.0f*_thrust_curve_expo*_batt_voltage_filt.get());
}
// scale to maximum thrust point
throttle_ratio *= _thrust_curve_max;
return constrain_float(throttle_ratio, 0.0f, _thrust_curve_max);
}
// update_lift_max from battery voltage - used for voltage compensation
void AP_MotorsMulticopter::update_lift_max_from_batt_voltage()
{

3
libraries/AP_Motors/AP_MotorsMulticopter.h
View File

@ -110,7 +110,8 @@ protected:
void current_limit_max_throttle();
// apply_thrust_curve_and_volt_scaling - thrust curve and voltage adjusted pwm value (i.e. 1000 ~ 2000)
int16_t apply_thrust_curve_and_volt_scaling(int16_t pwm_out, int16_t pwm_min, int16_t pwm_max) const;
int16_t apply_thrust_curve_and_volt_scaling_pwm(int16_t pwm_out, int16_t pwm_min, int16_t pwm_max) const;
float apply_thrust_curve_and_volt_scaling(float thrust) const;
// update_lift_max_from_batt_voltage - used for voltage compensation
void update_lift_max_from_batt_voltage();