From 30175dcba0a0a8de09ced177d1b500f722c2a419 Mon Sep 17 00:00:00 2001 From: borisbstyle Date: Fri, 26 Aug 2016 02:05:28 +0200 Subject: [PATCH] Redefine rate implementation // new power expo // remove super expo feature --- src/main/config/config.c | 2 +- src/main/config/config.h | 2 +- src/main/debug.h | 1 + src/main/io/rc_controls.c | 4 ---- src/main/io/rc_controls.h | 1 - src/main/io/rc_curves.c | 6 ------ src/main/io/rc_curves.h | 1 - src/main/io/serial_cli.c | 7 ++++--- src/main/mw.c | 44 +++++++++++++++++++++++++++++---------- 9 files changed, 40 insertions(+), 28 deletions(-) diff --git a/src/main/config/config.c b/src/main/config/config.c index 996dd1f20..f5aabf362 100755 --- a/src/main/config/config.c +++ b/src/main/config/config.c @@ -454,7 +454,7 @@ void createDefaultConfig(master_t *config) memset(config, 0, sizeof(master_t)); intFeatureClearAll(config); - intFeatureSet(DEFAULT_RX_FEATURE | FEATURE_FAILSAFE | FEATURE_SUPEREXPO_RATES, config); + intFeatureSet(DEFAULT_RX_FEATURE | FEATURE_FAILSAFE , config); #ifdef DEFAULT_FEATURES intFeatureSet(DEFAULT_FEATURES, config); #endif diff --git a/src/main/config/config.h b/src/main/config/config.h index 6570440c3..9f7da08d5 100644 --- a/src/main/config/config.h +++ b/src/main/config/config.h @@ -50,7 +50,7 @@ typedef enum { FEATURE_CHANNEL_FORWARDING = 1 << 20, FEATURE_TRANSPONDER = 1 << 21, FEATURE_AIRMODE = 1 << 22, - FEATURE_SUPEREXPO_RATES = 1 << 23, + //FEATURE_SUPEREXPO_RATES = 1 << 23, FEATURE_VTX = 1 << 24, FEATURE_RX_NRF24 = 1 << 25, FEATURE_SOFTSPI = 1 << 26, diff --git a/src/main/debug.h b/src/main/debug.h index 4431953f3..14f06fce4 100644 --- a/src/main/debug.h +++ b/src/main/debug.h @@ -54,5 +54,6 @@ typedef enum { DEBUG_RC_INTERPOLATION, DEBUG_VELOCITY, DEBUG_DTERM_FILTER, + DEBUG_ANGLERATE, DEBUG_COUNT } debugType_e; diff --git a/src/main/io/rc_controls.c b/src/main/io/rc_controls.c index c3ba253b8..39f270e0d 100644 --- a/src/main/io/rc_controls.c +++ b/src/main/io/rc_controls.c @@ -75,10 +75,6 @@ bool isAirmodeActive(void) { return (IS_RC_MODE_ACTIVE(BOXAIRMODE) || feature(FEATURE_AIRMODE)); } -bool isSuperExpoActive(void) { - return (feature(FEATURE_SUPEREXPO_RATES)); -} - void blackboxLogInflightAdjustmentEvent(adjustmentFunction_e adjustmentFunction, int32_t newValue) { #ifndef BLACKBOX UNUSED(adjustmentFunction); diff --git a/src/main/io/rc_controls.h b/src/main/io/rc_controls.h index aace839d9..38c093f53 100644 --- a/src/main/io/rc_controls.h +++ b/src/main/io/rc_controls.h @@ -254,7 +254,6 @@ typedef struct adjustmentState_s { #define MAX_ADJUSTMENT_RANGE_COUNT 15 bool isAirmodeActive(void); -bool isSuperExpoActive(void); void resetAdjustmentStates(void); void updateAdjustmentStates(adjustmentRange_t *adjustmentRanges); void processRcAdjustments(controlRateConfig_t *controlRateConfig, rxConfig_t *rxConfig); diff --git a/src/main/io/rc_curves.c b/src/main/io/rc_curves.c index 89b46522f..6f4b31ad9 100644 --- a/src/main/io/rc_curves.c +++ b/src/main/io/rc_curves.c @@ -45,12 +45,6 @@ void generateThrottleCurve(controlRateConfig_t *controlRateConfig, escAndServoCo } } -int16_t rcLookup(int32_t tmp, uint8_t expo, uint8_t rate) -{ - float tmpf = tmp / 100.0f; - return (int16_t)((2500.0f + (float)expo * (tmpf * tmpf - 25.0f)) * tmpf * (float)(rate) / 2500.0f ); -} - int16_t rcLookupThrottle(int32_t tmp) { const int32_t tmp2 = tmp / 100; diff --git a/src/main/io/rc_curves.h b/src/main/io/rc_curves.h index 747a934df..6b9161fb8 100644 --- a/src/main/io/rc_curves.h +++ b/src/main/io/rc_curves.h @@ -19,6 +19,5 @@ void generateThrottleCurve(controlRateConfig_t *controlRateConfig, escAndServoConfig_t *escAndServoConfig); -int16_t rcLookup(int32_t tmp, uint8_t expo, uint8_t rate); int16_t rcLookupThrottle(int32_t tmp); diff --git a/src/main/io/serial_cli.c b/src/main/io/serial_cli.c index b434fa430..3ce15c0f7 100644 --- a/src/main/io/serial_cli.c +++ b/src/main/io/serial_cli.c @@ -487,6 +487,7 @@ static const char * const lookupTableDebug[DEBUG_COUNT] = { "RC_INTERPOLATION", "VELOCITY", "DFILTER", + "ANGLERATE", }; #ifdef OSD @@ -801,9 +802,9 @@ const clivalue_t valueTable[] = { { "rc_yaw_expo", VAR_UINT8 | PROFILE_RATE_VALUE, &masterConfig.profile[0].controlRateProfile[0].rcYawExpo8, .config.minmax = { 0, 100 } }, { "thr_mid", VAR_UINT8 | PROFILE_RATE_VALUE, &masterConfig.profile[0].controlRateProfile[0].thrMid8, .config.minmax = { 0, 100 } }, { "thr_expo", VAR_UINT8 | PROFILE_RATE_VALUE, &masterConfig.profile[0].controlRateProfile[0].thrExpo8, .config.minmax = { 0, 100 } }, - { "roll_rate", VAR_UINT8 | PROFILE_RATE_VALUE, &masterConfig.profile[0].controlRateProfile[0].rates[FD_ROLL], .config.minmax = { 0, CONTROL_RATE_CONFIG_ROLL_PITCH_RATE_MAX } }, - { "pitch_rate", VAR_UINT8 | PROFILE_RATE_VALUE, &masterConfig.profile[0].controlRateProfile[0].rates[FD_PITCH], .config.minmax = { 0, CONTROL_RATE_CONFIG_ROLL_PITCH_RATE_MAX } }, - { "yaw_rate", VAR_UINT8 | PROFILE_RATE_VALUE, &masterConfig.profile[0].controlRateProfile[0].rates[FD_YAW], .config.minmax = { 0, CONTROL_RATE_CONFIG_YAW_RATE_MAX } }, + { "roll_srate", VAR_UINT8 | PROFILE_RATE_VALUE, &masterConfig.profile[0].controlRateProfile[0].rates[FD_ROLL], .config.minmax = { 0, CONTROL_RATE_CONFIG_ROLL_PITCH_RATE_MAX } }, + { "pitch_srate", VAR_UINT8 | PROFILE_RATE_VALUE, &masterConfig.profile[0].controlRateProfile[0].rates[FD_PITCH], .config.minmax = { 0, CONTROL_RATE_CONFIG_ROLL_PITCH_RATE_MAX } }, + { "yaw_srate", VAR_UINT8 | PROFILE_RATE_VALUE, &masterConfig.profile[0].controlRateProfile[0].rates[FD_YAW], .config.minmax = { 0, CONTROL_RATE_CONFIG_YAW_RATE_MAX } }, { "tpa_rate", VAR_UINT8 | PROFILE_RATE_VALUE, &masterConfig.profile[0].controlRateProfile[0].dynThrPID, .config.minmax = { 0, CONTROL_RATE_CONFIG_TPA_MAX} }, { "tpa_breakpoint", VAR_UINT16 | PROFILE_RATE_VALUE, &masterConfig.profile[0].controlRateProfile[0].tpa_breakpoint, .config.minmax = { PWM_RANGE_MIN, PWM_RANGE_MAX} }, { "airmode_activate_throttle", VAR_UINT16 | MASTER_VALUE, &masterConfig.rxConfig.airModeActivateThreshold, .config.minmax = {1000, 2000 } }, diff --git a/src/main/mw.c b/src/main/mw.c index d61601528..20580b66a 100644 --- a/src/main/mw.c +++ b/src/main/mw.c @@ -173,22 +173,44 @@ bool isCalibrating() return (!isAccelerationCalibrationComplete() && sensors(SENSOR_ACC)) || (!isGyroCalibrationComplete()); } +#define RC_RATE_INCREMENTAL 14.54f + float calculateSetpointRate(int axis, int16_t rc) { - float angleRate; + float angleRate, rcRate, rcSuperfactor, rcCommandf; + uint8_t rcExpo; - if (isSuperExpoActive()) { - rcInput[axis] = (axis == YAW) ? (ABS(rc) / (500.0f * (currentControlRateProfile->rcYawRate8 / 100.0f))) : (ABS(rc) / (500.0f * (currentControlRateProfile->rcRate8 / 100.0f))); - float rcFactor = 1.0f / (constrainf(1.0f - (rcInput[axis] * (currentControlRateProfile->rates[axis] / 100.0f)), 0.01f, 1.00f)); - - angleRate = rcFactor * ((27 * rc) / 16.0f); + if (axis != YAW) { + rcExpo = currentControlRateProfile->rcExpo8; + rcRate = currentControlRateProfile->rcRate8 / 100.0f; } else { - angleRate = (float)((currentControlRateProfile->rates[axis] + 27) * rc) / 16.0f; + rcExpo = currentControlRateProfile->rcYawExpo8; + rcRate = currentControlRateProfile->rcYawRate8 / 100.0f; + } + + if (rcRate > 2.0f) rcRate = rcRate + (RC_RATE_INCREMENTAL * (rcRate - 2.0f)); + rcCommandf = rc / 500.0f; + rcInput[axis] = ABS(rcCommandf); + + if (rcExpo) { + float expof = rcExpo / 100.0f; + rcCommandf = rcCommandf * (expof * (rcInput[axis] * rcInput[axis] * rcInput[axis]) + rcInput[axis]*(1-expof)); + } + + angleRate = 200.0f * rcRate * rcCommandf; + + if (currentControlRateProfile->rates[axis]) { + rcSuperfactor = 1.0f / (constrainf(1.0f - (rcInput[axis] * (currentControlRateProfile->rates[axis] / 100.0f)), 0.01f, 1.00f)); + angleRate *= rcSuperfactor; + } + + if (debugMode == DEBUG_ANGLERATE) { + debug[axis] = angleRate; } if (currentProfile->pidProfile.pidController == PID_CONTROLLER_LEGACY) - return constrainf(angleRate, -8190.0f, 8190.0f); // Rate limit protection + return constrainf(angleRate * 4.1f, -8190.0f, 8190.0f); // Rate limit protection else - return constrainf(angleRate / 4.1f, -1997.0f, 1997.0f); // Rate limit protection (deg/sec) + return constrainf(angleRate, -1998.0f, 1998.0f); // Rate limit protection (deg/sec) } void scaleRcCommandToFpvCamAngle(void) { @@ -298,14 +320,14 @@ static void updateRcCommands(void) } else { tmp = 0; } - rcCommand[axis] = rcLookup(tmp, currentControlRateProfile->rcExpo8, currentControlRateProfile->rcRate8); + rcCommand[axis] = tmp; } else if (axis == YAW) { if (tmp > masterConfig.rcControlsConfig.yaw_deadband) { tmp -= masterConfig.rcControlsConfig.yaw_deadband; } else { tmp = 0; } - rcCommand[axis] = rcLookup(tmp, currentControlRateProfile->rcYawExpo8, currentControlRateProfile->rcYawRate8) * -masterConfig.yaw_control_direction;; + rcCommand[axis] = tmp * -masterConfig.yaw_control_direction; } if (rcData[axis] < masterConfig.rxConfig.midrc) { rcCommand[axis] = -rcCommand[axis];