diff --git a/libraries/AC_AttitudeControl/AC_AttitudeControl.h b/libraries/AC_AttitudeControl/AC_AttitudeControl.h index 96781dba94..8332f828e4 100644 --- a/libraries/AC_AttitudeControl/AC_AttitudeControl.h +++ b/libraries/AC_AttitudeControl/AC_AttitudeControl.h @@ -35,9 +35,10 @@ #define AC_ATTITUDE_CONTROL_ANGLE_LIMIT_TC_DEFAULT 1.0f // Time constant used to limit lean angle so that vehicle does not lose altitude #define AC_ATTITUDE_CONTROL_ANGLE_LIMIT_THROTTLE_MAX 0.8f // Max throttle used to limit lean angle so that vehicle does not lose altitude -#define AC_ATTITUDE_CONTROL_MIN_DEFAULT 0.1f // minimum throttle mix -#define AC_ATTITUDE_CONTROL_MID_DEFAULT 0.5f // manual throttle mix +#define AC_ATTITUDE_CONTROL_MIN_DEFAULT 0.1f // minimum throttle mix default +#define AC_ATTITUDE_CONTROL_MAN_DEFAULT 0.5f // manual throttle mix default #define AC_ATTITUDE_CONTROL_MAX_DEFAULT 0.5f // maximum throttle mix default +#define AC_ATTITUDE_CONTROL_MAX 5.0f // maximum throttle mix default #define AC_ATTITUDE_CONTROL_THR_MIX_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 @@ -243,7 +244,7 @@ public: // control rpy throttle mix virtual void set_throttle_mix_min() {} - virtual void set_throttle_mix_mid() {} + virtual void set_throttle_mix_man() {} virtual void set_throttle_mix_max() {} // enable use of flybass passthrough on heli diff --git a/libraries/AC_AttitudeControl/AC_AttitudeControl_Multi.cpp b/libraries/AC_AttitudeControl/AC_AttitudeControl_Multi.cpp index 7dd702eb70..331b41ce81 100644 --- a/libraries/AC_AttitudeControl/AC_AttitudeControl_Multi.cpp +++ b/libraries/AC_AttitudeControl/AC_AttitudeControl_Multi.cpp @@ -135,6 +135,13 @@ const AP_Param::GroupInfo AC_AttitudeControl_Multi::var_info[] = { // @User: Advanced AP_GROUPINFO("THR_MIX_MAX", 5, AC_AttitudeControl_Multi, _thr_mix_max, AC_ATTITUDE_CONTROL_MAX_DEFAULT), + // @Param: THR_MIX_MAN + // @DisplayName: Throttle Mix Manual + // @Description: Throttle vs attitude control prioritisation used during manual flight (higher values mean we prioritise attitude control over throttle) + // @Range: 0.5 0.9 + // @User: Advanced + AP_GROUPINFO("THR_MIX_MAN", 6, AC_AttitudeControl_Multi, _thr_mix_man, AC_ATTITUDE_CONTROL_MAN_DEFAULT), + AP_GROUPEND }; @@ -219,7 +226,7 @@ void AC_AttitudeControl_Multi::update_throttle_rpy_mix() // reduce more slowly (from 0.9 to 0.1 in 1.6 seconds) _throttle_rpy_mix -= MIN(0.5f*_dt, _throttle_rpy_mix-_throttle_rpy_mix_desired); } - _throttle_rpy_mix = constrain_float(_throttle_rpy_mix, 0.1f, 1.0f); + _throttle_rpy_mix = constrain_float(_throttle_rpy_mix, 0.1f, AC_ATTITUDE_CONTROL_MAX); } void AC_AttitudeControl_Multi::rate_controller_run() @@ -238,12 +245,26 @@ void AC_AttitudeControl_Multi::rate_controller_run() void AC_AttitudeControl_Multi::parameter_sanity_check() { // sanity check throttle mix parameters + if (_thr_mix_man < 0.1f || _thr_mix_man > 4.0f) { + // parameter description recommends thr-mix-man be no higher than 0.9 but we allow up to 4.0 + // which can be useful for very high powered copters with very low hover throttle + _thr_mix_man = AC_ATTITUDE_CONTROL_MAN_DEFAULT; + _thr_mix_man.set_and_save(AC_ATTITUDE_CONTROL_MAN_DEFAULT); + } if (_thr_mix_min < 0.1f || _thr_mix_min > 0.25f) { _thr_mix_min = AC_ATTITUDE_CONTROL_MIN_DEFAULT; + _thr_mix_min.set_and_save(AC_ATTITUDE_CONTROL_MIN_DEFAULT); } - if (_thr_mix_max < 0.5f || _thr_mix_max > 2.0f) { - // parameter description recommends thr-mix-max be no higher than 0.9 but we allow up to 2.0 + if (_thr_mix_max < 0.5f || _thr_mix_max > AC_ATTITUDE_CONTROL_MAX) { + // parameter description recommends thr-mix-max be no higher than 0.9 but we allow up to 5.0 // which can be useful for very high powered copters with very low hover throttle _thr_mix_max = AC_ATTITUDE_CONTROL_MAX_DEFAULT; + _thr_mix_max.set_and_save(AC_ATTITUDE_CONTROL_MAX_DEFAULT); + } + if (_thr_mix_min > _thr_mix_max) { + _thr_mix_min = AC_ATTITUDE_CONTROL_MIN_DEFAULT; + _thr_mix_min.set_and_save(AC_ATTITUDE_CONTROL_MIN_DEFAULT); + _thr_mix_max = AC_ATTITUDE_CONTROL_MAX_DEFAULT; + _thr_mix_max.set_and_save(AC_ATTITUDE_CONTROL_MAX_DEFAULT); } } diff --git a/libraries/AC_AttitudeControl/AC_AttitudeControl_Multi.h b/libraries/AC_AttitudeControl/AC_AttitudeControl_Multi.h index 18c3977d33..3935cfac01 100644 --- a/libraries/AC_AttitudeControl/AC_AttitudeControl_Multi.h +++ b/libraries/AC_AttitudeControl/AC_AttitudeControl_Multi.h @@ -64,7 +64,7 @@ public: // low values favour pilot/autopilot throttle over attitude control, high values favour attitude control over throttle // has no effect when throttle is above hover throttle void set_throttle_mix_min() override { _throttle_rpy_mix_desired = _thr_mix_min; } - void set_throttle_mix_mid() override { _throttle_rpy_mix_desired = AC_ATTITUDE_CONTROL_MID_DEFAULT; } + void set_throttle_mix_man() override { _throttle_rpy_mix_desired = _thr_mix_man; } void set_throttle_mix_max() override { _throttle_rpy_mix_desired = _thr_mix_max; } // are we producing min throttle? @@ -92,6 +92,7 @@ protected: AC_PID _pid_rate_pitch; AC_PID _pid_rate_yaw; + AP_Float _thr_mix_man; // throttle vs attitude control prioritisation used when using manual throttle (higher values mean we prioritise attitude control over throttle) AP_Float _thr_mix_min; // throttle vs attitude control prioritisation used when landing (higher values mean we prioritise attitude control over throttle) AP_Float _thr_mix_max; // throttle vs attitude control prioritisation used during active flight (higher values mean we prioritise attitude control over throttle) };