From a3cf7e0cf7e32d73688461984ad798e0982f29ac Mon Sep 17 00:00:00 2001 From: mikeller Date: Sun, 27 Jan 2019 19:10:05 +1300 Subject: [PATCH] Added motor output limiting per profile. --- src/main/cli/settings.c | 2 ++ src/main/fc/config.c | 4 ++++ src/main/flight/mixer.c | 31 ++++++++++++++++++++----------- src/main/flight/pid.c | 1 + src/main/flight/pid.h | 1 + 5 files changed, 28 insertions(+), 11 deletions(-) diff --git a/src/main/cli/settings.c b/src/main/cli/settings.c index 5096a32d7..8e82eea8b 100644 --- a/src/main/cli/settings.c +++ b/src/main/cli/settings.c @@ -1013,6 +1013,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 60870a385..1eaf12fa8 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 19a2636a5..b9790049b 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,33 @@ 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); + } else { + motorOutputLow = DSHOT_MIN_THROTTLE + ((DSHOT_MAX_THROTTLE - DSHOT_MIN_THROTTLE) / 100.0f) * CONVERT_PARAMETER_TO_PERCENT(motorConfig()->digitalIdleOffsetValue); + } + motorOutputHigh = DSHOT_MAX_THROTTLE - outputLimitOffset; + 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; } - 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)); 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 613816af4..cfa09575f 100644 --- a/src/main/flight/pid.c +++ b/src/main/flight/pid.c @@ -202,6 +202,7 @@ void resetPidProfile(pidProfile_t *pidProfile) pidProfile->pid[PID_ROLL].D = 30; pidProfile->pid[PID_PITCH].D = 32; #endif + pidProfile->motor_output_limit = 100; } void pgResetFn_pidProfiles(pidProfile_t *pidProfiles) diff --git a/src/main/flight/pid.h b/src/main/flight/pid.h index 9335d68ae..7ff120844 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); -- 2.11.4.GIT