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

AP_Motors: allow for negative thrust expo 

This allows for motors with a thrust curve that decreases with demand.
This commit is contained in:
Leonard Hall 2017-06-05 21:39:29 +10:00 committed by Andrew Tridgell
parent 5646afac1c
commit d28182435e
1 changed files with 7 additions and 7 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

14
libraries/AP_Motors/AP_MotorsMulticopter.cpp
View File

@ -297,12 +297,11 @@ float AP_MotorsMulticopter::apply_thrust_curve_and_volt_scaling(float thrust) co
{
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){
if(!is_zero(_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*thrust))/(2.0f*_thrust_curve_expo*_batt_voltage_filt.get());
} else {
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);
}
float thrust_curve_expo = constrain_float(_thrust_curve_expo, -1.0f, 1.0f);
if(!is_zero(_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*thrust))/(2.0f*thrust_curve_expo*_batt_voltage_filt.get());
} else {
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);
}
return constrain_float(throttle_ratio, 0.0f, 1.0f);
@ -328,7 +327,8 @@ void AP_MotorsMulticopter::update_lift_max_from_batt_voltage()
float batt_voltage_filt = _batt_voltage_filt.apply(_batt_voltage_resting_estimate/_batt_voltage_max, 1.0f/_loop_rate);
// calculate lift max
_lift_max = batt_voltage_filt*(1-_thrust_curve_expo) + _thrust_curve_expo*batt_voltage_filt*batt_voltage_filt;
float thrust_curve_expo = constrain_float(_thrust_curve_expo, -1.0f, 1.0f);
_lift_max = batt_voltage_filt*(1-thrust_curve_expo) + thrust_curve_expo*batt_voltage_filt*batt_voltage_filt;
}
float AP_MotorsMulticopter::get_compensation_gain() const