From d28182435ed9cf18539027be9753d715dc4c40b5 Mon Sep 17 00:00:00 2001 From: Leonard Hall Date: Mon, 5 Jun 2017 21:39:29 +1000 Subject: [PATCH] AP_Motors: allow for negative thrust expo This allows for motors with a thrust curve that decreases with demand. --- libraries/AP_Motors/AP_MotorsMulticopter.cpp | 14 +++++++------- 1 file changed, 7 insertions(+), 7 deletions(-) diff --git a/libraries/AP_Motors/AP_MotorsMulticopter.cpp b/libraries/AP_Motors/AP_MotorsMulticopter.cpp index 22ec517523..090bc85a1a 100644 --- a/libraries/AP_Motors/AP_MotorsMulticopter.cpp +++ b/libraries/AP_Motors/AP_MotorsMulticopter.cpp @@ -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