From a74acccb84172e101fa10e29d983310bb0986c26 Mon Sep 17 00:00:00 2001 From: borisbstyle Date: Thu, 2 Jun 2016 17:09:32 +0200 Subject: [PATCH] Add rc_rate_yaw // SuperExpo feature renamed to SUPEREXPO_RATES --- src/main/config/config.c | 5 +++-- src/main/config/config.h | 2 +- src/main/flight/pid.c | 4 ++-- src/main/io/rc_controls.c | 2 +- src/main/io/rc_controls.h | 1 + src/main/io/rc_curves.c | 10 ++-------- src/main/io/rc_curves.h | 3 +-- src/main/io/serial_cli.c | 3 ++- src/main/mw.c | 4 ++-- 9 files changed, 15 insertions(+), 19 deletions(-) diff --git a/src/main/config/config.c b/src/main/config/config.c index af15469eb..46fcd292a 100755 --- a/src/main/config/config.c +++ b/src/main/config/config.c @@ -141,7 +141,7 @@ static uint32_t activeFeaturesLatch = 0; static uint8_t currentControlRateProfileIndex = 0; controlRateConfig_t *currentControlRateProfile; -static const uint8_t EEPROM_CONF_VERSION = 139; +static const uint8_t EEPROM_CONF_VERSION = 140; static void resetAccelerometerTrims(flightDynamicsTrims_t *accelerometerTrims) { @@ -310,6 +310,7 @@ void resetSerialConfig(serialConfig_t *serialConfig) static void resetControlRateConfig(controlRateConfig_t *controlRateConfig) { controlRateConfig->rcRate8 = 100; + controlRateConfig->rcYawRate8 = 100; controlRateConfig->rcExpo8 = 10; controlRateConfig->thrMid8 = 50; controlRateConfig->thrExpo8 = 0; @@ -399,7 +400,7 @@ static void resetConf(void) #endif featureSet(FEATURE_FAILSAFE); - featureSet(FEATURE_SUPEREXPO); + featureSet(FEATURE_SUPEREXPO_RATES); // global settings masterConfig.current_profile_index = 0; // default profile diff --git a/src/main/config/config.h b/src/main/config/config.h index 24fe20ea6..7bd7a2e05 100644 --- a/src/main/config/config.h +++ b/src/main/config/config.h @@ -45,7 +45,7 @@ typedef enum { FEATURE_CHANNEL_FORWARDING = 1 << 20, FEATURE_TRANSPONDER = 1 << 21, FEATURE_AIRMODE = 1 << 22, - FEATURE_SUPEREXPO = 1 << 23, + FEATURE_SUPEREXPO_RATES = 1 << 23, } features_e; void handleOneshotFeatureChangeOnRestart(void); diff --git a/src/main/flight/pid.c b/src/main/flight/pid.c index 99cc25ae0..f9f2a2231 100644 --- a/src/main/flight/pid.c +++ b/src/main/flight/pid.c @@ -80,14 +80,14 @@ float calculateRate(int axis, const controlRateConfig_t *controlRateConfig) { if (isSuperExpoActive()) { float rcFactor = (axis == YAW) ? (ABS(rcCommand[axis]) / 500.0f) : (ABS(rcCommand[axis]) / (500.0f * (controlRateConfig->rcRate8 / 100.0f))); - rcFactor = constrainf(1.0f / (1.0f - (rcFactor * (controlRateConfig->rates[axis] / 100.0f))), 0.01f, 1.00f); + rcFactor = 1.0f / (constrainf(1.0f - (rcFactor * (controlRateConfig->rates[axis] / 100.0f)), 0.01f, 1.00f)); angleRate = rcFactor * ((27 * rcCommand[axis]) / 16.0f); } else { angleRate = (float)((controlRateConfig->rates[axis] + 27) * rcCommand[axis]) / 16.0f; } - return constrainf(angleRate, -8190, 8290); // Rate limit protection + return constrainf(angleRate, -8190.0f, 8190.0f); // Rate limit protection } uint16_t getDynamicKp(int axis, const pidProfile_t *pidProfile) { diff --git a/src/main/io/rc_controls.c b/src/main/io/rc_controls.c index 4e7344ac9..0bbcadc09 100644 --- a/src/main/io/rc_controls.c +++ b/src/main/io/rc_controls.c @@ -76,7 +76,7 @@ bool isAirmodeActive(void) { } bool isSuperExpoActive(void) { - return (feature(FEATURE_SUPEREXPO)); + return (feature(FEATURE_SUPEREXPO_RATES)); } void blackboxLogInflightAdjustmentEvent(adjustmentFunction_e adjustmentFunction, int32_t newValue) { diff --git a/src/main/io/rc_controls.h b/src/main/io/rc_controls.h index a7e7286e2..a5e96e15c 100644 --- a/src/main/io/rc_controls.h +++ b/src/main/io/rc_controls.h @@ -136,6 +136,7 @@ typedef struct modeActivationCondition_s { typedef struct controlRateConfig_s { uint8_t rcRate8; + uint8_t rcYawRate8; uint8_t rcExpo8; uint8_t thrMid8; uint8_t thrExpo8; diff --git a/src/main/io/rc_curves.c b/src/main/io/rc_curves.c index 2ed391974..ee2fe326f 100644 --- a/src/main/io/rc_curves.c +++ b/src/main/io/rc_curves.c @@ -46,16 +46,10 @@ void generateThrottleCurve(controlRateConfig_t *controlRateConfig, escAndServoCo } } -int16_t rcLookupPitchRoll(int32_t tmp, controlRateConfig_t *controlRateConfig) +int16_t rcLookup(int32_t tmp, uint8_t expo, uint8_t rate) { float tmpf = tmp / 100.0f; - return (int16_t)((2500.0f + (float)controlRateConfig->rcExpo8 * (tmpf * tmpf - 25.0f)) * tmpf * (float)(controlRateConfig->rcRate8) / 2500.0f ); -} - -int16_t rcLookupYaw(int32_t tmp, controlRateConfig_t *controlRateConfig) -{ - float tmpf = tmp / 100.0f; - return (int16_t)((2500.0f + (float)controlRateConfig->rcYawExpo8* (tmpf * tmpf - 25.0f)) * tmpf / 25.0f ); + return (int16_t)((2500.0f + (float)expo * (tmpf * tmpf - 25.0f)) * tmpf * (float)(rate) / 2500.0f ); } int16_t rcLookupThrottle(int32_t tmp) diff --git a/src/main/io/rc_curves.h b/src/main/io/rc_curves.h index 428dbdc5c..747a934df 100644 --- a/src/main/io/rc_curves.h +++ b/src/main/io/rc_curves.h @@ -19,7 +19,6 @@ void generateThrottleCurve(controlRateConfig_t *controlRateConfig, escAndServoConfig_t *escAndServoConfig); -int16_t rcLookupPitchRoll(int32_t tmp, controlRateConfig_t *controlRateConfig); -int16_t rcLookupYaw(int32_t tmp, controlRateConfig_t *controlRateConfig); +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 b71ae0b10..d077b9614 100644 --- a/src/main/io/serial_cli.c +++ b/src/main/io/serial_cli.c @@ -199,7 +199,7 @@ static const char * const featureNames[] = { "SERVO_TILT", "SOFTSERIAL", "GPS", "FAILSAFE", "SONAR", "TELEMETRY", "CURRENT_METER", "3D", "RX_PARALLEL_PWM", "RX_MSP", "RSSI_ADC", "LED_STRIP", "DISPLAY", "ONESHOT125", - "BLACKBOX", "CHANNEL_FORWARDING", "TRANSPONDER", "AIRMODE", "SUPEREXPO", + "BLACKBOX", "CHANNEL_FORWARDING", "TRANSPONDER", "AIRMODE", "SUPEREXPO_RATES", NULL }; @@ -695,6 +695,7 @@ const clivalue_t valueTable[] = { #endif { "rc_rate", VAR_UINT8 | PROFILE_RATE_VALUE, &masterConfig.profile[0].controlRateProfile[0].rcRate8, .config.minmax = { 0, 250 } }, + { "rc_rate_yaw", VAR_UINT8 | PROFILE_RATE_VALUE, &masterConfig.profile[0].controlRateProfile[0].rcYawRate8, .config.minmax = { 0, 250 } }, { "rc_expo", VAR_UINT8 | PROFILE_RATE_VALUE, &masterConfig.profile[0].controlRateProfile[0].rcExpo8, .config.minmax = { 0, 100 } }, { "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 } }, diff --git a/src/main/mw.c b/src/main/mw.c index f5865a27b..771de46e2 100644 --- a/src/main/mw.c +++ b/src/main/mw.c @@ -244,14 +244,14 @@ static void updateRcCommands(void) } else { tmp = 0; } - rcCommand[axis] = rcLookupPitchRoll(tmp, currentControlRateProfile); + rcCommand[axis] = rcLookup(tmp, currentControlRateProfile->rcExpo8, currentControlRateProfile->rcRate8); } else if (axis == YAW) { if (tmp > masterConfig.rcControlsConfig.yaw_deadband) { tmp -= masterConfig.rcControlsConfig.yaw_deadband; } else { tmp = 0; } - rcCommand[axis] = rcLookupYaw(tmp, currentControlRateProfile) * -masterConfig.yaw_control_direction; + rcCommand[axis] = rcLookup(tmp, currentControlRateProfile->rcYawExpo8, currentControlRateProfile->rcYawRate8) * -masterConfig.yaw_control_direction;; } if (rcData[axis] < masterConfig.rxConfig.midrc) { rcCommand[axis] = -rcCommand[axis];