From d4c22f1c28e54420681362df6a90616932982d31 Mon Sep 17 00:00:00 2001 From: borisbstyle Date: Tue, 31 May 2016 22:30:47 +0200 Subject: [PATCH] Rework Super Expo Rate Implementation // On the fly Rc Expo --- src/main/blackbox/blackbox.c | 70 +++++++++++++---------------- src/main/config/config.c | 13 ++---- src/main/flight/pid.c | 104 ++++++++++++++++++------------------------- src/main/io/rc_controls.c | 4 +- src/main/io/rc_curves.c | 40 +++-------------- src/main/io/rc_curves.h | 6 +-- src/main/io/serial_cli.c | 3 -- src/main/io/serial_msp.c | 6 +-- src/main/mw.c | 4 +- src/main/rx/rx.h | 3 -- 10 files changed, 92 insertions(+), 161 deletions(-) diff --git a/src/main/blackbox/blackbox.c b/src/main/blackbox/blackbox.c index 9e8fdc1fd..bf43dcc35 100644 --- a/src/main/blackbox/blackbox.c +++ b/src/main/blackbox/blackbox.c @@ -1195,146 +1195,140 @@ static bool blackboxWriteSysinfo() case 18: blackboxPrintfHeaderLine("tpa_breakpoint:%d", masterConfig.profile[masterConfig.current_profile_index].controlRateProfile[masterConfig.profile[masterConfig.current_profile_index].activeRateProfile].tpa_breakpoint); break; - case 19: - blackboxPrintfHeaderLine("superExpoFactor:%d, %d", masterConfig.rxConfig.superExpoFactor, masterConfig.rxConfig.superExpoFactorYaw); - break; - case 20: + case 21: blackboxPrintfHeaderLine("rates:%d,%d,%d", masterConfig.profile[masterConfig.current_profile_index].controlRateProfile[masterConfig.profile[masterConfig.current_profile_index].activeRateProfile].rates[ROLL], masterConfig.profile[masterConfig.current_profile_index].controlRateProfile[masterConfig.profile[masterConfig.current_profile_index].activeRateProfile].rates[PITCH], masterConfig.profile[masterConfig.current_profile_index].controlRateProfile[masterConfig.profile[masterConfig.current_profile_index].activeRateProfile].rates[YAW]); break; - case 21: + case 22: blackboxPrintfHeaderLine("looptime:%d", targetLooptime); break; - case 22: + case 23: blackboxPrintfHeaderLine("pidController:%d", masterConfig.profile[masterConfig.current_profile_index].pidProfile.pidController); break; - case 23: + case 24: blackboxPrintfHeaderLine("rollPID:%d,%d,%d", masterConfig.profile[masterConfig.current_profile_index].pidProfile.P8[ROLL], masterConfig.profile[masterConfig.current_profile_index].pidProfile.I8[ROLL], masterConfig.profile[masterConfig.current_profile_index].pidProfile.D8[ROLL]); break; - case 24: + case 25: blackboxPrintfHeaderLine("pitchPID:%d,%d,%d", masterConfig.profile[masterConfig.current_profile_index].pidProfile.P8[PITCH], masterConfig.profile[masterConfig.current_profile_index].pidProfile.I8[PITCH], masterConfig.profile[masterConfig.current_profile_index].pidProfile.D8[PITCH]); break; - case 25: + case 26: blackboxPrintfHeaderLine("yawPID:%d,%d,%d", masterConfig.profile[masterConfig.current_profile_index].pidProfile.P8[YAW], masterConfig.profile[masterConfig.current_profile_index].pidProfile.I8[YAW], masterConfig.profile[masterConfig.current_profile_index].pidProfile.D8[YAW]); break; - case 26: + case 27: blackboxPrintfHeaderLine("altPID:%d,%d,%d", masterConfig.profile[masterConfig.current_profile_index].pidProfile.P8[PIDALT], masterConfig.profile[masterConfig.current_profile_index].pidProfile.I8[PIDALT], masterConfig.profile[masterConfig.current_profile_index].pidProfile.D8[PIDALT]); break; - case 27: + case 28: blackboxPrintfHeaderLine("posPID:%d,%d,%d", masterConfig.profile[masterConfig.current_profile_index].pidProfile.P8[PIDPOS], masterConfig.profile[masterConfig.current_profile_index].pidProfile.I8[PIDPOS], masterConfig.profile[masterConfig.current_profile_index].pidProfile.D8[PIDPOS]); break; - case 28: + case 29: blackboxPrintfHeaderLine("posrPID:%d,%d,%d", masterConfig.profile[masterConfig.current_profile_index].pidProfile.P8[PIDPOSR], masterConfig.profile[masterConfig.current_profile_index].pidProfile.I8[PIDPOSR], masterConfig.profile[masterConfig.current_profile_index].pidProfile.D8[PIDPOSR]); break; - case 29: + case 30: blackboxPrintfHeaderLine("navrPID:%d,%d,%d", masterConfig.profile[masterConfig.current_profile_index].pidProfile.P8[PIDNAVR], masterConfig.profile[masterConfig.current_profile_index].pidProfile.I8[PIDNAVR], masterConfig.profile[masterConfig.current_profile_index].pidProfile.D8[PIDNAVR]); break; - case 30: + case 31: blackboxPrintfHeaderLine("levelPID:%d,%d,%d", masterConfig.profile[masterConfig.current_profile_index].pidProfile.P8[PIDLEVEL], masterConfig.profile[masterConfig.current_profile_index].pidProfile.I8[PIDLEVEL], masterConfig.profile[masterConfig.current_profile_index].pidProfile.D8[PIDLEVEL]); break; - case 31: + case 32: blackboxPrintfHeaderLine("magPID:%d", masterConfig.profile[masterConfig.current_profile_index].pidProfile.P8[PIDMAG]); break; - case 32: + case 33: blackboxPrintfHeaderLine("velPID:%d,%d,%d", masterConfig.profile[masterConfig.current_profile_index].pidProfile.P8[PIDVEL], masterConfig.profile[masterConfig.current_profile_index].pidProfile.I8[PIDVEL], masterConfig.profile[masterConfig.current_profile_index].pidProfile.D8[PIDVEL]); break; - case 33: + case 34: blackboxPrintfHeaderLine("yaw_p_limit:%d", masterConfig.profile[masterConfig.current_profile_index].pidProfile.yaw_p_limit); break; - case 34: + case 35: blackboxPrintfHeaderLine("yaw_lpf_hz:%d", (int)(masterConfig.profile[masterConfig.current_profile_index].pidProfile.yaw_lpf_hz * 100.0f)); break; - case 35: + case 36: blackboxPrintfHeaderLine("dterm_average_count:%d", masterConfig.profile[masterConfig.current_profile_index].pidProfile.dterm_average_count); break; - case 36: + case 37: blackboxPrintfHeaderLine("dynamic_pid:%d", masterConfig.profile[masterConfig.current_profile_index].pidProfile.dynamic_pid); break; - case 37: + case 38: blackboxPrintfHeaderLine("rollPitchItermResetRate:%d", masterConfig.profile[masterConfig.current_profile_index].pidProfile.rollPitchItermIgnoreRate); break; - case 38: + case 39: blackboxPrintfHeaderLine("yawItermResetRate:%d", masterConfig.profile[masterConfig.current_profile_index].pidProfile.yawItermIgnoreRate); break; - case 39: + case 40: blackboxPrintfHeaderLine("dterm_lpf_hz:%d", (int)(masterConfig.profile[masterConfig.current_profile_index].pidProfile.dterm_lpf_hz * 100.0f)); break; - case 40: + case 41: blackboxPrintfHeaderLine("airmode_activate_throttle:%d", masterConfig.rxConfig.airModeActivateThreshold); break; - case 41: - blackboxPrintfHeaderLine("deadband:%d", masterConfig.rcControlsConfig.deadband); - break; case 42: - blackboxPrintfHeaderLine("yaw_deadband:%d", masterConfig.rcControlsConfig.yaw_deadband); + blackboxPrintfHeaderLine("deadband:%d", masterConfig.rcControlsConfig.deadband); break; case 43: - blackboxPrintfHeaderLine("gyro_lpf:%d", masterConfig.gyro_lpf); + blackboxPrintfHeaderLine("yaw_deadband:%d", masterConfig.rcControlsConfig.yaw_deadband); break; case 44: - blackboxPrintfHeaderLine("gyro_lowpass_hz:%d", (int)(masterConfig.gyro_soft_lpf_hz * 100.0f)); + blackboxPrintfHeaderLine("gyro_lpf:%d", masterConfig.gyro_lpf); break; case 45: - blackboxPrintfHeaderLine("acc_lpf_hz:%d", (int)(masterConfig.acc_lpf_hz * 100.0f)); + blackboxPrintfHeaderLine("gyro_lowpass_hz:%d", (int)(masterConfig.gyro_soft_lpf_hz * 100.0f)); break; case 46: - blackboxPrintfHeaderLine("acc_hardware:%d", masterConfig.acc_hardware); + blackboxPrintfHeaderLine("acc_lpf_hz:%d", (int)(masterConfig.acc_lpf_hz * 100.0f)); break; case 47: - blackboxPrintfHeaderLine("baro_hardware:%d", masterConfig.baro_hardware); + blackboxPrintfHeaderLine("acc_hardware:%d", masterConfig.acc_hardware); break; case 48: - blackboxPrintfHeaderLine("mag_hardware:%d", masterConfig.mag_hardware); + blackboxPrintfHeaderLine("baro_hardware:%d", masterConfig.baro_hardware); break; case 49: - blackboxPrintfHeaderLine("gyro_cal_on_first_arm:%d", masterConfig.gyro_cal_on_first_arm); + blackboxPrintfHeaderLine("mag_hardware:%d", masterConfig.mag_hardware); break; case 50: - blackboxPrintfHeaderLine("vbat_pid_compensation:%d", masterConfig.batteryConfig.vbatPidCompensation); + blackboxPrintfHeaderLine("gyro_cal_on_first_arm:%d", masterConfig.gyro_cal_on_first_arm); break; case 51: - blackboxPrintfHeaderLine("rc_smoothing:%d", masterConfig.rxConfig.rcSmoothing); + blackboxPrintfHeaderLine("vbat_pid_compensation:%d", masterConfig.batteryConfig.vbatPidCompensation); break; case 52: - blackboxPrintfHeaderLine("superExpoYawMode:%d", masterConfig.rxConfig.superExpoYawMode); + blackboxPrintfHeaderLine("rc_smoothing:%d", masterConfig.rxConfig.rcSmoothing); break; case 53: blackboxPrintfHeaderLine("features:%d", masterConfig.enabledFeatures); diff --git a/src/main/config/config.c b/src/main/config/config.c index 081cf9e8d..88fd49059 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 = 138; +static const uint8_t EEPROM_CONF_VERSION = 139; static void resetAccelerometerTrims(flightDynamicsTrims_t *accelerometerTrims) { @@ -310,15 +310,15 @@ void resetSerialConfig(serialConfig_t *serialConfig) static void resetControlRateConfig(controlRateConfig_t *controlRateConfig) { controlRateConfig->rcRate8 = 100; - controlRateConfig->rcExpo8 = 70; + controlRateConfig->rcExpo8 = 10; controlRateConfig->thrMid8 = 50; controlRateConfig->thrExpo8 = 0; controlRateConfig->dynThrPID = 20; - controlRateConfig->rcYawExpo8 = 20; + controlRateConfig->rcYawExpo8 = 10; controlRateConfig->tpa_breakpoint = 1650; for (uint8_t axis = 0; axis < FLIGHT_DYNAMICS_INDEX_COUNT; axis++) { - controlRateConfig->rates[axis] = 50; + controlRateConfig->rates[axis] = 70; } } @@ -460,10 +460,7 @@ static void resetConf(void) masterConfig.rxConfig.rcSmoothing = 0; masterConfig.rxConfig.fpvCamAngleDegrees = 0; masterConfig.rxConfig.max_aux_channel = 6; - masterConfig.rxConfig.superExpoFactor = 30; masterConfig.rxConfig.airModeActivateThreshold = 1350; - masterConfig.rxConfig.superExpoFactorYaw = 40; - masterConfig.rxConfig.superExpoYawMode = 1; resetAllRxChannelRangeConfigurations(masterConfig.rxConfig.channelRanges); @@ -746,8 +743,6 @@ static bool isEEPROMContentValid(void) void activateControlRateConfig(void) { - generatePitchRollCurve(currentControlRateProfile); - generateYawCurve(currentControlRateProfile); generateThrottleCurve(currentControlRateProfile, &masterConfig.escAndServoConfig); } diff --git a/src/main/flight/pid.c b/src/main/flight/pid.c index 8058cd34d..8dc3f28f1 100644 --- a/src/main/flight/pid.c +++ b/src/main/flight/pid.c @@ -74,19 +74,19 @@ void setTargetPidLooptime(uint8_t pidProcessDenom) { targetPidLooptime = targetLooptime * pidProcessDenom; } -float calculateExpoPlus(int axis, const rxConfig_t *rxConfig, const controlRateConfig_t *controlRateConfig) { - float propFactor, superExpoFactor, rcFactor; +float calculateRate(int axis, const controlRateConfig_t *controlRateConfig) { + float angleRate; - if (axis == YAW && !rxConfig->superExpoYawMode) { - propFactor = 1.0f; - } else { - superExpoFactor = (axis == YAW) ? rxConfig->superExpoFactorYaw : rxConfig->superExpoFactor; - rcFactor = (axis == YAW) ? (ABS(rcCommand[axis]) / 500.0f) : (ABS(rcCommand[axis]) / (500.0f * (controlRateConfig->rcRate8 / 100.0f))); + if (isSuperExpoActive()) { + float rcFactor = (axis == YAW) ? (ABS(rcCommand[axis]) / 500.0f) : (ABS(rcCommand[axis]) / (500.0f * (controlRateConfig->rcRate8 / 100.0f))); + rcFactor = 1.0f / (1.0f - (rcFactor * (controlRateConfig->rates[axis] / 100.0f))); - propFactor = constrainf(1.0f - ((superExpoFactor / 100.0f) * rcFactor * rcFactor * rcFactor), 0.0f, 1.0f); + angleRate = rcFactor * ((27 * rcCommand[axis]) / 16.0f); + } else { + angleRate = (float)((controlRateConfig->rates[axis] + 27) * rcCommand[axis]) / 16.0f; } - return propFactor; + return angleRate; } uint16_t getDynamicKp(int axis, const pidProfile_t *pidProfile) { @@ -169,31 +169,26 @@ static void pidLuxFloat(const pidProfile_t *pidProfile, const controlRateConfig_ // ----------PID controller---------- for (axis = 0; axis < 3; axis++) { - uint8_t rate = controlRateConfig->rates[axis]; - - if (axis == FD_YAW) { - // YAW is always gyro-controlled (MAG correction is applied to rcCommand) 100dps to 1100dps max yaw rate - AngleRate = (float)((rate + 47) * rcCommand[YAW]) / 32.0f; - } else { - // ACRO mode, control is GYRO based, direct sticks control is applied to rate PID - AngleRate = (float)((rate + 27) * rcCommand[axis]) / 16.0f; // 200dps to 1200dps max roll/pitch rate - if (FLIGHT_MODE(ANGLE_MODE) || FLIGHT_MODE(HORIZON_MODE)) { - // calculate error angle and limit the angle to the max inclination + + // ACRO mode, control is GYRO based, direct sticks control is applied to rate PID + AngleRate = calculateRate(axis, controlRateConfig); + + if (FLIGHT_MODE(ANGLE_MODE) || FLIGHT_MODE(HORIZON_MODE)) { + // calculate error angle and limit the angle to the max inclination #ifdef GPS - const float errorAngle = (constrain(2 * rcCommand[axis] + GPS_angle[axis], -((int) max_angle_inclination), - +max_angle_inclination) - attitude.raw[axis] + angleTrim->raw[axis]); // 16 bits is ok here + const float errorAngle = (constrain(2 * rcCommand[axis] + GPS_angle[axis], -((int) max_angle_inclination), + +max_angle_inclination) - attitude.raw[axis] + angleTrim->raw[axis]); // 16 bits is ok here #else - const float errorAngle = (constrain(2 * rcCommand[axis], -((int) max_angle_inclination), - +max_angle_inclination) - attitude.raw[axis] + angleTrim->raw[axis]); // 16 bits is ok here + const float errorAngle = (constrain(2 * rcCommand[axis], -((int) max_angle_inclination), + +max_angle_inclination) - attitude.raw[axis] + angleTrim->raw[axis]); // 16 bits is ok here #endif - if (FLIGHT_MODE(ANGLE_MODE)) { - // ANGLE mode - control is angle based, so control loop is needed - AngleRate = errorAngle * pidProfile->P8[PIDLEVEL] / 16.0f; - } else { - // HORIZON mode - direct sticks control is applied to rate PID - // mix up angle error to desired AngleRate to add a little auto-level feel - AngleRate += errorAngle * pidProfile->I8[PIDLEVEL] * horizonLevelStrength / 16.0f; - } + if (FLIGHT_MODE(ANGLE_MODE)) { + // ANGLE mode - control is angle based, so control loop is needed + AngleRate = errorAngle * pidProfile->P8[PIDLEVEL] / 16.0f; + } else { + // HORIZON mode - direct sticks control is applied to rate PID + // mix up angle error to desired AngleRate to add a little auto-level feel + AngleRate += errorAngle * pidProfile->I8[PIDLEVEL] * horizonLevelStrength / 16.0f; } } @@ -208,11 +203,7 @@ static void pidLuxFloat(const pidProfile_t *pidProfile, const controlRateConfig_ uint16_t kP = (pidProfile->dynamic_pid) ? getDynamicKp(axis, pidProfile) : pidProfile->P8[axis]; // -----calculate P component - if ((isSuperExpoActive() && axis != YAW) || (axis == YAW && rxConfig->superExpoYawMode == SUPEREXPO_YAW_ALWAYS)) { - PTerm = (luxPTermScale * kP * tpaFactor) * (AngleRate - gyroRate * calculateExpoPlus(axis, rxConfig, controlRateConfig)); - } else { - PTerm = luxPTermScale * RateError * kP * tpaFactor; - } + PTerm = luxPTermScale * RateError * kP * tpaFactor; // Constrain YAW by yaw_p_limit value if not servo driven in that case servolimits apply if((motorCount >= 4 && pidProfile->yaw_p_limit) && axis == YAW) { @@ -295,31 +286,26 @@ static void pidMultiWiiRewrite(const pidProfile_t *pidProfile, const controlRate // ----------PID controller---------- for (axis = 0; axis < 3; axis++) { - uint8_t rate = controlRateConfig->rates[axis]; // -----Get the desired angle rate depending on flight mode - if (axis == FD_YAW) { - // YAW is always gyro-controlled (MAG correction is applied to rcCommand) - AngleRateTmp = ((int32_t)(rate + 47) * rcCommand[YAW]) >> 5; - } else { - AngleRateTmp = ((int32_t)(rate + 27) * rcCommand[axis]) >> 4; - if (FLIGHT_MODE(ANGLE_MODE) || FLIGHT_MODE(HORIZON_MODE)) { - // calculate error angle and limit the angle to max configured inclination + AngleRateTmp = (int32_t)calculateRate(axis, controlRateConfig); + + if (FLIGHT_MODE(ANGLE_MODE) || FLIGHT_MODE(HORIZON_MODE)) { + // calculate error angle and limit the angle to max configured inclination #ifdef GPS - const int32_t errorAngle = constrain(2 * rcCommand[axis] + GPS_angle[axis], -((int) max_angle_inclination), - +max_angle_inclination) - attitude.raw[axis] + angleTrim->raw[axis]; + const int32_t errorAngle = constrain(2 * rcCommand[axis] + GPS_angle[axis], -((int) max_angle_inclination), + +max_angle_inclination) - attitude.raw[axis] + angleTrim->raw[axis]; #else - const int32_t errorAngle = constrain(2 * rcCommand[axis], -((int) max_angle_inclination), - +max_angle_inclination) - attitude.raw[axis] + angleTrim->raw[axis]; + const int32_t errorAngle = constrain(2 * rcCommand[axis], -((int) max_angle_inclination), + +max_angle_inclination) - attitude.raw[axis] + angleTrim->raw[axis]; #endif - if (FLIGHT_MODE(ANGLE_MODE)) { - // ANGLE mode - control is angle based, so control loop is needed - AngleRateTmp = (errorAngle * pidProfile->P8[PIDLEVEL]) >> 4; - } else { - // HORIZON mode - mix up angle error to desired AngleRateTmp to add a little auto-level feel, - // horizonLevelStrength is scaled to the stick input - AngleRateTmp += (errorAngle * pidProfile->I8[PIDLEVEL] * horizonLevelStrength / 100) >> 4; - } + if (FLIGHT_MODE(ANGLE_MODE)) { + // ANGLE mode - control is angle based, so control loop is needed + AngleRateTmp = (errorAngle * pidProfile->P8[PIDLEVEL]) >> 4; + } else { + // HORIZON mode - mix up angle error to desired AngleRateTmp to add a little auto-level feel, + // horizonLevelStrength is scaled to the stick input + AngleRateTmp += (errorAngle * pidProfile->I8[PIDLEVEL] * horizonLevelStrength / 100) >> 4; } } @@ -333,11 +319,7 @@ static void pidMultiWiiRewrite(const pidProfile_t *pidProfile, const controlRate uint16_t kP = (pidProfile->dynamic_pid) ? getDynamicKp(axis, pidProfile) : pidProfile->P8[axis]; // -----calculate P component - if ((isSuperExpoActive() && axis != YAW) || (axis == YAW && rxConfig->superExpoYawMode == SUPEREXPO_YAW_ALWAYS)) { - PTerm = (kP * PIDweight[axis] / 100) * (AngleRateTmp - (int32_t)(gyroRate * calculateExpoPlus(axis, rxConfig, controlRateConfig))) >> 7; - } else { - PTerm = (RateError * kP * PIDweight[axis] / 100) >> 7; - } + PTerm = (RateError * kP * PIDweight[axis] / 100) >> 7; // Constrain YAW by yaw_p_limit value if not servo driven in that case servolimits apply if((motorCount >= 4 && pidProfile->yaw_p_limit) && axis == YAW) { diff --git a/src/main/io/rc_controls.c b/src/main/io/rc_controls.c index 8f25d6f6c..4e7344ac9 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 (IS_RC_MODE_ACTIVE(BOXSUPEREXPO) || feature(FEATURE_SUPEREXPO)); + return (feature(FEATURE_SUPEREXPO)); } void blackboxLogInflightAdjustmentEvent(adjustmentFunction_e adjustmentFunction, int32_t newValue) { @@ -503,13 +503,11 @@ void applyStepAdjustment(controlRateConfig_t *controlRateConfig, uint8_t adjustm case ADJUSTMENT_RC_RATE: newValue = constrain((int)controlRateConfig->rcRate8 + delta, 0, 250); // FIXME magic numbers repeated in serial_cli.c controlRateConfig->rcRate8 = newValue; - generatePitchRollCurve(controlRateConfig); blackboxLogInflightAdjustmentEvent(ADJUSTMENT_RC_RATE, newValue); break; case ADJUSTMENT_RC_EXPO: newValue = constrain((int)controlRateConfig->rcExpo8 + delta, 0, 100); // FIXME magic numbers repeated in serial_cli.c controlRateConfig->rcExpo8 = newValue; - generatePitchRollCurve(controlRateConfig); blackboxLogInflightAdjustmentEvent(ADJUSTMENT_RC_EXPO, newValue); break; case ADJUSTMENT_THROTTLE_EXPO: diff --git a/src/main/io/rc_curves.c b/src/main/io/rc_curves.c index ac6e67579..2ed391974 100644 --- a/src/main/io/rc_curves.c +++ b/src/main/io/rc_curves.c @@ -26,37 +26,9 @@ #include "config/config.h" -#define PITCH_LOOKUP_LENGTH 31 -#define YAW_LOOKUP_LENGTH 31 #define THROTTLE_LOOKUP_LENGTH 12 - -static int16_t lookupPitchRollRC[PITCH_LOOKUP_LENGTH]; // lookup table for expo & RC rate PITCH+ROLL -static int16_t lookupYawRC[YAW_LOOKUP_LENGTH]; // lookup table for expo & RC rate YAW static int16_t lookupThrottleRC[THROTTLE_LOOKUP_LENGTH]; // lookup table for expo & mid THROTTLE - -void generatePitchRollCurve(controlRateConfig_t *controlRateConfig) -{ - uint8_t i; - float j = 0; - - for (i = 0; i < PITCH_LOOKUP_LENGTH; i++) { - lookupPitchRollRC[i] = (2500 + controlRateConfig->rcExpo8 * (j * j - 25)) * j * (int32_t) controlRateConfig->rcRate8 / 2500; - j += 0.2f; - } -} - -void generateYawCurve(controlRateConfig_t *controlRateConfig) -{ - uint8_t i; - float j = 0; - - for (i = 0; i < YAW_LOOKUP_LENGTH; i++) { - lookupYawRC[i] = (2500 + controlRateConfig->rcYawExpo8 * (j * j - 25)) * j / 25; - j += 0.2f; - } -} - void generateThrottleCurve(controlRateConfig_t *controlRateConfig, escAndServoConfig_t *escAndServoConfig) { uint8_t i; @@ -74,16 +46,16 @@ void generateThrottleCurve(controlRateConfig_t *controlRateConfig, escAndServoCo } } -int16_t rcLookupPitchRoll(int32_t tmp) +int16_t rcLookupPitchRoll(int32_t tmp, controlRateConfig_t *controlRateConfig) { - const int32_t tmp2 = tmp / 20; - return lookupPitchRollRC[tmp2] + (tmp - tmp2 * 20) * (lookupPitchRollRC[tmp2 + 1] - lookupPitchRollRC[tmp2]) / 20; + 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) +int16_t rcLookupYaw(int32_t tmp, controlRateConfig_t *controlRateConfig) { - const int32_t tmp2 = tmp / 20; - return lookupYawRC[tmp2] + (tmp - tmp2 * 20) * (lookupYawRC[tmp2 + 1] - lookupYawRC[tmp2]) / 20; + float tmpf = tmp / 100.0f; + return (int16_t)((2500.0f + (float)controlRateConfig->rcYawExpo8* (tmpf * tmpf - 25.0f)) * tmpf / 25.0f ); } int16_t rcLookupThrottle(int32_t tmp) diff --git a/src/main/io/rc_curves.h b/src/main/io/rc_curves.h index 4134f0a31..428dbdc5c 100644 --- a/src/main/io/rc_curves.h +++ b/src/main/io/rc_curves.h @@ -17,11 +17,9 @@ #pragma once -void generatePitchRollCurve(controlRateConfig_t *controlRateConfig); -void generateYawCurve(controlRateConfig_t *controlRateConfig); void generateThrottleCurve(controlRateConfig_t *controlRateConfig, escAndServoConfig_t *escAndServoConfig); -int16_t rcLookupPitchRoll(int32_t tmp); -int16_t rcLookupYaw(int32_t tmp); +int16_t rcLookupPitchRoll(int32_t tmp, controlRateConfig_t *controlRateConfig); +int16_t rcLookupYaw(int32_t tmp, controlRateConfig_t *controlRateConfig); int16_t rcLookupThrottle(int32_t tmp); diff --git a/src/main/io/serial_cli.c b/src/main/io/serial_cli.c index 7cc6ae32f..05aefaa7a 100644 --- a/src/main/io/serial_cli.c +++ b/src/main/io/serial_cli.c @@ -704,9 +704,6 @@ const clivalue_t valueTable[] = { { "yaw_rate", 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} }, - { "super_expo_factor", VAR_UINT8 | MASTER_VALUE, &masterConfig.rxConfig.superExpoFactor, .config.minmax = {1, 100 } }, - { "super_expo_factor_yaw", VAR_UINT8 | MASTER_VALUE, &masterConfig.rxConfig.superExpoFactorYaw, .config.minmax = {1, 100 } }, - { "super_expo_yaw", VAR_UINT8 | MASTER_VALUE | MODE_LOOKUP, &masterConfig.rxConfig.superExpoYawMode, .config.lookup = { TABLE_SUPEREXPO_YAW } }, { "airmode_activate_throttle", VAR_UINT16 | MASTER_VALUE, &masterConfig.rxConfig.airModeActivateThreshold, .config.minmax = {1000, 2000 } }, { "failsafe_delay", VAR_UINT8 | MASTER_VALUE, &masterConfig.failsafeConfig.failsafe_delay, .config.minmax = { 0, 200 } }, diff --git a/src/main/io/serial_msp.c b/src/main/io/serial_msp.c index f932fdb80..8133aac4e 100644 --- a/src/main/io/serial_msp.c +++ b/src/main/io/serial_msp.c @@ -216,7 +216,7 @@ static const box_t boxes[CHECKBOX_ITEM_COUNT + 1] = { { BOXBLACKBOX, "BLACKBOX;", 26 }, { BOXFAILSAFE, "FAILSAFE;", 27 }, { BOXAIRMODE, "AIR MODE;", 28 }, - { BOXSUPEREXPO, "SUPER EXPO;", 29 }, + //{ BOXSUPEREXPO, "SUPER EXPO;", 29 }, { BOX3DDISABLESWITCH, "DISABLE 3D SWITCH;", 30}, { CHECKBOX_ITEM_COUNT, NULL, 0xFF } }; @@ -544,7 +544,6 @@ void mspInit(serialConfig_t *serialConfig) } if (!feature(FEATURE_AIRMODE)) activeBoxIds[activeBoxIdCount++] = BOXAIRMODE; - if (!feature(FEATURE_SUPEREXPO)) activeBoxIds[activeBoxIdCount++] = BOXSUPEREXPO; activeBoxIds[activeBoxIdCount++] = BOX3DDISABLESWITCH; if (sensors(SENSOR_BARO)) { @@ -650,8 +649,7 @@ static uint32_t packFlightModeFlags(void) IS_ENABLED(ARMING_FLAG(ARMED)) << BOXARM | IS_ENABLED(IS_RC_MODE_ACTIVE(BOXBLACKBOX)) << BOXBLACKBOX | IS_ENABLED(FLIGHT_MODE(FAILSAFE_MODE)) << BOXFAILSAFE | - IS_ENABLED(IS_RC_MODE_ACTIVE(BOXAIRMODE)) << BOXAIRMODE | - IS_ENABLED(IS_RC_MODE_ACTIVE(BOXSUPEREXPO)) << BOXSUPEREXPO; + IS_ENABLED(IS_RC_MODE_ACTIVE(BOXAIRMODE)) << BOXAIRMODE; for (i = 0; i < activeBoxIdCount; i++) { int flag = (tmp & (1 << activeBoxIds[i])); diff --git a/src/main/mw.c b/src/main/mw.c index c5c5e0af5..cc515485b 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); + rcCommand[axis] = rcLookupPitchRoll(tmp, currentControlRateProfile); } else if (axis == YAW) { if (tmp > masterConfig.rcControlsConfig.yaw_deadband) { tmp -= masterConfig.rcControlsConfig.yaw_deadband; } else { tmp = 0; } - rcCommand[axis] = rcLookupYaw(tmp) * -masterConfig.yaw_control_direction; + rcCommand[axis] = rcLookupYaw(tmp, currentControlRateProfile) * -masterConfig.yaw_control_direction; } if (rcData[axis] < masterConfig.rxConfig.midrc) { rcCommand[axis] = -rcCommand[axis]; diff --git a/src/main/rx/rx.h b/src/main/rx/rx.h index b6e1cfea3..b76873fb1 100644 --- a/src/main/rx/rx.h +++ b/src/main/rx/rx.h @@ -124,9 +124,6 @@ typedef struct rxConfig_s { uint8_t rcSmoothing; uint8_t fpvCamAngleDegrees; // Camera angle to be scaled into rc commands uint8_t max_aux_channel; - uint8_t superExpoFactor; // Super Expo Factor - uint8_t superExpoFactorYaw; // Super Expo Factor Yaw - uint8_t superExpoYawMode; // Seperate Super expo for yaw uint16_t airModeActivateThreshold; // Throttle setpoint where airmode gets activated uint16_t rx_min_usec; -- 2.11.4.GIT