diff --git a/libraries/AP_Motors/AP_MotorsMatrix.cpp b/libraries/AP_Motors/AP_MotorsMatrix.cpp index 5890f8c33a..0a52e3f6d6 100644 --- a/libraries/AP_Motors/AP_MotorsMatrix.cpp +++ b/libraries/AP_Motors/AP_MotorsMatrix.cpp @@ -237,7 +237,7 @@ void AP_MotorsMatrix::output_armed() // We will choose #1 (the best throttle for yaw control) if that means reducing throttle to the motors (i.e. we favour reducing throttle *because* it provides better yaw control) // We will choose #2 (a mix of pilot and hover throttle) only when the throttle is quite low. We favour reducing throttle instead of better yaw control because the pilot has commanded it int16_t motor_mid = (rpy_low+rpy_high)/2; - out_best_thr_pwm = min(out_mid_pwm - motor_mid, max(_rc_throttle.radio_out, (_rc_throttle.radio_out+_hover_out)/2)); + out_best_thr_pwm = min(out_mid_pwm - motor_mid, max(_rc_throttle.radio_out, _rc_throttle.radio_out*max(0,1.0f-_throttle_low_comp)+_hover_out*_throttle_low_comp)); // calculate amount of yaw we can fit into the throttle range // this is always equal to or less than the requested yaw from the pilot or rate controller diff --git a/libraries/AP_Motors/AP_Motors_Class.cpp b/libraries/AP_Motors/AP_Motors_Class.cpp index 9bd48784fc..c1ac57761c 100644 --- a/libraries/AP_Motors/AP_Motors_Class.cpp +++ b/libraries/AP_Motors/AP_Motors_Class.cpp @@ -77,6 +77,13 @@ const AP_Param::GroupInfo AP_Motors::var_info[] PROGMEM = { // @User: Advanced AP_GROUPINFO("YAW_HEADROOM", 6, AP_Motors, _yaw_headroom, AP_MOTORS_YAW_HEADROOM_DEFAULT), + // @Param: THR_LOW_CMP + // @DisplayName: Motor low throttle compensation + // @Description: Ratio controlling the max throttle output during competing requests of low throttle from the pilot (or autopilot) and higher throttle for attitude control + // @Values: 0.2:Favour Throttle Control, 0.5:Equal Weighting, 1:Favour Attitude Control + // @User: Advanced + AP_GROUPINFO("THR_LOW_CMP", 7, AP_Motors, _throttle_low_comp, AP_MOTORS_THR_LOW_CMP_DEFAULT), + // @Param: THST_EXPO // @DisplayName: Thrust Curve Expo // @Description: Motor thrust curve exponent (from 0 for linear to 1.0 for second order curve) diff --git a/libraries/AP_Motors/AP_Motors_Class.h b/libraries/AP_Motors/AP_Motors_Class.h index 99241d188c..d965865962 100644 --- a/libraries/AP_Motors/AP_Motors_Class.h +++ b/libraries/AP_Motors/AP_Motors_Class.h @@ -56,6 +56,7 @@ #define AP_MOTORS_YAW_HEADROOM_DEFAULT 200 +#define AP_MOTORS_THR_LOW_CMP_DEFAULT 0.5f // ratio controlling the max throttle output during competing requests of low throttle from the pilot (or autopilot) and higher throttle for attitude control. Higher favours Attitude over pilot input #define AP_MOTORS_THST_EXPO_DEFAULT 0.5f // set to 0 for linear and 1 for second order approximation #define AP_MOTORS_THST_MAX_DEFAULT 0.95f // throttle which produces the maximum thrust. (i.e. 0 ~ 1 ) of the full throttle range #define AP_MOTORS_THST_BAT_MAX_DEFAULT 0.0f @@ -197,6 +198,7 @@ protected: AP_Int16 _spin_when_armed; // used to control whether the motors always spin when armed. pwm value above radio_min AP_Int16 _yaw_headroom; // yaw control is given at least this pwm range + AP_Float _throttle_low_comp; // mix between throttle and hover throttle for 0 to 1 and ratio above hover throttle for >1 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 _batt_voltage_max; // maximum voltage used to scale lift