diff --git a/src/main/blackbox/blackbox.c b/src/main/blackbox/blackbox.c index 2593547db7..a4bf5da8a1 100644 --- a/src/main/blackbox/blackbox.c +++ b/src/main/blackbox/blackbox.c @@ -1254,6 +1254,9 @@ static bool blackboxWriteSysinfo(void) BLACKBOX_PRINT_HEADER_LINE("rates", "%d,%d,%d", currentControlRateProfile->rates[ROLL], currentControlRateProfile->rates[PITCH], currentControlRateProfile->rates[YAW]); + BLACKBOX_PRINT_HEADER_LINE("rate_limits", "%d,%d,%d", currentControlRateProfile->rate_limit[ROLL], + currentControlRateProfile->rate_limit[PITCH], + currentControlRateProfile->rate_limit[YAW]); BLACKBOX_PRINT_HEADER_LINE("rollPID", "%d,%d,%d", currentPidProfile->pid[PID_ROLL].P, currentPidProfile->pid[PID_ROLL].I, currentPidProfile->pid[PID_ROLL].D); diff --git a/src/main/fc/controlrate_profile.c b/src/main/fc/controlrate_profile.c index e423ab4462..0d6b330343 100644 --- a/src/main/fc/controlrate_profile.c +++ b/src/main/fc/controlrate_profile.c @@ -33,6 +33,7 @@ #include "fc/config.h" #include "fc/controlrate_profile.h" #include "fc/rc.h" +#include "fc/rc_controls.h" controlRateConfig_t *currentControlRateProfile; @@ -57,7 +58,10 @@ void pgResetFn_controlRateProfiles(controlRateConfig_t *controlRateConfig) .rates[FD_PITCH] = 70, .rates[FD_YAW] = 70, .throttle_limit_type = THROTTLE_LIMIT_TYPE_OFF, - .throttle_limit_percent = 100 + .throttle_limit_percent = 100, + .rate_limit[FD_ROLL] = CONTROL_RATE_CONFIG_RATE_LIMIT_MAX, + .rate_limit[FD_PITCH] = CONTROL_RATE_CONFIG_RATE_LIMIT_MAX, + .rate_limit[FD_YAW] = CONTROL_RATE_CONFIG_RATE_LIMIT_MAX ); } } diff --git a/src/main/fc/controlrate_profile.h b/src/main/fc/controlrate_profile.h index ab4e4c2f12..865d1787b8 100644 --- a/src/main/fc/controlrate_profile.h +++ b/src/main/fc/controlrate_profile.h @@ -48,6 +48,7 @@ typedef struct controlRateConfig_s { uint16_t tpa_breakpoint; // Breakpoint where TPA is activated uint8_t throttle_limit_type; // Sets the throttle limiting type - off, scale or clip uint8_t throttle_limit_percent; // Sets the maximum pilot commanded throttle limit + uint16_t rate_limit[3]; // Sets the maximum rate for the axes } controlRateConfig_t; PG_DECLARE_ARRAY(controlRateConfig_t, CONTROL_RATE_PROFILE_COUNT, controlRateProfiles); diff --git a/src/main/fc/rc.c b/src/main/fc/rc.c index 5aff3de8ed..e76d0834d5 100644 --- a/src/main/fc/rc.c +++ b/src/main/fc/rc.c @@ -111,7 +111,9 @@ static int16_t rcLookupThrottle(int32_t tmp) return lookupThrottleRC[tmp2] + (tmp - tmp2 * 100) * (lookupThrottleRC[tmp2 + 1] - lookupThrottleRC[tmp2]) / 100; } -#define SETPOINT_RATE_LIMIT 1998.0f +#define SETPOINT_RATE_LIMIT 1998 +STATIC_ASSERT(CONTROL_RATE_CONFIG_RATE_LIMIT_MAX <= SETPOINT_RATE_LIMIT, CONTROL_RATE_CONFIG_RATE_LIMIT_MAX_too_large); + #define RC_RATE_INCREMENTAL 14.54f float applyBetaflightRates(const int axis, float rcCommandf, const float rcCommandfAbs) @@ -169,7 +171,8 @@ static void calculateSetpointRate(int axis) angleRate = applyRates(axis, rcCommandf, rcCommandfAbs); } - setpointRate[axis] = constrainf(angleRate, -SETPOINT_RATE_LIMIT, SETPOINT_RATE_LIMIT); // Rate limit protection (deg/sec) + // Rate limit from profile (deg/sec) + setpointRate[axis] = constrainf(angleRate, -1.0f * currentControlRateProfile->rate_limit[axis], 1.0f * currentControlRateProfile->rate_limit[axis]); DEBUG_SET(DEBUG_ANGLERATE, axis, angleRate); } @@ -189,8 +192,8 @@ static void scaleRcCommandToFpvCamAngle(void) float roll = setpointRate[ROLL]; float yaw = setpointRate[YAW]; - setpointRate[ROLL] = constrainf(roll * cosFactor - yaw * sinFactor, -SETPOINT_RATE_LIMIT, SETPOINT_RATE_LIMIT); - setpointRate[YAW] = constrainf(yaw * cosFactor + roll * sinFactor, -SETPOINT_RATE_LIMIT, SETPOINT_RATE_LIMIT); + setpointRate[ROLL] = constrainf(roll * cosFactor - yaw * sinFactor, -SETPOINT_RATE_LIMIT * 1.0f, SETPOINT_RATE_LIMIT * 1.0f); + setpointRate[YAW] = constrainf(yaw * cosFactor + roll * sinFactor, -SETPOINT_RATE_LIMIT * 1.0f, SETPOINT_RATE_LIMIT * 1.0f); } #define THROTTLE_BUFFER_MAX 20 diff --git a/src/main/fc/rc_controls.h b/src/main/fc/rc_controls.h index f1e60843f0..557aef58d1 100644 --- a/src/main/fc/rc_controls.h +++ b/src/main/fc/rc_controls.h @@ -100,6 +100,9 @@ typedef enum { #define CONTROL_RATE_CONFIG_RC_RATES_MAX 255 +#define CONTROL_RATE_CONFIG_RATE_LIMIT_MIN 200 +#define CONTROL_RATE_CONFIG_RATE_LIMIT_MAX 1998 + // (Super) rates are constrained to [0, 100] for Betaflight rates, so values higher than 100 won't make a difference. Range extended for RaceFlight rates. #define CONTROL_RATE_CONFIG_RATE_MAX 255 diff --git a/src/main/interface/settings.c b/src/main/interface/settings.c index eee874ffb8..49eb1b356d 100644 --- a/src/main/interface/settings.c +++ b/src/main/interface/settings.c @@ -749,6 +749,9 @@ const clivalue_t valueTable[] = { { "tpa_breakpoint", VAR_UINT16 | PROFILE_RATE_VALUE, .config.minmax = { PWM_PULSE_MIN, PWM_PULSE_MAX }, PG_CONTROL_RATE_PROFILES, offsetof(controlRateConfig_t, tpa_breakpoint) }, { "throttle_limit_type", VAR_UINT8 | PROFILE_RATE_VALUE | MODE_LOOKUP, .config.lookup = { TABLE_THROTTLE_LIMIT_TYPE }, PG_CONTROL_RATE_PROFILES, offsetof(controlRateConfig_t, throttle_limit_type) }, { "throttle_limit_percent", VAR_UINT8 | PROFILE_RATE_VALUE, .config.minmax = { 25, 100 }, PG_CONTROL_RATE_PROFILES, offsetof(controlRateConfig_t, throttle_limit_percent) }, + { "roll_rate_limit", VAR_UINT16 | PROFILE_RATE_VALUE, .config.minmax = { CONTROL_RATE_CONFIG_RATE_LIMIT_MIN, CONTROL_RATE_CONFIG_RATE_LIMIT_MAX }, PG_CONTROL_RATE_PROFILES, offsetof(controlRateConfig_t, rate_limit[FD_ROLL]) }, + { "pitch_rate_limit", VAR_UINT16 | PROFILE_RATE_VALUE, .config.minmax = { CONTROL_RATE_CONFIG_RATE_LIMIT_MIN, CONTROL_RATE_CONFIG_RATE_LIMIT_MAX }, PG_CONTROL_RATE_PROFILES, offsetof(controlRateConfig_t, rate_limit[FD_PITCH]) }, + { "yaw_rate_limit", VAR_UINT16 | PROFILE_RATE_VALUE, .config.minmax = { CONTROL_RATE_CONFIG_RATE_LIMIT_MIN, CONTROL_RATE_CONFIG_RATE_LIMIT_MAX }, PG_CONTROL_RATE_PROFILES, offsetof(controlRateConfig_t, rate_limit[FD_YAW]) }, // PG_SERIAL_CONFIG { "reboot_character", VAR_UINT8 | MASTER_VALUE, .config.minmax = { 48, 126 }, PG_SERIAL_CONFIG, offsetof(serialConfig_t, reboot_character) },