From 751dbb7df75ed2964c22600bcaf3c0f2f8ba4ee1 Mon Sep 17 00:00:00 2001 From: Leonard Hall Date: Sat, 21 Feb 2015 16:55:13 +0900 Subject: [PATCH] Motors: move thrust curve and volt scaling to parent Moving from MotorsMatrix to Motors allows it to be used from other frames --- libraries/AP_Motors/AP_MotorsMatrix.cpp | 15 +++++---------- libraries/AP_Motors/AP_Motors_Class.cpp | 10 ++++++++++ libraries/AP_Motors/AP_Motors_Class.h | 3 +++ 3 files changed, 18 insertions(+), 10 deletions(-) diff --git a/libraries/AP_Motors/AP_MotorsMatrix.cpp b/libraries/AP_Motors/AP_MotorsMatrix.cpp index e4cde1ce25..b31ffba83b 100644 --- a/libraries/AP_Motors/AP_MotorsMatrix.cpp +++ b/libraries/AP_Motors/AP_MotorsMatrix.cpp @@ -352,18 +352,13 @@ void AP_MotorsMatrix::output_armed() } } - // adjust for throttle curve - if (_throttle_curve_enabled) { - for (i=0; i 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); - } - motor_out[i] = temp_out*(_thrust_curve_max*out_max_pwm-out_min_pwm)+out_min_pwm; - } + // apply thrust curve and voltage scaling + for (i=0; i 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); + } + return (temp_out*(_thrust_curve_max*pwm_max-pwm_min)+pwm_min); +} diff --git a/libraries/AP_Motors/AP_Motors_Class.h b/libraries/AP_Motors/AP_Motors_Class.h index b66e7d9d93..6de0a93596 100644 --- a/libraries/AP_Motors/AP_Motors_Class.h +++ b/libraries/AP_Motors/AP_Motors_Class.h @@ -180,6 +180,9 @@ protected: // update_max_throttle - updates the limits on _max_throttle if necessary taking into account slow_start_throttle flag void update_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; + // flag bitmask struct AP_Motors_flags { uint8_t armed : 1; // 1 if the motors are armed, 0 if disarmed