From 38e812a5a505323212b8f9ea48c8f484e3d23d10 Mon Sep 17 00:00:00 2001 From: borisbstyle Date: Fri, 5 Aug 2016 00:55:12 +0200 Subject: [PATCH] Add roll pitch velocity --- src/main/config/config.c | 3 ++- src/main/flight/pid.c | 14 ++++++++------ src/main/flight/pid.h | 3 ++- src/main/io/serial_cli.c | 5 +++-- src/main/io/serial_msp.c | 4 ++-- 5 files changed, 17 insertions(+), 12 deletions(-) diff --git a/src/main/config/config.c b/src/main/config/config.c index 2f047d64e..f1a88d02f 100755 --- a/src/main/config/config.c +++ b/src/main/config/config.c @@ -172,7 +172,7 @@ static uint32_t activeFeaturesLatch = 0; static uint8_t currentControlRateProfileIndex = 0; controlRateConfig_t *currentControlRateProfile; -static const uint8_t EEPROM_CONF_VERSION = 142; +static const uint8_t EEPROM_CONF_VERSION = 143; static void resetAccelerometerTrims(flightDynamicsTrims_t *accelerometerTrims) { @@ -246,6 +246,7 @@ static void resetPidProfile(pidProfile_t *pidProfile) pidProfile->ptermSetpointWeight = 75; pidProfile->dtermSetpointWeight = 120; pidProfile->pidMaxVelocityYaw = 200; + pidProfile->pidMaxVelocityRollPitch = 0; pidProfile->toleranceBand = 20; pidProfile->toleranceBandReduction = 40; pidProfile->zeroCrossAllowanceCount = 2; diff --git a/src/main/flight/pid.c b/src/main/flight/pid.c index b405a9ab4..e14742341 100644 --- a/src/main/flight/pid.c +++ b/src/main/flight/pid.c @@ -133,7 +133,7 @@ static void pidBetaflight(const pidProfile_t *pidProfile, uint16_t max_angle_inc float errorRate = 0, rP = 0, rD = 0, PVRate = 0; float ITerm,PTerm,DTerm; static float lastRateError[2]; - static float Kp[3], Ki[3], Kd[3], b[3], c[3], yawMaxVelocity, yawPreviousRate; + static float Kp[3], Ki[3], Kd[3], b[3], c[3], rollPitchMaxVelocity, yawMaxVelocity, previousSetpoint[3]; float delta; int axis; float horizonLevelStrength = 1; @@ -188,6 +188,7 @@ static void pidBetaflight(const pidProfile_t *pidProfile, uint16_t max_angle_inc b[axis] = pidProfile->ptermSetpointWeight / 100.0f; c[axis] = pidProfile->dtermSetpointWeight / 100.0f; yawMaxVelocity = pidProfile->pidMaxVelocityYaw * 1000 * getdT(); + rollPitchMaxVelocity = pidProfile->pidMaxVelocityRollPitch * 1000 * getdT(); configP[axis] = pidProfile->P8[axis]; configI[axis] = pidProfile->I8[axis]; @@ -195,12 +196,13 @@ static void pidBetaflight(const pidProfile_t *pidProfile, uint16_t max_angle_inc } // Limit abrupt yaw inputs / stops - if (axis == YAW && pidProfile->pidMaxVelocityYaw) { - float yawCurrentVelocity = setpointRate[axis] - yawPreviousRate; - if (ABS(yawCurrentVelocity) > yawMaxVelocity) { - setpointRate[axis] = (yawCurrentVelocity > 0) ? yawPreviousRate + yawMaxVelocity : yawPreviousRate - yawMaxVelocity; + float maxVelocity = (axis == YAW) ? yawMaxVelocity : rollPitchMaxVelocity; + if (maxVelocity) { + float currentVelocity = setpointRate[axis] - previousSetpoint[axis]; + if (ABS(currentVelocity) > maxVelocity) { + setpointRate[axis] = (currentVelocity > 0) ? previousSetpoint[axis] + maxVelocity : previousSetpoint[axis] - maxVelocity; } - yawPreviousRate = setpointRate[axis]; + previousSetpoint[axis] = setpointRate[axis]; } // Yaw control is GYRO based, direct sticks control is applied to rate PID diff --git a/src/main/flight/pid.h b/src/main/flight/pid.h index 4f3623c31..9aa625a33 100644 --- a/src/main/flight/pid.h +++ b/src/main/flight/pid.h @@ -100,7 +100,8 @@ typedef struct pidProfile_s { uint8_t itermThrottleGain; // Throttle coupling to iterm. Quick throttle changes will bump iterm uint8_t ptermSetpointWeight; // Setpoint weight for Pterm (lower means more PV tracking) uint8_t dtermSetpointWeight; // Setpoint weight for Dterm (0= measurement, 1= full error, 1 > agressive derivative) - uint16_t pidMaxVelocityYaw; // velocity limiter for pid controller deg/sec/ms + uint16_t pidMaxVelocityYaw; // velocity yaw limiter for pid controller deg/sec/ms + uint16_t pidMaxVelocityRollPitch; // velocity roll/pitch limiter for pid controller deg/sec/ms #ifdef GTUNE uint8_t gtune_lolimP[3]; // [0..200] Lower limit of P during G tune diff --git a/src/main/io/serial_cli.c b/src/main/io/serial_cli.c index 5aef0f539..15de60362 100644 --- a/src/main/io/serial_cli.c +++ b/src/main/io/serial_cli.c @@ -773,8 +773,8 @@ const clivalue_t valueTable[] = { { "servo_lowpass_enable", VAR_INT8 | MASTER_VALUE | MODE_LOOKUP, &masterConfig.mixerConfig.servo_lowpass_enable, .config.lookup = { TABLE_OFF_ON } }, #endif - { "rc_rate", VAR_UINT8 | PROFILE_RATE_VALUE, &masterConfig.profile[0].controlRateProfile[0].rcRate8, .config.minmax = { 0, 250 } }, - { "rc_rate_yaw", VAR_UINT8 | PROFILE_RATE_VALUE, &masterConfig.profile[0].controlRateProfile[0].rcYawRate8, .config.minmax = { 0, 250 } }, + { "rc_rate", VAR_UINT8 | PROFILE_RATE_VALUE, &masterConfig.profile[0].controlRateProfile[0].rcRate8, .config.minmax = { 0, 300 } }, + { "rc_rate_yaw", VAR_UINT8 | PROFILE_RATE_VALUE, &masterConfig.profile[0].controlRateProfile[0].rcYawRate8, .config.minmax = { 0, 300 } }, { "rc_expo", VAR_UINT8 | PROFILE_RATE_VALUE, &masterConfig.profile[0].controlRateProfile[0].rcExpo8, .config.minmax = { 0, 100 } }, { "rc_yaw_expo", VAR_UINT8 | PROFILE_RATE_VALUE, &masterConfig.profile[0].controlRateProfile[0].rcYawExpo8, .config.minmax = { 0, 100 } }, { "thr_mid", VAR_UINT8 | PROFILE_RATE_VALUE, &masterConfig.profile[0].controlRateProfile[0].thrMid8, .config.minmax = { 0, 100 } }, @@ -833,6 +833,7 @@ const clivalue_t valueTable[] = { { "pterm_setpoint_weight", VAR_UINT8 | PROFILE_VALUE, &masterConfig.profile[0].pidProfile.ptermSetpointWeight, .config.minmax = {30, 100 } }, { "dterm_setpoint_weight", VAR_UINT8 | PROFILE_VALUE, &masterConfig.profile[0].pidProfile.dtermSetpointWeight, .config.minmax = {0, 300 } }, { "max_yaw_acceleration", VAR_UINT16 | PROFILE_VALUE, &masterConfig.profile[0].pidProfile.pidMaxVelocityYaw, .config.minmax = {0, 1000 } }, + { "max_acceleration", VAR_UINT16 | PROFILE_VALUE, &masterConfig.profile[0].pidProfile.pidMaxVelocityRollPitch, .config.minmax = {0, 1000 } }, { "iterm_ignore_threshold", VAR_UINT16 | PROFILE_VALUE, &masterConfig.profile[0].pidProfile.rollPitchItermIgnoreRate, .config.minmax = {15, 1000 } }, { "yaw_iterm_ignore_threshold", VAR_UINT16 | PROFILE_VALUE, &masterConfig.profile[0].pidProfile.yawItermIgnoreRate, .config.minmax = {15, 1000 } }, diff --git a/src/main/io/serial_msp.c b/src/main/io/serial_msp.c index b5d170cfb..0d36fda47 100644 --- a/src/main/io/serial_msp.c +++ b/src/main/io/serial_msp.c @@ -1268,8 +1268,8 @@ static bool processOutCommand(uint8_t cmdMSP) serialize8(currentProfile->pidProfile.toleranceBand); serialize8(currentProfile->pidProfile.toleranceBandReduction); serialize8(currentProfile->pidProfile.itermThrottleGain); + serialize16(currentProfile->pidProfile.pidMaxVelocityRollPitch); serialize16(currentProfile->pidProfile.pidMaxVelocityYaw); - serialize16(1000); // Reserved for future break; case MSP_SENSOR_CONFIG: headSerialReply(3); @@ -1858,8 +1858,8 @@ static bool processInCommand(void) currentProfile->pidProfile.toleranceBand = read8(); currentProfile->pidProfile.toleranceBandReduction = read8(); currentProfile->pidProfile.itermThrottleGain = read8(); + currentProfile->pidProfile.pidMaxVelocityRollPitch = read16(); currentProfile->pidProfile.pidMaxVelocityYaw = read16(); - read16(); // reserved for future purposes break; case MSP_SET_SENSOR_CONFIG: masterConfig.acc_hardware = read8();