rusefi
/
atbetaflight
mirror of https://github.com/rusefi/atbetaflight.git
1
0
Fork 0
Code Issues Packages Projects Releases Wiki Activity

Add roll pitch velocity

This commit is contained in:
borisbstyle 2016-08-05 00:55:12 +02:00
parent 06be182e50
commit 38e812a5a5
5 changed files with 17 additions and 12 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

3
src/main/config/config.c
View File

@ -172,7 +172,7 @@ static uint32_t activeFeaturesLatch = 0;
static uint8_t currentControlRateProfileIndex = 0; static uint8_t currentControlRateProfileIndex = 0;
controlRateConfig_t *currentControlRateProfile; 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) static void resetAccelerometerTrims(flightDynamicsTrims_t *accelerometerTrims)
{ {
@ -246,6 +246,7 @@ static void resetPidProfile(pidProfile_t *pidProfile)
pidProfile->ptermSetpointWeight = 75; pidProfile->ptermSetpointWeight = 75;
pidProfile->dtermSetpointWeight = 120; pidProfile->dtermSetpointWeight = 120;
pidProfile->pidMaxVelocityYaw = 200; pidProfile->pidMaxVelocityYaw = 200;
pidProfile->pidMaxVelocityRollPitch = 0;
pidProfile->toleranceBand = 20; pidProfile->toleranceBand = 20;
pidProfile->toleranceBandReduction = 40; pidProfile->toleranceBandReduction = 40;
pidProfile->zeroCrossAllowanceCount = 2; pidProfile->zeroCrossAllowanceCount = 2;

14
src/main/flight/pid.c
View File

@ -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 errorRate = 0, rP = 0, rD = 0, PVRate = 0;
float ITerm,PTerm,DTerm; float ITerm,PTerm,DTerm;
static float lastRateError[2]; 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; float delta;
int axis; int axis;
float horizonLevelStrength = 1; 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; b[axis] = pidProfile->ptermSetpointWeight / 100.0f;
c[axis] = pidProfile->dtermSetpointWeight / 100.0f; c[axis] = pidProfile->dtermSetpointWeight / 100.0f;
yawMaxVelocity = pidProfile->pidMaxVelocityYaw * 1000 * getdT(); yawMaxVelocity = pidProfile->pidMaxVelocityYaw * 1000 * getdT();
rollPitchMaxVelocity = pidProfile->pidMaxVelocityRollPitch * 1000 * getdT();
configP[axis] = pidProfile->P8[axis]; configP[axis] = pidProfile->P8[axis];
configI[axis] = pidProfile->I8[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 // Limit abrupt yaw inputs / stops
if (axis == YAW && pidProfile->pidMaxVelocityYaw) { float maxVelocity = (axis == YAW) ? yawMaxVelocity : rollPitchMaxVelocity;
float yawCurrentVelocity = setpointRate[axis] - yawPreviousRate; if (maxVelocity) {
if (ABS(yawCurrentVelocity) > yawMaxVelocity) { float currentVelocity = setpointRate[axis] - previousSetpoint[axis];
setpointRate[axis] = (yawCurrentVelocity > 0) ? yawPreviousRate + yawMaxVelocity : yawPreviousRate - yawMaxVelocity; 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 // Yaw control is GYRO based, direct sticks control is applied to rate PID

3
src/main/flight/pid.h
View File

@ -100,7 +100,8 @@ typedef struct pidProfile_s {
uint8_t itermThrottleGain; // Throttle coupling to iterm. Quick throttle changes will bump iterm 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 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) 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 #ifdef GTUNE
uint8_t gtune_lolimP[3]; // [0..200] Lower limit of P during G tune uint8_t gtune_lolimP[3]; // [0..200] Lower limit of P during G tune

5
src/main/io/serial_cli.c
View File

@ -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 } }, { "servo_lowpass_enable", VAR_INT8 | MASTER_VALUE | MODE_LOOKUP, &masterConfig.mixerConfig.servo_lowpass_enable, .config.lookup = { TABLE_OFF_ON } },
#endif #endif
{ "rc_rate", VAR_UINT8 | PROFILE_RATE_VALUE, &masterConfig.profile[0].controlRateProfile[0].rcRate8, .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, 250 } }, { "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_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 } }, { "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 } }, { "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 } }, { "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 } }, { "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_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 } }, { "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 } }, { "yaw_iterm_ignore_threshold", VAR_UINT16 | PROFILE_VALUE, &masterConfig.profile[0].pidProfile.yawItermIgnoreRate, .config.minmax = {15, 1000 } },

4
src/main/io/serial_msp.c
View File

@ -1268,8 +1268,8 @@ static bool processOutCommand(uint8_t cmdMSP)
serialize8(currentProfile->pidProfile.toleranceBand); serialize8(currentProfile->pidProfile.toleranceBand);
serialize8(currentProfile->pidProfile.toleranceBandReduction); serialize8(currentProfile->pidProfile.toleranceBandReduction);
serialize8(currentProfile->pidProfile.itermThrottleGain); serialize8(currentProfile->pidProfile.itermThrottleGain);
serialize16(currentProfile->pidProfile.pidMaxVelocityRollPitch);
serialize16(currentProfile->pidProfile.pidMaxVelocityYaw); serialize16(currentProfile->pidProfile.pidMaxVelocityYaw);
serialize16(1000); // Reserved for future
break; break;
case MSP_SENSOR_CONFIG: case MSP_SENSOR_CONFIG:
headSerialReply(3); headSerialReply(3);
@ -1858,8 +1858,8 @@ static bool processInCommand(void)
currentProfile->pidProfile.toleranceBand = read8(); currentProfile->pidProfile.toleranceBand = read8();
currentProfile->pidProfile.toleranceBandReduction = read8(); currentProfile->pidProfile.toleranceBandReduction = read8();
currentProfile->pidProfile.itermThrottleGain = read8(); currentProfile->pidProfile.itermThrottleGain = read8();
currentProfile->pidProfile.pidMaxVelocityRollPitch = read16();
currentProfile->pidProfile.pidMaxVelocityYaw = read16(); currentProfile->pidProfile.pidMaxVelocityYaw = read16();
read16(); // reserved for future purposes
break; break;
case MSP_SET_SENSOR_CONFIG: case MSP_SET_SENSOR_CONFIG:
masterConfig.acc_hardware = read8(); masterConfig.acc_hardware = read8();