diff --git a/src/main/cli/settings.c b/src/main/cli/settings.c index e83107806b..59f72f0acd 100644 --- a/src/main/cli/settings.c +++ b/src/main/cli/settings.c @@ -1011,6 +1011,8 @@ const clivalue_t valueTable[] = { { "dterm_cut_lowpass_hz", VAR_UINT8 | PROFILE_VALUE, .config.minmax = { 1, 20 }, PG_PID_PROFILE, offsetof(pidProfile_t, dterm_cut_lowpass_hz) }, #endif + { "motor_output_limit", VAR_UINT8 | PROFILE_VALUE, .config.minmax = { 1, 100 }, PG_PID_PROFILE, offsetof(pidProfile_t, motor_output_limit) }, + #ifdef USE_LAUNCH_CONTROL { "launch_control_mode", VAR_UINT8 | PROFILE_VALUE | MODE_LOOKUP, .config.lookup = { TABLE_LAUNCH_CONTROL_MODE }, PG_PID_PROFILE, offsetof(pidProfile_t, launchControlMode) }, { "launch_trigger_allow_reset", VAR_UINT8 | PROFILE_VALUE | MODE_LOOKUP, .config.lookup = { TABLE_OFF_ON }, PG_PID_PROFILE, offsetof(pidProfile_t, launchControlAllowTriggerReset) }, diff --git a/src/main/fc/config.c b/src/main/fc/config.c index 60870a3852..1eaf12fa8d 100644 --- a/src/main/fc/config.c +++ b/src/main/fc/config.c @@ -208,6 +208,10 @@ static void validateAndFixConfig(void) } #endif + if (currentPidProfile->motor_output_limit > 100 || currentPidProfile->motor_output_limit == 0) { + currentPidProfile->motor_output_limit = 100; + } + if (motorConfig()->dev.motorPwmProtocol == PWM_TYPE_BRUSHED) { featureDisable(FEATURE_3D); diff --git a/src/main/flight/mixer.c b/src/main/flight/mixer.c index 19a2636a57..aace0fe4ca 100644 --- a/src/main/flight/mixer.c +++ b/src/main/flight/mixer.c @@ -367,6 +367,11 @@ bool mixerIsTricopter(void) // DSHOT scaling is done to the actual dshot range void initEscEndpoints(void) { + float motorOutputLimit = 1.0f; + if (currentPidProfile->motor_output_limit < 100) { + motorOutputLimit = currentPidProfile->motor_output_limit / 100.0f; + } + // Can't use 'isMotorProtocolDshot()' here since motors haven't been initialised yet switch (motorConfig()->dev.motorPwmProtocol) { #ifdef USE_DSHOT @@ -375,29 +380,34 @@ void initEscEndpoints(void) case PWM_TYPE_DSHOT600: case PWM_TYPE_DSHOT300: case PWM_TYPE_DSHOT150: - disarmMotorOutput = DSHOT_CMD_MOTOR_STOP; - if (featureIsEnabled(FEATURE_3D)) { - motorOutputLow = DSHOT_MIN_THROTTLE + ((DSHOT_3D_FORWARD_MIN_THROTTLE - 1 - DSHOT_MIN_THROTTLE) / 100.0f) * CONVERT_PARAMETER_TO_PERCENT(motorConfig()->digitalIdleOffsetValue); - } else { - motorOutputLow = DSHOT_MIN_THROTTLE + ((DSHOT_MAX_THROTTLE - DSHOT_MIN_THROTTLE) / 100.0f) * CONVERT_PARAMETER_TO_PERCENT(motorConfig()->digitalIdleOffsetValue); + { + float outputLimitOffset = (DSHOT_MAX_THROTTLE - DSHOT_MIN_THROTTLE) * (1 - motorOutputLimit); + disarmMotorOutput = DSHOT_CMD_MOTOR_STOP; + if (featureIsEnabled(FEATURE_3D)) { + motorOutputLow = DSHOT_MIN_THROTTLE + ((DSHOT_3D_FORWARD_MIN_THROTTLE - 1 - DSHOT_MIN_THROTTLE) / 100.0f) * CONVERT_PARAMETER_TO_PERCENT(motorConfig()->digitalIdleOffsetValue); + motorOutputHigh = DSHOT_MAX_THROTTLE - outputLimitOffset / 2; + deadbandMotor3dHigh = DSHOT_3D_FORWARD_MIN_THROTTLE + ((DSHOT_MAX_THROTTLE - DSHOT_3D_FORWARD_MIN_THROTTLE) / 100.0f) * CONVERT_PARAMETER_TO_PERCENT(motorConfig()->digitalIdleOffsetValue); + deadbandMotor3dLow = DSHOT_3D_FORWARD_MIN_THROTTLE - 1 - outputLimitOffset / 2; + } else { + motorOutputLow = DSHOT_MIN_THROTTLE + ((DSHOT_MAX_THROTTLE - DSHOT_MIN_THROTTLE) / 100.0f) * CONVERT_PARAMETER_TO_PERCENT(motorConfig()->digitalIdleOffsetValue); + motorOutputHigh = DSHOT_MAX_THROTTLE - outputLimitOffset; + } } - motorOutputHigh = DSHOT_MAX_THROTTLE; - deadbandMotor3dHigh = DSHOT_3D_FORWARD_MIN_THROTTLE + ((DSHOT_MAX_THROTTLE - DSHOT_3D_FORWARD_MIN_THROTTLE) / 100.0f) * CONVERT_PARAMETER_TO_PERCENT(motorConfig()->digitalIdleOffsetValue); - deadbandMotor3dLow = DSHOT_3D_FORWARD_MIN_THROTTLE - 1; break; #endif default: if (featureIsEnabled(FEATURE_3D)) { + float outputLimitOffset = (flight3DConfig()->limit3d_high - flight3DConfig()->limit3d_low) * (1 - motorOutputLimit) / 2; disarmMotorOutput = flight3DConfig()->neutral3d; - motorOutputLow = flight3DConfig()->limit3d_low; - motorOutputHigh = flight3DConfig()->limit3d_high; + motorOutputLow = flight3DConfig()->limit3d_low + outputLimitOffset; + motorOutputHigh = flight3DConfig()->limit3d_high - outputLimitOffset; deadbandMotor3dHigh = flight3DConfig()->deadband3d_high; deadbandMotor3dLow = flight3DConfig()->deadband3d_low; } else { disarmMotorOutput = motorConfig()->mincommand; motorOutputLow = motorConfig()->minthrottle; - motorOutputHigh = motorConfig()->maxthrottle; + motorOutputHigh = motorConfig()->maxthrottle - ((motorConfig()->maxthrottle - motorConfig()->minthrottle) * (1 - motorOutputLimit)); } break; } diff --git a/src/main/flight/pid.c b/src/main/flight/pid.c index 613816af47..09ef8955b3 100644 --- a/src/main/flight/pid.c +++ b/src/main/flight/pid.c @@ -191,6 +191,7 @@ void resetPidProfile(pidProfile_t *pidProfile) .dterm_cut_gain = 15, .dterm_cut_range_hz = 40, .dterm_cut_lowpass_hz = 7, + .motor_output_limit = 100, ); #ifdef USE_DYN_LPF pidProfile->dterm_lowpass_hz = 150; diff --git a/src/main/flight/pid.h b/src/main/flight/pid.h index 9335d68ae3..7ff1208441 100644 --- a/src/main/flight/pid.h +++ b/src/main/flight/pid.h @@ -164,6 +164,7 @@ typedef struct pidProfile_s { uint8_t dterm_cut_gain; // Gain factor for amount of gyro activity required to remove the dterm cut uint8_t dterm_cut_range_hz; // Biquad to prevent high frequency gyro noise from removing the dterm cut uint8_t dterm_cut_lowpass_hz; // First order lowpass to delay and smooth dterm cut factor + uint8_t motor_output_limit; // Upper limit of the motor output (percent) } pidProfile_t; PG_DECLARE_ARRAY(pidProfile_t, MAX_PROFILE_COUNT, pidProfiles);