diff --git a/docs/Cli.md b/docs/Cli.md index 6c9be5ea4d..5d85fd7bba 100644 --- a/docs/Cli.md +++ b/docs/Cli.md @@ -187,13 +187,13 @@ Re-apply any new defaults as desired. | `yaw_motor_direction` | | -1 | 1 | 1 | Profile | INT8 | | `tri_unarmed_servo` | On tricopter mix only, if this is set to 1, servo will always be correcting regardless of armed state. to disable this, set it to 0. | OFF | ON | ON | Profile | INT8 | | `default_rate_profile` | Default = profile number | 0 | 2 | | Profile | UINT8 | -| `rc_rate` | | 0 | 250 | 90 | Rate Profile | UINT8 | | `rc_expo` | | 0 | 100 | 65 | Rate Profile | UINT8 | | `rc_yaw_expo` | | 0 | 100 | 0 | Rate Profile | UINT8 | | `thr_mid` | | 0 | 100 | 50 | Rate Profile | UINT8 | | `thr_expo` | | 0 | 100 | 0 | Rate Profile | UINT8 | -| `roll_pitch_rate` | | 0 | 100 | 0 | Rate Profile | UINT8 | -| `yaw_rate` | | 0 | 100 | 0 | Rate Profile | UINT8 | +| `roll_rate` | Defines rotation rate on ROLL axis that UAV will try to archive on max. stick deflection. Rates are defined in tenths of degrees per second [dps/10]. That means, rate 20 represents 200dps rotation speed. Default 20 (200dps) is more less equivalent of old Cleanflight/Baseflight rate 0. Max. 180 (1800dps) is what gyro can measure. +| `pitch_rate` | Defines rotation rate on PITCH axis that UAV will try to archive on max. stick deflection. Rates are defined in tenths of degrees per second [dps/10]. That means, rate 20 represents 200dps rotation speed. Default 20 (200dps) is more less equivalent of old Cleanflight/Baseflight rate 0. Max. 180 (1800dps) is what gyro can measure. | 6 | 180 | 20 | Rate Profile | UINT8 | +| `yaw_rate` | Defines rotation rate on YAW axis that UAV will try to archive on max. stick deflection. Rates are defined in tenths of degrees per second [dps/10]. That means, rate 20 represents 200dps rotation speed. Default 20 (200dps) is more less equivalent of old Cleanflight/Baseflight rate 0. Max. 180 (1800dps) is what gyro can measure. | 2 | 180 | 20 | Rate Profile | UINT8 | | `tpa_rate` | Throttle PID attenuation reduces influence of P on ROLL and PITCH as throttle increases. For every 1% throttle after the TPA breakpoint, P is reduced by the TPA rate. | 0 | 100 | 0 | Rate Profile | UINT8 | | `tpa_breakpoint` | See tpa_rate. | 1000 | 2000 | 1500 | Rate Profile | UINT16 | | `failsafe_delay` | Time in deciseconds to wait before activating failsafe when signal is lost. See [Failsafe documentation](Failsafe.md#failsafe_delay). | 0 | 200 | 10 | Profile | UINT8 | diff --git a/docs/Profiles.md b/docs/Profiles.md index 06d4546a1b..890a47a946 100644 --- a/docs/Profiles.md +++ b/docs/Profiles.md @@ -54,7 +54,6 @@ e.g # rateprofile rateprofile 0 -set rc_rate = 90 set rc_expo = 65 set thr_mid = 50 set thr_expo = 0 diff --git a/src/main/blackbox/blackbox.c b/src/main/blackbox/blackbox.c index b4efe3ddad..0512306f71 100644 --- a/src/main/blackbox/blackbox.c +++ b/src/main/blackbox/blackbox.c @@ -685,7 +685,7 @@ static void writeInterframe(void) arraySubInt32(deltas, blackboxCurrent->axisPID_P, blackboxLast->axisPID_P, XYZ_AXIS_COUNT); blackboxWriteSignedVBArray(deltas, XYZ_AXIS_COUNT); - /* + /* * The PID I field changes very slowly, most of the time +-2, so use an encoding * that can pack all three fields into one byte in that situation. */ @@ -920,7 +920,7 @@ void startBlackbox(void) * cache those now. */ blackboxBuildConditionCache(); - + blackboxModeActivationConditionPresent = isModeActivationConditionPresent(currentProfile->modeActivationConditions, BOXBLACKBOX); blackboxIteration = 0; @@ -1228,7 +1228,7 @@ static bool blackboxWriteSysinfo() blackboxPrintfHeaderLine("P interval:%d/%d", masterConfig.blackbox_rate_num, masterConfig.blackbox_rate_denom); break; case 4: - blackboxPrintfHeaderLine("rcRate:%d", masterConfig.controlRateProfiles[masterConfig.current_profile_index].rcRate8); + blackboxPrintfHeaderLine("rcRate:%d", 100); //For compatibility reasons write rc_rate 100 break; case 5: blackboxPrintfHeaderLine("minthrottle:%d", masterConfig.escAndServoConfig.minthrottle); @@ -1323,7 +1323,7 @@ static void blackboxCheckAndLogArmingBeep() } } -/* +/* * Use the user's num/denom settings to decide if the P-frame of the given index should be logged, allowing the user to control * the portion of logged loop iterations. */ @@ -1367,7 +1367,7 @@ static void blackboxLogIteration() writeIntraframe(); } else { blackboxCheckAndLogArmingBeep(); - + if (blackboxShouldLogPFrame(blackboxPFrameIndex)) { /* * We assume that slow frames are only interesting in that they aid the interpretation of the main data stream. @@ -1499,7 +1499,7 @@ void handleBlackbox(void) blackboxLogEvent(FLIGHT_LOG_EVENT_LOGGING_RESUME, (flightLogEventData_t *) &resume); blackboxSetState(BLACKBOX_STATE_RUNNING); - + blackboxLogIteration(); } diff --git a/src/main/common/maths.c b/src/main/common/maths.c index d613853be7..6b5886d2d8 100644 --- a/src/main/common/maths.c +++ b/src/main/common/maths.c @@ -187,6 +187,12 @@ int scaleRange(int x, int srcMin, int srcMax, int destMin, int destMax) { return ((a / b) - (destMax - destMin)) + destMax; } +int scaleRangef(float x, float srcMin, float srcMax, float destMin, float destMax) { + float a = (destMax - destMin) * (x - srcMin); + float b = srcMax - srcMin; + return ((a / b) - (destMax - destMin)) + destMax; +} + // Normalize a vector void normalizeV(struct fp_vector *src, struct fp_vector *dest) { diff --git a/src/main/common/maths.h b/src/main/common/maths.h index c59e8f4061..40d48d9af4 100644 --- a/src/main/common/maths.h +++ b/src/main/common/maths.h @@ -139,6 +139,7 @@ float devStandardDeviation(stdev_t *dev); float degreesToRadians(int16_t degrees); int scaleRange(int x, int srcMin, int srcMax, int destMin, int destMax); +int scaleRangef(float x, float srcMin, float srcMax, float destMin, float destMax); void normalizeV(struct fp_vector *src, struct fp_vector *dest); diff --git a/src/main/config/config.c b/src/main/config/config.c index 6b2fa1f1ba..31b88aa1d7 100755 --- a/src/main/config/config.c +++ b/src/main/config/config.c @@ -362,7 +362,6 @@ void resetSerialConfig(serialConfig_t *serialConfig) } static void resetControlRateConfig(controlRateConfig_t *controlRateConfig) { - controlRateConfig->rcRate8 = 90; controlRateConfig->rcExpo8 = 70; controlRateConfig->thrMid8 = 50; controlRateConfig->thrExpo8 = 0; @@ -371,7 +370,11 @@ static void resetControlRateConfig(controlRateConfig_t *controlRateConfig) { controlRateConfig->tpa_breakpoint = 1500; for (uint8_t axis = 0; axis < FLIGHT_DYNAMICS_INDEX_COUNT; axis++) { - controlRateConfig->rates[axis] = 0; + if (axis == FD_YAW) { + controlRateConfig->rates[axis] = CONTROL_RATE_CONFIG_YAW_RATE_DEFAULT; + } else { + controlRateConfig->rates[axis] = CONTROL_RATE_CONFIG_ROLL_PITCH_RATE_DEFAULT; + } } } @@ -634,10 +637,9 @@ static void resetConf(void) currentProfile->pidProfile.P8[PITCH] = 36; masterConfig.failsafeConfig.failsafe_delay = 2; masterConfig.failsafeConfig.failsafe_off_delay = 0; - currentControlRateProfile->rcRate8 = 130; - currentControlRateProfile->rates[FD_PITCH] = 20; - currentControlRateProfile->rates[FD_ROLL] = 20; - currentControlRateProfile->rates[FD_YAW] = 100; + currentControlRateProfile->rates[FD_PITCH] = CONTROL_RATE_CONFIG_ROLL_PITCH_RATE_DEFAULT; + currentControlRateProfile->rates[FD_ROLL] = CONTROL_RATE_CONFIG_ROLL_PITCH_RATE_DEFAULT; + currentControlRateProfile->rates[FD_YAW] = CONTROL_RATE_CONFIG_YAW_RATE_DEFAULT; parseRcChannels("TAER1234", &masterConfig.rxConfig); // { 1.0f, -0.414178f, 1.0f, -1.0f }, // REAR_R @@ -738,8 +740,7 @@ static bool isEEPROMContentValid(void) void activateControlRateConfig(void) { - generatePitchRollCurve(currentControlRateProfile); - generateYawCurve(currentControlRateProfile); + generateRcCurves(currentControlRateProfile); generateThrottleCurve(currentControlRateProfile, &masterConfig.escAndServoConfig); } diff --git a/src/main/flight/pid.c b/src/main/flight/pid.c index 783c6e9ea0..b411695c82 100644 --- a/src/main/flight/pid.c +++ b/src/main/flight/pid.c @@ -115,10 +115,14 @@ int16_t pidAngleToRcCommand(float angleDeciDegrees) return angleDeciDegrees / 2.0f; } +/* +Map stick positions to desired rotatrion rate in given axis. +Rotation rate in dps at full stick deflection is defined by axis rate measured in dps/10 +Rate 20 means 200dps at full stick deflection +*/ float pidRcCommandToRate(int16_t stick, uint8_t rate) { - // Map stick position from 200dps to 1200dps - return (float)((rate + 20) * stick) / 50.0f; + return scaleRangef((float) stick, (float) -500, (float) 500, (float) -rate, (float) rate) * 10; } #define FP_PID_RATE_P_MULTIPLIER 40.0f // betaflight - 40.0 diff --git a/src/main/io/rc_controls.c b/src/main/io/rc_controls.c index 9969d012d7..c16691876f 100644 --- a/src/main/io/rc_controls.c +++ b/src/main/io/rc_controls.c @@ -284,12 +284,12 @@ bool isModeActivationConditionPresent(modeActivationCondition_t *modeActivationC for (index = 0; index < MAX_MODE_ACTIVATION_CONDITION_COUNT; index++) { modeActivationCondition_t *modeActivationCondition = &modeActivationConditions[index]; - + if (modeActivationCondition->modeId == modeId && IS_RANGE_USABLE(&modeActivationCondition->range)) { return true; } } - + return false; } @@ -456,16 +456,10 @@ void applyStepAdjustment(controlRateConfig_t *controlRateConfig, uint8_t adjustm beeperConfirmationBeeps(1); } switch(adjustmentFunction) { - 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); + generateRcCurves(controlRateConfig); blackboxLogInflightAdjustmentEvent(ADJUSTMENT_RC_EXPO, newValue); break; case ADJUSTMENT_THROTTLE_EXPO: @@ -476,7 +470,7 @@ void applyStepAdjustment(controlRateConfig_t *controlRateConfig, uint8_t adjustm break; case ADJUSTMENT_PITCH_ROLL_RATE: case ADJUSTMENT_PITCH_RATE: - newValue = constrain((int)controlRateConfig->rates[FD_PITCH] + delta, 0, CONTROL_RATE_CONFIG_ROLL_PITCH_RATE_MAX); + newValue = constrain((int)controlRateConfig->rates[FD_PITCH] + delta, CONTROL_RATE_CONFIG_ROLL_PITCH_RATE_MIN, CONTROL_RATE_CONFIG_ROLL_PITCH_RATE_MAX); controlRateConfig->rates[FD_PITCH] = newValue; blackboxLogInflightAdjustmentEvent(ADJUSTMENT_PITCH_RATE, newValue); if (adjustmentFunction == ADJUSTMENT_PITCH_RATE) { @@ -484,12 +478,12 @@ void applyStepAdjustment(controlRateConfig_t *controlRateConfig, uint8_t adjustm } // follow though for combined ADJUSTMENT_PITCH_ROLL_RATE case ADJUSTMENT_ROLL_RATE: - newValue = constrain((int)controlRateConfig->rates[FD_ROLL] + delta, 0, CONTROL_RATE_CONFIG_ROLL_PITCH_RATE_MAX); + newValue = constrain((int)controlRateConfig->rates[FD_ROLL] + delta, CONTROL_RATE_CONFIG_ROLL_PITCH_RATE_MIN, CONTROL_RATE_CONFIG_ROLL_PITCH_RATE_MAX); controlRateConfig->rates[FD_ROLL] = newValue; blackboxLogInflightAdjustmentEvent(ADJUSTMENT_ROLL_RATE, newValue); break; case ADJUSTMENT_YAW_RATE: - newValue = constrain((int)controlRateConfig->rates[FD_YAW] + delta, 0, CONTROL_RATE_CONFIG_YAW_RATE_MAX); + newValue = constrain((int)controlRateConfig->rates[FD_YAW] + delta, CONTROL_RATE_CONFIG_YAW_RATE_MIN, CONTROL_RATE_CONFIG_YAW_RATE_MAX); controlRateConfig->rates[FD_YAW] = newValue; blackboxLogInflightAdjustmentEvent(ADJUSTMENT_YAW_RATE, newValue); break; @@ -676,4 +670,3 @@ void resetAdjustmentStates(void) { memset(adjustmentStates, 0, sizeof(adjustmentStates)); } - diff --git a/src/main/io/rc_controls.h b/src/main/io/rc_controls.h index bb372a0473..41e9f72bb4 100644 --- a/src/main/io/rc_controls.h +++ b/src/main/io/rc_controls.h @@ -108,11 +108,21 @@ typedef enum { #define MIN_MODE_RANGE_STEP 0 #define MAX_MODE_RANGE_STEP ((CHANNEL_RANGE_MAX - CHANNEL_RANGE_MIN) / 25) -// Roll/pitch rates are a proportion used for mixing, so it tops out at 1.0: -#define CONTROL_RATE_CONFIG_ROLL_PITCH_RATE_MAX 100 +/* +Max and min available values for rates are now stored as absolute +tenths of degrees-per-second [dsp/10] +That means, max. rotation rate 180 equals 1800dps -/* Meaningful yaw rates are effectively unbounded because they are treated as a rotation rate multiplier: */ -#define CONTROL_RATE_CONFIG_YAW_RATE_MAX 255 +New defaults of 200dps for pitch,roll and yaw are more less +equivalent of rates 0 from previous versions of iNav, Cleanflight, Baseflight +and so on. +*/ +#define CONTROL_RATE_CONFIG_ROLL_PITCH_RATE_MAX 180 +#define CONTROL_RATE_CONFIG_ROLL_PITCH_RATE_MIN 6 +#define CONTROL_RATE_CONFIG_ROLL_PITCH_RATE_DEFAULT 20 +#define CONTROL_RATE_CONFIG_YAW_RATE_MAX 180 +#define CONTROL_RATE_CONFIG_YAW_RATE_MIN 2 +#define CONTROL_RATE_CONFIG_YAW_RATE_DEFAULT 20 #define CONTROL_RATE_CONFIG_TPA_MAX 100 @@ -134,7 +144,6 @@ typedef struct modeActivationCondition_s { #define IS_RANGE_USABLE(range) ((range)->startStep < (range)->endStep) typedef struct controlRateConfig_s { - uint8_t rcRate8; 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 53fb6a71ae..02cb7f3ef1 100644 --- a/src/main/io/rc_curves.c +++ b/src/main/io/rc_curves.c @@ -33,17 +33,21 @@ static int16_t lookupYawRC[YAW_LOOKUP_LENGTH]; // lookup table for static int16_t lookupThrottleRC[THROTTLE_LOOKUP_LENGTH]; // lookup table for expo & mid THROTTLE int16_t lookupThrottleRCMid; // THROTTLE curve mid point -void generatePitchRollCurve(controlRateConfig_t *controlRateConfig) +int16_t computeRcCurvePoint(uint8_t expo, uint8_t i) { - for (int i = 0; i < PITCH_LOOKUP_LENGTH; i++) { - lookupPitchRollRC[i] = (2500 + controlRateConfig->rcExpo8 * (i * i - 25)) * i * (int32_t) controlRateConfig->rcRate8 / 2500; - } + return (2500 + expo * (i * i - 25)) * i / 25; } -void generateYawCurve(controlRateConfig_t *controlRateConfig) +void generateRcCurves(controlRateConfig_t *controlRateConfig) { - for (int i = 0; i < YAW_LOOKUP_LENGTH; i++) { - lookupYawRC[i] = (2500 + controlRateConfig->rcYawExpo8 * (i * i - 25)) * i / 25; + uint8_t i; + + for (i = 0; i < PITCH_LOOKUP_LENGTH; i++) { + lookupPitchRollRC[i] = computeRcCurvePoint(controlRateConfig->rcExpo8, i); + } + + for (i = 0; i < YAW_LOOKUP_LENGTH; i++) { + lookupYawRC[i] = computeRcCurvePoint(controlRateConfig->rcYawExpo8, i); } } @@ -63,22 +67,25 @@ void generateThrottleCurve(controlRateConfig_t *controlRateConfig, escAndServoCo } } -int16_t rcLookupPitchRoll(int32_t tmp) +int16_t rcLookupTable(int16_t lookupTable[], int32_t absoluteDeflection) { - const int32_t tmp2 = tmp / 100; - return lookupPitchRollRC[tmp2] + (tmp - tmp2 * 100) * (lookupPitchRollRC[tmp2 + 1] - lookupPitchRollRC[tmp2]) / 100; + const int32_t lookupStep = absoluteDeflection / 100; + return lookupTable[lookupStep] + (absoluteDeflection - lookupStep * 100) * (lookupTable[lookupStep + 1] - lookupTable[lookupStep]) / 100; } -int16_t rcLookupYaw(int32_t tmp) +int16_t rcLookupPitchRoll(int32_t absoluteDeflection) { - const int32_t tmp2 = tmp / 100; - return (lookupYawRC[tmp2] + (tmp - tmp2 * 100) * (lookupYawRC[tmp2 + 1] - lookupYawRC[tmp2]) / 100); + return rcLookupTable(lookupPitchRollRC, absoluteDeflection); } -int16_t rcLookupThrottle(int32_t tmp) +int16_t rcLookupYaw(int32_t absoluteDeflection) { - const int32_t tmp2 = tmp / 100; - return lookupThrottleRC[tmp2] + (tmp - tmp2 * 100) * (lookupThrottleRC[tmp2 + 1] - lookupThrottleRC[tmp2]) / 100; // [0;1000] -> expo -> [MINTHROTTLE;MAXTHROTTLE] + return rcLookupTable(lookupYawRC, absoluteDeflection); +} + +int16_t rcLookupThrottle(int32_t absoluteDeflection) +{ + return rcLookupTable(lookupThrottleRC, absoluteDeflection); } int16_t rcLookupThrottleMid(void) diff --git a/src/main/io/rc_curves.h b/src/main/io/rc_curves.h index 0a0f19b5ec..0f4b40a676 100644 --- a/src/main/io/rc_curves.h +++ b/src/main/io/rc_curves.h @@ -17,8 +17,7 @@ #pragma once -void generatePitchRollCurve(controlRateConfig_t *controlRateConfig); -void generateYawCurve(controlRateConfig_t *controlRateConfig); +void generateRcCurves(controlRateConfig_t *controlRateConfig); void generateThrottleCurve(controlRateConfig_t *controlRateConfig, escAndServoConfig_t *escAndServoConfig); int16_t rcLookupPitchRoll(int32_t tmp); diff --git a/src/main/io/serial_cli.c b/src/main/io/serial_cli.c index ae90315416..bfbc2472a7 100644 --- a/src/main/io/serial_cli.c +++ b/src/main/io/serial_cli.c @@ -708,14 +708,19 @@ const clivalue_t valueTable[] = { #endif { "default_rate_profile", VAR_UINT8 | PROFILE_VALUE , &masterConfig.profile[0].defaultRateProfileIndex, .config.minmax = { 0, MAX_CONTROL_RATE_PROFILE_COUNT - 1 }, 0 }, - { "rc_rate", VAR_UINT8 | CONTROL_RATE_VALUE, &masterConfig.controlRateProfiles[0].rcRate8, .config.minmax = { 0, 250 }, 0 }, { "rc_expo", VAR_UINT8 | CONTROL_RATE_VALUE, &masterConfig.controlRateProfiles[0].rcExpo8, .config.minmax = { 0, 100 }, 0 }, { "rc_yaw_expo", VAR_UINT8 | CONTROL_RATE_VALUE, &masterConfig.controlRateProfiles[0].rcYawExpo8, .config.minmax = { 0, 100 }, 0 }, { "thr_mid", VAR_UINT8 | CONTROL_RATE_VALUE, &masterConfig.controlRateProfiles[0].thrMid8, .config.minmax = { 0, 100 }, 0 }, { "thr_expo", VAR_UINT8 | CONTROL_RATE_VALUE, &masterConfig.controlRateProfiles[0].thrExpo8, .config.minmax = { 0, 100 }, 0 }, - { "roll_rate", VAR_UINT8 | CONTROL_RATE_VALUE, &masterConfig.controlRateProfiles[0].rates[FD_ROLL], .config.minmax = { 0, CONTROL_RATE_CONFIG_ROLL_PITCH_RATE_MAX }, 0 }, - { "pitch_rate", VAR_UINT8 | CONTROL_RATE_VALUE, &masterConfig.controlRateProfiles[0].rates[FD_PITCH], .config.minmax = { 0, CONTROL_RATE_CONFIG_ROLL_PITCH_RATE_MAX }, 0 }, - { "yaw_rate", VAR_UINT8 | CONTROL_RATE_VALUE, &masterConfig.controlRateProfiles[0].rates[FD_YAW], .config.minmax = { 0, CONTROL_RATE_CONFIG_YAW_RATE_MAX }, 0 }, + + /* + New rates are in dps/10. That means, Rate of 20 means 200dps of rotation speed on given axis. + Rate 180 (1800dps) is max. value gyro can measure reliably + */ + { "roll_rate", VAR_UINT8 | CONTROL_RATE_VALUE, &masterConfig.controlRateProfiles[0].rates[FD_ROLL], .config.minmax = { CONTROL_RATE_CONFIG_ROLL_PITCH_RATE_MIN, CONTROL_RATE_CONFIG_ROLL_PITCH_RATE_MAX }, 0 }, + { "pitch_rate", VAR_UINT8 | CONTROL_RATE_VALUE, &masterConfig.controlRateProfiles[0].rates[FD_PITCH], .config.minmax = { CONTROL_RATE_CONFIG_ROLL_PITCH_RATE_MIN, CONTROL_RATE_CONFIG_ROLL_PITCH_RATE_MAX }, 0 }, + { "yaw_rate", VAR_UINT8 | CONTROL_RATE_VALUE, &masterConfig.controlRateProfiles[0].rates[FD_YAW], .config.minmax = { CONTROL_RATE_CONFIG_YAW_RATE_MIN, CONTROL_RATE_CONFIG_YAW_RATE_MAX }, 0 }, + { "tpa_rate", VAR_UINT8 | CONTROL_RATE_VALUE, &masterConfig.controlRateProfiles[0].dynThrPID, .config.minmax = { 0, CONTROL_RATE_CONFIG_TPA_MAX}, 0 }, { "tpa_breakpoint", VAR_UINT16 | CONTROL_RATE_VALUE, &masterConfig.controlRateProfiles[0].tpa_breakpoint, .config.minmax = { PWM_RANGE_MIN, PWM_RANGE_MAX}, 0 }, diff --git a/src/main/io/serial_msp.c b/src/main/io/serial_msp.c index 328257f911..2cfc9c8941 100644 --- a/src/main/io/serial_msp.c +++ b/src/main/io/serial_msp.c @@ -737,7 +737,7 @@ static bool processOutCommand(uint8_t cmdMSP) break; case MSP_RC_TUNING: headSerialReply(11); - serialize8(currentControlRateProfile->rcRate8); + serialize8(100); //rcRate8 kept for compatibity reasons, this setting is no longer used serialize8(currentControlRateProfile->rcExpo8); for (i = 0 ; i < 3; i++) { serialize8(currentControlRateProfile->rates[i]); // R,P,Y see flight_dynamics_index_t @@ -1221,11 +1221,16 @@ static bool processInCommand(void) case MSP_SET_RC_TUNING: if (currentPort->dataSize >= 10) { - currentControlRateProfile->rcRate8 = read8(); + read8(); //Read rcRate8, kept for protocol compatibility reasons currentControlRateProfile->rcExpo8 = read8(); for (i = 0; i < 3; i++) { rate = read8(); - currentControlRateProfile->rates[i] = MIN(rate, i == FD_YAW ? CONTROL_RATE_CONFIG_YAW_RATE_MAX : CONTROL_RATE_CONFIG_ROLL_PITCH_RATE_MAX); + if (i == FD_YAW) { + currentControlRateProfile->rates[i] = constrain(rate, CONTROL_RATE_CONFIG_YAW_RATE_MIN, CONTROL_RATE_CONFIG_YAW_RATE_MAX); + } + else { + currentControlRateProfile->rates[i] = constrain(rate, CONTROL_RATE_CONFIG_ROLL_PITCH_RATE_MIN, CONTROL_RATE_CONFIG_ROLL_PITCH_RATE_MAX); + } } rate = read8(); currentControlRateProfile->dynThrPID = MIN(rate, CONTROL_RATE_CONFIG_TPA_MAX); diff --git a/src/main/mw.c b/src/main/mw.c index e7adf96caa..f8e64418ee 100755 --- a/src/main/mw.c +++ b/src/main/mw.c @@ -126,40 +126,46 @@ bool isCalibrating() return (!isAccelerationCalibrationComplete() && sensors(SENSOR_ACC)) || (!isGyroCalibrationComplete()); } -void annexCode(void) +int16_t getAxisRcCommand(int16_t rawData, int16_t (*loopupTable)(int32_t), int16_t deadband) { - int32_t tmp; + int16_t command, absoluteDeflection; - for (int axis = 0; axis < 3; axis++) { - tmp = MIN(ABS(rcData[axis] - masterConfig.rxConfig.midrc), 500); - if (axis == ROLL || axis == PITCH) { - if (currentProfile->rcControlsConfig.deadband) { - if (tmp > currentProfile->rcControlsConfig.deadband) { - tmp -= currentProfile->rcControlsConfig.deadband; - } else { - tmp = 0; - } - } - rcCommand[axis] = rcLookupPitchRoll(tmp); - } else if (axis == YAW) { - if (currentProfile->rcControlsConfig.yaw_deadband) { - if (tmp > currentProfile->rcControlsConfig.yaw_deadband) { - tmp -= currentProfile->rcControlsConfig.yaw_deadband; - } else { - tmp = 0; - } - } - rcCommand[axis] = rcLookupYaw(tmp) * -1; - } + absoluteDeflection = MIN(ABS(rawData - masterConfig.rxConfig.midrc), 500); - if (rcData[axis] < masterConfig.rxConfig.midrc) { - rcCommand[axis] = -rcCommand[axis]; + if (deadband) { + if (absoluteDeflection > deadband) { + absoluteDeflection -= deadband; + } else { + absoluteDeflection = 0; } } - tmp = constrain(rcData[THROTTLE], masterConfig.rxConfig.mincheck, PWM_RANGE_MAX); - tmp = (uint32_t)(tmp - masterConfig.rxConfig.mincheck) * PWM_RANGE_MIN / (PWM_RANGE_MAX - masterConfig.rxConfig.mincheck); // [MINCHECK;2000] -> [0;1000] - rcCommand[THROTTLE] = rcLookupThrottle(tmp); + /* + Get command from lookup table after applying deadband + */ + command = loopupTable(absoluteDeflection); + + if (rawData < masterConfig.rxConfig.midrc) { + command = -command; + } + + return command; +} + +void annexCode(void) +{ + + int32_t throttleValue; + + // Compute ROLL PITCH and YAW command + rcCommand[ROLL] = getAxisRcCommand(rcData[ROLL], rcLookupPitchRoll, currentProfile->rcControlsConfig.deadband); + rcCommand[PITCH] = getAxisRcCommand(rcData[PITCH], rcLookupPitchRoll, currentProfile->rcControlsConfig.deadband); + rcCommand[YAW] = getAxisRcCommand(rcData[YAW], rcLookupYaw, currentProfile->rcControlsConfig.yaw_deadband); + + //Compute THROTTLE command + throttleValue = constrain(rcData[THROTTLE], masterConfig.rxConfig.mincheck, PWM_RANGE_MAX); + throttleValue = (uint32_t)(throttleValue - masterConfig.rxConfig.mincheck) * PWM_RANGE_MIN / (PWM_RANGE_MAX - masterConfig.rxConfig.mincheck); // [MINCHECK;2000] -> [0;1000] + rcCommand[THROTTLE] = rcLookupThrottle(throttleValue); if (FLIGHT_MODE(HEADFREE_MODE)) { const float radDiff = degreesToRadians(DECIDEGREES_TO_DEGREES(attitude.values.yaw) - headFreeModeHold); diff --git a/src/test/unit/rc_controls_unittest.cc b/src/test/unit/rc_controls_unittest.cc index 61a4b1e90c..8e744dba89 100644 --- a/src/test/unit/rc_controls_unittest.cc +++ b/src/test/unit/rc_controls_unittest.cc @@ -186,7 +186,7 @@ static int callCounts[CALL_COUNT_ITEM_COUNT]; #define CALL_COUNTER(item) (callCounts[item]) extern "C" { -void generatePitchRollCurve(controlRateConfig_t *) { +void generateRcCurves(controlRateConfig_t *) { callCounts[COUNTER_GENERATE_PITCH_ROLL_CURVE]++; } @@ -237,7 +237,6 @@ static const adjustmentConfig_t rateAdjustmentConfig = { class RcControlsAdjustmentsTest : public ::testing::Test { protected: controlRateConfig_t controlRateConfig = { - .rcRate8 = 90, .rcExpo8 = 0, .thrMid8 = 0, .thrExpo8 = 0, @@ -256,7 +255,6 @@ protected: rxConfig.maxcheck = DEFAULT_MAX_CHECK; rxConfig.midrc = 1500; - controlRateConfig.rcRate8 = 90; controlRateConfig.rcExpo8 = 0; controlRateConfig.thrMid8 = 0; controlRateConfig.thrExpo8 = 0; @@ -289,7 +287,6 @@ TEST_F(RcControlsAdjustmentsTest, processRcAdjustmentsSticksInMiddle) processRcAdjustments(&controlRateConfig, &rxConfig); // then - EXPECT_EQ(controlRateConfig.rcRate8, 90); EXPECT_EQ(CALL_COUNTER(COUNTER_GENERATE_PITCH_ROLL_CURVE), 0); EXPECT_EQ(CALL_COUNTER(COUNTER_QUEUE_CONFIRMATION_BEEP), 0); EXPECT_EQ(adjustmentStateMask, 0); @@ -299,7 +296,6 @@ TEST_F(RcControlsAdjustmentsTest, processRcAdjustmentsWithRcRateFunctionSwitchUp { // given controlRateConfig_t controlRateConfig = { - .rcRate8 = 90, .rcExpo8 = 0, .thrMid8 = 0, .thrExpo8 = 0, @@ -344,7 +340,6 @@ TEST_F(RcControlsAdjustmentsTest, processRcAdjustmentsWithRcRateFunctionSwitchUp processRcAdjustments(&controlRateConfig, &rxConfig); // then - EXPECT_EQ(controlRateConfig.rcRate8, 91); EXPECT_EQ(CALL_COUNTER(COUNTER_GENERATE_PITCH_ROLL_CURVE), 1); EXPECT_EQ(CALL_COUNTER(COUNTER_QUEUE_CONFIRMATION_BEEP), 1); EXPECT_EQ(adjustmentStateMask, expectedAdjustmentStateMask); @@ -360,7 +355,6 @@ TEST_F(RcControlsAdjustmentsTest, processRcAdjustmentsWithRcRateFunctionSwitchUp // when processRcAdjustments(&controlRateConfig, &rxConfig); - EXPECT_EQ(controlRateConfig.rcRate8, 91); EXPECT_EQ(adjustmentStateMask, expectedAdjustmentStateMask); @@ -382,7 +376,6 @@ TEST_F(RcControlsAdjustmentsTest, processRcAdjustmentsWithRcRateFunctionSwitchUp // when processRcAdjustments(&controlRateConfig, &rxConfig); - EXPECT_EQ(controlRateConfig.rcRate8, 91); EXPECT_EQ(adjustmentStateMask, expectedAdjustmentStateMask); @@ -403,7 +396,6 @@ TEST_F(RcControlsAdjustmentsTest, processRcAdjustmentsWithRcRateFunctionSwitchUp processRcAdjustments(&controlRateConfig, &rxConfig); // then - EXPECT_EQ(controlRateConfig.rcRate8, 92); EXPECT_EQ(CALL_COUNTER(COUNTER_GENERATE_PITCH_ROLL_CURVE), 2); EXPECT_EQ(CALL_COUNTER(COUNTER_QUEUE_CONFIRMATION_BEEP), 2); EXPECT_EQ(adjustmentStateMask, expectedAdjustmentStateMask); @@ -420,7 +412,6 @@ TEST_F(RcControlsAdjustmentsTest, processRcAdjustmentsWithRcRateFunctionSwitchUp processRcAdjustments(&controlRateConfig, &rxConfig); // then - EXPECT_EQ(controlRateConfig.rcRate8, 92); EXPECT_EQ(adjustmentStateMask, expectedAdjustmentStateMask); // @@ -434,7 +425,6 @@ TEST_F(RcControlsAdjustmentsTest, processRcAdjustmentsWithRcRateFunctionSwitchUp processRcAdjustments(&controlRateConfig, &rxConfig); // then - EXPECT_EQ(controlRateConfig.rcRate8, 92); EXPECT_EQ(adjustmentStateMask, expectedAdjustmentStateMask); // @@ -450,7 +440,6 @@ TEST_F(RcControlsAdjustmentsTest, processRcAdjustmentsWithRcRateFunctionSwitchUp processRcAdjustments(&controlRateConfig, &rxConfig); // then - EXPECT_EQ(controlRateConfig.rcRate8, 93); EXPECT_EQ(CALL_COUNTER(COUNTER_GENERATE_PITCH_ROLL_CURVE), 3); EXPECT_EQ(CALL_COUNTER(COUNTER_QUEUE_CONFIRMATION_BEEP), 3); EXPECT_EQ(adjustmentStateMask, expectedAdjustmentStateMask); @@ -714,4 +703,4 @@ int16_t heading; uint8_t stateFlags = 0; int16_t rcData[MAX_SUPPORTED_RC_CHANNEL_COUNT]; rxRuntimeConfig_t rxRuntimeConfig; -} \ No newline at end of file +}