From da8bb6f6fc663c568678cfe1fa743bd68fcdbbe9 Mon Sep 17 00:00:00 2001 From: borisbstyle Date: Tue, 5 May 2015 16:43:27 +0200 Subject: [PATCH] Yaw Expo Feature --- docs/Cli.md | 1 + src/main/config/config.c | 2 ++ src/main/io/rc_controls.h | 1 + src/main/io/rc_curves.c | 9 +++++++++ src/main/io/rc_curves.h | 3 +++ src/main/io/serial_cli.c | 1 + src/main/io/serial_msp.c | 6 ++++-- src/main/mw.c | 3 ++- 8 files changed, 23 insertions(+), 3 deletions(-) diff --git a/docs/Cli.md b/docs/Cli.md index 55cfddbcd..05ab6c828 100644 --- a/docs/Cli.md +++ b/docs/Cli.md @@ -185,6 +185,7 @@ Re-apply any new defaults as desired. | 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 | diff --git a/src/main/config/config.c b/src/main/config/config.c index 5d1c27ca9..831fc280c 100644 --- a/src/main/config/config.c +++ b/src/main/config/config.c @@ -301,6 +301,7 @@ static void resetControlRateConfig(controlRateConfig_t *controlRateConfig) { controlRateConfig->thrMid8 = 50; controlRateConfig->thrExpo8 = 0; controlRateConfig->dynThrPID = 0; + controlRateConfig->rcYawExpo8 = 0; controlRateConfig->tpa_breakpoint = 1500; for (uint8_t axis = 0; axis < FLIGHT_DYNAMICS_INDEX_COUNT; axis++) { @@ -650,6 +651,7 @@ static bool isEEPROMContentValid(void) void activateControlRateConfig(void) { generatePitchRollCurve(currentControlRateProfile); + generateYawCurve(currentControlRateProfile); generateThrottleCurve(currentControlRateProfile, &masterConfig.escAndServoConfig); } diff --git a/src/main/io/rc_controls.h b/src/main/io/rc_controls.h index 9305364ec..be2eda7e6 100644 --- a/src/main/io/rc_controls.h +++ b/src/main/io/rc_controls.h @@ -132,6 +132,7 @@ typedef struct controlRateConfig_s { uint8_t thrExpo8; uint8_t rates[3]; uint8_t dynThrPID; + uint8_t rcYawExpo8; uint16_t tpa_breakpoint; // Breakpoint where TPA is activated } controlRateConfig_t; diff --git a/src/main/io/rc_curves.c b/src/main/io/rc_curves.c index 85330d7fa..9ac65ce4a 100644 --- a/src/main/io/rc_curves.c +++ b/src/main/io/rc_curves.c @@ -25,6 +25,7 @@ #include "io/rc_curves.h" int16_t lookupPitchRollRC[PITCH_LOOKUP_LENGTH]; // lookup table for expo & RC rate PITCH+ROLL +int16_t lookupYawRC[YAW_LOOKUP_LENGTH]; // lookup table for expo & RC rate YAW int16_t lookupThrottleRC[THROTTLE_LOOKUP_LENGTH]; // lookup table for expo & mid THROTTLE @@ -36,6 +37,14 @@ void generatePitchRollCurve(controlRateConfig_t *controlRateConfig) lookupPitchRollRC[i] = (2500 + controlRateConfig->rcExpo8 * (i * i - 25)) * i * (int32_t) controlRateConfig->rcRate8 / 2500; } +void generateYawCurve(controlRateConfig_t *controlRateConfig) +{ + uint8_t i; + + for (i = 0; i < PITCH_LOOKUP_LENGTH; i++) + lookupYawRC[i] = (2500 + controlRateConfig->rcYawExpo8 * (i * i - 25)) * i / 25; +} + void generateThrottleCurve(controlRateConfig_t *controlRateConfig, escAndServoConfig_t *escAndServoConfig) { uint8_t i; diff --git a/src/main/io/rc_curves.h b/src/main/io/rc_curves.h index fa98c957b..f79b50599 100644 --- a/src/main/io/rc_curves.h +++ b/src/main/io/rc_curves.h @@ -18,9 +18,12 @@ #pragma once #define PITCH_LOOKUP_LENGTH 7 +#define YAW_LOOKUP_LENGTH 7 #define THROTTLE_LOOKUP_LENGTH 12 extern int16_t lookupPitchRollRC[PITCH_LOOKUP_LENGTH]; // lookup table for expo & RC rate PITCH+ROLL +extern int16_t lookupYawRC[YAW_LOOKUP_LENGTH]; // lookup table for expo & RC rate YAW extern int16_t lookupThrottleRC[THROTTLE_LOOKUP_LENGTH]; // lookup table for expo & mid THROTTLE void generatePitchRollCurve(controlRateConfig_t *controlRateConfig); +void generateYawCurve(controlRateConfig_t *controlRateConfig); void generateThrottleCurve(controlRateConfig_t *controlRateConfig, escAndServoConfig_t *escAndServoConfig); diff --git a/src/main/io/serial_cli.c b/src/main/io/serial_cli.c index 7862fa4fb..f4497417f 100644 --- a/src/main/io/serial_cli.c +++ b/src/main/io/serial_cli.c @@ -400,6 +400,7 @@ const clivalue_t valueTable[] = { { "default_rate_profile", VAR_UINT8 | PROFILE_VALUE , &masterConfig.profile[0].defaultRateProfileIndex, 0, MAX_CONTROL_RATE_PROFILE_COUNT - 1 }, { "rc_rate", VAR_UINT8 | CONTROL_RATE_VALUE, &masterConfig.controlRateProfiles[0].rcRate8, 0, 250 }, { "rc_expo", VAR_UINT8 | CONTROL_RATE_VALUE, &masterConfig.controlRateProfiles[0].rcExpo8, 0, 100 }, + { "rc_yaw_expo", VAR_UINT8 | CONTROL_RATE_VALUE, &masterConfig.controlRateProfiles[0].rcYawExpo8, 0, 100 }, { "thr_mid", VAR_UINT8 | CONTROL_RATE_VALUE, &masterConfig.controlRateProfiles[0].thrMid8, 0, 100 }, { "thr_expo", VAR_UINT8 | CONTROL_RATE_VALUE, &masterConfig.controlRateProfiles[0].thrExpo8, 0, 100 }, { "roll_rate", VAR_UINT8 | CONTROL_RATE_VALUE, &masterConfig.controlRateProfiles[0].rates[FD_ROLL], 0, CONTROL_RATE_CONFIG_ROLL_PITCH_RATE_MAX }, diff --git a/src/main/io/serial_msp.c b/src/main/io/serial_msp.c index b62ad7893..86042971b 100644 --- a/src/main/io/serial_msp.c +++ b/src/main/io/serial_msp.c @@ -281,7 +281,7 @@ const char *boardIdentifier = TARGET_BOARD_IDENTIFIER; #define MSP_SET_RAW_GPS 201 //in message fix, numsat, lat, lon, alt, speed #define MSP_SET_PID 202 //in message P I D coeff (9 are used currently) #define MSP_SET_BOX 203 //in message BOX setup (number is dependant of your setup) -#define MSP_SET_RC_TUNING 204 //in message rc rate, rc expo, rollpitch rate, yaw rate, dyn throttle PID +#define MSP_SET_RC_TUNING 204 //in message rc rate, rc expo, rollpitch rate, yaw rate, dyn throttle PID, yaw expo #define MSP_ACC_CALIBRATION 205 //in message no param #define MSP_MAG_CALIBRATION 206 //in message no param #define MSP_SET_MISC 207 //in message powermeter trig + 8 free for future use @@ -918,6 +918,7 @@ static bool processOutCommand(uint8_t cmdMSP) serialize8(currentControlRateProfile->thrMid8); serialize8(currentControlRateProfile->thrExpo8); serialize16(currentControlRateProfile->tpa_breakpoint); + serialize8(currentControlRateProfile->rcYawExpo8); break; case MSP_PID: headSerialReply(3 * PID_ITEM_COUNT); @@ -1364,7 +1365,7 @@ static bool processInCommand(void) break; case MSP_SET_RC_TUNING: - if (currentPort->dataSize == 10) {//allow for tpa_breakpoint + if (currentPort->dataSize >= 11) {//allow for yaw expo currentControlRateProfile->rcRate8 = read8(); currentControlRateProfile->rcExpo8 = read8(); for (i = 0; i < 3; i++) { @@ -1376,6 +1377,7 @@ static bool processInCommand(void) currentControlRateProfile->thrMid8 = read8(); currentControlRateProfile->thrExpo8 = read8(); currentControlRateProfile->tpa_breakpoint = read16(); + currentControlRateProfile->rcYawExpo8 = read8(); } else { headSerialError(0); } diff --git a/src/main/mw.c b/src/main/mw.c index b18a8529f..e00de9940 100644 --- a/src/main/mw.c +++ b/src/main/mw.c @@ -209,7 +209,8 @@ void annexCode(void) tmp = 0; } } - rcCommand[axis] = tmp * -masterConfig.yaw_control_direction; + tmp2 = tmp / 100; + rcCommand[axis] = (lookupYawRC[tmp2] + (tmp - tmp2 * 100) * (lookupYawRC[tmp2 + 1] - lookupYawRC[tmp2]) / 100) * -masterConfig.yaw_control_direction; prop1 = 100 - (uint16_t)currentControlRateProfile->rates[axis] * ABS(tmp) / 500; } // FIXME axis indexes into pids. use something like lookupPidIndex(rc_alias_e alias) to reduce coupling.