From d534f99fb3514136f9bc4572e75000ca2f9bf3ed Mon Sep 17 00:00:00 2001 From: "Pawel Spychalski (DzikuVx)" Date: Fri, 13 May 2016 12:09:42 +0200 Subject: [PATCH 01/10] rates constrained to new min and max --- src/main/io/rc_controls.c | 11 +++++------ src/main/io/rc_controls.h | 8 ++++++-- src/main/io/serial_cli.c | 6 +++--- src/main/io/serial_msp.c | 2 +- 4 files changed, 15 insertions(+), 12 deletions(-) diff --git a/src/main/io/rc_controls.c b/src/main/io/rc_controls.c index 9969d012d7..1ba97fe554 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; } @@ -476,7 +476,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 +484,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 +676,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..1f37692ff4 100644 --- a/src/main/io/rc_controls.h +++ b/src/main/io/rc_controls.h @@ -109,10 +109,14 @@ typedef enum { #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 +#define CONTROL_RATE_CONFIG_ROLL_PITCH_RATE_MAX 180 +#define CONTROL_RATE_CONFIG_ROLL_PITCH_RATE_MIN 10 +#define CONTROL_RATE_CONFIG_ROLL_PITCH_RATE_DEFAULT 20 /* Meaningful yaw rates are effectively unbounded because they are treated as a rotation rate multiplier: */ -#define CONTROL_RATE_CONFIG_YAW_RATE_MAX 255 +#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 diff --git a/src/main/io/serial_cli.c b/src/main/io/serial_cli.c index d9ee608a87..6027ddb2f9 100644 --- a/src/main/io/serial_cli.c +++ b/src/main/io/serial_cli.c @@ -675,9 +675,9 @@ const clivalue_t valueTable[] = { { "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 }, + { "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 d50b321e22..111b4d965e 100644 --- a/src/main/io/serial_msp.c +++ b/src/main/io/serial_msp.c @@ -1219,7 +1219,7 @@ static bool processInCommand(void) 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); + currentControlRateProfile->rates[i] = constrain(rate, i == FD_YAW ? CONTROL_RATE_CONFIG_YAW_RATE_MIN : CONTROL_RATE_CONFIG_ROLL_PITCH_RATE_MIN, i == FD_YAW ? CONTROL_RATE_CONFIG_YAW_RATE_MAX : CONTROL_RATE_CONFIG_ROLL_PITCH_RATE_MAX); } rate = read8(); currentControlRateProfile->dynThrPID = MIN(rate, CONTROL_RATE_CONFIG_TPA_MAX); From 058c2bf72e2c77f080fb5f648b61c393b1592cf7 Mon Sep 17 00:00:00 2001 From: "Pawel Spychalski (DzikuVx)" Date: Fri, 13 May 2016 14:09:05 +0200 Subject: [PATCH 02/10] new defaults applied to rates when PID profile is changed --- src/main/config/config.c | 12 ++++++++---- 1 file changed, 8 insertions(+), 4 deletions(-) diff --git a/src/main/config/config.c b/src/main/config/config.c index ee21d663eb..0b6fe4d3ad 100755 --- a/src/main/config/config.c +++ b/src/main/config/config.c @@ -362,7 +362,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; + } } } @@ -625,9 +629,9 @@ static void resetConf(void) 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 From 122c3f5a29687a393eba91223660b7294765db8c Mon Sep 17 00:00:00 2001 From: "Pawel Spychalski (DzikuVx)" Date: Fri, 13 May 2016 14:47:35 +0200 Subject: [PATCH 03/10] new rcCommand scaling to desired rotation rates --- src/main/common/maths.c | 6 ++++++ src/main/common/maths.h | 1 + src/main/flight/pid.c | 8 ++++++-- 3 files changed, 13 insertions(+), 2 deletions(-) diff --git a/src/main/common/maths.c b/src/main/common/maths.c index 79c1d2f81d..7367d73465 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 eddb5773b4..6569ff2d06 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/flight/pid.c b/src/main/flight/pid.c index 783c6e9ea0..4dfd61418a 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); } #define FP_PID_RATE_P_MULTIPLIER 40.0f // betaflight - 40.0 From 7eb2419ef9afb30b60cbd3b7e7640d1fcc6f9c7a Mon Sep 17 00:00:00 2001 From: "Pawel Spychalski (DzikuVx)" Date: Fri, 13 May 2016 21:50:03 +0200 Subject: [PATCH 04/10] rc curves generation simplified --- src/main/config/config.c | 3 +-- src/main/io/rc_controls.c | 4 ++-- src/main/io/rc_curves.c | 18 +++++++++++------- src/main/io/rc_curves.h | 3 +-- src/test/unit/rc_controls_unittest.cc | 4 ++-- 5 files changed, 17 insertions(+), 15 deletions(-) diff --git a/src/main/config/config.c b/src/main/config/config.c index 0b6fe4d3ad..d6fb64eeb0 100755 --- a/src/main/config/config.c +++ b/src/main/config/config.c @@ -732,8 +732,7 @@ static bool isEEPROMContentValid(void) void activateControlRateConfig(void) { - generatePitchRollCurve(currentControlRateProfile); - generateYawCurve(currentControlRateProfile); + generateRcCurves(currentControlRateProfile); generateThrottleCurve(currentControlRateProfile, &masterConfig.escAndServoConfig); } diff --git a/src/main/io/rc_controls.c b/src/main/io/rc_controls.c index 1ba97fe554..a711e109b2 100644 --- a/src/main/io/rc_controls.c +++ b/src/main/io/rc_controls.c @@ -459,13 +459,13 @@ 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); + generateRcCurves(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: diff --git a/src/main/io/rc_curves.c b/src/main/io/rc_curves.c index 53fb6a71ae..636913aefd 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); } } 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/test/unit/rc_controls_unittest.cc b/src/test/unit/rc_controls_unittest.cc index 61a4b1e90c..c7e552b5f1 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]++; } @@ -714,4 +714,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 +} From 0f19dcc70f21b6e73e829a848229a8097cd6b32c Mon Sep 17 00:00:00 2001 From: "Pawel Spychalski (DzikuVx)" Date: Fri, 13 May 2016 22:07:28 +0200 Subject: [PATCH 05/10] rc_rate setting removed --- docs/Cli.md | 1 - docs/Profiles.md | 3 +-- src/main/blackbox/blackbox.c | 12 ++++++------ src/main/config/config.c | 2 -- src/main/io/rc_controls.c | 6 ------ src/main/io/rc_controls.h | 1 - src/main/io/serial_cli.c | 1 - src/main/io/serial_msp.c | 4 ++-- src/test/unit/rc_controls_unittest.cc | 11 ----------- 9 files changed, 9 insertions(+), 32 deletions(-) diff --git a/docs/Cli.md b/docs/Cli.md index 064812382e..c468b71d45 100644 --- a/docs/Cli.md +++ b/docs/Cli.md @@ -187,7 +187,6 @@ 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 | diff --git a/docs/Profiles.md b/docs/Profiles.md index a8fb592e29..4ff574092e 100644 --- a/docs/Profiles.md +++ b/docs/Profiles.md @@ -31,7 +31,7 @@ Three rate profiles are supported. Rate profiles can be selected while flying using the inflight adjustments feature. Each normal profile has a setting called 'default_rate_profile`. When a profile is activated the -corresponding rate profile is also activated. +corresponding rate profile is also activated. Profile 0 has a default rate profile of 0. Profile 1 has a default rate profile of 1. @@ -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 d0149ed374..841d896e20 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/config/config.c b/src/main/config/config.c index d6fb64eeb0..44fd94c611 100755 --- a/src/main/config/config.c +++ b/src/main/config/config.c @@ -353,7 +353,6 @@ void resetSerialConfig(serialConfig_t *serialConfig) } static void resetControlRateConfig(controlRateConfig_t *controlRateConfig) { - controlRateConfig->rcRate8 = 90; controlRateConfig->rcExpo8 = 70; controlRateConfig->thrMid8 = 50; controlRateConfig->thrExpo8 = 0; @@ -628,7 +627,6 @@ 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] = 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; diff --git a/src/main/io/rc_controls.c b/src/main/io/rc_controls.c index a711e109b2..c16691876f 100644 --- a/src/main/io/rc_controls.c +++ b/src/main/io/rc_controls.c @@ -456,12 +456,6 @@ 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; - generateRcCurves(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; diff --git a/src/main/io/rc_controls.h b/src/main/io/rc_controls.h index 1f37692ff4..f7f99516e6 100644 --- a/src/main/io/rc_controls.h +++ b/src/main/io/rc_controls.h @@ -138,7 +138,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/serial_cli.c b/src/main/io/serial_cli.c index 6027ddb2f9..e1fc9f5926 100644 --- a/src/main/io/serial_cli.c +++ b/src/main/io/serial_cli.c @@ -670,7 +670,6 @@ 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 }, diff --git a/src/main/io/serial_msp.c b/src/main/io/serial_msp.c index 111b4d965e..9581748017 100644 --- a/src/main/io/serial_msp.c +++ b/src/main/io/serial_msp.c @@ -732,7 +732,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 @@ -1215,7 +1215,7 @@ 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(); diff --git a/src/test/unit/rc_controls_unittest.cc b/src/test/unit/rc_controls_unittest.cc index c7e552b5f1..8e744dba89 100644 --- a/src/test/unit/rc_controls_unittest.cc +++ b/src/test/unit/rc_controls_unittest.cc @@ -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); From 7c99f9db56a21947be2a4ed1270fbef585ea6da7 Mon Sep 17 00:00:00 2001 From: "Pawel Spychalski (DzikuVx)" Date: Sat, 14 May 2016 08:17:14 +0200 Subject: [PATCH 06/10] rcCommand correctly scaled to dps --- src/main/flight/pid.c | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/src/main/flight/pid.c b/src/main/flight/pid.c index 4dfd61418a..b411695c82 100644 --- a/src/main/flight/pid.c +++ b/src/main/flight/pid.c @@ -122,7 +122,7 @@ Rate 20 means 200dps at full stick deflection */ float pidRcCommandToRate(int16_t stick, uint8_t rate) { - return scaleRangef((float) stick, (float) -500, (float) 500, (float) -rate, (float) rate); + return scaleRangef((float) stick, (float) -500, (float) 500, (float) -rate, (float) rate) * 10; } #define FP_PID_RATE_P_MULTIPLIER 40.0f // betaflight - 40.0 From 0818bad36ee7e8a45ec4ebec09eeaa93b35e53f3 Mon Sep 17 00:00:00 2001 From: "Pawel Spychalski (DzikuVx)" Date: Sat, 14 May 2016 09:28:19 +0200 Subject: [PATCH 07/10] comments and docs updated for new meaning of rates --- docs/Cli.md | 5 +++-- src/main/io/rc_controls.h | 16 +++++++++++----- src/main/io/serial_cli.c | 6 ++++++ 3 files changed, 20 insertions(+), 7 deletions(-) diff --git a/docs/Cli.md b/docs/Cli.md index c468b71d45..73260aff29 100644 --- a/docs/Cli.md +++ b/docs/Cli.md @@ -191,8 +191,9 @@ Re-apply any new defaults as desired. | `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/src/main/io/rc_controls.h b/src/main/io/rc_controls.h index f7f99516e6..41e9f72bb4 100644 --- a/src/main/io/rc_controls.h +++ b/src/main/io/rc_controls.h @@ -108,12 +108,18 @@ 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 180 -#define CONTROL_RATE_CONFIG_ROLL_PITCH_RATE_MIN 10 -#define CONTROL_RATE_CONFIG_ROLL_PITCH_RATE_DEFAULT 20 +/* +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: */ +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 diff --git a/src/main/io/serial_cli.c b/src/main/io/serial_cli.c index e1fc9f5926..4e71b5ed25 100644 --- a/src/main/io/serial_cli.c +++ b/src/main/io/serial_cli.c @@ -674,9 +674,15 @@ const clivalue_t valueTable[] = { { "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 }, + + /* + 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 }, From 9032cb846e75e5b42a8e32677226d66609811db1 Mon Sep 17 00:00:00 2001 From: "Pawel Spychalski (DzikuVx)" Date: Tue, 17 May 2016 14:15:57 +0200 Subject: [PATCH 08/10] rcCommand computation cleanup --- src/main/mw.c | 72 +++++++++++++++++++++++++++++++-------------------- 1 file changed, 44 insertions(+), 28 deletions(-) diff --git a/src/main/mw.c b/src/main/mw.c index ab3a55d05c..5befaca28d 100755 --- a/src/main/mw.c +++ b/src/main/mw.c @@ -125,40 +125,56 @@ bool isCalibrating() return (!isAccelerationCalibrationComplete() && sensors(SENSOR_ACC)) || (!isGyroCalibrationComplete()); } -void annexCode(void) +int16_t computeCommandFromLookup(int16_t (*loopupTable)(int32_t), int16_t absoluteDeflection) { - int32_t tmp; - 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; - } + int16_t lookupStep; - if (rcData[axis] < masterConfig.rxConfig.midrc) { - rcCommand[axis] = -rcCommand[axis]; + lookupStep = absoluteDeflection / 100; + + return loopupTable(lookupStep) + (absoluteDeflection - lookupStep * 100) * (loopupTable(lookupStep + 1) - loopupTable(lookupStep)) / 100; +} + +int16_t getAxisRcCommand(int16_t rawData, int16_t (*loopupTable)(int32_t), int16_t deadband) +{ + int16_t command, absoluteDeflection; + + absoluteDeflection = MIN(ABS(rawData - masterConfig.rxConfig.midrc), 500); + + 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 = computeCommandFromLookup(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] = computeCommandFromLookup(rcLookupThrottle, throttleValue); if (FLIGHT_MODE(HEADFREE_MODE)) { const float radDiff = degreesToRadians(DECIDEGREES_TO_DEGREES(attitude.values.yaw) - headFreeModeHold); From 00882e1b4942568e977aca6be40738c296126aa6 Mon Sep 17 00:00:00 2001 From: "Pawel Spychalski (DzikuVx)" Date: Wed, 25 May 2016 20:46:40 +0200 Subject: [PATCH 09/10] lookup table access simplified --- src/main/io/rc_curves.c | 21 ++++++++++++--------- src/main/mw.c | 14 ++------------ 2 files changed, 14 insertions(+), 21 deletions(-) diff --git a/src/main/io/rc_curves.c b/src/main/io/rc_curves.c index 636913aefd..02cb7f3ef1 100644 --- a/src/main/io/rc_curves.c +++ b/src/main/io/rc_curves.c @@ -67,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/mw.c b/src/main/mw.c index 5befaca28d..c065f80e1f 100755 --- a/src/main/mw.c +++ b/src/main/mw.c @@ -125,16 +125,6 @@ bool isCalibrating() return (!isAccelerationCalibrationComplete() && sensors(SENSOR_ACC)) || (!isGyroCalibrationComplete()); } -int16_t computeCommandFromLookup(int16_t (*loopupTable)(int32_t), int16_t absoluteDeflection) -{ - - int16_t lookupStep; - - lookupStep = absoluteDeflection / 100; - - return loopupTable(lookupStep) + (absoluteDeflection - lookupStep * 100) * (loopupTable(lookupStep + 1) - loopupTable(lookupStep)) / 100; -} - int16_t getAxisRcCommand(int16_t rawData, int16_t (*loopupTable)(int32_t), int16_t deadband) { int16_t command, absoluteDeflection; @@ -152,7 +142,7 @@ int16_t getAxisRcCommand(int16_t rawData, int16_t (*loopupTable)(int32_t), int16 /* Get command from lookup table after applying deadband */ - command = computeCommandFromLookup(loopupTable, absoluteDeflection); + command = loopupTable(absoluteDeflection); if (rawData < masterConfig.rxConfig.midrc) { command = -command; @@ -174,7 +164,7 @@ void annexCode(void) //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] = computeCommandFromLookup(rcLookupThrottle, throttleValue); + rcCommand[THROTTLE] = rcLookupThrottle(throttleValue); if (FLIGHT_MODE(HEADFREE_MODE)) { const float radDiff = degreesToRadians(DECIDEGREES_TO_DEGREES(attitude.values.yaw) - headFreeModeHold); From 6bf5c147b6693b5a9b10251f40ff1ed3560303ab Mon Sep 17 00:00:00 2001 From: "Konstantin (DigitalEntity) Sharlaimov" Date: Wed, 15 Jun 2016 20:16:07 +0300 Subject: [PATCH 10/10] Refactor MSP_SET_RC_TUNING for better readability --- src/main/io/serial_msp.c | 7 ++++++- 1 file changed, 6 insertions(+), 1 deletion(-) diff --git a/src/main/io/serial_msp.c b/src/main/io/serial_msp.c index 5d65422ff2..7e50346e10 100644 --- a/src/main/io/serial_msp.c +++ b/src/main/io/serial_msp.c @@ -1224,7 +1224,12 @@ static bool processInCommand(void) currentControlRateProfile->rcExpo8 = read8(); for (i = 0; i < 3; i++) { rate = read8(); - currentControlRateProfile->rates[i] = constrain(rate, i == FD_YAW ? CONTROL_RATE_CONFIG_YAW_RATE_MIN : CONTROL_RATE_CONFIG_ROLL_PITCH_RATE_MIN, 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);