From 5cace33ca59f3c15b86407aec7d1ef31e44c4a8c Mon Sep 17 00:00:00 2001 From: Leonard Hall Date: Thu, 9 Jun 2016 15:34:47 +0900 Subject: [PATCH] AP_MotorsMulticopter: SPIN_MIN replaces min_throttle --- libraries/AP_Motors/AP_MotorsMulticopter.cpp | 25 ++++++++++++-------- libraries/AP_Motors/AP_MotorsMulticopter.h | 9 +++---- 2 files changed, 20 insertions(+), 14 deletions(-) diff --git a/libraries/AP_Motors/AP_MotorsMulticopter.cpp b/libraries/AP_Motors/AP_MotorsMulticopter.cpp index 6079b4a309..b2d08a3ef0 100644 --- a/libraries/AP_Motors/AP_MotorsMulticopter.cpp +++ b/libraries/AP_Motors/AP_MotorsMulticopter.cpp @@ -102,6 +102,13 @@ const AP_Param::GroupInfo AP_MotorsMulticopter::var_info[] = { // @User: Advanced AP_GROUPINFO("PWM_MAX", 17, AP_MotorsMulticopter, _pwm_max, 0), + // @Param: SPIN_MIN + // @DisplayName: Motor Spin minimum + // @Description: Point at which the thrust starts expressed as a number from 0 to 1 in the entire output range + // @Values: 0.0:Low, 0.15:Default, 0.3:High + // @User: Advanced + AP_GROUPINFO("SPIN_MIN", 18, AP_MotorsMulticopter, _thrust_curve_min, AP_MOTORS_SPIN_MIN_DEFAULT), + // @Param: SPIN_ARM // @DisplayName: Motor Spin armed // @Description: Point at which the motors start to spin expressed as a number from 0 to 1 in the entire output range @@ -145,8 +152,8 @@ AP_MotorsMulticopter::AP_MotorsMulticopter(uint16_t loop_rate, uint16_t speed_hz _batt_voltage_filt.set_cutoff_frequency(AP_MOTORS_BATT_VOLT_FILT_HZ); _batt_voltage_filt.reset(1.0f); - // default throttle ranges (i.e. _min_throttle, _throttle_radio_min, _throttle_radio_max) - set_throttle_range(130, 1100, 1900); + // default throttle ranges (i.e. _throttle_radio_min, _throttle_radio_max) + set_throttle_range(1100, 1900); }; // output - sends commands to the motors @@ -309,8 +316,8 @@ float AP_MotorsMulticopter::get_compensation_gain() const int16_t AP_MotorsMulticopter::calc_thrust_to_pwm(float thrust_in) const { thrust_in = constrain_float(thrust_in, 0, 1); - return constrain_int16((get_pwm_output_min() + _min_throttle + apply_thrust_curve_and_volt_scaling(thrust_in) * - (get_pwm_output_max() - (get_pwm_output_min() + _min_throttle))), get_pwm_output_min() + _min_throttle, get_pwm_output_max()); + return get_pwm_output_min() + (get_pwm_output_max()-get_pwm_output_min()) * (_thrust_curve_min + (_thrust_curve_max-_thrust_curve_min)*apply_thrust_curve_and_volt_scaling(thrust_in)); +} int16_t AP_MotorsMulticopter::calc_spin_up_to_pwm() const { @@ -338,15 +345,13 @@ int16_t AP_MotorsMulticopter::get_pwm_output_max() const // set_throttle_range - sets the minimum throttle that will be sent to the engines when they're not off (i.e. to prevents issues with some motors spinning and some not at very low throttle) // also sets throttle channel minimum and maximum pwm -void AP_MotorsMulticopter::set_throttle_range(uint16_t min_throttle, int16_t radio_min, int16_t radio_max) +void AP_MotorsMulticopter::set_throttle_range(int16_t radio_min, int16_t radio_max) { // sanity check - if ((radio_max > radio_min) && (min_throttle < (radio_max - radio_min))) { + if ((radio_max > radio_min)) { _throttle_radio_min = radio_min; _throttle_radio_max = radio_max; } - // update _min_throttle - _min_throttle = (float)min_throttle * ((get_pwm_output_max() - get_pwm_output_min()) / 1000.0f); } // update the throttle input filter. should be called at 100hz @@ -415,8 +420,8 @@ void AP_MotorsMulticopter::output_logic() } } else { // _spool_desired == SPIN_WHEN_ARMED float spin_up_armed_ratio = 0.0f; - if (_min_throttle > 0) { - spin_up_armed_ratio = _thrust_curve_arm / _min_throttle; + if (_thrust_curve_min > 0.0f) { + spin_up_armed_ratio = _thrust_curve_arm / _thrust_curve_min; } _spin_up_ratio += constrain_float(spin_up_armed_ratio-_spin_up_ratio, -spool_step, spool_step); } diff --git a/libraries/AP_Motors/AP_MotorsMulticopter.h b/libraries/AP_Motors/AP_MotorsMulticopter.h index fc5e43e247..c8f076be85 100644 --- a/libraries/AP_Motors/AP_MotorsMulticopter.h +++ b/libraries/AP_Motors/AP_MotorsMulticopter.h @@ -17,6 +17,7 @@ #define AP_MOTORS_THST_EXPO_DEFAULT 0.65f // set to 0 for linear and 1 for second order approximation #define AP_MOTORS_THST_HOVER_DEFAULT 0.5f // the estimated hover throttle, 0 ~ 1 #define AP_MOTORS_THST_HOVER_TC 10.0f // time constant used to update estimated hover throttle, 0 ~ 1 +#define AP_MOTORS_SPIN_MIN_DEFAULT 0.15f // throttle out ratio which produces the minimum thrust. (i.e. 0 ~ 1 ) of the full throttle range #define AP_MOTORS_SPIN_MAX_DEFAULT 0.95f // throttle out ratio which produces the maximum thrust. (i.e. 0 ~ 1 ) of the full throttle range #define AP_MOTORS_SPIN_ARM_DEFAULT 0.10f // throttle out ratio which produces the armed spin rate. (i.e. 0 ~ 1 ) of the full throttle range #define AP_MOTORS_BAT_VOLT_MAX_DEFAULT 0.0f // voltage limiting max default @@ -49,7 +50,7 @@ public: // set_throttle_range - sets the minimum throttle that will be sent to the engines when they're not off (i.e. to prevents issues with some motors spinning and some not at very low throttle) // also sets minimum and maximum pwm values that will be sent to the motors - void set_throttle_range(uint16_t min_throttle, int16_t radio_min, int16_t radio_max); + void set_throttle_range(int16_t radio_min, int16_t radio_max); // update estimated throttle required to hover void update_throttle_hover(float dt); @@ -146,7 +147,8 @@ protected: // parameters AP_Int16 _yaw_headroom; // yaw control is given at least this pwm range AP_Float _thrust_curve_expo; // curve used to linearize pwm to thrust conversion. set to 0 for linear and 1 for second order approximation - AP_Float _thrust_curve_max; // throttle which produces the maximum thrust. (i.e. 0 ~ 1 ) of the full throttle range + AP_Float _thrust_curve_min; // throttle out ratio which produces the minimum thrust. (i.e. 0 ~ 1 ) of the full throttle range + AP_Float _thrust_curve_max; // throttle out ratio which produces the maximum thrust. (i.e. 0 ~ 1 ) of the full throttle range AP_Float _thrust_curve_arm; // throttle out ratio which produces the armed spin rate. (i.e. 0 ~ 1 ) of the full throttle range AP_Float _batt_voltage_max; // maximum voltage used to scale lift AP_Float _batt_voltage_min; // minimum voltage used to scale lift @@ -158,10 +160,8 @@ protected: // internal variables bool motor_enabled[AP_MOTORS_MAX_NUM_MOTORS]; // true if motor is enabled - int16_t _min_throttle; // the minimum throttle to be sent to the motors when they're on (prevents motors stalling while flying) int16_t _throttle_radio_min; // minimum PWM from RC input's throttle channel (i.e. minimum PWM input from receiver, RC3_MIN) int16_t _throttle_radio_max; // maximum PWM from RC input's throttle channel (i.e. maximum PWM input from receiver, RC3_MAX) - float _throttle_thrust_max; // the maximum allowed throttle thrust 0.0 to 1.0 in the range throttle_min to throttle_max // spool variables float _spin_up_ratio; // throttle percentage (0 ~ 1) between zero and throttle_min @@ -174,6 +174,7 @@ protected: int16_t _batt_timer; // timer used in battery resistance calcs float _lift_max; // maximum lift ratio from battery voltage float _throttle_limit; // ratio of throttle limit between hover and maximum + float _throttle_thrust_max; // the maximum allowed throttle thrust 0.0 to 1.0 in the range throttle_min to throttle_max // vehicle supplied callback for thrust compensation. Used for tiltrotors and tiltwings thrust_compensation_fn_t _thrust_compensation_callback;