From e4cdb077b2dab494a4255032dc5d7806d6c51c30 Mon Sep 17 00:00:00 2001 From: ianrmurphy Date: Thu, 19 Jul 2018 22:57:10 +0100 Subject: [PATCH] Added rate_limit feature to rateprofiles. Updated to conform to style guide. Fixed issue with statement ordering. Minimum rates of 200deg/s. Added rate limits to MSP for use in Configurator. Refactoring to improve efficiency. Revert 'msp.c' changes. Added BUILD_BUG_ON(CONTROL_RATE_CONFIG_RATE_LIMIT_MAX > SETPOINT_RATE_LIMIT). Added rate_limits to log header to faciliate support for blackbox-log-viewer to correctly handle the rate calculation. Use STATIC_ASSERT() for build protection. --- src/main/blackbox/blackbox.c | 3 +++ src/main/fc/controlrate_profile.c | 6 +++++- src/main/fc/controlrate_profile.h | 1 + src/main/fc/rc.c | 5 ++++- src/main/fc/rc_controls.h | 3 +++ src/main/interface/settings.c | 3 +++ 6 files changed, 19 insertions(+), 2 deletions(-) diff --git a/src/main/blackbox/blackbox.c b/src/main/blackbox/blackbox.c index 2593547db..a4bf5da8a 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 e423ab446..0d6b33034 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 ab4e4c2f1..865d1787b 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 5aff3de8e..4c95d9b4d 100644 --- a/src/main/fc/rc.c +++ b/src/main/fc/rc.c @@ -112,6 +112,8 @@ static int16_t rcLookupThrottle(int32_t tmp) } #define SETPOINT_RATE_LIMIT 1998.0f +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); } diff --git a/src/main/fc/rc_controls.h b/src/main/fc/rc_controls.h index f1e60843f..557aef58d 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 9503497d9..30298c73c 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) }, -- 2.11.4.GIT